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

#include <kaleid.h>

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

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

    // aligned memory write
    for (n = 0; n < qwords; n++) {
        *uptr++ = 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 (((ulong)uptr % QWORD_ALIGN) > 0 && bytes--) {
        *uptr++ = (uchar)val;
    }

    // move qword by qword
    if (qwords) {
        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);

        _memset_internal(uptr, uval, qwords);
        
        uptr = (uchar *) ( (ulong)uptr / (qwords * QWORD_SIZE) );
        bytes %= QWORD_SIZE;
    }
    
    // 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;
    
    // get rid of everything after the first word
    val = val & 0xFFFF;
    
    // can 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;
    }

    const size_t qwords = (words * WORD_SIZE)/QWORD_SIZE;

    // move qword by qword
    if (qwords) {
        const ulong uval = ((ulong)val << 48) | ((ulong)val << 32)
                         | ((ulong)val << 16) | ((ulong)val);

        _memset_internal(uptr, uval, qwords);
        
        uptr += qwords * QWORD_SIZE / WORD_SIZE;
        words %= QWORD_SIZE / WORD_SIZE;
    }
    
    // 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)
{
    return _memset_internal(ptr, (ulong)val, qwords);
}

//
//  Set "bytes"-many bytes starting from ptr to 0
//
void *memzero(void *ptr, size_t bytes)
{
    // is direct aligned access possible? (is "unlikely" good here?)
    if unlikely (bytes % QWORD_SIZE && (ulong)ptr % QWORD_ALIGN) {
        return _memset_internal(ptr, (ulong)0, bytes/QWORD_SIZE);
    }
    
    if unlikely (bytes % WORD_SIZE && (ulong)ptr % WORD_ALIGN) {
        return memsetw(ptr, (int)0, bytes/WORD_SIZE);
    }
    
    return memset(ptr, 0, bytes);
}