/*
 * Copyright (c) 2008-2015 ARM Ltd
 * 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.
 * 3. The name of the company may not be used to endorse or promote
 *    products derived from this software without specific prior written
 *    permission.
 *
 * THIS SOFTWARE IS PROVIDED BY ARM LTD ``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 ARM LTD 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 <picolibc.h>

#include "arm_asm.h"
#include <string.h>
#include <limits.h>

#if defined __OPTIMIZE_SIZE__ || defined __PREFER_SIZE_OVER_SPEED
#if __ARM_ARCH_ISA_THUMB == 2
/* Implemented in strlen.S.  */

#elif defined(__thumb__)
/* Implemented in strlen.S.  */

#else
#include "../../string/strlen.c"

#endif

#else /* defined __OPTIMIZE_SIZE__ || defined __PREFER_SIZE_OVER_SPEED */
#if defined __thumb__ && !defined __thumb2__
#include "../../string/strlen.c"

#elif __ARM_ARCH_ISA_THUMB >= 2 && defined __ARM_FEATURE_DSP
/* Implemented in strlen.S.  */

#else
#ifdef __GNUCLIKE_PRAGMA_DIAGNOSTIC
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

size_t __naked
strlen(const char *str)
{
    __asm__("len .req r0\n\t"
            "data .req r3\n\t"
            "addr .req r1\n\t"

#ifdef _ISA_ARM_7
            "pld [r0]\n\t"
#endif
            /* Word-align address */
            "bic	addr, r0, #3\n\t"
            /* Get adjustment for start ... */
            "ands	len, r0, #3\n\t"
            "neg	len, len\n\t"
            /* First word of data */
            "ldr	data, [addr], #4\n\t"
            /* Ensure bytes preceeding start ... */
            "add	ip, len, #4\n\t"
            "mov	ip, ip, asl #3\n\t"
            "mvn	r2, #0\n\t"
    /* ... are masked out */
#ifdef __thumb__
            "itt	ne\n\t"
#ifdef __ARMEB__
            "lslne	r2, ip\n\t"
#else
            "lsrne	r2, ip\n\t"
#endif
            "orrne	data, data, r2\n\t"
#else
            "it	ne\n\t"
#ifdef __ARMEB__
            "orrne	data, data, r2, lsl ip\n\t"
#else
            "orrne	data, data, r2, lsr ip\n\t"
#endif
#endif
    /* Magic const 0x01010101 */
#ifdef _ISA_ARM_7
            "movw	ip, #0x101\n\t"
#else
            "mov	ip, #0x1\n\t"
            "orr	ip, ip, ip, lsl #8\n\t"
#endif
            "orr	ip, ip, ip, lsl #16\n"

            /* This is the main loop.  We subtract one from each byte in
               the word: the sign bit changes iff the byte was zero or
               0x80 -- we eliminate the latter case by anding the result
               with the 1-s complement of the data.  */
            "1:\n\t"
            /* test (data - 0x01010101)  */
            "sub	r2, data, ip\n\t"
            /* ... & ~data */
            "bic	r2, r2, data\n\t"
            /* ... & 0x80808080 == 0? */
            "ands	r2, r2, ip, lsl #7\n\t"
#ifdef _ISA_ARM_7
            /* yes, get more data... */
            "itt	eq\n\t"
            "ldreq	data, [addr], #4\n\t"
            /* and 4 more bytes  */
            "addeq	len, len, #4\n\t"
            /* Unroll the loop a bit.  */
            "pld	[addr, #8]\n\t"
            /*  test (data - 0x01010101)  */
            "ittt	eq\n\t"
            "subeq	r2, data, ip\n\t"
            /* ... & ~data */
            "biceq	r2, r2, data\n\t"
            /* ... & 0x80808080 == 0? */
            "andeqs	r2, r2, ip, lsl #7\n\t"
#endif
            "itt	eq\n\t"
            /* yes, get more data... */
            "ldreq	data, [addr], #4\n\t"
            /* and 4 more bytes  */
            "addeq	len, len, #4\n\t"
            "beq	1b\n\t"
#ifdef __ARMEB__
            "tst	data, #0xff000000\n\t"
            "itttt	ne\n\t"
            "addne	len, len, #1\n\t"
            "tstne	data, #0xff0000\n\t"
            "addne	len, len, #1\n\t"
            "tstne	data, #0xff00\n\t"
            "it	ne\n\t"
            "addne	len, len, #1\n\t"
#else
#ifdef _ISA_ARM_5
    /* R2 is the residual sign bits from the above test.  All we
    need to do now is establish the position of the first zero
    byte... */
    /* Little-endian is harder, we need the number of trailing
    zeros / 8 */
#ifdef _ISA_ARM_7
            "rbit	r2, r2\n\t"
            "clz	r2, r2\n\t"
#else
            "rsb	r1, r2, #0\n\t"
            "and	r2, r2, r1\n\t"
            "clz	r2, r2\n\t"
            "rsb	r2, r2, #31\n\t"
#endif
            "add	len, len, r2, lsr #3\n\t"
#else /* No CLZ instruction */
            "tst	data, #0xff\n\t"
            "itttt	ne\n\t"
            "addne	len, len, #1\n\t"
            "tstne	data, #0xff00\n\t"
            "addne	len, len, #1\n\t"
            "tstne	data, #0xff0000\n\t"
            "it	ne\n\t"
            "addne	len, len, #1\n\t"
#endif
#endif
            "bx	lr\n\t");
}
#endif
#endif
