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 } 136