/* Copyright (c) 2005-2013 Rich Felker. All rights reserved.
 * Copyright (c) 2013 The F9 Microkernel Project. All rights reserved.
 * Use of this source code is governed by MIT license that can be found
 * in the LICENSE file.
 */

#include <stdint.h>
#include <string.h>

void *memcpy(void *restrict dest, const void *restrict src, size_t n)
{
	unsigned char *d = dest;
	const unsigned char *s = src;

#ifdef __GNUC__

#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
#define LS >>
#define RS <<
#else
#define LS <<
#define RS >>
#endif

	typedef uint32_t __attribute__((__may_alias__)) u32;
	uint32_t w, x;

	for (; (uintptr_t) s % 4 && n; n--)
		*d++ = *s++;

	if ((uintptr_t) d % 4 == 0) {
		for (; n >= 16; s += 16, d += 16, n -= 16) {
			*(u32 *)(d + 0) = *(u32 *)(s + 0);
			*(u32 *)(d + 4) = *(u32 *)(s + 4);
			*(u32 *)(d + 8) = *(u32 *)(s + 8);
			*(u32 *)(d + 12) = *(u32 *)(s + 12);
		}
		if (n & 8) {
			*(u32 *)(d + 0) = *(u32 *)(s + 0);
			*(u32 *)(d + 4) = *(u32 *)(s + 4);
			d += 8;
			s += 8;
		}
		if (n & 4) {
			*(u32 *)(d + 0) = *(u32 *)(s + 0);
			d += 4;
			s += 4;
		}
		if (n & 2) {
			*d++ = *s++;
			*d++ = *s++;
		}
		if (n & 1) {
			*d = *s;
		}
		return dest;
	}

	if (n >= 32)
		switch ((uintptr_t) d % 4) {
		case 1:
			w = *(u32 *) s;
			*d++ = *s++;
			*d++ = *s++;
			*d++ = *s++;
			n -= 3;
			for (; n >= 17; s += 16, d += 16, n -= 16) {
				x = *(u32 *)(s + 1);
				*(u32 *)(d + 0) = (w LS 24) | (x RS 8);
				w = *(u32 *)(s + 5);
				*(u32 *)(d + 4) = (x LS 24) | (w RS 8);
				x = *(u32 *)(s + 9);
				*(u32 *)(d + 8) = (w LS 24) | (x RS 8);
				w = *(u32 *)(s + 13);
				*(u32 *)(d + 12) = (x LS 24) | (w RS 8);
			}
			break;
		case 2:
			w = *(u32 *) s;
			*d++ = *s++;
			*d++ = *s++;
			n -= 2;
			for (; n >= 18; s += 16, d += 16, n -= 16) {
				x = *(u32 *)(s + 2);
				*(u32 *)(d + 0) = (w LS 16) | (x RS 16);
				w = *(u32 *)(s + 6);
				*(u32 *)(d + 4) = (x LS 16) | (w RS 16);
				x = *(u32 *)(s + 10);
				*(u32 *)(d + 8) = (w LS 16) | (x RS 16);
				w = *(u32 *)(s + 14);
				*(u32 *)(d + 12) = (x LS 16) | (w RS 16);
			}
			break;
		case 3:
			w = *(u32 *) s;
			*d++ = *s++;
			n -= 1;
			for (; n >= 19; s += 16, d += 16, n -= 16) {
				x = *(u32 *)(s + 3);
				*(u32 *)(d + 0) = (w LS 8) | (x RS 24);
				w = *(u32 *)(s + 7);
				*(u32 *)(d + 4) = (x LS 8) | (w RS 24);
				x = *(u32 *)(s + 11);
				*(u32 *)(d + 8) = (w LS 8) | (x RS 24);
				w = *(u32 *)(s + 15);
				*(u32 *)(d + 12) = (x LS 8) | (w RS 24);
			}
			break;
		}
	if (n & 16) {
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
	}
	if (n & 8) {
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
	}
	if (n & 4) {
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
	}
	if (n & 2) {
		*d++ = *s++;
		*d++ = *s++;
	}
	if (n & 1) {
		*d = *s;
	}
	return dest;
#endif

	for (; n; n--)
		*d++ = *s++;

	return dest;
}