common/lib/libc/arch/or1k/string/memcpy.c

   1 /* $NetBSD: memcpy.c,v 1.1 2014/09/03 19:34:25 matt Exp $ */
   2 /*-
   3  * Copyright (c) 2014 The NetBSD Foundation, Inc.
   4  * All rights reserved.
   5  *
   6  * This code is derived from software contributed to The NetBSD Foundation
   7  * by Matt Thomas of 3am Software Foundry.
   8  *
   9  * Redistribution and use in source and binary forms, with or without
  10  * modification, are permitted provided that the following conditions
  11  * are met:
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions and the following disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in the
  16  *    documentation and/or other materials provided with the distribution.
  17  *
  18  * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
  19  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  20  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  21  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
  22  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  28  * POSSIBILITY OF SUCH DAMAGE.
  29  */
  30
  31 #include <sys/cdefs.h>
  32
  33 __RCSID("$NetBSD: memcpy.c,v 1.1 2014/09/03 19:34:25 matt Exp $");
  34
  35 #include <stddef.h>
  36 #include <stdint.h>
  37 #include <string.h>
  38
  39 static inline unsigned long
  40 combine_words(unsigned long w1, unsigned long w2, int shift1, int shift2)
  41 {
  42 #if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
  43         return (w1 << shift1) | (w2 >> shift2);
  44 #else
  45         return (w1 >> shift1) | (w2 << shift2);
  46 #endif
  47 }
  48
  49 void *
  50 memcpy(void * restrict a, const void * restrict b, size_t len)
  51 {
  52         const unsigned char *cb = b;
  53         unsigned char *ca = a;
  54
  55         if (len == 0)
  56                 return a;
  57
  58         /*
  59          * Make sure the destination is long aligned.
  60          */
  61         while ((uintptr_t)ca & (sizeof(long) - 1)) {
  62                 *ca++ = *cb++;
  63                 if (--len == 0)
  64                         return a;
  65         }
  66
  67         unsigned long *la = (long *)ca;
  68         const int offset = (uintptr_t)cb & (sizeof(*la) - 1);
  69         const unsigned long *lb = (const unsigned long *) (cb - offset);
  70         unsigned long * const ea = la + len / sizeof(*la);
  71
  72         if (offset == 0) {
  73                 /*
  74                  * a & b are now both long alignment.
  75                  * First try to copy 4 longs at a time,
  76                  */
  77                 for (; la + 4 <= ea; la += 4, lb += 4) {
  78                         la[0] = lb[0];
  79                         la[1] = lb[1];
  80                         la[2] = lb[2];
  81                         la[3] = lb[3];
  82                 }
  83                 /*
  84                  * Now try to copy one long at a time.
  85                  */
  86                 while (la <= ea) {
  87                         *la++ = *lb++;
  88                 }
  89         } else {
  90                 const int shift1 = offset * 8;
  91                 const int shift2 = sizeof(*la) * 8 - shift1;
  92                 unsigned long w1 = *lb++;
  93
  94                 /*
  95                  * We try to write 4 words per loop.
  96                  */
  97                 for (; la + 4 <= ea; la += 4, lb += 4) {
  98                         unsigned long w2 = lb[0];
  99
 100                         la[0] = combine_words(w1, w2, shift1, shift2);
 101
 102                         w1 = lb[1];
 103
 104                         la[1] = combine_words(w2, w1, shift1, shift2);
 105
 106                         w2 = lb[2];
 107
 108                         la[2] = combine_words(w1, w2, shift1, shift2);
 109
 110                         w1 = lb[3];
 111
 112                         la[3] = combine_words(w2, w1, shift1, shift2);
 113                 }
 114
 115                 /*
 116                  * Now try to copy one long at a time.
 117                  */
 118                 while (la <= ea) {
 119                         unsigned long w2 = *lb++;
 120
 121                         *la++ = combine_words(w1, w2, shift1, shift2);
 122
 123                         w1 = w2;
 124                 }
 125         }
 126         len &= sizeof(*la) - 1;
 127         if (len) {
 128                 cb = (const unsigned char *)lb + offset;
 129                 ca = (unsigned char *)la;
 130                 while (len-- > 0) {
 131                         *ca++ = *cb++;
 132                 }
 133         }
 134         return a;
 135 }