/*-
 * Written by Mateusz Guzik <mjg@freebsd.org>
 * Copyright (c) 2023 The FreeBSD Foundation
 *
 * Portions of this software were developed by Robert Clausecker
 * <fuz@FreeBSD.org> under sponsorship from the FreeBSD Foundation.
 *
 * Public domain.
 */

#include <machine/asm.h>
#include "amd64_archlevel.h"

/*
 * Note: this routine was written with kernel use in mind (read: no simd),
 * it is only present in userspace as a temporary measure until something
 * better gets imported.
 */

#define ALIGN_TEXT      .p2align 4,0x90 /* 16-byte alignment, nop filled */

ARCHFUNCS(strlen)
        ARCHFUNC(strlen, scalar)
        ARCHFUNC(strlen, baseline)
ENDARCHFUNCS(strlen)

/*
 * strlen(string)
 *        %rdi
 *
 * Uses the ((x - 0x01....01) & ~x & 0x80....80) trick.
 *
 * 0x01....01 is replaced with 0x0 - 0x01....01 so that it can be added
 * with leaq.
 *
 * For a description see either:
 * - "Hacker's Delight" by Henry S. Warren, Jr.
 * - "Optimizing subroutines in assembly language: An optimization guide for x86 platforms"
 *   by Agner Fog
 *
 * The latter contains a 32-bit variant of the same algorithm coded in assembly for i386.
 */
ARCHENTRY(strlen, scalar)
        movabsq $0xfefefefefefefeff,%r8
        movabsq $0x8080808080808080,%r9

        movq    %rdi,%r10
        movq    %rdi,%rcx
        testb   $7,%dil
        jz      2f

        /*
         * Handle misaligned reads: align to 8 and fill
         * the spurious bytes.
         */
        andq    $~7,%rdi
        movq    (%rdi),%r11
        shlq    $3,%rcx
        movq    $-1,%rdx
        shlq    %cl,%rdx
        notq    %rdx
        orq     %rdx,%r11

        leaq    (%r11,%r8),%rcx
        notq    %r11
        andq    %r11,%rcx
        andq    %r9,%rcx
        jnz     3f

        /*
         * Main loop.
         */
        ALIGN_TEXT
1:
        leaq    8(%rdi),%rdi
2:
        movq    (%rdi),%r11
        leaq    (%r11,%r8),%rcx
        notq    %r11
        andq    %r11,%rcx
        andq    %r9,%rcx
        jz      1b
3:
        bsfq    %rcx,%rcx
        shrq    $3,%rcx
        leaq    (%rcx,%rdi),%rax
        subq    %r10,%rax
        ret
ARCHEND(strlen, scalar)

ARCHENTRY(strlen, baseline)
        mov     %rdi, %rcx
        pxor    %xmm1, %xmm1
        and     $~0xf, %rdi                     # align string
        pcmpeqb (%rdi), %xmm1                   # compare head (with junk before string)
        mov     %rcx, %rsi                      # string pointer copy for later
        and     $0xf, %ecx                      # amount of bytes rdi is past 16 byte alignment
        pmovmskb %xmm1, %eax
        add     $32, %rdi                       # advance to next iteration
        shr     %cl, %eax                       # clear out matches in junk bytes
        test    %eax, %eax                      # any match? (can't use ZF from SHR as CL=0 is possible)
        jnz     2f

        ALIGN_TEXT
1:      pxor    %xmm1, %xmm1
        pcmpeqb -16(%rdi), %xmm1                # find NUL bytes
        pmovmskb %xmm1, %eax
        test    %eax, %eax                      # were any NUL bytes present?
        jnz     3f

        /* the same unrolled once more */
        pxor    %xmm1, %xmm1
        pcmpeqb (%rdi), %xmm1
        pmovmskb %xmm1, %eax
        add     $32, %rdi                       # advance to next iteration
        test    %eax, %eax
        jz      1b

        /* match found in loop body */
        sub     $16, %rdi                       # undo half the advancement
3:      tzcnt   %eax, %eax                      # find the first NUL byte
        sub     %rsi, %rdi                      # string length until beginning of (%rdi)
        lea     -16(%rdi, %rax, 1), %rax        # that plus loc. of NUL byte: full string length
        ret

        /* match found in head */
2:      tzcnt   %eax, %eax                      # compute string length
        ret
ARCHEND(strlen, baseline)

        .section .note.GNU-stack,"",%progbits