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

@ vim:filetype=armasm

@ Generic routines for mixing audio samples
@ (c) Copyright 2007, Grazvydas "notaz" Ignotas


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