#include <stdint.h>
#include <arm_neon.h>

#define restrict __restrict__

void I420_to_BGRx (unsigned char * restrict in_data, unsigned char * restrict out_data,
unsigned int in_width, unsigned int in_height, unsigned int out_width,
unsigned int cx, unsigned int cy, unsigned int lx, unsigned int ly
)
{

unsigned char * restrict ype = in_data;
unsigned char * restrict ypo = in_data + in_width;
unsigned char * restrict up = in_data + in_width*in_height;
unsigned char * restrict vp = in_data + in_width*in_height+((in_width*in_height) >> 2);
unsigned int * restrict ope = (int*)( out_data + ((cx + cy * out_width) << 2));
unsigned int * restrict opo = (int*)( out_data + ((cx + (cy+1) * out_width) << 2));

unsigned int i,j;

const int16_t c1[] = {298, 409, -100, -208};
const int16_t c2[] = {516, 0, 0, 0};
int16x4_t su_16 = vdup_n_s16(-16);
int16x4_t vc1 = vld1_s16(c1);
int16x4_t vc2 = vld1_s16(c2);
uint16x4_t m255 = vdup_n_u16(255);

int16x4_t vu, vv, vy;
int32x4_t vr, vg, vb, ys;
uint16x4_t vsr, vsg, vsb;
uint8x8x2_t t1;
uint16x4x2_t t2;

for (j=0; j<(ly>>1); j++) {
// convert two lines in parallel, 2x2 pixel blk per iteration
for (i=0; i<(lx>>1); i++) {

// U
vu = vdup_n_s16(*up++ - 128);
// V
vv = vdup_n_s16(*vp++ - 128);
// Y
vy = vset_lane_s16(*ype++, vy, 0);
vy = vset_lane_s16(*ype++, vy, 1);
vy = vset_lane_s16(*ypo++, vy, 2);
vy = vset_lane_s16(*ypo++, vy, 3);
vy = vadd_s16(vy, su_16);
// scaled Y
ys = vmull_lane_s16(vy, vc1, 0);
// R
vr = vmlal_lane_s16(ys, vv, vc1, 1);
// G
vg = vmlal_lane_s16(ys, vu, vc1, 2);;
vg = vmlal_lane_s16(vg, vv, vc1, 3);
// B
vb = vmlal_lane_s16(ys, vu, vc2, 0);

// saturate and shift
vsr = vqrshrun_n_s32(vr, 8);
vsg = vqrshrun_n_s32(vg, 8);
vsb = vqrshrun_n_s32(vb, 8);
vsr = vmin_u16(vsr,m255);
vsg = vmin_u16(vsg,m255);
vsb = vmin_u16(vsb,m255);

// Convert to BGRx:

// 0B0B0B0B vtrn 0G0G0G0G => GBGBGBGB
t1 = vtrn_u8(vreinterpret_u8_u16(vsb),vreinterpret_u8_u16(vsg));
// GBGBGBGB vzip 0R0R0R0R => 0RGB0RGB 0RGB0RGB
t2 = vzip_u16(vreinterpret_u16_u8(t1.val[0]),vsr);

// store result
vst1_u32(ope, vreinterpret_u32_u16(t2.val[0]));
vst1_u32(opo, vreinterpret_u32_u16(t2.val[1]));
ope += 2;
opo += 2;

}
ype += (in_width << 1) - lx;
ypo += (in_width << 1) - lx;
up += (in_width - lx) >> 1;
vp += (in_width - lx) >> 1;
ope += (out_width << 1) - lx;
opo += (out_width << 1) - lx;
}
}

.cpu cortex-a8
.eabi_attribute 27, 3
.fpu neon
.eabi_attribute 23, 1
.eabi_attribute 24, 1
.eabi_attribute 25, 1
.eabi_attribute 26, 2
.eabi_attribute 30, 2
.eabi_attribute 18, 4
.file "1.c"
.text
.align 2
.global I420_to_BGRx
.type I420_to_BGRx, %function
I420_to_BGRx:
@ args = 20, pretend = 0, frame = 96
@ frame_needed = 0, uses_anonymous_args = 0
stmfd sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr}
fstmfdd sp!, {d8}
sub sp, sp, #100
add sl, sp, #80
mov ip, r1
ldr r7, [sp, #160]
mov r8, r0
mov r4, r2
mov r0, sl
mov r2, #8
ldr r1, .L8
mov r9, r3
str ip, [sp, #0]
ldr r6, [sp, #144]
ldr fp, [sp, #152]
ldr r5, [sp, #156]
bl memcpy
mov r2, #516
movs r7, r7, lsr #1
strh r2, [sp, #88] @ movhi
add r2, sp, #88
mov r3, #0
str r7, [sp, #44]
strh r3, [sp, #90] @ movhi
vmov.i16 d27, #65520 @ v4hi
strh r3, [sp, #92] @ movhi
vmov.i16 d26, #255 @ v4hi
strh r3, [sp, #94] @ movhi
ldr ip, [sp, #0]
vld1.16 {d7}, [sl]
vld1.16 {d6}, [r2]
beq .L1
mul r1, r4, r9
mov r2, r5, lsr #1
ldr sl, [sp, #148]
rsb r7, r5, r4
str r2, [sp, #20]
add r9, r8, r4
mla r2, fp, r6, r6
str r3, [sp, #16]
mla r0, r6, fp, sl
ldr r3, [sp, #20]
add r2, r2, sl
add fp, r1, r1, lsr #2
rsb r6, r5, r6, asl #1
add r1, r8, r1
add r2, ip, r2, asl #2
str r2, [sp, #4]
ldr r2, [sp, #20]
add fp, r8, fp
add r0, ip, r0, asl #2
rsb r5, r5, r4, asl #1
mov r7, r7, lsr #1
mov r6, r6, asl #2
mov r2, r2, asl #1
mov r3, r3, asl #3
str r0, [sp, #8]
str r5, [sp, #28]
str r7, [sp, #24]
str r6, [sp, #32]
str r8, [sp, #12]
str r2, [sp, #36]
str r3, [sp, #40]
.L3:
ldr sl, [sp, #20]
cmp sl, #0
beq .L5
ldr ip, [sp, #4]
add sl, r1, sl
ldr r0, [sp, #8]
mov r4, fp
ldr r3, [sp, #12]
mov r2, r9
.L4:
ldrb r5, [r3, #0] @ zero_extendqisi2
ldrb r7, [r3, #1] @ zero_extendqisi2
add r3, r3, #2
ldrb r8, [r2, #0] @ zero_extendqisi2
vmov.16 d8[0], r5
ldrb r5, [r2, #1] @ zero_extendqisi2
ldrb r6, [r1], #1 @ zero_extendqisi2
add r2, r2, #2
vmov.16 d8[1], r7
ldrb r7, [r4], #1 @ zero_extendqisi2
sub r6, r6, #128
cmp r1, sl
vmov.16 d8[2], r8
sub r7, r7, #128
vdup.16 d18, r6
vmov.16 d8[3], r5
vadd.i16 d8, d8, d27
vdup.16 d19, r7
vmull.s16 q8, d8, d7[0]
vmov q10, q8 @ v4si
vmov q11, q8 @ v4si
vmov q12, q8 @ v4si
vmlal.s16 q10, d18, d7[2]
vmlal.s16 q11, d18, d6[0]
vmlal.s16 q12, d19, d7[1]
vmlal.s16 q10, d19, d7[3]
vqrshrun.s32 d17, q11, #8
vqrshrun.s32 d16, q12, #8
vmin.u16 d17, d17, d26
vqrshrun.s32 d20, q10, #8
vmin.u16 d16, d16, d26
vmin.u16 d20, d20, d26
vmov d18, d16 @ v4hi
vtrn.8 d17, d20
fstd d17, [sp, #48]
fstd d20, [sp, #56]
vzip.16 d17, d18
fstd d17, [sp, #64]
fstd d18, [sp, #72]
vst1.32 {d17}, [r0]
add r0, r0, #8
vst1.32 {d18}, [ip]
add ip, ip, #8
bne .L4
ldr r3, [sp, #36]
ldr sl, [sp, #20]
ldr r2, [sp, #12]
add r9, r9, r3
add fp, fp, sl
ldr sl, [sp, #8]
add r2, r2, r3
ldr r3, [sp, #4]
str r2, [sp, #12]
ldr r2, [sp, #40]
add sl, sl, r2
add r3, r3, r2
str sl, [sp, #8]
str r3, [sp, #4]
.L5:
ldr r3, [sp, #28]
ldr r2, [sp, #12]
ldr sl, [sp, #16]
add r9, r9, r3
add r2, r2, r3
ldr r3, [sp, #24]
str r2, [sp, #12]
add sl, sl, #1
ldr r2, [sp, #44]
add r1, r1, r3
str sl, [sp, #16]
add fp, fp, r3
cmp sl, r2
ldr r3, [sp, #4]
ldr sl, [sp, #8]
ldr r2, [sp, #32]
add sl, sl, r2
add r3, r3, r2
str sl, [sp, #8]
str r3, [sp, #4]
bne .L3
.L1:
add sp, sp, #100
fldmfdd sp!, {d8}
ldmfd sp!, {r4, r5, r6, r7, r8, r9, sl, fp, pc}
.L9:
.align 2
.L8:
.word .LANCHOR0
.size I420_to_BGRx, .-I420_to_BGRx
.section .rodata
.align 3
.LANCHOR0 = . + 0
.LC0:
.short 298
.short 409
.short -100
.short -208
.ident "GCC: (GNU) 4.6.1"
.section .note.GNU-stack,"",%progbits

CC="arm-unknown-linux-gnueabi-gcc -O3 -mcpu=cortex-a8 -mfpu=neon -mfloat-abi=softfp -funsafe-math-optimizations -mtune=cortex-a8 -ffast-math -ftree-vectorize -S"