1 /*
2 * Copyright (c) 2017 Imagination Technologies.
3 *
4 * All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 *
10 * * Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * * Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer
14 * in the documentation and/or other materials provided with
15 * the distribution.
16 * * Neither the name of Imagination Technologies nor the names of its
17 * contributors may be used to endorse or promote products derived
18 * from this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
24 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <string.h>
34
35 #define op_t unsigned long int
36 #define op_size sizeof (op_t)
37
38 #if __mips64 || __mips_isa_rev >= 2
39 static inline size_t __attribute__ ((always_inline))
do_bytes(const char * base,const char * p,op_t inval)40 do_bytes (const char *base, const char *p, op_t inval)
41 {
42 op_t outval = 0;
43 #if __mips64
44 __asm__ volatile (
45 "dsbh %1, %0 \n\t"
46 "dshd %0, %1 \n\t"
47 "dclz %1, %0 \n\t"
48 : "+r" (inval), "+r" (outval)
49 );
50 #else
51 __asm__ volatile (
52 "wsbh %1, %0 \n\t"
53 "rotr %0, %1, 16 \n\t"
54 "clz %1, %0 \n\t"
55 : "+r" (inval), "+r" (outval)
56 );
57 #endif
58 p += (outval >> 3);
59 return (size_t) (p - base);
60 }
61
62 #define DO_WORD(in, val) { \
63 op_t tmp = ((val - mask_1) & ~val) & mask_128; \
64 if (tmp) \
65 return do_bytes(str, (const char *)(in), tmp); \
66 }
67 #else
68 static inline size_t __attribute__ ((always_inline))
do_bytes(const char * base,const char * p)69 do_bytes (const char *base, const char *p)
70 {
71 for (; *p; ++p);
72 return (size_t) (p - base);
73 }
74
75 #define DO_WORD(in, val) { \
76 if (((val - mask_1) & ~val) & mask_128) { \
77 return do_bytes(str, (const char *)(in)); \
78 } \
79 }
80 #endif
81
strnlen(const char * str,size_t n)82 size_t strnlen (const char *str, size_t n) {
83 if (n != 0) {
84 const char *p = (const char *) str;
85 const op_t *w;
86 op_t mask_1, mask_128;
87
88 for (; n > 0 && ((size_t) p % op_size) != 0; --n, ++p) {
89 if (!(*p))
90 return (p - str);
91 }
92
93 w = (const op_t *) p;
94
95 __asm__ volatile (
96 "li %0, 0x01010101 \n\t"
97 : "=r" (mask_1)
98 );
99 #if __mips64
100 mask_1 |= mask_1 << 32;
101 #endif
102 mask_128 = mask_1 << 7;
103
104 /*
105 * Check op_size byteswize after initial alignment
106 */
107 while (n >= 4 * op_size) {
108 const op_t w0 = w[0];
109 const op_t w1 = w[1];
110 const op_t w2 = w[2];
111 const op_t w3 = w[3];
112 DO_WORD(w + 0, w0)
113 DO_WORD(w + 1, w1)
114 DO_WORD(w + 2, w2)
115 DO_WORD(w + 3, w3)
116 w += 4;
117 n -= 4 * op_size;
118 }
119
120 while (n >= op_size) {
121 DO_WORD(w, w[0]);
122 w++;
123 n -= op_size;
124 }
125
126 /*
127 * Check bytewize for remaining bytes
128 */
129 p = (const char *) w;
130 for (; n > 0; --n, ++p) {
131 if (!(*p))
132 return (p - str);
133 }
134
135 return (p - str);
136 }
137
138 return 0;
139 }
140