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

@ vim:filetype=armasm

.global memcpy32 @ int *dest, int *src, int count

memcpy32:
    stmfd   sp!, {r4,lr}

    subs    r2, r2, #4
    bmi     mcp32_fin

mcp32_loop:
    ldmia   r1!, {r3,r4,r12,lr}
    subs    r2, r2, #4
    stmia   r0!, {r3,r4,r12,lr}
    bpl     mcp32_loop

mcp32_fin:
    tst     r2, #3
    ldmeqfd sp!, {r4,pc}
    tst     r2, #1
    ldrne   r3, [r1], #4
    strne   r3, [r0], #4

mcp32_no_unal1:
    tst     r2, #2
    ldmneia r1!, {r3,r12}
    ldmfd   sp!, {r4,lr}
    stmneia r0!, {r3,r12}
    bx      lr


.global memset32 @ int *dest, int c, int count

memset32:
    stmfd   sp!, {lr}

    mov     r3, r1
    subs    r2, r2, #4
    bmi     mst32_fin

    mov     r12,r1
    mov     lr, r1

mst32_loop:
    subs    r2, r2, #4
    stmia   r0!, {r1,r3,r12,lr}
    bpl     mst32_loop

mst32_fin:
    tst     r2, #1
    strne   r1, [r0], #4

    tst     r2, #2
    stmneia r0!, {r1,r3}

    ldmfd   sp!, {lr}
    bx      lr


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