diff options
Diffstat (limited to 'firmware/common/memset16.c')
-rwxr-xr-x | firmware/common/memset16.c | 79 |
1 files changed, 79 insertions, 0 deletions
diff --git a/firmware/common/memset16.c b/firmware/common/memset16.c new file mode 100755 index 0000000000..0aee97a169 --- /dev/null +++ b/firmware/common/memset16.c | |||
@@ -0,0 +1,79 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006 by Jens Arnold | ||
11 | * | ||
12 | * All files in this archive are subject to the GNU General Public License. | ||
13 | * See the file COPYING in the source tree root for full license agreement. | ||
14 | * | ||
15 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
16 | * KIND, either express or implied. | ||
17 | * | ||
18 | ****************************************************************************/ | ||
19 | |||
20 | #include <string.h> | ||
21 | #define LBLOCKSIZE (sizeof(long)/2) | ||
22 | #define UNALIGNED(X) ((long)X & (LBLOCKSIZE - 1)) | ||
23 | #define TOO_SMALL(LEN) ((LEN) < LBLOCKSIZE) | ||
24 | |||
25 | void *memset16(void *dst, int val, size_t len) | ||
26 | { | ||
27 | #if defined(PREFER_SIZE_OVER_SPEED) || defined(__OPTIMIZE_SIZE__) | ||
28 | unsigned short *p = (unsigned short *)dst; | ||
29 | |||
30 | while (len--) | ||
31 | *p++ = val; | ||
32 | |||
33 | return dst; | ||
34 | #else | ||
35 | unsigned short *p = (unsigned short *)dst; | ||
36 | unsigned int i; | ||
37 | unsigned long buffer; | ||
38 | unsigned long *aligned_addr; | ||
39 | |||
40 | if (!TOO_SMALL(len) && !UNALIGNED(dst)) | ||
41 | { | ||
42 | aligned_addr = (unsigned long *)dst; | ||
43 | |||
44 | val &= 0xffff; | ||
45 | if (LBLOCKSIZE == 2) | ||
46 | { | ||
47 | buffer = (val << 16) | val; | ||
48 | } | ||
49 | else | ||
50 | { | ||
51 | buffer = 0; | ||
52 | for (i = 0; i < LBLOCKSIZE; i++) | ||
53 | buffer = (buffer << 16) | val; | ||
54 | } | ||
55 | |||
56 | while (len >= LBLOCKSIZE*4) | ||
57 | { | ||
58 | *aligned_addr++ = buffer; | ||
59 | *aligned_addr++ = buffer; | ||
60 | *aligned_addr++ = buffer; | ||
61 | *aligned_addr++ = buffer; | ||
62 | len -= 4*LBLOCKSIZE; | ||
63 | } | ||
64 | |||
65 | while (len >= LBLOCKSIZE) | ||
66 | { | ||
67 | *aligned_addr++ = buffer; | ||
68 | len -= LBLOCKSIZE; | ||
69 | } | ||
70 | |||
71 | p = (unsigned short *)aligned_addr; | ||
72 | } | ||
73 | |||
74 | while (len--) | ||
75 | *p++ = val; | ||
76 | |||
77 | return dst; | ||
78 | #endif /* not PREFER_SIZE_OVER_SPEED */ | ||
79 | } | ||