//----------------------------------------------------------------------------//
//                           GNU GPL OS/K                                     //
//                                                                            //
//  Authors:    spectral`                                                     //
//              NeoX                                                          //
//                                                                            //
//  Desc:       mem*() functions                                              //
//----------------------------------------------------------------------------//

// XXX to be improved before being brought back
// to be tested with more alignment sizes

#include <kaleid/common/stdlib.h>

// to be moved
#define QWORD_SIZE 8
#define QWORD_ALIGN 8

//
//  Set "qwords"-many aligned qwords starting from ptr to val
//
static inline void *memsetq(void *ptr, ullong uval, size_t qwords)
{
    size_t n;
    ullong *uptr = (ullong *)ptr;

    // aligned memory write
    for (n = 0; n < qwords; n++) {
        *(uptr + n) = uval;
    }

    return ptr;
}

//
//  Set "bytes"-many bytes starting from ptr to val
//
void *memset(void *ptr, int val, size_t bytes)
{
    uchar *uptr = (uchar *)ptr;
    const size_t qwords = bytes/QWORD_SIZE;
    
    // get rid of everything after the first byte
    val = val & 0xFF;
    
    // deal with bytes before start of the first aligned qword
    while (((ullong)uptr % QWORD_ALIGN) > 0 && bytes--) {
        *uptr++ = (uchar)val;
    }

    // move qword by qword
    if (qwords) {
        const ullong uval = ((ullong)val << 56) | ((ullong)val << 48)
                          | ((ullong)val << 40) | ((ullong)val << 32)
                          | ((ullong)val << 24) | ((ullong)val << 16)
                          | ((ullong)val <<  8) | ((ullong)val);

        memsetq(uptr, uval, qwords);
        
        uptr += qwords * QWORD_SIZE;
        bytes %= QWORD_SIZE;
    }
    
    // deal with what's left
    while (bytes--) {
        *uptr++ = (uchar)val;
    }
    
    return ptr;
}

//
//  Set "bytes"-many bytes starting from ptr to 0
//
void *memzero(void *ptr, size_t bytes)
{
    return memset(ptr, 0, bytes);
}