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

#include <kaleid.h>
#include <kallims.h>

//------------------------------------------//
//            memset() family               //
//------------------------------------------//

//
//  Set "bytes"-many bytes starting from ptr to val
//
void *memsetb(void *ptr, int val, size_t bytes)
{
    uchar *uptr = (uchar *)ptr;
    
    // deal with bytes before start of the first aligned qword
    while (((ulong)uptr % QWORD_ALIGN) > 0 && bytes--) {
        *uptr++ = (uchar)val;
    }

    // we're qword-aligned now
    if (bytes > QWORD_SIZE) {
        const ulong uval = ((ulong)val << 56) | ((ulong)val << 48)
                         | ((ulong)val << 40) | ((ulong)val << 32)
                         | ((ulong)val << 24) | ((ulong)val << 16)
                         | ((ulong)val <<  8) | ((ulong)val);

        ulong *uqptr = (ulong *)ptr;
        
        // move qword by qword
        while (bytes > QWORD_SIZE) {
            *uqptr++ = uval;
            bytes -= QWORD_SIZE;
        }
        
        uptr = (uchar *)(ulong)uqptr;
    }
    
    // deal with what's left
    while (bytes--) {
        *uptr++ = (uchar)val;
    }
    
    return ptr;
}

//
//  Set "words"-many words starting from ptr to val
//
void *memsetw(void *ptr, int val, size_t words)
{
    ushort *uptr = (ushort *)ptr;
    
    // can't we do this an aligned way?
    if unlikely (((ulong)uptr % WORD_ALIGN) > 0) {
        // no, we can't align ourselves
        while (words--) {
            // do it the hard way
            *uptr++ = (ushort)val;
        }
        // too bad '-'
        return uptr;
    }
    
    // deal with words before start of the first aligned qword
    while (((ulong)uptr % QWORD_ALIGN) > 0 && words--) {
        *uptr++ = (ushort)val;
    }

    // we're aligned for sure
    if (words > QWORDS_TO_WORDS(1)) {
        const ulong uval = ((ulong)val << 48) | ((ulong)val << 32)
                         | ((ulong)val << 16) | ((ulong)val);

        ulong *uqptr = (ulong *)uptr;

        // move qword by qword
        while (words > QWORDS_TO_WORDS(1)) {
            words -= QWORDS_TO_WORDS(1);
            *uqptr++ = uval;
        }
        
        uptr = (ushort *)(ulong)uqptr;
    }
    
    // deal with what's left
    while (words--) {
        *uptr++ = (ushort)val;
    }
    
    return ptr;
}

//
//  Set "dwords"-many dwords starting from ptr to val
//  XXX unimplemented
//
void *memsetd(void *ptr, int val, size_t dwords)
{
    (void)val;
    (void)dwords;

    return ptr;
}

//
//  Set "qwords"-many qwords starting from ptr to val
//
void *memsetq(void *ptr, long val, size_t qwords)
{
    ulong *uptr = (ulong *)ptr;
    
    while (qwords--) *uptr++ = (ulong)val;
    
    return ptr;
}

//
//  Set "bytes"-many bytes starting from ptr to 0
//  
//  WARNING
//      Assume "bytes" is large, for small sizes
//      use memset(ptr, 0, bytes) directly
//
void *memzero(void *ptr, size_t bytes)
{
    return memsetb(ptr, 0, bytes);
}


//
//  Copy "bytes"-many bytes of src to dst
//  Does not deal with overlapping blocks (memmove's job)
//
void *memcpy(void *dst, const void *src, size_t bytes)
{
    const ulong  *usrc = (const ulong  *)src;
    ulong        *udst = (ulong        *)dst;
    
    if unlikely (bytes == 0) return dst;

    // can we align them both at once?
    if unlikely ((ulong)src % WORD_ALIGN == 1
              && (ulong)dst % WORD_ALIGN == 1) {
        const uchar *ubsrc = (const uchar *)usrc;
        uchar       *ubdst = (uchar       *)udst;

        *ubdst++ = *ubsrc++;
        bytes--;

        udst = (ulong *)ubdst;
        usrc = (ulong *)ubsrc;
    }

    const ushort *uwsrc = (const ushort *)usrc;
    ushort       *uwdst = (ushort       *)udst;

    // align either dst or src for qword access
    while ((ulong)dst % QWORD_ALIGN > 0
        && (ulong)src % QWORD_ALIGN > 0
        && bytes > WORD_SIZE) {

        *uwdst++ = *uwsrc++;
        bytes -= WORD_SIZE;
    }

    udst = (ulong *)uwdst;
    usrc = (ulong *)uwsrc;

    // should be most of the job
    while (bytes > QWORD_SIZE) {
        *udst++ = *usrc++;
        bytes -= QWORD_SIZE;
    }

    const uchar  *ubsrc = (const uchar  *)usrc;
    ushort       *ubdst = (ushort       *)udst;

    // deal with what's left
    while (bytes--) {
        *ubdst ++ = *ubsrc++;
    }

    return dst;
}