1
0
mirror of https://gitlab.os-k.eu/os-k-team/os-k.git synced 2023-08-25 14:03:10 +02:00
os-k/kaleid/crtlib/memory.c
2019-02-06 14:07:38 +01:00

214 lines
5.5 KiB
C

//----------------------------------------------------------------------------//
// GNU GPL OS/K //
// //
// Authors: spectral` //
// NeoX //
// //
// Desc: mem*() functions //
//----------------------------------------------------------------------------//
#include <kalbase.h>
//------------------------------------------//
// 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;
}