//----------------------------------------------------------------------------//
//                           GNU GPL OS/K                                     //
//                                                                            //
//  Desc:       mem*() family                                                 //
//                                                                            //
//                                                                            //
//  Copyright © 2018-2019 The OS/K Team                                       //
//                                                                            //
//  This file is part of OS/K.                                                //
//                                                                            //
//  OS/K is free software: you can redistribute it and/or modify              //
//  it under the terms of the GNU General Public License as published by      //
//  the Free Software Foundation, either version 3 of the License, or         //
//  any later version.                                                        //
//                                                                            //
//  OS/K is distributed in the hope that it will be useful,                   //
//  but WITHOUT ANY WARRANTY//without even the implied warranty of            //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             //
//  GNU General Public License for more details.                              //
//                                                                            //
//  You should have received a copy of the GNU General Public License         //
//  along with OS/K.  If not, see <https://www.gnu.org/licenses/>.            //
//----------------------------------------------------------------------------//

#include <kalbase.h>
#include <extras/malloc.h>

/* DO NOT compile with strict aliasing on */

//------------------------------------------//
//            Memory allocation             //
//------------------------------------------//

void *malloc(size_t n)
{
    void *ptr;
    error_t rc;

    rc = KalAllocMemory(&ptr, n, 0, 0);
    __set_errno(rc);
   (void)rc;

    return ptr;
}

void free(void *ptr)
{
    error_t rc = KalFreeMemory(ptr);
    (void)rc;
}

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

//
// Set "bytes"-many bytes starting from ptr to val
//
void *memset(void *ptr, int val, size_t bytes)
{
    uchar *uptr = (uchar *)ptr;

    // Deal with bytes before start of the first aligned qword
    while (((ulong)uptr % alignof(QWORD)) > 0 && bytes--) {
        *uptr++ = (uchar)val;
    }

    // At this point we're qword-aligned
    if (bytes > sizeof(QWORD)) {
        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;

        // Moving fast, qword by qword
        while (bytes > sizeof(QWORD)) {
            bytes -= sizeof(QWORD);
            *uqptr++ = uval;
        }

        uptr = (uchar *)(ulong)uqptr;
    }

    // Deal with the few remaining bytes
    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;

    // Check whether we can we do this a word-aligned way
    if unlikely (((ulong)uptr % alignof(WORD)) > 0) {
        // We can't, so we write word by word all the way up
        while (words--) *uptr++ = (ushort)val;
        return uptr;
    }

    while (((ulong)uptr % alignof(QWORD)) > 0 && words--) {
        *uptr++ = (ushort)val;
    }

    if (words > QWORDS_TO_WORDS(1)) {
        const ulong uval = ((ulong)val << 48) | ((ulong)val << 32)
                         | ((ulong)val << 16) | ((ulong)val);

        ulong *uqptr = (ulong *)uptr;

        while (words > QWORDS_TO_WORDS(1)) {
            words -= QWORDS_TO_WORDS(1);
            *uqptr++ = uval;
        }

        uptr = (ushort *)(ulong)uqptr;
    }

    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;
    __set_errno(ENOSYS);
    return ptr;
}

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

    // There's no need to check for alignment
    while (qwords--) *uptr++ = (ulong)val;

    return ptr;
}

//------------------------------------------//
//          Other mem*() functions          //
//------------------------------------------//

//
// Set "bytes"-many bytes starting from ptr to 0
//
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 *restrict dst, const void *restrict src, size_t bytes)
{
    const ulong  *usrc = (const ulong  *)src;
    ulong        *udst = (ulong        *)dst;

    if unlikely (bytes == 0) return dst;

    // Can align both src and dst at once at once?
    if unlikely ((ulong)src % alignof(WORD) == 1
              && (ulong)dst % alignof(WORD) == 1) {
        const uchar *ubsrc = (const uchar *)usrc;
        uchar       *ubdst = (uchar       *)udst;

        // Yes we can, we're guaranteed to be word-aligned after that
        *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 % alignof(QWORD) > 0
        && (ulong)src % alignof(QWORD) > 0
        && bytes > sizeof(WORD)) {

        bytes -= sizeof(WORD);
        *uwdst++ = *uwsrc++;
    }

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

    // Copy fast
    while (bytes > sizeof(QWORD)) {
        bytes -= sizeof(QWORD);
        *udst++ = *usrc++;
    }

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

    // Deal with the few bytes left
    while (bytes--) *ubdst ++ = *ubsrc++;

    return dst;
}

//
// Move memory from src to dest, even if they overlap
//
void *memmove(void *dst, const void *src, size_t bytes)
{
    const uchar *usrc = src;
    uchar       *udst = dst;

    // Can we use memcpy() safely?
    if (udst < usrc) {
        return memcpy(dst, src, bytes);
    }

    // No, so we go backwards
    uchar *usrc_end = (uchar *)usrc + bytes - 1;
    uchar *udst_end = udst + bytes - 1;
    while (bytes--) *udst_end-- = *usrc_end--;

    return dst;
}

//
// Compare memory areas
//
int memcmp(const void *ptr1, const void *ptr2, size_t bytes)
{
    const uchar *uptr1 = ptr1;
    const uchar *uptr2 = ptr2;

    while (bytes--) {
        if (*uptr1++ != *uptr2++) {
            return uptr1[-1] < uptr2[-1] ? -1 : 1;
        }
    }

    return 0;
}