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
combine_words(unsigned long w1,unsigned long w2,int shift1,int shift2)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 *
memcpy(void * restrict a,const void * restrict b,size_t len)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