libc.c 9.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457
  1. /* vim: tabstop=4 shiftwidth=4 noexpandtab
  2. * This file is part of ToaruOS and is released under the terms
  3. * of the NCSA / University of Illinois License - see LICENSE.md
  4. * Copyright (C) 2015 Dale Weiler
  5. *
  6. * Standard C library for kernel
  7. *
  8. */
  9. #include <kernel/system.h>
  10. #ifndef UCHAR_MAX
  11. #define UCHAR_MAX 255
  12. #endif
  13. #define ALIGN (sizeof(size_t))
  14. #define ONES ((size_t)-1/UCHAR_MAX)
  15. #define HIGHS (ONES * (UCHAR_MAX/2+1))
  16. #define HASZERO(X) (((X)-ONES) & ~(X) & HIGHS)
  17. #define BITOP(A, B, OP) \
  18. ((A)[(size_t)(B)/(8*sizeof *(A))] OP (size_t)1<<((size_t)(B)%(8*sizeof *(A))))
  19. void * memcpy(void * restrict dest, const void * restrict src, size_t n) {
  20. asm volatile("cld; rep movsb"
  21. : "=c"((int){0})
  22. : "D"(dest), "S"(src), "c"(n)
  23. : "flags", "memory");
  24. return dest;
  25. }
  26. void * memset(void * dest, int c, size_t n) {
  27. asm volatile("cld; rep stosb"
  28. : "=c"((int){0})
  29. : "D"(dest), "a"(c), "c"(n)
  30. : "flags", "memory");
  31. return dest;
  32. }
  33. int memcmp(const void * vl, const void * vr, size_t n) {
  34. const unsigned char *l = vl;
  35. const unsigned char *r = vr;
  36. for (; n && *l == *r; n--, l++, r++);
  37. return n ? *l-*r : 0;
  38. }
  39. void * memchr(const void * src, int c, size_t n) {
  40. const unsigned char * s = src;
  41. c = (unsigned char)c;
  42. for (; ((uintptr_t)s & (ALIGN - 1)) && n && *s != c; s++, n--);
  43. if (n && *s != c) {
  44. const size_t * w;
  45. size_t k = ONES * c;
  46. for (w = (const void *)s; n >= sizeof(size_t) && !HASZERO(*w^k); w++, n -= sizeof(size_t));
  47. for (s = (const void *)w; n && *s != c; s++, n--);
  48. }
  49. return n ? (void *)s : 0;
  50. }
  51. void * memrchr(const void * m, int c, size_t n) {
  52. const unsigned char * s = m;
  53. c = (unsigned char)c;
  54. while (n--) {
  55. if (s[n] == c) {
  56. return (void*)(s+n);
  57. }
  58. }
  59. return 0;
  60. }
  61. void * memmove(void * dest, const void * src, size_t n) {
  62. char * d = dest;
  63. const char * s = src;
  64. if (d==s) {
  65. return d;
  66. }
  67. if (s+n <= d || d+n <= s) {
  68. return memcpy(d, s, n);
  69. }
  70. if (d<s) {
  71. if ((uintptr_t)s % sizeof(size_t) == (uintptr_t)d % sizeof(size_t)) {
  72. while ((uintptr_t)d % sizeof(size_t)) {
  73. if (!n--) {
  74. return dest;
  75. }
  76. *d++ = *s++;
  77. }
  78. for (; n >= sizeof(size_t); n -= sizeof(size_t), d += sizeof(size_t), s += sizeof(size_t)) {
  79. *(size_t *)d = *(size_t *)s;
  80. }
  81. }
  82. for (; n; n--) {
  83. *d++ = *s++;
  84. }
  85. } else {
  86. if ((uintptr_t)s % sizeof(size_t) == (uintptr_t)d % sizeof(size_t)) {
  87. while ((uintptr_t)(d+n) % sizeof(size_t)) {
  88. if (!n--) {
  89. return dest;
  90. }
  91. d[n] = s[n];
  92. }
  93. while (n >= sizeof(size_t)) {
  94. n -= sizeof(size_t);
  95. *(size_t *)(d+n) = *(size_t *)(s+n);
  96. }
  97. }
  98. while (n) {
  99. n--;
  100. d[n] = s[n];
  101. }
  102. }
  103. return dest;
  104. }
  105. int strcmp(const char * l, const char * r) {
  106. for (; *l == *r && *l; l++, r++);
  107. return *(unsigned char *)l - *(unsigned char *)r;
  108. }
  109. size_t strlen(const char * s) {
  110. const char * a = s;
  111. const size_t * w;
  112. for (; (uintptr_t)s % ALIGN; s++) {
  113. if (!*s) {
  114. return s-a;
  115. }
  116. }
  117. for (w = (const void *)s; !HASZERO(*w); w++);
  118. for (s = (const void *)w; *s; s++);
  119. return s-a;
  120. }
  121. char * strdup(const char * s) {
  122. size_t l = strlen(s);
  123. return memcpy(malloc(l+1), s, l+1);
  124. }
  125. char * stpcpy(char * restrict d, const char * restrict s) {
  126. size_t * wd;
  127. const size_t * ws;
  128. if ((uintptr_t)s % ALIGN == (uintptr_t)d % ALIGN) {
  129. for (; (uintptr_t)s % ALIGN; s++, d++) {
  130. if (!(*d = *s)) {
  131. return d;
  132. }
  133. }
  134. wd = (void *)d;
  135. ws = (const void *)s;
  136. for (; !HASZERO(*ws); *wd++ = *ws++);
  137. d = (void *)wd;
  138. s = (const void *)ws;
  139. }
  140. for (; (*d=*s); s++, d++);
  141. return d;
  142. }
  143. char * strcpy(char * restrict dest, const char * restrict src) {
  144. stpcpy(dest, src);
  145. return dest;
  146. }
  147. size_t strspn(const char * s, const char * c) {
  148. const char * a = s;
  149. size_t byteset[32/sizeof(size_t)] = { 0 };
  150. if (!c[0]) {
  151. return 0;
  152. }
  153. if (!c[1]) {
  154. for (; *s == *c; s++);
  155. return s-a;
  156. }
  157. for (; *c && BITOP(byteset, *(unsigned char *)c, |=); c++);
  158. for (; *s && BITOP(byteset, *(unsigned char *)s, &); s++);
  159. return s-a;
  160. }
  161. char * strchrnul(const char * s, int c) {
  162. size_t * w;
  163. size_t k;
  164. c = (unsigned char)c;
  165. if (!c) {
  166. return (char *)s + strlen(s);
  167. }
  168. for (; (uintptr_t)s % ALIGN; s++) {
  169. if (!*s || *(unsigned char *)s == c) {
  170. return (char *)s;
  171. }
  172. }
  173. k = ONES * c;
  174. for (w = (void *)s; !HASZERO(*w) && !HASZERO(*w^k); w++);
  175. for (s = (void *)w; *s && *(unsigned char *)s != c; s++);
  176. return (char *)s;
  177. }
  178. char * strchr(const char * s, int c) {
  179. char *r = strchrnul(s, c);
  180. return *(unsigned char *)r == (unsigned char)c ? r : 0;
  181. }
  182. char * strrchr(const char * s, int c) {
  183. return memrchr(s, c, strlen(s) + 1);
  184. }
  185. size_t strcspn(const char * s, const char * c) {
  186. const char *a = s;
  187. if (c[0] && c[1]) {
  188. size_t byteset[32/sizeof(size_t)] = { 0 };
  189. for (; *c && BITOP(byteset, *(unsigned char *)c, |=); c++);
  190. for (; *s && !BITOP(byteset, *(unsigned char *)s, &); s++);
  191. return s-a;
  192. }
  193. return strchrnul(s, *c)-a;
  194. }
  195. char * strpbrk(const char * s, const char * b) {
  196. s += strcspn(s, b);
  197. return *s ? (char *)s : 0;
  198. }
  199. static char *strstr_2b(const unsigned char * h, const unsigned char * n) {
  200. uint16_t nw = n[0] << 8 | n[1];
  201. uint16_t hw = h[0] << 8 | h[1];
  202. for (h++; *h && hw != nw; hw = hw << 8 | *++h);
  203. return *h ? (char *)h-1 : 0;
  204. }
  205. static char *strstr_3b(const unsigned char * h, const unsigned char * n) {
  206. uint32_t nw = n[0] << 24 | n[1] << 16 | n[2] << 8;
  207. uint32_t hw = h[0] << 24 | h[1] << 16 | h[2] << 8;
  208. for (h += 2; *h && hw != nw; hw = (hw|*++h) << 8);
  209. return *h ? (char *)h-2 : 0;
  210. }
  211. static char *strstr_4b(const unsigned char * h, const unsigned char * n) {
  212. uint32_t nw = n[0] << 24 | n[1] << 16 | n[2] << 8 | n[3];
  213. uint32_t hw = h[0] << 24 | h[1] << 16 | h[2] << 8 | h[3];
  214. for (h += 3; *h && hw != nw; hw = hw << 8 | *++h);
  215. return *h ? (char *)h-3 : 0;
  216. }
  217. static char *strstr_twoway(const unsigned char * h, const unsigned char * n) {
  218. size_t mem;
  219. size_t mem0;
  220. size_t byteset[32 / sizeof(size_t)] = { 0 };
  221. size_t shift[256];
  222. size_t l;
  223. /* Computing length of needle and fill shift table */
  224. for (l = 0; n[l] && h[l]; l++) {
  225. BITOP(byteset, n[l], |=);
  226. shift[n[l]] = l+1;
  227. }
  228. if (n[l]) {
  229. return 0; /* hit the end of h */
  230. }
  231. /* Compute maximal suffix */
  232. size_t ip = -1;
  233. size_t jp = 0;
  234. size_t k = 1;
  235. size_t p = 1;
  236. while (jp+k<l) {
  237. if (n[ip+k] == n[jp+k]) {
  238. if (k == p) {
  239. jp += p;
  240. k = 1;
  241. } else {
  242. k++;
  243. }
  244. } else if (n[ip+k] > n[jp+k]) {
  245. jp += k;
  246. k = 1;
  247. p = jp - ip;
  248. } else {
  249. ip = jp++;
  250. k = p = 1;
  251. }
  252. }
  253. size_t ms = ip;
  254. size_t p0 = p;
  255. /* And with the opposite comparison */
  256. ip = -1;
  257. jp = 0;
  258. k = p = 1;
  259. while (jp+k<l) {
  260. if (n[ip+k] == n[jp+k]) {
  261. if (k == p) {
  262. jp += p;
  263. k = 1;
  264. } else {
  265. k++;
  266. }
  267. } else if (n[ip+k] < n[jp+k]) {
  268. jp += k;
  269. k = 1;
  270. p = jp - ip;
  271. } else {
  272. ip = jp++;
  273. k = p = 1;
  274. }
  275. }
  276. if (ip+1 > ms+1) {
  277. ms = ip;
  278. } else {
  279. p = p0;
  280. }
  281. /* Periodic needle? */
  282. if (memcmp(n, n+p, ms+1)) {
  283. mem0 = 0;
  284. p = MAX(ms, l-ms-1) + 1;
  285. } else {
  286. mem0 = l-p;
  287. }
  288. mem = 0;
  289. /* Initialize incremental end-of-haystack pointer */
  290. const unsigned char * z = h;
  291. /* Search loop */
  292. for (;;) {
  293. /* Update incremental end-of-haystack pointer */
  294. if ((size_t)(z-h) < l) {
  295. /* Fast estimate for MIN(l,63) */
  296. size_t grow = l | 63;
  297. const unsigned char *z2 = memchr(z, 0, grow);
  298. if (z2) {
  299. z = z2;
  300. if ((size_t)(z-h) < l) {
  301. return 0;
  302. }
  303. } else {
  304. z += grow;
  305. }
  306. }
  307. /* Check last byte first; advance by shift on mismatch */
  308. if (BITOP(byteset, h[l-1], &)) {
  309. k = l-shift[h[l-1]];
  310. if (k) {
  311. if (mem0 && mem && k < p) k = l-p;
  312. h += k;
  313. mem = 0;
  314. continue;
  315. }
  316. } else {
  317. h += l;
  318. mem = 0;
  319. continue;
  320. }
  321. /* Compare right half */
  322. for (k=MAX(ms+1,mem); n[k] && n[k] == h[k]; k++);
  323. if (n[k]) {
  324. h += k-ms;
  325. mem = 0;
  326. continue;
  327. }
  328. /* Compare left half */
  329. for (k=ms+1; k>mem && n[k-1] == h[k-1]; k--);
  330. if (k <= mem) {
  331. return (char *)h;
  332. }
  333. h += p;
  334. mem = mem0;
  335. }
  336. }
  337. char *strstr(const char * h, const char * n) {
  338. /* Return immediately on empty needle */
  339. if (!n[0]) {
  340. return (char *)h;
  341. }
  342. /* Use faster algorithms for short needles */
  343. h = strchr(h, *n);
  344. if (!h || !n[1]) {
  345. return (char *)h;
  346. }
  347. if (!h[1]) return 0;
  348. if (!n[2]) return strstr_2b((void *)h, (void *)n);
  349. if (!h[2]) return 0;
  350. if (!n[3]) return strstr_3b((void *)h, (void *)n);
  351. if (!h[3]) return 0;
  352. if (!n[4]) return strstr_4b((void *)h, (void *)n);
  353. /* Two-way on large needles */
  354. return strstr_twoway((void *)h, (void *)n);
  355. }
  356. static inline int isdigit(int ch) {
  357. return (unsigned int)ch-'0' < 10;
  358. }
  359. static inline int isspace(int ch) {
  360. return ch == ' ' || (unsigned int)ch-'\t' < 5;
  361. }
  362. int atoi(const char * s) {
  363. int n = 0;
  364. int neg = 0;
  365. while (isspace(*s)) {
  366. s++;
  367. }
  368. switch (*s) {
  369. case '-':
  370. neg = 1;
  371. /* Fallthrough is intentional here */
  372. case '+':
  373. s++;
  374. }
  375. while (isdigit(*s)) {
  376. n = 10*n - (*s++ - '0');
  377. }
  378. /* The sign order may look incorrect here but this is correct as n is calculated
  379. * as a negative number to avoid overflow on INT_MAX.
  380. */
  381. return neg ? n : -n;
  382. }
  383. char * strtok_r(char * str, const char * delim, char ** saveptr) {
  384. char * token;
  385. if (str == NULL) {
  386. str = *saveptr;
  387. }
  388. str += strspn(str, delim);
  389. if (*str == '\0') {
  390. *saveptr = str;
  391. return NULL;
  392. }
  393. token = str;
  394. str = strpbrk(token, delim);
  395. if (str == NULL) {
  396. *saveptr = (char *)lfind(token, '\0');
  397. } else {
  398. *str = '\0';
  399. *saveptr = str + 1;
  400. }
  401. return token;
  402. }