/*-
 * Copyright (c) 2010 Xin LI <delphij@FreeBSD.org>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include <sys/cdefs.h>
__FBSDID("$FreeBSD$");

#include <sys/limits.h>
#include <sys/types.h>
#include <string.h>

/*
 * Portable memchr() for 32-bit and 64-bit systems.
 *
 * The algorithm used here is mostly derived from strlen(3).
 */

/* Magic numbers for the algorithm */
#if LONG_BIT == 32
static const unsigned long mask01 = 0x01010101;
static const unsigned long mask80 = 0x80808080;
#elif LONG_BIT == 64
static const unsigned long mask01 = 0x0101010101010101;
static const unsigned long mask80 = 0x8080808080808080;
#else
#error Unsupported word size
#endif

#define	LONGPTR_MASK (sizeof(long) - 1)


/*
 * Helper macro to return string length if we caught the zero
 * byte.
 */
#define testbyte(x)					\
	do {						\
		if (p[x] == pattern)			\
		    return (__DECONST(void *, p) + x);	\
	} while (0)

void *
memchr(const void *str, int c, size_t n)
{
	const unsigned char pattern = (unsigned char)c;
	const unsigned char *p;
	const unsigned long *lp;
	const unsigned long xormask = c * mask01;
	long va, vb, wd;

	/*
	 * The real scan starts from str.
	 */
	p = str;

	/*
	 * Use the word algorithm only when there is at least two words to
	 * look at.
	 */
	if (n > 2 * sizeof(unsigned long)) {
		/*
		 * The first word requires some special handling, since
		 * the pointer is not necessarily be aligned in the first
		 * place.
		 */
		lp = (const unsigned long *)((uintptr_t)str & ~LONGPTR_MASK);
		wd = *lp ^ xormask;
		va = (wd - mask01);
		vb = ((~wd) & mask80);

		/* Point to the next word (as a boundary) */
		lp++;

		/* Mark the first (boundary- str) bytes as read */
		n -= (uintptr_t)((const unsigned char *)lp - (const unsigned char *)str);

		/*
		 * Test whether the first word contain the desired pattern.
		 *
		 * Note that because the first byte might not be aligned to a
		 * word boundary, we scan only str thru lp.
		 */
		if (va & vb)
			for (; p < (const unsigned char *)lp; p++)
				if (*p == pattern)
					return (__DECONST(void *, p));

		/*
		 * Scan as long as we have at least one whole word.
		 */
		while (n >= sizeof(unsigned long)) {
			wd = (*lp) ^ xormask;
			va = (wd - mask01);
			vb = ((~wd) & mask80);
			if (va & vb) {
				p = (const unsigned char *)(lp);
				testbyte(0);
				testbyte(1);
				testbyte(2);
				testbyte(3);
#if (LONG_BIT >= 64)
				testbyte(4);
				testbyte(5);
				testbyte(6);
				testbyte(7);
#endif
 			} else {
				lp++;
				n -= sizeof(unsigned long);
			}
 		}

		/* Prepare to look at the tail as no match was found. */
		p = (const unsigned char *)lp;
	}

	/*
	 * Scan characters that is left to be scanned
	 */
	if (n > 0)
		for (; n > 0; p++, n--)
			if (*p == pattern)
				return (__DECONST(void *, p));

	/* Nothing found or n == 0 */
	return (NULL);
}