[picodrive.git] / pico / sound / mix_arm.s

/*
 * Generic routines for mixing audio samples
 * (C) notaz, 2007,2008
 *
 * This work is licensed under the terms of MAME license.
 * See COPYING file in the top-level directory.
 */

.text
.align 4

@ 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


.data
.align 4

.global mix_32_to_16l_level
mix_32_to_16l_level:
    .word   0

.text
.align 4

@ same as mix_32_to_16l_stereo, but with additional shift
.global mix_32_to_16l_stereo_lvl @ short *dest, int *src, int count

mix_32_to_16l_stereo_lvl:
    stmfd   sp!, {r4-r9,lr}

    ldr     r9, =mix_32_to_16l_level
    mov     lr, #1
    ldr     r9, [r9]

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

m32_16l_st_l_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
    mov     r4, r4, asr r9
    mov     r5, r5, asr r9
    mov     r6, r6, asr r9
    mov     r7, r7, asr r9
    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_l_loop

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

m32_16l_st_l_no_unal2:
    ldmfd   sp!, {r4-r9,lr}
    bx      lr

@ vim:filetype=armasm
Commit	Line	Data
cff531af	1	/*
	2	* Generic routines for mixing audio samples
	3	* (C) notaz, 2007,2008
	4	*
	5	* This work is licensed under the terms of MAME license.
	6	* See COPYING file in the top-level directory.
	7	*/
4f265db7	8
4a32f01f	9	.text
	10	.align 4
	11
4f265db7	12	@ this assumes src is word aligned
	13	.global mix_16h_to_32 @ int dest, short src, int count
	14
	15	mix_16h_to_32:
	16	stmfd sp!, {r4-r6,lr}
	17	/*
	18	tst r1, #2
	19	beq m16_32_mo_unalw
	20	ldrsh r4, [r1], #2
	21	ldr r3, [r0]
	22	sub r2, r2, #1
	23	add r3, r3, r4, asr #1
	24	str r3, [r0], #4
	25	*/
	26	m16_32_mo_unalw:
	27	subs r2, r2, #4
	28	bmi m16_32_end
	29
	30	m16_32_loop:
	31	ldmia r0, {r3-r6}
	32	ldmia r1!,{r12,lr}
	33	subs r2, r2, #4
	34	add r4, r4, r12,asr #17 @ we use half volume
	35	mov r12,r12,lsl #16
	36	add r3, r3, r12,asr #17
	37	add r6, r6, lr, asr #17
	38	mov lr, lr, lsl #16
	39	add r5, r5, lr, asr #17
	40	stmia r0!,{r3-r6}
	41	bpl m16_32_loop
	42
	43	m16_32_end:
	44	tst r2, #2
	45	beq m16_32_no_unal2
	46	ldr r5, [r1], #4
	47	ldmia r0, {r3,r4}
	48	mov r12,r5, lsl #16
	49	add r3, r3, r12,asr #17
	50	add r4, r4, r5, asr #17
	51	stmia r0!,{r3,r4}
	52
	53	m16_32_no_unal2:
	54	tst r2, #1
	55	ldmeqfd sp!, {r4-r6,pc}
	56	ldrsh r4, [r1], #2
	57	ldr r3, [r0]
	58	add r3, r3, r4, asr #1
	59	str r3, [r0], #4
	60
	61	ldmfd sp!, {r4-r6,lr}
	62	bx lr
	63
	64
	65
	66	.global mix_16h_to_32_s1 @ int dest, short src, int count
	67
	68	mix_16h_to_32_s1:
	69	stmfd sp!, {r4-r6,lr}
	70
	71	subs r2, r2, #4
	72	bmi m16_32_s1_end
	73
	74	m16_32_s1_loop:
	75	ldmia r0, {r3-r6}
76	ldr r12,[r1], #8
77	ldr lr, [r1], #8
78	subs r2, r2, #4
79	add r4, r4, r12,asr #17
80	mov r12,r12,lsl #16
81	add r3, r3, r12,asr #17 @ we use half volume
82	add r6, r6, lr, asr #17
83	mov lr, lr, lsl #16
84	add r5, r5, lr, asr #17
85	stmia r0!,{r3-r6}
86	bpl m16_32_s1_loop
87
88	m16_32_s1_end:
89	tst r2, #2
90	beq m16_32_s1_no_unal2
91	ldr r5, [r1], #8
92	ldmia r0, {r3,r4}
93	mov r12,r5, lsl #16
94	add r3, r3, r12,asr #17
95	add r4, r4, r5, asr #17
96	stmia r0!,{r3,r4}
97
98	m16_32_s1_no_unal2:
99	tst r2, #1
100	ldmeqfd sp!, {r4-r6,pc}
101	ldrsh r4, [r1], #2
102	ldr r3, [r0]
103	add r3, r3, r4, asr #1
104	str r3, [r0], #4
105
106	ldmfd sp!, {r4-r6,lr}
107	bx lr
108
109
110
111	.global mix_16h_to_32_s2 @ int dest, short src, int count
112
113	mix_16h_to_32_s2:
114	stmfd sp!, {r4-r6,lr}
115
116	subs r2, r2, #4
117	bmi m16_32_s2_end
118
119	m16_32_s2_loop:
120	ldmia r0, {r3-r6}
121	ldr r12,[r1], #16
122	ldr lr, [r1], #16
123	subs r2, r2, #4
124	add r4, r4, r12,asr #17
125	mov r12,r12,lsl #16
126	add r3, r3, r12,asr #17 @ we use half volume
127	add r6, r6, lr, asr #17
128	mov lr, lr, lsl #16
129	add r5, r5, lr, asr #17
130	stmia r0!,{r3-r6}
131	bpl m16_32_s2_loop
132
133	m16_32_s2_end:
134	tst r2, #2
135	beq m16_32_s2_no_unal2
136	ldr r5, [r1], #16
137	ldmia r0, {r3,r4}
138	mov r12,r5, lsl #16
139	add r3, r3, r12,asr #17
140	add r4, r4, r5, asr #17
141	stmia r0!,{r3,r4}
142
143	m16_32_s2_no_unal2:
144	tst r2, #1
145	ldmeqfd sp!, {r4-r6,pc}
146	ldrsh r4, [r1], #2
147	ldr r3, [r0]
148	add r3, r3, r4, asr #1
149	str r3, [r0], #4
150
151	ldmfd sp!, {r4-r6,lr}
152	bx lr
153
154
155
156	@ limit
157	@ reg=int_sample, lr=1, r3=tmp, kills flags
158	.macro Limit reg
7a93adeb	159	add r3, lr, \reg, asr #15
4f265db7	160	bics r3, r3, #1 @ in non-overflow conditions r3 is 0 or 1
4f265db7	161	movne \reg, #0x8000
7a93adeb	162	subpl \reg, \reg, #1
4f265db7	163	.endm
	164
	165
	166	@ limit and shift up by 16
	167	@ reg=int_sample, lr=1, r3=tmp, kills flags
	168	.macro Limitsh reg
	169	@ movs r4, r3, asr #16
	170	@ cmnne r4, #1
	171	@ beq c32_16_no_overflow
	172	@ tst r4, r4
	173	@ mov r3, #0x8000
	174	@ subpl r3, r3, #1
	175
7a93adeb	176	add r3, lr, \reg, asr #15
4f265db7	177	bics r3, r3, #1 @ in non-overflow conditions r3 is 0 or 1
	178	moveq \reg, \reg, lsl #16
	179	movne \reg, #0x80000000
7a93adeb	180	subpl \reg, \reg, #0x00010000
4f265db7	181	.endm
	182
	183
	184	@ mix 32bit audio (with 16bits really used, upper bits indicate overflow) with normal 16 bit audio with left channel only
	185	@ warning: this function assumes dest is word aligned
	186	.global mix_32_to_16l_stereo @ short dest, int src, int count
	187
	188	mix_32_to_16l_stereo:
	189	stmfd sp!, {r4-r8,lr}
	190
	191	mov lr, #1
	192
	193	mov r2, r2, lsl #1
	194	subs r2, r2, #4
	195	bmi m32_16l_st_end
	196
	197	m32_16l_st_loop:
	198	ldmia r0, {r8,r12}
	199	ldmia r1!, {r4-r7}
	200	mov r8, r8, lsl #16
	201	mov r12,r12,lsl #16
	202	add r4, r4, r8, asr #16
	203	add r5, r5, r8, asr #16
	204	add r6, r6, r12,asr #16
	205	add r7, r7, r12,asr #16
	206	Limitsh r4
	207	Limitsh r5
	208	Limitsh r6
	209	Limitsh r7
	210	subs r2, r2, #4
	211	orr r4, r5, r4, lsr #16
	212	orr r5, r7, r6, lsr #16
	213	stmia r0!, {r4,r5}
	214	bpl m32_16l_st_loop
	215
	216	m32_16l_st_end:
	217	@ check for remaining bytes to convert
	218	tst r2, #2
	219	beq m32_16l_st_no_unal2
	220	ldrsh r6, [r0]
	221	ldmia r1!,{r4,r5}
	222	add r4, r4, r6
	223	add r5, r5, r6
	224	Limitsh r4
	225	Limitsh r5
	226	orr r4, r5, r4, lsr #16
	227	str r4, [r0], #4
	228
	229	m32_16l_st_no_unal2:
	230	ldmfd sp!, {r4-r8,lr}
	231	bx lr
	232
	233
	234	@ mix 32bit audio (with 16bits really used, upper bits indicate overflow) with normal 16 bit audio (for mono sound)
	235	.global mix_32_to_16_mono @ short dest, int src, int count
	236
	237	mix_32_to_16_mono:
	238	stmfd sp!, {r4-r8,lr}
	239
	240	mov lr, #1
	241
	242	@ check if dest is word aligned
	243	tst r0, #2
	244	beq m32_16_mo_no_unalw
7a93adeb	245	ldrsh r5, [r0]
4f265db7	246	ldr r4, [r1], #4
	247	sub r2, r2, #1
	248	add r4, r4, r5
	249	Limit r4
	250	strh r4, [r0], #2
	251
	252	m32_16_mo_no_unalw:
	253	subs r2, r2, #4
	254	bmi m32_16_mo_end
	255
	256	m32_16_mo_loop:
	257	ldmia r0, {r8,r12}
	258	ldmia r1!, {r4-r7}
	259	add r5, r5, r8, asr #16
	260	mov r8, r8, lsl #16
	261	add r4, r4, r8, asr #16
	262	add r7, r7, r12,asr #16
	263	mov r12,r12,lsl #16
	264	add r6, r6, r12,asr #16
	265	Limitsh r4
	266	Limitsh r5
	267	Limitsh r6
	268	Limitsh r7
	269	subs r2, r2, #4
	270	orr r4, r5, r4, lsr #16
	271	orr r5, r7, r6, lsr #16
	272	stmia r0!, {r4,r5}
	273	bpl m32_16_mo_loop
	274
	275	m32_16_mo_end:
	276	@ check for remaining bytes to convert
	277	tst r2, #2
	278	beq m32_16_mo_no_unal2
	279	ldr r6, [r0]
	280	ldmia r1!,{r4,r5}
	281	add r5, r5, r6, asr #16
	282	mov r6, r6, lsl #16
	283	add r4, r4, r6, asr #16
	284	Limitsh r4
	285	Limitsh r5
	286	orr r4, r5, r4, lsr #16
	287	str r4, [r0], #4
	288
	289	m32_16_mo_no_unal2:
	290	tst r2, #1
	291	ldmeqfd sp!, {r4-r8,pc}
7a93adeb	292	ldrsh r5, [r0]
4f265db7	293	ldr r4, [r1], #4
	294	add r4, r4, r5
	295	Limit r4
	296	strh r4, [r0], #2
	297
	298	ldmfd sp!, {r4-r8,lr}
	299	bx lr
	300
4a32f01f	301
	302
	303	.data
	304	.align 4
	305
	306	.global mix_32_to_16l_level
	307	mix_32_to_16l_level:
	308	.word 0
	309
	310	.text
	311	.align 4
	312
	313	@ same as mix_32_to_16l_stereo, but with additional shift
	314	.global mix_32_to_16l_stereo_lvl @ short dest, int src, int count
	315
	316	mix_32_to_16l_stereo_lvl:
	317	stmfd sp!, {r4-r9,lr}
	318
	319	ldr r9, =mix_32_to_16l_level
	320	mov lr, #1
	321	ldr r9, [r9]
	322
	323	mov r2, r2, lsl #1
	324	subs r2, r2, #4
	325	bmi m32_16l_st_l_end
	326
	327	m32_16l_st_l_loop:
	328	ldmia r0, {r8,r12}
	329	ldmia r1!, {r4-r7}
	330	mov r8, r8, lsl #16
	331	mov r12,r12,lsl #16
	332	add r4, r4, r8, asr #16
	333	add r5, r5, r8, asr #16
	334	add r6, r6, r12,asr #16
	335	add r7, r7, r12,asr #16
	336	mov r4, r4, asr r9
	337	mov r5, r5, asr r9
	338	mov r6, r6, asr r9
	339	mov r7, r7, asr r9
	340	Limitsh r4
	341	Limitsh r5
	342	Limitsh r6
	343	Limitsh r7
	344	subs r2, r2, #4
	345	orr r4, r5, r4, lsr #16
	346	orr r5, r7, r6, lsr #16
	347	stmia r0!, {r4,r5}
	348	bpl m32_16l_st_l_loop
	349
	350	m32_16l_st_l_end:
	351	@ check for remaining bytes to convert
	352	tst r2, #2
	353	beq m32_16l_st_l_no_unal2
	354	ldrsh r6, [r0]
	355	ldmia r1!,{r4,r5}
	356	add r4, r4, r6
	357	add r5, r5, r6
	358	mov r4, r4, asr r9
	359	mov r5, r5, asr r9
	360	Limitsh r4
	361	Limitsh r5
	362	orr r4, r5, r4, lsr #16
	363	str r4, [r0], #4
	364
365	m32_16l_st_l_no_unal2:
366	ldmfd sp!, {r4-r9,lr}
367	bx lr
368
cff531af	369	@ vim:filetype=armasm