[picodrive.git] / Pico / sound / mix.s

@ vim:filetype=armasm


@ this assumes src is word aligned
.global mix_16h_to_32 @ int *dest, short *src, int count

mix_16h_to_32:
    stmfd   sp!, {r4-r6,lr}
/*
    tst     r1, #2
    beq     m16_32_mo_unalw
    ldrsh   r4, [r1], #2
    ldr     r3, [r0]
    sub     r2, r2, #1
    add     r3, r3, r4, asr #1
    str     r3, [r0], #4
*/
m16_32_mo_unalw:
    subs    r2, r2, #4
    bmi     m16_32_end

m16_32_loop:
    ldmia   r0, {r3-r6}
    ldmia   r1!,{r12,lr}
    subs    r2, r2, #4
    add     r4, r4, r12,asr #17 @ we use half volume
    mov     r12,r12,lsl #16
    add     r3, r3, r12,asr #17
    add     r6, r6, lr, asr #17
    mov     lr, lr, lsl #16
    add     r5, r5, lr, asr #17
    stmia   r0!,{r3-r6}
    bpl     m16_32_loop

m16_32_end:
    tst     r2, #2
    beq     m16_32_no_unal2
    ldr     r5, [r1], #4
    ldmia   r0, {r3,r4}
    mov     r12,r5, lsl #16
    add     r3, r3, r12,asr #17
    add     r4, r4, r5, asr #17
    stmia   r0!,{r3,r4}

m16_32_no_unal2:
    tst     r2, #1
    ldmeqfd sp!, {r4-r6,pc}
    ldrsh   r4, [r1], #2
    ldr     r3, [r0]
    add     r3, r3, r4, asr #1
    str     r3, [r0], #4

    ldmfd   sp!, {r4-r6,lr}
    bx      lr


.global mix_16h_to_32_s1 @ int *dest, short *src, int count

mix_16h_to_32_s1:
    stmfd   sp!, {r4-r6,lr}

    subs    r2, r2, #4
    bmi     m16_32_s1_end

m16_32_s1_loop:
    ldmia   r0, {r3-r6}
    ldr     r12,[r1], #8
    ldr     lr, [r1], #8
    subs    r2, r2, #4
    add     r4, r4, r12,asr #17
    mov     r12,r12,lsl #16
    add     r3, r3, r12,asr #17 @ we use half volume
    add     r6, r6, lr, asr #17
    mov     lr, lr, lsl #16
    add     r5, r5, lr, asr #17
    stmia   r0!,{r3-r6}
    bpl     m16_32_s1_loop

m16_32_s1_end:
    tst     r2, #2
    beq     m16_32_s1_no_unal2
    ldr     r5, [r1], #8
    ldmia   r0, {r3,r4}
    mov     r12,r5, lsl #16
    add     r3, r3, r12,asr #17
    add     r4, r4, r5, asr #17
    stmia   r0!,{r3,r4}

m16_32_s1_no_unal2:
    tst     r2, #1
    ldmeqfd sp!, {r4-r6,pc}
    ldrsh   r4, [r1], #2
    ldr     r3, [r0]
    add     r3, r3, r4, asr #1
    str     r3, [r0], #4

    ldmfd   sp!, {r4-r6,lr}
    bx      lr


.global mix_16h_to_32_s2 @ int *dest, short *src, int count

mix_16h_to_32_s2:
    stmfd   sp!, {r4-r6,lr}

    subs    r2, r2, #4
    bmi     m16_32_s2_end

m16_32_s2_loop:
    ldmia   r0, {r3-r6}
    ldr     r12,[r1], #16
    ldr     lr, [r1], #16
    subs    r2, r2, #4
    add     r4, r4, r12,asr #17
    mov     r12,r12,lsl #16
    add     r3, r3, r12,asr #17 @ we use half volume
    add     r6, r6, lr, asr #17
    mov     lr, lr, lsl #16
    add     r5, r5, lr, asr #17
    stmia   r0!,{r3-r6}
    bpl     m16_32_s2_loop

m16_32_s2_end:
    tst     r2, #2
    beq     m16_32_s2_no_unal2
    ldr     r5, [r1], #16
    ldmia   r0, {r3,r4}
    mov     r12,r5, lsl #16
    add     r3, r3, r12,asr #17
    add     r4, r4, r5, asr #17
    stmia   r0!,{r3,r4}

m16_32_s2_no_unal2:
    tst     r2, #1
    ldmeqfd sp!, {r4-r6,pc}
    ldrsh   r4, [r1], #2
    ldr     r3, [r0]
    add     r3, r3, r4, asr #1
    str     r3, [r0], #4

    ldmfd   sp!, {r4-r6,lr}
    bx      lr


@ limit
@ reg=int_sample, lr=1, r3=tmp, kills flags
.macro Limit reg
    add     r3, lr, \reg, asr #15
    bics    r3, r3, #1			@ in non-overflow conditions r3 is 0 or 1
    movne   \reg, #0x8000
    subpl   \reg, \reg, #1
.endm


@ limit and shift up by 16
@ reg=int_sample, lr=1, r3=tmp, kills flags
.macro Limitsh reg
@    movs    r4, r3, asr #16
@    cmnne   r4, #1
@    beq     c32_16_no_overflow
@    tst     r4, r4
@    mov     r3, #0x8000
@    subpl   r3, r3, #1

    add     r3, lr, \reg, asr #15
    bics    r3, r3, #1			@ in non-overflow conditions r3 is 0 or 1
    moveq   \reg, \reg, lsl #16
    movne   \reg, #0x80000000
    subpl   \reg, \reg, #0x00010000
.endm


@ mix 32bit audio (with 16bits really used, upper bits indicate overflow) with normal 16 bit audio with left channel only
@ warning: this function assumes dest is word aligned
.global mix_32_to_16l_stereo @ short *dest, int *src, int count

mix_32_to_16l_stereo:
    stmfd   sp!, {r4-r8,lr}

    mov     lr, #1

    mov     r2, r2, lsl #1
    subs    r2, r2, #4
    bmi     m32_16l_st_end

m32_16l_st_loop:
    ldmia   r0,  {r8,r12}
    ldmia   r1!, {r4-r7}
    mov     r8, r8, lsl #16
    mov     r12,r12,lsl #16
    add     r4, r4, r8, asr #16
    add     r5, r5, r8, asr #16
    add     r6, r6, r12,asr #16
    add     r7, r7, r12,asr #16
    Limitsh r4
    Limitsh r5
    Limitsh r6
    Limitsh r7
    subs    r2, r2, #4
    orr     r4, r5, r4, lsr #16
    orr     r5, r7, r6, lsr #16
    stmia   r0!, {r4,r5}
    bpl     m32_16l_st_loop

m32_16l_st_end:
    @ check for remaining bytes to convert
    tst     r2, #2
    beq     m32_16l_st_no_unal2
    ldrsh   r6, [r0]
    ldmia   r1!,{r4,r5}
    add     r4, r4, r6
    add     r5, r5, r6
    Limitsh r4
    Limitsh r5
    orr     r4, r5, r4, lsr #16
    str     r4, [r0], #4

m32_16l_st_no_unal2:
    ldmfd   sp!, {r4-r8,lr}
    bx      lr


@ mix 32bit audio (with 16bits really used, upper bits indicate overflow) with normal 16 bit audio (for mono sound)
.global mix_32_to_16_mono @ short *dest, int *src, int count

mix_32_to_16_mono:
    stmfd   sp!, {r4-r8,lr}

    mov     lr, #1

    @ check if dest is word aligned
    tst     r0, #2
    beq     m32_16_mo_no_unalw
    ldrsh   r5, [r0]
    ldr     r4, [r1], #4
    sub     r2, r2, #1
    add     r4, r4, r5
    Limit   r4
    strh    r4, [r0], #2

m32_16_mo_no_unalw:
    subs    r2, r2, #4
    bmi     m32_16_mo_end

m32_16_mo_loop:
    ldmia   r0,  {r8,r12}
    ldmia   r1!, {r4-r7}
    add     r5, r5, r8, asr #16
    mov     r8, r8, lsl #16
    add     r4, r4, r8, asr #16
    add     r7, r7, r12,asr #16
    mov     r12,r12,lsl #16
    add     r6, r6, r12,asr #16
    Limitsh r4
    Limitsh r5
    Limitsh r6
    Limitsh r7
    subs    r2, r2, #4
    orr     r4, r5, r4, lsr #16
    orr     r5, r7, r6, lsr #16
    stmia   r0!, {r4,r5}
    bpl     m32_16_mo_loop

m32_16_mo_end:
    @ check for remaining bytes to convert
    tst     r2, #2
    beq     m32_16_mo_no_unal2
    ldr     r6, [r0]
    ldmia   r1!,{r4,r5}
    add     r5, r5, r6, asr #16
    mov     r6, r6, lsl #16
    add     r4, r4, r6, asr #16
    Limitsh r4
    Limitsh r5
    orr     r4, r5, r4, lsr #16
    str     r4, [r0], #4

m32_16_mo_no_unal2:
    tst     r2, #1
    ldmeqfd sp!, {r4-r8,pc}
    ldrsh   r5, [r0]
    ldr     r4, [r1], #4
    add     r4, r4, r5
    Limit   r4
    strh    r4, [r0], #2

    ldmfd   sp!, {r4-r8,lr}
    bx      lr
Commit	Line	Data
4f265db7	1	@ vim:filetype=armasm
4f265db7	2
4f265db7	3
	4	@ this assumes src is word aligned
	5	.global mix_16h_to_32 @ int dest, short src, int count
	6
	7	mix_16h_to_32:
	8	stmfd sp!, {r4-r6,lr}
	9	/*
	10	tst r1, #2
	11	beq m16_32_mo_unalw
	12	ldrsh r4, [r1], #2
	13	ldr r3, [r0]
	14	sub r2, r2, #1
	15	add r3, r3, r4, asr #1
	16	str r3, [r0], #4
	17	*/
	18	m16_32_mo_unalw:
	19	subs r2, r2, #4
	20	bmi m16_32_end
	21
	22	m16_32_loop:
	23	ldmia r0, {r3-r6}
	24	ldmia r1!,{r12,lr}
	25	subs r2, r2, #4
	26	add r4, r4, r12,asr #17 @ we use half volume
	27	mov r12,r12,lsl #16
	28	add r3, r3, r12,asr #17
	29	add r6, r6, lr, asr #17
	30	mov lr, lr, lsl #16
	31	add r5, r5, lr, asr #17
	32	stmia r0!,{r3-r6}
	33	bpl m16_32_loop
	34
	35	m16_32_end:
	36	tst r2, #2
	37	beq m16_32_no_unal2
	38	ldr r5, [r1], #4
	39	ldmia r0, {r3,r4}
	40	mov r12,r5, lsl #16
	41	add r3, r3, r12,asr #17
	42	add r4, r4, r5, asr #17
	43	stmia r0!,{r3,r4}
	44
	45	m16_32_no_unal2:
	46	tst r2, #1
	47	ldmeqfd sp!, {r4-r6,pc}
	48	ldrsh r4, [r1], #2
	49	ldr r3, [r0]
	50	add r3, r3, r4, asr #1
	51	str r3, [r0], #4
	52
	53	ldmfd sp!, {r4-r6,lr}
	54	bx lr
	55
	56
	57
	58	.global mix_16h_to_32_s1 @ int dest, short src, int count
	59
	60	mix_16h_to_32_s1:
	61	stmfd sp!, {r4-r6,lr}
	62
	63	subs r2, r2, #4
	64	bmi m16_32_s1_end
	65
	66	m16_32_s1_loop:
67	ldmia r0, {r3-r6}
68	ldr r12,[r1], #8
69	ldr lr, [r1], #8
70	subs r2, r2, #4
71	add r4, r4, r12,asr #17
72	mov r12,r12,lsl #16
73	add r3, r3, r12,asr #17 @ we use half volume
74	add r6, r6, lr, asr #17
75	mov lr, lr, lsl #16
76	add r5, r5, lr, asr #17
77	stmia r0!,{r3-r6}
78	bpl m16_32_s1_loop
79
80	m16_32_s1_end:
81	tst r2, #2
82	beq m16_32_s1_no_unal2
83	ldr r5, [r1], #8
84	ldmia r0, {r3,r4}
85	mov r12,r5, lsl #16
86	add r3, r3, r12,asr #17
87	add r4, r4, r5, asr #17
88	stmia r0!,{r3,r4}
89
90	m16_32_s1_no_unal2:
91	tst r2, #1
92	ldmeqfd sp!, {r4-r6,pc}
93	ldrsh r4, [r1], #2
94	ldr r3, [r0]
95	add r3, r3, r4, asr #1
96	str r3, [r0], #4
97
98	ldmfd sp!, {r4-r6,lr}
99	bx lr
100
101
102
103	.global mix_16h_to_32_s2 @ int dest, short src, int count
104
105	mix_16h_to_32_s2:
106	stmfd sp!, {r4-r6,lr}
107
108	subs r2, r2, #4
109	bmi m16_32_s2_end
110
111	m16_32_s2_loop:
112	ldmia r0, {r3-r6}
113	ldr r12,[r1], #16
114	ldr lr, [r1], #16
115	subs r2, r2, #4
116	add r4, r4, r12,asr #17
117	mov r12,r12,lsl #16
118	add r3, r3, r12,asr #17 @ we use half volume
119	add r6, r6, lr, asr #17
120	mov lr, lr, lsl #16
121	add r5, r5, lr, asr #17
122	stmia r0!,{r3-r6}
123	bpl m16_32_s2_loop
124
125	m16_32_s2_end:
126	tst r2, #2
127	beq m16_32_s2_no_unal2
128	ldr r5, [r1], #16
129	ldmia r0, {r3,r4}
130	mov r12,r5, lsl #16
131	add r3, r3, r12,asr #17
132	add r4, r4, r5, asr #17
133	stmia r0!,{r3,r4}
134
135	m16_32_s2_no_unal2:
136	tst r2, #1
137	ldmeqfd sp!, {r4-r6,pc}
138	ldrsh r4, [r1], #2
139	ldr r3, [r0]
140	add r3, r3, r4, asr #1
141	str r3, [r0], #4
142
143	ldmfd sp!, {r4-r6,lr}
144	bx lr
145
146
147
148	@ limit
149	@ reg=int_sample, lr=1, r3=tmp, kills flags
150	.macro Limit reg
7a93adeb	151	add r3, lr, \reg, asr #15
4f265db7	152	bics r3, r3, #1 @ in non-overflow conditions r3 is 0 or 1
4f265db7	153	movne \reg, #0x8000
7a93adeb	154	subpl \reg, \reg, #1
4f265db7	155	.endm
	156
	157
	158	@ limit and shift up by 16
	159	@ reg=int_sample, lr=1, r3=tmp, kills flags
	160	.macro Limitsh reg
	161	@ movs r4, r3, asr #16
	162	@ cmnne r4, #1
	163	@ beq c32_16_no_overflow
	164	@ tst r4, r4
	165	@ mov r3, #0x8000
	166	@ subpl r3, r3, #1
	167
7a93adeb	168	add r3, lr, \reg, asr #15
4f265db7	169	bics r3, r3, #1 @ in non-overflow conditions r3 is 0 or 1
	170	moveq \reg, \reg, lsl #16
	171	movne \reg, #0x80000000
7a93adeb	172	subpl \reg, \reg, #0x00010000
4f265db7	173	.endm
	174
	175
	176	@ mix 32bit audio (with 16bits really used, upper bits indicate overflow) with normal 16 bit audio with left channel only
	177	@ warning: this function assumes dest is word aligned
	178	.global mix_32_to_16l_stereo @ short dest, int src, int count
	179
	180	mix_32_to_16l_stereo:
	181	stmfd sp!, {r4-r8,lr}
	182
	183	mov lr, #1
	184
	185	mov r2, r2, lsl #1
	186	subs r2, r2, #4
	187	bmi m32_16l_st_end
	188
	189	m32_16l_st_loop:
	190	ldmia r0, {r8,r12}
	191	ldmia r1!, {r4-r7}
	192	mov r8, r8, lsl #16
	193	mov r12,r12,lsl #16
	194	add r4, r4, r8, asr #16
	195	add r5, r5, r8, asr #16
	196	add r6, r6, r12,asr #16
	197	add r7, r7, r12,asr #16
	198	Limitsh r4
	199	Limitsh r5
	200	Limitsh r6
	201	Limitsh r7
	202	subs r2, r2, #4
	203	orr r4, r5, r4, lsr #16
	204	orr r5, r7, r6, lsr #16
	205	stmia r0!, {r4,r5}
	206	bpl m32_16l_st_loop
	207
	208	m32_16l_st_end:
	209	@ check for remaining bytes to convert
	210	tst r2, #2
	211	beq m32_16l_st_no_unal2
	212	ldrsh r6, [r0]
	213	ldmia r1!,{r4,r5}
	214	add r4, r4, r6
	215	add r5, r5, r6
	216	Limitsh r4
	217	Limitsh r5
	218	orr r4, r5, r4, lsr #16
	219	str r4, [r0], #4
	220
	221	m32_16l_st_no_unal2:
	222	ldmfd sp!, {r4-r8,lr}
	223	bx lr
	224
	225
	226	@ mix 32bit audio (with 16bits really used, upper bits indicate overflow) with normal 16 bit audio (for mono sound)
	227	.global mix_32_to_16_mono @ short dest, int src, int count
	228
	229	mix_32_to_16_mono:
	230	stmfd sp!, {r4-r8,lr}
	231
	232	mov lr, #1
	233
	234	@ check if dest is word aligned
	235	tst r0, #2
	236	beq m32_16_mo_no_unalw
7a93adeb	237	ldrsh r5, [r0]
4f265db7	238	ldr r4, [r1], #4
	239	sub r2, r2, #1
	240	add r4, r4, r5
	241	Limit r4
	242	strh r4, [r0], #2
	243
	244	m32_16_mo_no_unalw:
	245	subs r2, r2, #4
	246	bmi m32_16_mo_end
	247
	248	m32_16_mo_loop:
	249	ldmia r0, {r8,r12}
	250	ldmia r1!, {r4-r7}
	251	add r5, r5, r8, asr #16
	252	mov r8, r8, lsl #16
	253	add r4, r4, r8, asr #16
	254	add r7, r7, r12,asr #16
	255	mov r12,r12,lsl #16
	256	add r6, r6, r12,asr #16
	257	Limitsh r4
	258	Limitsh r5
	259	Limitsh r6
	260	Limitsh r7
	261	subs r2, r2, #4
	262	orr r4, r5, r4, lsr #16
	263	orr r5, r7, r6, lsr #16
	264	stmia r0!, {r4,r5}
	265	bpl m32_16_mo_loop
	266
	267	m32_16_mo_end:
	268	@ check for remaining bytes to convert
	269	tst r2, #2
	270	beq m32_16_mo_no_unal2
	271	ldr r6, [r0]
	272	ldmia r1!,{r4,r5}
	273	add r5, r5, r6, asr #16
	274	mov r6, r6, lsl #16
	275	add r4, r4, r6, asr #16
	276	Limitsh r4
	277	Limitsh r5
	278	orr r4, r5, r4, lsr #16
	279	str r4, [r0], #4
	280
	281	m32_16_mo_no_unal2:
	282	tst r2, #1
	283	ldmeqfd sp!, {r4-r8,pc}
7a93adeb	284	ldrsh r5, [r0]
4f265db7	285	ldr r4, [r1], #4
	286	add r4, r4, r5
	287	Limit r4
	288	strh r4, [r0], #2
	289
	290	ldmfd sp!, {r4-r8,lr}
	291	bx lr
	292