;**************************************************************************
;* Copyright (C) 2007 Loren Merritt <lorenm@u.washington.edu>
;*
;* This program is free software; you can redistribute it and/or modify
;* it under the terms of the GNU General Public License as published by
;* the Free Software Foundation; either version 2 of the License, or
;* (at your option) any later version.
;*
;* This program 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 General Public License for more details.
;*
;* You should have received a copy of the GNU General Public License
;* along with this program; if not, write to the Free Software
;* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
;**************************************************************************

%include "amd64inc.asm"

section .text align=16

; int xlsc_sad_16_mmx2( uint8_t *fenc, uint8_t *fref, int stride )
cglobal xlsc_sad_16_mmx2
    pxor   mm0, mm0
%rep 16
    movq   mm1, [parm1q]
    movq   mm2, [parm1q+8]
    psadbw mm1, [parm2q]
    psadbw mm2, [parm2q+8]
    paddw  mm0, mm1
    paddw  mm0, mm2
    add parm1q, FENC_STRIDE
    add parm2q, parm3q
%endrep
    movd   eax, mm0
    ret


; int xlsc_ssd_16_sse2( uint8_t *fenc, uint8_t *fref, int stride )
cglobal xlsc_ssd_16_sse2
    pxor      xmm0, xmm0
    pxor      xmm7, xmm7
%rep 16
    movdqa    xmm1, [parm1q]
    movdqu    xmm3, [parm2q]
    movdqa    xmm2, xmm1
    movdqa    xmm4, xmm3
    punpcklbw xmm1, xmm7
    punpcklbw xmm3, xmm7
    punpckhbw xmm2, xmm7
    punpckhbw xmm4, xmm7
    psubw     xmm1, xmm3
    psubw     xmm2, xmm4
    pmaddwd   xmm1, xmm1
    pmaddwd   xmm2, xmm2
    paddd     xmm0, xmm1
    paddd     xmm0, xmm2
    add     parm1q, FENC_STRIDE
    add     parm2q, parm3q
%endrep
    movhlps   xmm1, xmm0
    paddd     xmm0, xmm1
    pshuflw   xmm1, xmm0, 0xE
    paddd     xmm0, xmm1
    movd      eax,  xmm0
    ret


%macro SAD_X4_1x8P 2
    movq    mm7, [parm1q+%1]
    movq    mm4, [parm2q+%2]
    movq    mm5, [parm3q+%2]
    movq    mm6, [parm4q+%2]
    psadbw  mm4, mm7
    psadbw  mm5, mm7
    psadbw  mm6, mm7
    psadbw  mm7, [parm5q+%2]
    paddw   mm0, mm4
    paddw   mm1, mm5
    paddw   mm2, mm6
    paddw   mm3, mm7
%endmacro

; void xlsc_sad_x4_16_mmx2( uint8_t *fenc, uint8_t *r0, uint8_t *r1, uint8_t *r2, uint8_t *r3, int stride, int res[4] )
cglobal xlsc_sad_x4_16_mmx2
    pxor    mm0, mm0
    pxor    mm1, mm1
    pxor    mm2, mm2
    pxor    mm3, mm3
    mov     eax, 16
.loop:
    SAD_X4_1x8P 0, 0
    SAD_X4_1x8P 8, 8
    SAD_X4_1x8P FENC_STRIDE, parm6q
    SAD_X4_1x8P FENC_STRIDE+8, parm6q+8
    add  parm1q, 2*FENC_STRIDE
    lea  parm2q, [parm2q+2*parm6q]
    lea  parm3q, [parm3q+2*parm6q]
    lea  parm4q, [parm4q+2*parm6q]
    lea  parm5q, [parm5q+2*parm6q]
    sub     eax, byte 2
    jg .loop
    mov     rax, parm7q
    punpckldq mm0, mm1
    punpckldq mm2, mm3
    movq  [rax], mm0
    movq  [rax+8], mm2
    ret


%macro LOAD_DIFF_4x4 1
    movd      mm0, [parm1q+%1]
    movd      mm1, [parm1q+%1+FENC_STRIDE]
    movd      mm4, [parm2q+%1]
    movd      mm5, [parm2q+%1+parm3q]
    punpcklbw mm0, mm4
    punpcklbw mm1, mm5
    punpcklbw mm4, mm4
    punpcklbw mm5, mm5
    psubw     mm0, mm4
    psubw     mm1, mm5
    movd      mm2, [parm1q+%1+2*FENC_STRIDE]
    movd      mm3, [parm1q+%1+3*FENC_STRIDE]
    movd      mm4, [parm2q+%1+2*parm3q]
    movd      mm5, [parm2q+%1+rax]
    punpcklbw mm2, mm4
    punpcklbw mm3, mm5
    punpcklbw mm4, mm4
    punpcklbw mm5, mm5
    psubw     mm2, mm4
    psubw     mm3, mm5
%endmacro

%macro SUMSUB_BADC 4
    paddw %1, %2
    paddw %3, %4
    paddw %2, %2
    paddw %4, %4
    psubw %2, %1
    psubw %4, %3
%endmacro

%macro SBUTTERFLY 4
    movq      %4, %2
    punpckl%1 %2, %3
    punpckh%1 %4, %3
%endmacro

%macro TRANSPOSE_4x4W 5 ; abcd -> adec
    SBUTTERFLY wd, %1, %2, %5
    SBUTTERFLY wd, %3, %4, %2
    SBUTTERFLY dq, %1, %3, %4
    SBUTTERFLY dq, %5, %2, %3
%endmacro

%macro HADAMARD_4x4 5
    SUMSUB_BADC %1, %2, %3, %4
    SUMSUB_BADC %1, %3, %2, %4
    TRANSPOSE_4x4W %1, %2, %3, %4, %5
    SUMSUB_BADC %1, %4, %5, %3
    SUMSUB_BADC %1, %5, %4, %3
%endmacro

%macro ABS_4x2 4
    pxor   %3, %3
    pxor   %4, %4
    psubw  %3, %1
    psubw  %4, %2
    pmaxsw %1, %3
    pmaxsw %2, %4
%endmacro

%macro SATD_4x4 2
    LOAD_DIFF_4x4 %1
    HADAMARD_4x4 mm0, mm1, mm2, mm3, mm4
    ABS_4x2 mm0, mm2, mm1, mm5
    ABS_4x2 mm3, mm4, mm1, mm5
    paddusw mm0, mm2
    paddusw mm3, mm4
    pavgw   mm0, mm3
    paddusw mm7, mm0
%if %2
    add  parm1q, 4*FENC_STRIDE
    lea  parm2q, [parm2q+4*parm3q]
%endif
%endmacro

; int xlsc_satd_16_mmx2( uint8_t *fenc, uint8_t *fref, int stride )
cglobal xlsc_satd_16_mmx2
    pxor      mm7, mm7
    lea       rax, [parm3q*3]
    mov       ecx, 4
.loop:
    SATD_4x4    0, 0
    SATD_4x4    4, 0
    SATD_4x4    8, 0
    SATD_4x4   12, 1
    sub       ecx, byte 1
    jg .loop
    pshufw    mm0, mm7, 0x4E
    pxor      mm1, mm1
    paddusw   mm7, mm0
    punpcklwd mm7, mm1
    pshufw    mm0, mm7, 0x4E
    paddd     mm0, mm7
    movd      eax, mm0
    ret

