#include <stdint.h>
#include <string.h>
#include <emmintrin.h>

#define ONES UINT64_C(0x0101010101010101)
#define HIGHS UINT64_C(0x8080808080808080)
#define HASZERO(x) (((x) - ONES) & ~(x) & HIGHS)

__declspec(dllexport) void *swar_memchr(const void *s, int c, size_t n) {
  const unsigned char *p = s;
  uint64_t k = ONES * (unsigned char)c;
  while (n >= 8) {
    uint64_t v;
    memcpy(&v, p, 8);
    uint64_t m = HASZERO(v ^ k);
    if (m) return (void *)(p + (__builtin_ctzll(m) >> 3));
    p += 8;
    n -= 8;
  }
  for (; n; p++, n--) if (*p == (unsigned char)c) return (void *)p;
  return NULL;
}

__declspec(dllexport) void *sse2_memchr(const void *s, int c, size_t n) {
  const unsigned char *p = s;
  __m128i needle = _mm_set1_epi8((char)c);
  while (n >= 16) {
    int mask = _mm_movemask_epi8(_mm_cmpeq_epi8(_mm_loadu_si128((const __m128i *)p), needle));
    if (mask) return (void *)(p + __builtin_ctz(mask));
    p += 16;
    n -= 16;
  }
  for (; n; p++, n--) if (*p == (unsigned char)c) return (void *)p;
  return NULL;
}