/* $NetBSD: memcpy.c,v 1.1 2014/09/03 19:34:25 matt Exp $ */
/*-
 * Copyright (c) 2014 The NetBSD Foundation, Inc.
 * All rights reserved.
 *
 * This code is derived from software contributed to The NetBSD Foundation
 * by Matt Thomas of 3am Software Foundry.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/cdefs.h>

__RCSID("$NetBSD: memcpy.c,v 1.1 2014/09/03 19:34:25 matt Exp $");

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

static inline unsigned long
combine_words(unsigned long w1, unsigned long w2, int shift1, int shift2)
{
#if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
        return (w1 << shift1) | (w2 >> shift2);
#else
        return (w1 >> shift1) | (w2 << shift2);
#endif
}

void *
memcpy(void * restrict a, const void * restrict b, size_t len)
{
        const unsigned char *cb = b;
        unsigned char *ca = a;

        if (len == 0)
                return a;

        /*
         * Make sure the destination is long aligned.
         */
        while ((uintptr_t)ca & (sizeof(long) - 1)) {
                *ca++ = *cb++;
                if (--len == 0)
                        return a;
        }

        unsigned long *la = (long *)ca;
        const int offset = (uintptr_t)cb & (sizeof(*la) - 1);
        const unsigned long *lb = (const unsigned long *) (cb - offset);
        unsigned long * const ea = la + len / sizeof(*la);

        if (offset == 0) {
                /*
                 * a & b are now both long alignment.
                 * First try to copy 4 longs at a time,
                 */
                for (; la + 4 <= ea; la += 4, lb += 4) {
                        la[0] = lb[0];
                        la[1] = lb[1];
                        la[2] = lb[2];
                        la[3] = lb[3];
                }
                /*
                 * Now try to copy one long at a time.
                 */
                while (la <= ea) {
                        *la++ = *lb++;
                }
        } else {
                const int shift1 = offset * 8;
                const int shift2 = sizeof(*la) * 8 - shift1;
                unsigned long w1 = *lb++;

                /*
                 * We try to write 4 words per loop.
                 */
                for (; la + 4 <= ea; la += 4, lb += 4) {
                        unsigned long w2 = lb[0];

                        la[0] = combine_words(w1, w2, shift1, shift2);

                        w1 = lb[1];

                        la[1] = combine_words(w2, w1, shift1, shift2);

                        w2 = lb[2];

                        la[2] = combine_words(w1, w2, shift1, shift2);

                        w1 = lb[3];

                        la[3] = combine_words(w2, w1, shift1, shift2);
                }

                /*
                 * Now try to copy one long at a time.
                 */
                while (la <= ea) {
                        unsigned long w2 = *lb++;

                        *la++ = combine_words(w1, w2, shift1, shift2);

                        w1 = w2;
                }
        }
        len &= sizeof(*la) - 1;
        if (len) {
                cb = (const unsigned char *)lb + offset;
                ca = (unsigned char *)la;
                while (len-- > 0) {
                        *ca++ = *cb++;
                }
        }
        return a;
}