klib: add memchr

klib/include/klib.h

@@ -24,6 +24,7 @@ char *strcpy(char *dst, const char *src);
 char *strncpy(char *dst, const char *src, size_t n);
 int strcmp(const char *s1, const char *s2);
 int strncmp(const char *s1, const char *s2, size_t n);
+void *memchr(const void *src, int c, size_t n);
 
 
 //stdlib.h

klib/src/stdlib.c

@@ -54,4 +54,31 @@ void *malloc(size_t size) {
 void free(void *ptr) {
 }
 
+#include <stdint.h>
+#include <limits.h>
+
+#define SS (sizeof(size_t))
+#define ALIGN (sizeof(size_t)-1)
+#define ONES ((size_t)-1/UCHAR_MAX)
+#define HIGHS (ONES * (UCHAR_MAX/2+1))
+#define HASZERO(x) ((x)-ONES & ~(x) & HIGHS)
+
+void *memchr(const void *src, int c, size_t n)
+{
+	const unsigned char *s = src;
+	c = (unsigned char)c;
+#ifdef __GNUC__
+	for (; ((uintptr_t)s & ALIGN) && n && *s != c; s++, n--);
+	if (n && *s != c) {
+		typedef size_t __attribute__((__may_alias__)) word;
+		const word *w;
+		size_t k = ONES * c;
+		for (w = (const void *)s; n>=SS && !HASZERO(*w^k); w++, n-=SS);
+		s = (const void *)w;
+	}
+#endif
+	for (; n && *s != c; s++, n--);
+	return n ? (void *)s : 0;
+}
+
 #endif