1 files changed, 141 insertions, 4 deletions
diff --git a/apps/dsp_cf.S b/apps/dsp_cf.S
index 719d1db1d5..233be82860 100644
--- a/apps/dsp_cf.S
+++ b/apps/dsp_cf.S
@@ -17,8 +17,11 @@
 *
 ****************************************************************************/
-    .section .text
+/****************************************************************************
-    .global apply_crossfeed 
+ * apply_crossfeed(int32_t* src[], int count)
+ */
+    .section    .text
+    .global     apply_crossfeed 
 apply_crossfeed:
    lea.l (-44, %sp), %sp
    movem.l %d2-%d7/%a2-%a6, (%sp)      | save all regs
@@ -67,11 +70,11 @@ apply_crossfeed:
    addq.l #1, %d4                      | index++
    moveq.l #13, %d6
    cmp.l %d6, %d4                      | wrap index to 0 if it overflows
-    jlt .nowrap
+    jlt .cfnowrap
    moveq.l #13*8, %d4
    sub.l %d4, %a0                      | wrap back delay line ptr as well
    clr.l %d4
-.nowrap:
+.cfnowrap:
    subq.l #1, %d7
    jne .cfloop
    | save data back to struct
@@ -81,4 +84,138 @@ apply_crossfeed:
    movem.l (%sp), %d2-%d7/%a2-%a6
    lea.l (44, %sp), %sp
    rts
+.cfend:
+    .size       apply_crossfeed,.cfend-apply_crossfeed
+/****************************************************************************
+ * dsp_downsample(int channels, int count, struct resample_data *r,
+ *                in32_t **src, int32_t **dst)
+ */
+    .section    .text
+    .global     dsp_downsample
+dsp_downsample:
+        lea.l           -40(%sp), %sp           | save non-clobberables
+        movem.l     %d2-%d7/%a2-%a5, (%sp)  |
+        movem.l         44(%sp), %d2-%d3/%a0-%a2| %d2 = ch = channels
+                                        | %d3 = count
+                                        | %a0 = r
+                                                | %a1 = src
+                                        | %a2 = dst
+        move.l          4(%a0), %d4                             | %d4 = delta = r->delta
+        move.l          #16, %d7                | %d7 = shift
+.dschannel_loop:
+        move.l          (%a0), %d5                              | %d5 = phase = r->phase
+        move.l      -4(%a1, %d2.l*4), %a3       | %a3 = s = src[ch-1]
+        move.l      -4(%a2, %d2.l*4), %a4       | %a4 = d = dst[ch-1]
+        lea.l       4(%a0, %d2.l*4), %a5    | %a5 = &r->last_sample[ch-1]
+        move.l          (%a5), %d0                              | %d0 = last = r->last_sample[ch-1]
+        move.l      -4(%a3, %d3.l*4), %d1   | r->last_sample[ch-1] = s[count-1]
+        move.l      %d1, (%a5)                          |
+        move.l      %d5, %d6                            | %d6 = pos = phase >> 16
+        lsr.l       %d7, %d6                |
+        cmp.l       %d3, %d6                            | past end of samples?
+        bge.b       .dsloop_skip                        | yes? skip loop
+        tst.l       %d6                     | need last sample of prev. frame?
+        bne.b       .dsloop                             | no? start main loop
+        move.l      (%a3, %d6.l*4), %d1         | %d1 = s[pos]
+        bra.b       .dsuse_last_start           | start with last (last in %d0)
+.dsloop:
+        lea.l       -4(%a3, %d6.l*4), %a5   | load s[pos-1] and s[pos]
+        movem.l     (%a5), %d0-%d1          |
+.dsuse_last_start:
+        sub.l       %d0, %d1                | %d1 = diff = s[pos] - s[pos-1]
+        move.l      %d0, %acc0              | %acc0 = previous sample
+        move.l      %d5, %d0                | frac = (phase << 16) >> 1
+        lsl.l       %d7, %d0                |
+        lsr.l       #1, %d0                 |
+        mac.l       %d0, %d1, %acc0         | %acc0 += frac * diff
+        move.l      %acc0, %d0              |
+        add.l       %d4, %d5                | phase += delta
+        move.l      %d5, %d6                | pos = phase >> 16
+        lsr.l       %d7, %d6                |
+        move.l      %d0, (%a4)+             | *d++ = %d0
+        cmp.l       %d3, %d6                | pos < count?
+        blt.b       .dsloop                 | yes? continue resampling
+.dsloop_skip:
+        subq.l      #1, %d2                                     | ch > 0?
+        bgt.b           .dschannel_loop                 | yes? process next channel
+        asl.l       %d7, %d3                            | wrap phase to start of next frame
+        sub.l       %d3, %d5                | r->phase = phase - (count << 16)
+        move.l      %d5, (%a0)                          |
+        move.l      %a4, %d0                            | return d - d[0]
+        sub.l       (%a2), %d0                          |
+        asr.l       #2, %d0                 | convert bytes->samples
+        movem.l     (%sp), %d2-%d7/%a2-%a5  | restore non-clobberables
+        move.l          %acc1, %acc0                    | clear %acc0
+        lea.l       40(%sp), %sp            | cleanup stack
+        rts                                 | buh-bye
+.dsend:
+    .size       dsp_downsample,.dsend-dsp_downsample
+/****************************************************************************
+ * dsp_upsample(int channels, int count, struct resample_data *r,
+ *              in32_t **src, int32_t **dst)
+ */
+    .section    .text
+    .global     dsp_upsample
+dsp_upsample:
+        lea.l       -40(%sp), %sp                       | save non-clobberables
+        movem.l     %d2-%d7/%a2-%a5, (%sp)  |
+        movem.l         44(%sp), %d2-%d3/%a0-%a2| %d2 = ch = channels
+                                        | %d3 = count
+                                        | %a0 = r
+                                                | %a1 = src
+                                        | %a2 = dst
+        move.l          4(%a0), %d4                             | %d4 = delta = r->delta
+        swap        %d4                     | swap delta to high word to use
+                                        | carries to increment position
+.uschannel_loop:
+        move.l          (%a0), %d5                              | %d5 = phase = r->phase
+        move.l      -4(%a1, %d2.l*4), %a3       | %a3 = s = src[ch-1]
+        move.l      -4(%a2, %d2.l*4), %a4       | %a4 = d = dst[ch-1]
+        lea.l       4(%a0, %d2.l*4), %a5    | %a5 = &r->last_sample[ch-1]
+        move.l          (%a5), %d0                              | %d0 = last = r->last_sample[ch-1]
+        move.l      -4(%a3, %d3.l*4), %d1   | r->last_sample[ch-1] = s[count-1]
+        move.l      %d1, (%a5)                          |
+        moveq.l         #16, %d1                | %d0 = shift
+        move.l      %d5, %d6                            | %d6 = pos = phase >> 16
+        lsl.l       %d1, %d5                | swap phase to high word to use
+                                        | carries to increment position
+        lsr.l       %d1, %d6                | pos == 0?
+        bne.b       .usstart_1                  | no? transistion from down
+        move.l      (%a3), %d1                          | %d1 = s[0]
+        sub.l       %d0, %d1                | diff = s[pos] - last
+        bra.b       .usloop_0               | jump to typical start point
+.usstart_1:
+        cmp.l       %d3, %d6                | past end of samples?
+        bge.b       .usloop_skip            | yes? skip loop
+.usloop_1:
+        lea.l       -4(%a3, %d6.l*4), %a5   | load s[pos-1] and s[pos]
+        movem.l     (%a5), %d0-%d1          |
+        sub.l       %d0, %d1                | %d1 = diff = s[pos] - s[pos-1]
+.usloop_0:
+        move.l      %d0, %acc0                          | %acc0 = previous sample
+        lsr.l       #1, %d5                 | make phase into frac
+        mac.l       %d1, %d5, %acc0         | %acc0 += diff * frac
+        move.l          %acc0, %d7              |
+        lsl.l       #1, %d5                 | restore frac to phase
+        move.l      %d7, (%a4)+                         | *d++ = %d0
+        add.l       %d4, %d5                | phase += delta
+        bcc.b       .usloop_0                   | load next values?
+        addq.l      #1, %d6                 | increment position
+        cmp.l       %d3, %d6                            | pos < count?
+        blt.b       .usloop_1                   | yes? continue resampling
+.usloop_skip:
+        subq.l      #1, %d2                                     | ch > 0?
+        bgt.b           .uschannel_loop                 | yes? process next channel
+        swap        %d5                                         | wrap phase to start of next frame
+        move.l      %d5, (%a0)                          | ...and save in r->phase
+        move.l      %a4, %d0                            | return d - d[0]
+        sub.l       (%a2), %d0                          |
+        asr.l       #2, %d0                 | convert bytes->samples
+        movem.l     (%sp), %d2-%d7/%a2-%a5  | restore non-clobberables
+        move.l          %acc1, %acc0                    | clear %acc0
+        lea.l       40(%sp), %sp            | cleanup stack
+        rts                                 | buh-bye
+.usend:
+    .size       dsp_upsample,.usend-dsp_upsample

diff --git a/apps/dsp_cf.S b/apps/dsp_cf.S index 719d1db1d5..233be82860 100644 --- a/apps/dsp_cf.S +++ b/apps/dsp_cf.S
@@ -17,8 +17,11 @@
17	*	17	*
18	****************************************************************************/	18	****************************************************************************/
19		19
20	.section .text	20	/****************************************************************************
21	.global apply_crossfeed	21	* apply_crossfeed(int32_t* src[], int count)
		22	*/
		23	.section .text
		24	.global apply_crossfeed
22	apply_crossfeed:	25	apply_crossfeed:
23	lea.l (-44, %sp), %sp	26	lea.l (-44, %sp), %sp
24	movem.l %d2-%d7/%a2-%a6, (%sp) \| save all regs	27	movem.l %d2-%d7/%a2-%a6, (%sp) \| save all regs
@@ -67,11 +70,11 @@ apply_crossfeed:
67	addq.l #1, %d4 \| index++	70	addq.l #1, %d4 \| index++
68	moveq.l #13, %d6	71	moveq.l #13, %d6
69	cmp.l %d6, %d4 \| wrap index to 0 if it overflows	72	cmp.l %d6, %d4 \| wrap index to 0 if it overflows
70	jlt .nowrap	73	jlt .cfnowrap
71	moveq.l #13*8, %d4	74	moveq.l #13*8, %d4
72	sub.l %d4, %a0 \| wrap back delay line ptr as well	75	sub.l %d4, %a0 \| wrap back delay line ptr as well
73	clr.l %d4	76	clr.l %d4
74	.nowrap:	77	.cfnowrap:
75	subq.l #1, %d7	78	subq.l #1, %d7
76	jne .cfloop	79	jne .cfloop
77	\| save data back to struct	80	\| save data back to struct
@@ -81,4 +84,138 @@ apply_crossfeed:
81	movem.l (%sp), %d2-%d7/%a2-%a6	84	movem.l (%sp), %d2-%d7/%a2-%a6
82	lea.l (44, %sp), %sp	85	lea.l (44, %sp), %sp
83	rts	86	rts
		87	.cfend:
		88	.size apply_crossfeed,.cfend-apply_crossfeed
84		89
		90	/****************************************************************************
		91	* dsp_downsample(int channels, int count, struct resample_data *r,
		92	* in32_t src, int32_t dst)
		93	*/
		94	.section .text
		95	.global dsp_downsample
		96	dsp_downsample:
		97	lea.l -40(%sp), %sp \| save non-clobberables
		98	movem.l %d2-%d7/%a2-%a5, (%sp) \|
		99	movem.l 44(%sp), %d2-%d3/%a0-%a2\| %d2 = ch = channels
		100	\| %d3 = count
		101	\| %a0 = r
		102	\| %a1 = src
		103	\| %a2 = dst
		104	move.l 4(%a0), %d4 \| %d4 = delta = r->delta
		105	move.l #16, %d7 \| %d7 = shift
		106	.dschannel_loop:
		107	move.l (%a0), %d5 \| %d5 = phase = r->phase
		108	move.l -4(%a1, %d2.l*4), %a3 \| %a3 = s = src[ch-1]
		109	move.l -4(%a2, %d2.l*4), %a4 \| %a4 = d = dst[ch-1]
		110	lea.l 4(%a0, %d2.l*4), %a5 \| %a5 = &r->last_sample[ch-1]
		111	move.l (%a5), %d0 \| %d0 = last = r->last_sample[ch-1]
		112	move.l -4(%a3, %d3.l*4), %d1 \| r->last_sample[ch-1] = s[count-1]
		113	move.l %d1, (%a5) \|
		114	move.l %d5, %d6 \| %d6 = pos = phase >> 16
		115	lsr.l %d7, %d6 \|
		116	cmp.l %d3, %d6 \| past end of samples?
		117	bge.b .dsloop_skip \| yes? skip loop
		118	tst.l %d6 \| need last sample of prev. frame?
		119	bne.b .dsloop \| no? start main loop
		120	move.l (%a3, %d6.l*4), %d1 \| %d1 = s[pos]
		121	bra.b .dsuse_last_start \| start with last (last in %d0)
		122	.dsloop:
		123	lea.l -4(%a3, %d6.l*4), %a5 \| load s[pos-1] and s[pos]
		124	movem.l (%a5), %d0-%d1 \|
		125	.dsuse_last_start:
		126	sub.l %d0, %d1 \| %d1 = diff = s[pos] - s[pos-1]
		127	move.l %d0, %acc0 \| %acc0 = previous sample
		128	move.l %d5, %d0 \| frac = (phase << 16) >> 1
		129	lsl.l %d7, %d0 \|
		130	lsr.l #1, %d0 \|
		131	mac.l %d0, %d1, %acc0 \| %acc0 += frac * diff
		132	move.l %acc0, %d0 \|
		133	add.l %d4, %d5 \| phase += delta
		134	move.l %d5, %d6 \| pos = phase >> 16
		135	lsr.l %d7, %d6 \|
		136	move.l %d0, (%a4)+ \| *d++ = %d0
		137	cmp.l %d3, %d6 \| pos < count?
		138	blt.b .dsloop \| yes? continue resampling
		139	.dsloop_skip:
		140	subq.l #1, %d2 \| ch > 0?
		141	bgt.b .dschannel_loop \| yes? process next channel
		142	asl.l %d7, %d3 \| wrap phase to start of next frame
		143	sub.l %d3, %d5 \| r->phase = phase - (count << 16)
		144	move.l %d5, (%a0) \|
		145	move.l %a4, %d0 \| return d - d[0]
		146	sub.l (%a2), %d0 \|
		147	asr.l #2, %d0 \| convert bytes->samples
		148	movem.l (%sp), %d2-%d7/%a2-%a5 \| restore non-clobberables
		149	move.l %acc1, %acc0 \| clear %acc0
		150	lea.l 40(%sp), %sp \| cleanup stack
		151	rts \| buh-bye
		152	.dsend:
		153	.size dsp_downsample,.dsend-dsp_downsample
		154
		155	/****************************************************************************
		156	* dsp_upsample(int channels, int count, struct resample_data *r,
		157	* in32_t src, int32_t dst)
		158	*/
		159	.section .text
		160	.global dsp_upsample
		161	dsp_upsample:
		162	lea.l -40(%sp), %sp \| save non-clobberables
		163	movem.l %d2-%d7/%a2-%a5, (%sp) \|
		164	movem.l 44(%sp), %d2-%d3/%a0-%a2\| %d2 = ch = channels
		165	\| %d3 = count
		166	\| %a0 = r
		167	\| %a1 = src
		168	\| %a2 = dst
		169	move.l 4(%a0), %d4 \| %d4 = delta = r->delta
		170	swap %d4 \| swap delta to high word to use
		171	\| carries to increment position
		172	.uschannel_loop:
		173	move.l (%a0), %d5 \| %d5 = phase = r->phase
		174	move.l -4(%a1, %d2.l*4), %a3 \| %a3 = s = src[ch-1]
		175	move.l -4(%a2, %d2.l*4), %a4 \| %a4 = d = dst[ch-1]
		176	lea.l 4(%a0, %d2.l*4), %a5 \| %a5 = &r->last_sample[ch-1]
		177	move.l (%a5), %d0 \| %d0 = last = r->last_sample[ch-1]
		178	move.l -4(%a3, %d3.l*4), %d1 \| r->last_sample[ch-1] = s[count-1]
		179	move.l %d1, (%a5) \|
		180	moveq.l #16, %d1 \| %d0 = shift
		181	move.l %d5, %d6 \| %d6 = pos = phase >> 16
		182	lsl.l %d1, %d5 \| swap phase to high word to use
		183	\| carries to increment position
		184	lsr.l %d1, %d6 \| pos == 0?
		185	bne.b .usstart_1 \| no? transistion from down
		186	move.l (%a3), %d1 \| %d1 = s[0]
		187	sub.l %d0, %d1 \| diff = s[pos] - last
		188	bra.b .usloop_0 \| jump to typical start point
		189	.usstart_1:
		190	cmp.l %d3, %d6 \| past end of samples?
		191	bge.b .usloop_skip \| yes? skip loop
		192	.usloop_1:
		193	lea.l -4(%a3, %d6.l*4), %a5 \| load s[pos-1] and s[pos]
		194	movem.l (%a5), %d0-%d1 \|
		195	sub.l %d0, %d1 \| %d1 = diff = s[pos] - s[pos-1]
		196	.usloop_0:
		197	move.l %d0, %acc0 \| %acc0 = previous sample
		198	lsr.l #1, %d5 \| make phase into frac
		199	mac.l %d1, %d5, %acc0 \| %acc0 += diff * frac
		200	move.l %acc0, %d7 \|
		201	lsl.l #1, %d5 \| restore frac to phase
		202	move.l %d7, (%a4)+ \| *d++ = %d0
		203	add.l %d4, %d5 \| phase += delta
		204	bcc.b .usloop_0 \| load next values?
		205	addq.l #1, %d6 \| increment position
		206	cmp.l %d3, %d6 \| pos < count?
		207	blt.b .usloop_1 \| yes? continue resampling
		208	.usloop_skip:
		209	subq.l #1, %d2 \| ch > 0?
		210	bgt.b .uschannel_loop \| yes? process next channel
		211	swap %d5 \| wrap phase to start of next frame
		212	move.l %d5, (%a0) \| ...and save in r->phase
		213	move.l %a4, %d0 \| return d - d[0]
		214	sub.l (%a2), %d0 \|
		215	asr.l #2, %d0 \| convert bytes->samples
		216	movem.l (%sp), %d2-%d7/%a2-%a5 \| restore non-clobberables
		217	move.l %acc1, %acc0 \| clear %acc0
		218	lea.l 40(%sp), %sp \| cleanup stack
		219	rts \| buh-bye
		220	.usend:
		221	.size dsp_upsample,.usend-dsp_upsample