kern/src/string.c

   1 // Basic string routines.  Not hardware optimized, but not shabby.
   2
   3 #include <stdio.h>
   4 #include <string.h>
   5 #include <ros/memlayout.h>
   6 #include <assert.h>
   7
   8 int
   9 strlen(const char *s)
  10 {
  11         int n;
  12
  13         for (n = 0; *s != '\0'; s++)
  14                 n++;
  15         return n;
  16 }
  17
  18 int
  19 strnlen(const char *s, size_t size)
  20 {
  21         int n;
  22
  23         for (n = 0; size > 0 && *s != '\0'; s++, size--)
  24                 n++;
  25         return n;
  26 }
  27
  28 /* zra: These aren't being used, and they are dangerous, so I'm rm'ing them
  29 char *
  30 strcpy(char *dst, const char *src)
  31 {
  32         char *ret;
  33
  34         ret = dst;
  35         while ((*dst++ = *src++) != '\0')
  36                 ;
  37         return ret;
  38 }
  39
  40 char *
  41 strcat(char *dst, const char *src)
  42 {
  43         strcpy(dst+strlen(dst),src);
  44         return dst;
  45 }
  46 */
  47
  48 char *
  49 strncpy(char *dst, const char *src, size_t size) {
  50         size_t i;
  51         char *ret;
  52
  53         ret = dst;
  54         for (i = 0; i < size; i++) {
  55                 *dst++ = *src;
  56                 // If strlen(src) < size, null-pad 'dst' out to 'size' chars
  57                 if (*src != '\0')
  58                         src++;
  59         }
  60         return ret;
  61 }
  62
  63 size_t
  64 strlcpy(char *dst, const char *src, size_t size)
  65 {
  66         if (size > 0) {
  67                 while (--size > 0 && *src != '\0')
  68                         *dst++ = *src++;
  69                 *dst = '\0';
  70         }
  71
  72         return strlen(src);
  73 }
  74
  75 size_t
  76 strlcat(char *dst, const char *src, size_t size)
  77 {
  78         size_t rem;     /* Buffer space remaining after null in dst. */
  79
  80         /* We must find the terminating NUL byte in dst, but abort the
  81          * search if we go past 'size' bytes.  At the end of this loop,
  82          * 'dst' will point to either the NUL byte in the original
  83          * destination or to one byte beyond the end of the buffer.
  84          *
  85          * 'rem' will be the amount of 'size' remaining beyond the NUL byte;
  86          * potentially zero. This implies that 'size - rem' is equal to the
  87          * distance from the beginning of the destination buffer to 'dst'.
  88          *
  89          * The return value of strlcat is the sum of the length of the
  90          * original destination buffer (size - rem) plus the size of the
  91          * src string (the return value of strlcpy). */
  92         rem = size;
  93         while ((rem > 0) && (*dst != '\0')) {
  94                 rem--;
  95                 dst++;
  96         }
  97
  98         return (size - rem) + strlcpy(dst, src, rem);
  99 }
 100
 101 int
 102 strcmp(const char *p, const char *q)
 103 {
 104         while (*p && *p == *q)
 105                 p++, q++;
 106         return (int) ((unsigned char) *p - (unsigned char) *q);
 107 }
 108
 109 int
 110 strncmp(const char *p, const char *q, size_t n)
 111 {
 112         while (n > 0 && *p && *p == *q)
 113                 n--, p++, q++;
 114         if (n == 0)
 115                 return 0;
 116         else
 117                 return (int) ((unsigned char) *p - (unsigned char) *q);
 118 }
 119
 120 // Return a pointer to the first occurrence of 'c' in 's',
 121 // or a null pointer if the string has no 'c'.
 122 char *
 123 strchr(const char *s, char c)
 124 {
 125         for (; *s; s++)
 126                 if (*s == c)
 127                         return (char *) s;
 128         return 0;
 129 }
 130
 131 // Return a pointer to the last occurrence of 'c' in 's',
 132 // or a null pointer if the string has no 'c'.
 133 char *
 134 strrchr(const char *s, char c)
 135 {
 136         char *lastc = NULL;
 137         for (; *s; s++)
 138                 if (*s == c){
 139                         lastc = (char*)s;
 140                 }
 141         return lastc;
 142 }
 143
 144 void *memchr(const void *mem, int chr, int len)
 145 {
 146         char *s = (char*) mem;
 147
 148         for (int i = 0; i < len; i++)
 149                 if (s[i] == (char) chr)
 150                         return s + i;
 151         return NULL;
 152 }
 153
 154 // Return a pointer to the first occurrence of 'c' in 's',
 155 // or a pointer to the string-ending null character if the string has no 'c'.
 156 char *
 157 strfind(const char *s, char c)
 158 {
 159         for (; *s; s++)
 160                 if (*s == c)
 161                         break;
 162         return (char *) s;
 163 }
 164
 165 // memset aligned words.
 166 static inline void *
 167 memsetw(long* _v, long c, size_t n)
 168 {
 169         long *start, *end, *v;
 170
 171         start = _v;
 172         end = _v + n/sizeof(long);
 173         v = _v;
 174         c = c & 0xff;
 175         c = c | c<<8;
 176         c = c | c<<16;
 177         #if NUM_ADDR_BITS == 64
 178         c = c | c<<32;
 179         #elif NUM_ADDR_BITS != 32
 180         # error
 181         #endif
 182
 183         while(v < end - (8-1))
 184         {
 185                 v[3] = v[2] = v[1] = v[0] = c;
 186                 v += 4;
 187                 v[3] = v[2] = v[1] = v[0] = c;
 188                 v += 4;
 189         }
 190
 191         while(v < end)
 192           *v++ = c;
 193
 194         return start;
 195 }
 196
 197 // copy aligned words.
 198 // unroll 9 ways to get multiple misses in flight
 199 #define memcpyw(type, _dst, _src, n) \
 200   do { \
 201         type* restrict src = (type*)(_src); \
 202         type* restrict dst = (type*)(_dst); \
 203         type* srcend = src + (n)/sizeof(type); \
 204         type* dstend = dst + (n)/sizeof(type); \
 205         while (dst < dstend - (9-1)) { \
 206                 dst[0] = src[0]; \
 207                 dst[1] = src[1]; \
 208                 dst[2] = src[2]; \
 209                 dst[3] = src[3]; \
 210                 dst[4] = src[4]; \
 211                 dst[5] = src[5]; \
 212                 dst[6] = src[6]; \
 213                 dst[7] = src[7]; \
 214                 dst[8] = src[8]; \
 215                 src += 9; \
 216                 dst += 9; \
 217         } \
 218         while(dst < dstend) \
 219           *dst++ = *src++; \
 220   } while(0)
 221
 222 void *
 223 memset(void *v, int c, size_t _n)
 224 {
 225         char *p;
 226         size_t n0;
 227         size_t n = _n;
 228
 229         if (n == 0) return NULL; // zra: complain here?
 230
 231         p = v;
 232
 233     while (n > 0 && ((uintptr_t)p & (sizeof(long)-1)))
 234         {
 235                 *p++ = c;
 236                 n--;
 237         }
 238
 239         if (n >= sizeof(long))
 240         {
 241                 n0 = n / sizeof(long) * sizeof(long);
 242                 memsetw((long*)p, c, n0);
 243                 n -= n0;
 244                 p += n0;
 245         }
 246
 247         while (n > 0)
 248         {
 249                 *p++ = c;
 250                 n--;
 251         }
 252
 253         return v;
 254 }
 255
 256 void *
 257 memcpy(void* dst, const void* src, size_t _n)
 258 {
 259         const char* s;
 260         char* d;
 261         size_t n0 = 0;
 262         size_t n = _n;
 263         int align = sizeof(long)-1;
 264
 265         s = src;
 266         d = dst;
 267
 268         if ((((uintptr_t)s | (uintptr_t)d) & (sizeof(long)-1)) == 0)
 269         {
 270                 n0 = n / sizeof(long) * sizeof(long);
 271                 memcpyw(long, d, s, n0);
 272         }
 273         else if ((((uintptr_t)s | (uintptr_t)d) & (sizeof(int)-1)) == 0)
 274         {
 275                 n0 = n / sizeof(int) * sizeof(int);
 276                 memcpyw(int, d, s, n0);
 277         }
 278         else if ((((uintptr_t)s | (uintptr_t)d) & (sizeof(short)-1)) == 0)
 279         {
 280                 n0 = n / sizeof(short) * sizeof(short);
 281                 memcpyw(short, d, s, n0);
 282         }
 283
 284         n -= n0;
 285         s += n0;
 286         d += n0;
 287
 288         while (n-- > 0)
 289                 *d++ = *s++;
 290
 291         return dst;
 292 }
 293
 294 void *
 295 memmove(void *dst, const void *src, size_t _n)
 296 {
 297 #ifdef CONFIG_X86
 298         bcopy(src, dst, _n);
 299         return dst;
 300 #else
 301         const char *s;
 302         char *d;
 303         size_t n = _n;
 304
 305         s = src;
 306         d = dst;
 307         if (s < d && s + n > d) {
 308                 s += n;
 309                 d += n;
 310                 while (n-- > 0)
 311                         *--d = *--s;
 312         } else
 313                 while (n-- > 0)
 314                         *d++ = *s++;
 315
 316         return dst;
 317 #endif
 318 }
 319
 320 int
 321 memcmp(const void *v1, const void *v2, size_t n)
 322 {
 323         const uint8_t *s1 = (const uint8_t *) v1;
 324         const uint8_t *s2 = (const uint8_t *) v2;
 325
 326         while (n-- > 0) {
 327                 if (*s1 != *s2)
 328                         return (int) *s1 - (int) *s2;
 329                 s1++, s2++;
 330         }
 331
 332         return 0;
 333 }
 334
 335 void *
 336 memfind(const void *_s, int c, size_t n)
 337 {
 338         const void *ends = (const char *) _s + n;
 339         const void *s = _s;
 340         for (; s < ends; s++)
 341                 if (*(const unsigned char *) s == (unsigned char) c)
 342                         break;
 343         return (void *)s;
 344 }
 345
 346 long
 347 strtol(const char *s, char **endptr, int base)
 348 {
 349         int neg = 0;
 350         long val = 0;
 351
 352         // gobble initial whitespace
 353         while (*s == ' ' || *s == '\t')
 354                 s++;
 355
 356         // plus/minus sign
 357         if (*s == '+')
 358                 s++;
 359         else if (*s == '-')
 360                 s++, neg = 1;
 361
 362         // hex or octal base prefix
 363         if ((base == 0 || base == 16) && (s[0] == '0' && s[1] == 'x'))
 364                 s += 2, base = 16;
 365         else if (base == 0 && s[0] == '0')
 366                 s++, base = 8;
 367         else if (base == 0)
 368                 base = 10;
 369
 370         // digits
 371         while (1) {
 372                 int dig;
 373
 374                 if (*s >= '0' && *s <= '9')
 375                         dig = *s - '0';
 376                 else if (*s >= 'a' && *s <= 'z')
 377                         dig = *s - 'a' + 10;
 378                 else if (*s >= 'A' && *s <= 'Z')
 379                         dig = *s - 'A' + 10;
 380                 else
 381                         break;
 382                 if (dig >= base)
 383                         break;
 384                 s++, val = (val * base) + dig;
 385                 // we don't properly detect overflow!
 386         }
 387
 388         if (endptr)
 389                 *endptr = (char *) s;
 390         return (neg ? -val : val);
 391 }
 392
 393 unsigned long
 394 strtoul(const char *s, char **endptr, int base)
 395 {
 396         int neg = 0;
 397         unsigned long val = 0;
 398
 399         // gobble initial whitespace
 400         while (*s == ' ' || *s == '\t')
 401                 s++;
 402
 403         // plus/minus sign
 404         if (*s == '+')
 405                 s++;
 406         else if (*s == '-')
 407                 s++, neg = 1;
 408
 409         // hex or octal base prefix
 410         if ((base == 0 || base == 16) && (s[0] == '0' && s[1] == 'x'))
 411                 s += 2, base = 16;
 412         else if (base == 0 && s[0] == '0')
 413                 s++, base = 8;
 414         else if (base == 0)
 415                 base = 10;
 416
 417         // digits
 418         while (1) {
 419                 int dig;
 420
 421                 if (*s >= '0' && *s <= '9')
 422                         dig = *s - '0';
 423                 else if (*s >= 'a' && *s <= 'z')
 424                         dig = *s - 'a' + 10;
 425                 else if (*s >= 'A' && *s <= 'Z')
 426                         dig = *s - 'A' + 10;
 427                 else
 428                         break;
 429                 if (dig >= base)
 430                         break;
 431                 s++, val = (val * base) + dig;
 432                 // we don't properly detect overflow!
 433         }
 434
 435         if (endptr)
 436                 *endptr = (char *) s;
 437         return (neg ? -val : val);
 438 }
 439
 440 int atoi(const char *s)
 441 {
 442         if (!s)
 443                 return 0;
 444         if (s[0] == '0' && s[1] == 'x')
 445                 warn("atoi() used on a hex string!");
 446         // no overflow detection
 447         return (int)strtol(s,NULL,10);
 448 }
 449
 450 int sigchecksum(void *address, int length)
 451 {
 452         uint8_t *p, sum;
 453
 454         sum = 0;
 455         for (p = address; length-- > 0; p++)
 456                 sum += *p;
 457
 458         return sum;
 459 }
 460
 461 void *sigscan(uint8_t *address, int length, char *signature)
 462 {
 463         uint8_t *e, *p;
 464         int siglength;
 465
 466         e = address + length;
 467         siglength = strlen(signature);
 468         for (p = address; p + siglength < e; p += 16) {
 469                 if (memcmp(p, signature, siglength))
 470                         continue;
 471                 return p;
 472         }
 473
 474         return NULL;
 475 }