/*
 * Simple string operations using longword operations.
 * The ones in newlib-nano are bytewise...
 */
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include "fw.h"

void *memset(void *s, int c, size_t n)
{
    xptr_t q;
    uint32_t cc;

    if (unlikely(!n))
	return s;

    cc = (uint8_t)c;
    cc = cc | (cc << 16);
    cc = cc | (cc << 8);
    q.b = s;

    if (unlikely(q.a & 1)) {
	*q.b++ = cc;
	n--;
    }

    if (likely(n >= 2)) {
	if (unlikely(q.a & 2)) {
	    *q.w++ = cc;
	    n -= 2;
	}

	while (n >= 4*8) {
	    q.l[0] = cc;
	    q.l[1] = cc;
	    q.l[2] = cc;
	    q.l[3] = cc;
	    q.l[4] = cc;
	    q.l[5] = cc;
	    q.l[6] = cc;
	    q.l[7] = cc;
	    q.l += 8;
	    n -= 32;
	}
	while (n >= 4) {
	    *q.l++ = cc;
	    n -= 4;
	}

	if (unlikely(n & 2))
	    *q.w++ = c;
    }

    if (unlikely(n & 1))
	*q.b++ = c;

    return s;
}
