/* Function hypot vectorized with AVX2.
   Copyright (C) 2021-2022 Free Software Foundation, Inc.
   This file is part of the GNU C Library.

   The GNU C Library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.

   The GNU C Library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with the GNU C Library; if not, see
   https://www.gnu.org/licenses/.  */

/*
 * ALGORITHM DESCRIPTION:
 *
 *      HIGH LEVEL OVERVIEW
 *
 *      Calculate z = (x*x+y*y)
 *      Calculate reciplicle sqrt (z)
 *      Calculate error = z*(rsqrt(z)*rsqrt(z)) - 1
 *      Calculate fixing part p with polynom
 *      Fix answer with sqrt(z) = z * rsqrt(z) + error * p * z
 *
 *      ALGORITHM DETAILS
 *
 *    Multiprecision branch for _HA_ only
 *      Remove sigm from both arguments
 *      Find maximum (_x) and minimum (_y) (by abs value) between arguments
 *      Split _x int _a and _b for multiprecision
 *      If _x >> _y we will we will not split _y for multiprecision
 *      all _y will be put into lower part (_d) and higher part (_c = 0)
 *      Fixing _hilo_mask for the case _x >> _y
 *      Split _y into _c and _d for multiprecision with fixed mask
 *
 *      compute Hi and Lo parts of _z = _x*_x + _y*_y
 *
 *      _zHi = _a*_a + _c*_c
 *      _zLo = (_x + _a)*_b + _d*_y + _d*_c
 *      _z = _zHi + _zLo
 *
 *    No multiprecision branch for _LA_ and _EP_
 *      _z = _VARG1 * _VARG1 + _VARG2 * _VARG2
 *
 *    Check _z exponent to be withing borders [3BC ; 441] else goto Callout
 *
 *    _s  ~ 1.0/sqrt(_z)
 *    _s2 ~ 1.0/(sqrt(_z)*sqrt(_z)) ~ 1.0/_z = (1.0/_z + O)
 *    _e[rror]  =  (1.0/_z + O) * _z - 1.0
 *    calculate fixing part _p
 *    _p = (((_POLY_C5*_e + _POLY_C4)*_e +_POLY_C3)*_e +_POLY_C2)*_e + _POLY_C1
 *    some parts of polynom are skipped for lower flav
 *
 *    result = _z * (1.0/sqrt(_z) + O) + _p * _e[rror] * _z
 *
 *
 */

/* Offsets for data table __svml_dhypot_data_internal
 */
#define _dHiLoMask                    	0
#define _dAbsMask                     	32
#define _dOne                         	64
#define _POLY_C5                      	96
#define _POLY_C4                      	128
#define _POLY_C3                      	160
#define _POLY_C2                      	192
#define _POLY_C1                      	224
#define _LowBoundary                  	256
#define _HighBoundary                 	288

#include <sysdep.h>

        .text
	.section .text.avx2,"ax",@progbits
ENTRY(_ZGVdN4vv_hypot_avx2)
        pushq     %rbp
        cfi_def_cfa_offset(16)
        movq      %rsp, %rbp
        cfi_def_cfa(6, 16)
        cfi_offset(6, -16)
        andq      $-32, %rsp
        subq      $128, %rsp
        vmovapd   %ymm1, %ymm2
        vmovapd   %ymm0, %ymm1

/*
 *  Defines
 *  Implementation
 * Multiprecision branch for _HA_ only
 * _z = _VARG1 * _VARG1 + _VARG2 * _VARG2
 */
        vmulpd    %ymm1, %ymm1, %ymm0

/*
 * calculate fixing part _p
 * _p = (((_POLY_C5*_e + _POLY_C4)*_e +_POLY_C3)*_e +_POLY_C2)*_e + _POLY_C1
 * some parts of polynom are skipped for lower flav
 */
        vmovupd   _POLY_C4+__svml_dhypot_data_internal(%rip), %ymm15
        vmovups   _LowBoundary+__svml_dhypot_data_internal(%rip), %xmm4
        vfmadd231pd %ymm2, %ymm2, %ymm0

/*
 * _s  ~ 1.0/sqrt(_z)
 * _s2 ~ 1.0/(sqrt(_z)*sqrt(_z)) ~ 1.0/_z
 */
        vcvtpd2ps %ymm0, %xmm12

/* Check _z exponent to be withing borders [3BC ; 441] else goto Callout */
        vextractf128 $1, %ymm0, %xmm3
        vrsqrtps  %xmm12, %xmm13
        vshufps   $221, %xmm3, %xmm0, %xmm5
        vcvtps2pd %xmm13, %ymm3
        vpcmpgtd  %xmm5, %xmm4, %xmm6
        vpcmpgtd  _HighBoundary+__svml_dhypot_data_internal(%rip), %xmm5, %xmm7
        vpor      %xmm7, %xmm6, %xmm9
        vpshufd   $80, %xmm9, %xmm8
        vmulpd    %ymm3, %ymm3, %ymm14
        vpshufd   $250, %xmm9, %xmm10

/* _e[rror]  ~  (1.0/_z + O) * _z - 1.0 */
        vfmsub213pd _dOne+__svml_dhypot_data_internal(%rip), %ymm0, %ymm14
        vfmadd213pd _POLY_C3+__svml_dhypot_data_internal(%rip), %ymm14, %ymm15
        vfmadd213pd _POLY_C2+__svml_dhypot_data_internal(%rip), %ymm14, %ymm15
        vfmadd213pd _POLY_C1+__svml_dhypot_data_internal(%rip), %ymm14, %ymm15

/* result = _z * (1.0/sqrt(_z) + O) + _p * _e[rror] * _z */
        vmulpd    %ymm15, %ymm14, %ymm14
        vmulpd    %ymm14, %ymm3, %ymm15
        vmulpd    %ymm15, %ymm0, %ymm4
        vfmadd213pd %ymm4, %ymm3, %ymm0
        vinsertf128 $1, %xmm10, %ymm8, %ymm11
        vmovmskpd %ymm11, %edx

/*  The end of implementation  */
        testl     %edx, %edx

/* Go to special inputs processing branch */
        jne       L(SPECIAL_VALUES_BRANCH)
                                # LOE rbx r12 r13 r14 r15 edx ymm0 ymm1 ymm2

/* Restore registers
 * and exit the function
 */

L(EXIT):
        movq      %rbp, %rsp
        popq      %rbp
        cfi_def_cfa(7, 8)
        cfi_restore(6)
        ret
        cfi_def_cfa(6, 16)
        cfi_offset(6, -16)

/* Branch to process
 * special inputs
 */

L(SPECIAL_VALUES_BRANCH):
        vmovupd   %ymm1, 32(%rsp)
        vmovupd   %ymm2, 64(%rsp)
        vmovupd   %ymm0, 96(%rsp)
                                # LOE rbx r12 r13 r14 r15 edx ymm0

        xorl      %eax, %eax
                                # LOE rbx r12 r13 r14 r15 eax edx

        vzeroupper
        movq      %r12, 16(%rsp)
        /*  DW_CFA_expression: r12 (r12) (DW_OP_lit8; DW_OP_minus; DW_OP_const4s: -32; DW_OP_and; DW_OP_const4s: -112; DW_OP_plus)  */
        .cfi_escape 0x10, 0x0c, 0x0e, 0x38, 0x1c, 0x0d, 0xe0, 0xff, 0xff, 0xff, 0x1a, 0x0d, 0x90, 0xff, 0xff, 0xff, 0x22
        movl      %eax, %r12d
        movq      %r13, 8(%rsp)
        /*  DW_CFA_expression: r13 (r13) (DW_OP_lit8; DW_OP_minus; DW_OP_const4s: -32; DW_OP_and; DW_OP_const4s: -120; DW_OP_plus)  */
        .cfi_escape 0x10, 0x0d, 0x0e, 0x38, 0x1c, 0x0d, 0xe0, 0xff, 0xff, 0xff, 0x1a, 0x0d, 0x88, 0xff, 0xff, 0xff, 0x22
        movl      %edx, %r13d
        movq      %r14, (%rsp)
        /*  DW_CFA_expression: r14 (r14) (DW_OP_lit8; DW_OP_minus; DW_OP_const4s: -32; DW_OP_and; DW_OP_const4s: -128; DW_OP_plus)  */
        .cfi_escape 0x10, 0x0e, 0x0e, 0x38, 0x1c, 0x0d, 0xe0, 0xff, 0xff, 0xff, 0x1a, 0x0d, 0x80, 0xff, 0xff, 0xff, 0x22
                                # LOE rbx r15 r12d r13d

/* Range mask
 * bits check
 */

L(RANGEMASK_CHECK):
        btl       %r12d, %r13d

/* Call scalar math function */
        jc        L(SCALAR_MATH_CALL)
                                # LOE rbx r15 r12d r13d

/* Special inputs
 * processing loop
 */

L(SPECIAL_VALUES_LOOP):
        incl      %r12d
        cmpl      $4, %r12d

/* Check bits in range mask */
        jl        L(RANGEMASK_CHECK)
                                # LOE rbx r15 r12d r13d

        movq      16(%rsp), %r12
        cfi_restore(12)
        movq      8(%rsp), %r13
        cfi_restore(13)
        movq      (%rsp), %r14
        cfi_restore(14)
        vmovupd   96(%rsp), %ymm0

/* Go to exit */
        jmp       L(EXIT)
        /*  DW_CFA_expression: r12 (r12) (DW_OP_lit8; DW_OP_minus; DW_OP_const4s: -32; DW_OP_and; DW_OP_const4s: -112; DW_OP_plus)  */
        .cfi_escape 0x10, 0x0c, 0x0e, 0x38, 0x1c, 0x0d, 0xe0, 0xff, 0xff, 0xff, 0x1a, 0x0d, 0x90, 0xff, 0xff, 0xff, 0x22
        /*  DW_CFA_expression: r13 (r13) (DW_OP_lit8; DW_OP_minus; DW_OP_const4s: -32; DW_OP_and; DW_OP_const4s: -120; DW_OP_plus)  */
        .cfi_escape 0x10, 0x0d, 0x0e, 0x38, 0x1c, 0x0d, 0xe0, 0xff, 0xff, 0xff, 0x1a, 0x0d, 0x88, 0xff, 0xff, 0xff, 0x22
        /*  DW_CFA_expression: r14 (r14) (DW_OP_lit8; DW_OP_minus; DW_OP_const4s: -32; DW_OP_and; DW_OP_const4s: -128; DW_OP_plus)  */
        .cfi_escape 0x10, 0x0e, 0x0e, 0x38, 0x1c, 0x0d, 0xe0, 0xff, 0xff, 0xff, 0x1a, 0x0d, 0x80, 0xff, 0xff, 0xff, 0x22
                                # LOE rbx r12 r13 r14 r15 ymm0

/* Scalar math fucntion call
 * to process special input
 */

L(SCALAR_MATH_CALL):
        movl      %r12d, %r14d
        movsd     32(%rsp,%r14,8), %xmm0
        movsd     64(%rsp,%r14,8), %xmm1
        call      hypot@PLT
                                # LOE rbx r14 r15 r12d r13d xmm0

        movsd     %xmm0, 96(%rsp,%r14,8)

/* Process special inputs in loop */
        jmp       L(SPECIAL_VALUES_LOOP)
                                # LOE rbx r15 r12d r13d
END(_ZGVdN4vv_hypot_avx2)

        .section .rodata, "a"
        .align 32

#ifdef __svml_dhypot_data_internal_typedef
typedef unsigned int VUINT32;
typedef struct
{
        __declspec(align(32)) VUINT32 _dHiLoMask[4][2];
        __declspec(align(32)) VUINT32 _dAbsMask[4][2];
        __declspec(align(32)) VUINT32 _dOne[4][2];
        __declspec(align(32)) VUINT32 _POLY_C5[4][2];
        __declspec(align(32)) VUINT32 _POLY_C4[4][2];
        __declspec(align(32)) VUINT32 _POLY_C3[4][2];
        __declspec(align(32)) VUINT32 _POLY_C2[4][2];
        __declspec(align(32)) VUINT32 _POLY_C1[4][2];
        __declspec(align(32)) VUINT32 _LowBoundary[8][1];
        __declspec(align(32)) VUINT32 _HighBoundary[8][1];
} __svml_dhypot_data_internal;
#endif
__svml_dhypot_data_internal:
        /* legacy algorithm */
        .quad 0xffffc00000000000, 0xffffc00000000000, 0xffffc00000000000, 0xffffc00000000000       /* _dHiLoMask     */
        .align 32
        .quad 0x7fffffffffffffff, 0x7fffffffffffffff, 0x7fffffffffffffff, 0x7fffffffffffffff       /* _dAbsMask      */
        .align 32
        .quad 0x3FF0000000000000, 0x3FF0000000000000, 0x3FF0000000000000, 0x3FF0000000000000       /* _dOne          */
        .align 32
        .quad 0xBFCF800000000000, 0xBFCF800000000000, 0xBFCF800000000000, 0xBFCF800000000000       /* _POLY_C5            */
        .align 32
        .quad 0x3FD1800000000000, 0x3FD1800000000000, 0x3FD1800000000000, 0x3FD1800000000000       /* _POLY_C4            */
        .align 32
        .quad 0xBFD4000000000000, 0xBFD4000000000000, 0xBFD4000000000000, 0xBFD4000000000000       /* _POLY_C3            */
        .align 32
        .quad 0x3FD8000000000000, 0x3FD8000000000000, 0x3FD8000000000000, 0x3FD8000000000000       /* _POLY_C2            */
        .align 32
        .quad 0xBFE0000000000000, 0xBFE0000000000000, 0xBFE0000000000000, 0xBFE0000000000000       /* _POLY_C1            */
        .align 32
        .long 0x3BC00000, 0x3BC00000, 0x3BC00000, 0x3BC00000, 0x3BC00000, 0x3BC00000, 0x3BC00000, 0x3BC00000       /* _LowBoundary   */
        .align 32
        .long 0x44100000, 0x44100000, 0x44100000, 0x44100000, 0x44100000, 0x44100000, 0x44100000, 0x44100000       /* _HighBoundary  */
        .align 32
        .type	__svml_dhypot_data_internal,@object
        .size	__svml_dhypot_data_internal,.-__svml_dhypot_data_internal
