len0rd/rockbox
1
0
Fork 1
mirror of https://github.com/Rockbox/rockbox.git synced 2025-10-14 10:37:38 -04:00
Code Issues Projects Releases Packages Wiki Activity Actions

Introduce new hermite polynomial resampler.

Browse source 
Uses the Catmull-Rom case of Hermite cubic splines.

Vastly improves the quality and accuracy of audio resampling with a
rather minor additional overhead compared to the previous linear
implementation.

ARM and Coldfire assembly implementations included.

Change-Id: Ic45d84bc66c5b312ef373198297a952167a4be26
Reviewed-on: http://gerrit.rockbox.org/304
Reviewed-by: Michael Sevakis <jethead71@rockbox.org>
Tested-by: Michael Sevakis <jethead71@rockbox.org>
This commit is contained in:
Michael Sevakis 2012-05-07 03:12:56 -04:00
parent 91b850ec42
commit a7dee7f447
3 changed files with 537 additions and 254 deletions
Download patch file Download diff file Expand all files Collapse all files

296
lib/rbcodec/dsp/dsp_arm.S
View file

@ -289,114 +289,224 @@ crossfeed_meier_process:
ldmpc regs=r4-r10 @ restore non-volatile context, return ldmpc regs=r4-r10 @ restore non-volatile context, return
.size crossfeed_meier_process, .-crossfeed_meier_process .size crossfeed_meier_process, .-crossfeed_meier_process
/**************************************************************************** /****************************************************************************
* int resample_linear(struct resample_data *data, struct dsp_buffer *src, * int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
* struct dsp_buffer *dst) * struct dsp_buffer *dst)
*/ */
.section .text, "ax",%progbits .section .text, "ax",%progbits
.global resample_linear .global resample_hermite
resample_linear: resample_hermite:
@input: r0 = data, r1 = src, r2 = dst @input: r0 = data, r1 = src, r2 = dst
stmfd sp!, { r4-r11, lr } @ stack modified regs stmfd sp!, { r0-r2, r4-r11, lr } @ stack parms, modified regs
ldr r4, [r0] @ r4 = data->delta ldr r9, [r1] @ r9 = srcrem = src->remcount
add r10, r0, #4 @ r10 = &data->phase ldrb r10, [r1, #17] @ r10 = ch = num_channels
ldrb r3, [r1, #17] @ r3 = num_channels, ldr r14, [r0] @ r14 = data->delta, r0 = data
stmfd sp!, { r1, r10 } @ stack src, &data->phase
.lrs_channel_loop:
ldr r5, [r10] @ r5 = data->phase
ldr r6, [r1] @ r6 = srcrem = src->remcount
ldr r7, [r1, r3, lsl #2] @ r7 = src->p32[ch]
ldr r8, [r2, r3, lsl #2] @ r8 = dst->p32[ch]
ldr r9, [r2, #12] @ r9 = dstrem = dst->bufcount
cmp r6, #0x8000 @ srcrem = MIN(srcrem, 0x8000) cmp r9, #0x8000 @ srcrem = MIN(srcrem, 0x8000)
movgt r6, #0x8000 @ movgt r9, #0x8000 @
mov r0, r5, lsr #16 @ pos = MIN(pos, srcrem)
cmp r0, r6 @
movgt r0, r6 @ r0 = pos = phase >> 16
cmp r0, #0 @
ldrle r11, [r10, r3, lsl #2] @ pos <= 0? r11 = last = last_sample[ch]
addgt r12, r7, r0, lsl #2 @ pos > 0? r1 = last = s[pos - 1]
ldrgt r11, [r12, #-4] @
cmp r0, r6 @
bge .lrs_channel_done @ pos >= count? channel complete
cmp r4, #0x10000 @ delta >= 1.0? @ Channels are processed high to low while history is saved low to high
ldrhs r12, [r7, r0, lsl #2] @ yes? r12 = s[pos] @ It's really noone's business how we do this
bhs .lrs_dsstart @ yes? is downsampling add r12, r0, #8 @ r12 = h = data->history
.hrs_channel_loop:
stmfd sp!, { r10, r12 } @ push ch, h
ldr r5, [r0, #4] @ r5 = data->phase
ldr r6, [r1, r10, lsl #2] @ r6 = src->p32[ch]
ldr r7, [r2, r10, lsl #2] @ r7 = dst->p32[ch]
ldr r8, [r2, #12] @ r8 = dstrem = dst->bufcount
mov r0, r5, lsr #16 @ r0 = pos = phase >> 16
cmp r0, r9 @ r0 = pos = MIN(pos, srcrem)
movgt r0, r9 @
add r6, r6, r0, lsl #2 @ r6 = &s[pos]
cmp r0, #3 @ pos >= 3? history not needed
ldmgedb r6, { r1-r3 } @ x3..x1 = s[pos-3]..s[pos-1]
bge .hrs_loadhist_done @
add r10, r0, r0, lsl #1 @ branch pc + pos*12
add pc, pc, r10, lsl #2 @
nop @
ldmia r12, { r1-r3 } @ x3..x1 = h[0]..h[2]
b .hrs_loadhist_done @
nop @
ldmib r12, { r1-r2 } @ x3..x2 = h[1]..h[2]
ldr r3, [r6, #-4] @ x1 = s[0]
b .hrs_loadhist_done @
ldr r1, [r12, #8] @ x3 = h[2]
ldmdb r6, { r2-r3 } @ x2..x1 = s[0]..s[1]
.hrs_loadhist_done:
cmp r0, r9 @ pos past end?
bge .hrs_channel_done
cmp r14, #0x10000 @ delta >= 1.0?
bhs .hrs_dsstart @ yes? is downsampling
/** Upsampling **/ /** Upsampling **/
mov r5, r5, lsl #16 @ Move phase into high halfword str r9, [sp, #-4]! @ push srcrem
add r7, r7, r0, lsl #2 @ r7 = &s[pos] mov r5, r5, lsl #16 @ r5 = phase << 16
sub r0, r6, r0 @ r0 = dte = srcrem - pos sub r0, r9, r0 @ r0 = dte = srcrem - pos
.lrs_usloop_1: mov r14, r14, lsl #16 @ r14 = delta << 16
ldr r12, [r7], #4 @ r12 = s[pos]
sub r14, r12, r11 @ r14 = diff = s[pos] - s[pos - 1]
.lrs_usloop_0:
mov r1, r5, lsr #16 @ r1 = frac = phase >> 16
@ keep frac in Rs to take advantage of multiplier early termination
smull r1, r10, r14, r1 @ r1, r10 = diff * frac (lo, hi)
add r1, r11, r1, lsr #16 @ r1 = out = last + frac*diff
add r1, r1, r10, lsl #16 @
str r1, [r8], #4 @ *d++ = out
subs r9, r9, #1 @ destination full?
bls .lrs_usfull @ yes? channel is done
adds r5, r5, r4, lsl #16 @ phase += delta << 16
bcc .lrs_usloop_0 @ if carry is set, pos is incremented
subs r0, r0, #1 @ if srcrem > 0, do another sample
mov r11, r12 @ r11 = last = s[pos-1] (pos changed)
bgt .lrs_usloop_1
b .lrs_usdone
.lrs_usfull: @ Register usage in loop:
adds r5, r5, r4, lsl #16 @ do missed phase increment @ r0 = dte
subcs r0, r0, #1 @ do missed srcrem decrement @ r1 = x3, r2 = x2, r3 = x1, r4 = x0
movcs r11, r12 @ r11 = s[pos-1] (pos changed) @ r5 = phase << 16/frac, r6 = &s[pos], r7 = d, r8 = dstrem
@ r9 = scratch/acclo, r10 = scratch/acchi
@ r11 = c2, r12 = c3, c1 calculated in frac loop
@ r14 = delta << 16
@
@ Try to avoid overflow as much as possible and at the same time preserve
@ accuracy. Same formulas apply to downsampling but registers and
@ instruction order differ due to specific constraints.
@ c1 = -0.5*x3 + 0.5*x1
@ = 0.5*(x1 - x3) <--
@
@ v = x1 - x2, -v = x2 - x1
@ c2 = x3 - 2.5*x2 + 2*x1 - 0.5*x0
@ = x3 + 2*(x1 - x2) - 0.5*(x0 + x2)
@ = x3 + 2*v - 0.5*(x0 + x2) <--
@
@ c3 = -0.5*x3 + 1.5*x2 - 1.5*x1 + 0.5*x0
@ = 0.5*(x0 - x3 + (x2 - x1)) + (x2 - x1)
@ = 0.5*(x0 - x3 - v) - v <--
.hrs_usloop_carry:
ldr r4, [r6], #4 @ x0 = s[pos]
sub r9, r3, r2 @ r9 = v, r11 = c2, r12 = c3
add r11, r1, r9, asl #1 @
add r10, r4, r2 @
sub r12, r4, r1 @
sub r12, r12, r9 @
sub r11, r11, r10, asr #1 @
rsb r12, r9, r12, asr #1 @
.hrs_usloop_frac:
mov r5, r5, lsr #16 @ r5 = phase -> frac
smull r9, r10, r12, r5 @ acc = frac * c3 + c2
add r9, r11, r9, lsr #16 @
add r9, r9, r10, asl #16 @
smull r9, r10, r5, r9 @ acc = frac * acc + c1
mov r9, r9, lsr #16 @
orr r9, r9, r10, asl #16 @
sub r10, r3, r1 @
add r9, r9, r10, asr #1 @
smull r9, r10, r5, r9 @ acc = frac * acc + x2
subs r8, r8, #1 @ destination full?
add r9, r2, r9, lsr #16 @
add r9, r9, r10, asl #16 @
str r9, [r7], #4 @ *d++ = acc
bls .hrs_usfull @ yes? channel is done
adds r5, r14, r5, lsl #16 @ frac += delta
bcc .hrs_usloop_frac @ if carry is set, pos is incremented
.lrs_usdone: subs r0, r0, #1 @ if dte > 0, do another sample
sub r0, r6, r0 @ r0 = pos = srcrem - dte mov r1, r2 @ x3 = x2
orr r5, r5, r0 @ reconstruct swapped phase mov r2, r3 @ x2 = x1
mov r5, r5, ror #16 @ swap pos and frac for phase mov r3, r4 @ x1 = x0
b .lrs_channel_done @ bgt .hrs_usloop_carry
b .hrs_usdone
.hrs_usfull:
adds r5, r14, r5, lsl #16 @ do missed phase increment
bcc .hrs_usdone @
sub r0, r0, #1 @ do missed dte decrement
mov r1, r2 @ do missed history update
mov r2, r3 @
mov r3, r4 @
.hrs_usdone:
ldr r9, [sp], #4 @ r9 = pop srcrem
mov r14, r14, lsr #16 @ restore delta for next round
sub r0, r9, r0 @ r0 = pos = srcrem - dte
orr r5, r5, r0 @ reconstruct swapped phase
mov r5, r5, ror #16 @ swap pos and frac for phase
b .hrs_channel_done
/** Downsampling **/ /** Downsampling **/
.lrs_dsloop: @ Register usage in loop:
add r10, r7, r0, lsl #2 @ r10 = &s[pos] @ r0 = pos/frac
ldmda r10, { r11, r12 } @ r11 = last, r12 = s[pos] @ r1 = x3, r2 = x2, r3 = x1, r4 = x0
.lrs_dsstart: @ r5 = phase, r6 = &s[pos], r7 = d, r8 = dstrem
sub r14, r12, r11 @ r14 = diff = s[pos] - s[pos - 1] @ r9 = srcrem, r10 = scratch/acclo
@ keep frac in Rs to take advantage of multiplier early termination @ r11 = c2/scratch, r12 = c3/acchi
bic r1, r5, r0, lsl #16 @ frac = phase & 0xffff @ r14 = delta
smull r1, r10, r14, r1 @ r1, r10 = diff * frac (lo, hi) .hrs_dsloop_4:
add r5, r5, r4 @ phase += delta ldmdb r6, { r1-r3 } @ x3..x0 = s[pos-3]..s[pos-1]
subs r9, r9, #1 @ destination full? ... b .hrs_dsloop
mov r0, r5, lsr #16 @ pos = phase >> 16 .hrs_dsloop_3:
add r1, r11, r1, lsr #16 @ r1 = out = last + frac*diff ldmdb r6, { r2-r3 } @ x2..x0 = s[pos-2]..s[pos-1]
add r1, r1, r10, lsl #16 @ mov r1, r4 @ x3 = x0
str r1, [r8], #4 @ *d++ = out b .hrs_dsloop
cmpgt r6, r0 @ ... || pos >= srcrem? ... .hrs_dsloop_2:
bgt .lrs_dsloop @ ... no, do more samples mov r1, r3 @ x3 = x1
ldr r3, [r6, #-4] @ x1 = s[pos-1]
mov r2, r4 @ x2 = x0
b .hrs_dsloop
.hrs_dsloop_1: @ expected loop destination
mov r1, r2 @ x3 = x2
mov r2, r3 @ x2 = x1
mov r3, r4 @ x1 = x0
.hrs_dsloop:
subs r8, r8, #1 @ destination full?
cmpgt r9, r0 @ ... || pos >= srcrem?
ble .hrs_channel_done
.hrs_dsstart:
ldr r4, [r6] @ x0 = s[pos]
sub r10, r3, r2 @ r10 = v, r11 = c2, r12 = c3
add r11, r4, r2 @
bic r0, r5, r0, lsl #16 @ r0 = frac = phase & 0xffff
sub r11, r1, r11, asr #1 @
add r11, r11, r10, asl #1 @
sub r12, r4, r1 @
sub r12, r12, r10 @
rsb r12, r10, r12, asr #1 @
smull r10, r12, r0, r12 @ acc = frac * c3 + c2
add r10, r11, r10, lsr #16 @
add r10, r10, r12, asl #16 @
sub r11, r3, r1 @
smull r10, r12, r0, r10 @ acc = frac * acc + c1
mov r11, r11, asr #1 @
add r10, r11, r10, lsr #16 @
add r10, r10, r12, asl #16 @
smull r10, r12, r0, r10 @ acc = frac * acc + x2
mov r11, r5, lsr #16 @ r11 = last_pos
add r5, r5, r14 @ phase += delta
mov r0, r5, lsr #16 @ r0 = pos = phase >> 16
add r10, r2, r10, lsr #16 @
add r10, r10, r12, asl #16 @
str r10, [r7], #4 @ *d++ = acc
cmp r0, r6 @ pos = MIN(pos, srcrem) cmp r0, r9 @ r0 = pos = MIN(pos, srcrem)
movgt r0, r6 @ movgt r0, r9 @
sub r1, r0, #1 @ pos must always be > 0 since step >= 1.0 sub r11, r0, r11 @ shift = pos - last_pos
ldr r11, [r7, r1, lsl #2] @ r11 = s[pos - 1] cmp r11, #4 @
add r6, r6, r11, lsl #2 @ r6 += shift * 4
bge .hrs_dsloop_4 @
ldr pc, [pc, r11, lsl #2] @ branch to corresponding loop address
.word 0, 0
.word .hrs_dsloop_1
.word .hrs_dsloop_2
.word .hrs_dsloop_3
.lrs_channel_done: .hrs_channel_done:
ldmia sp, { r1, r10 } @ recover src, &data->phase ldmfd sp!, { r10, r12 } @ recover ch, h
str r11, [r10, r3, lsl #2] @ last_sample[ch] = last subs r10, r10, #1 @ --ch
subs r3, r3, #1 @ stmia r12!, { r1-r3 } @ h[0..2] = x3..x1
bgt .lrs_channel_loop @ ldmgtia sp, { r0-r2 } @ load data, src, dst
bgt .hrs_channel_loop
ldr r6, [r2, #12] @ r6 = dst->bufcount ldmfd sp!, { r1-r3 } @ pop data, src, dst
sub r5, r5, r0, lsl #16 @ r5 = phase - (pos << 16) sub r5, r5, r0, lsl #16 @ r5 = phase - (pos << 16)
str r5, [r10] @ data->phase = r5 ldr r2, [r3, #12] @ r2 = dst->bufcount
sub r6, r6, r9 @ r6 = dst->bufcount - dstrem = dstcount str r5, [r1, #4] @ data->phase = r5
str r6, [r2] @ dst->remcount = dstcount sub r2, r2, r8 @ r2 = dst->bufcount - dstrem
add sp, sp, #8 @ adjust stack for temp variables str r2, [r3] @ dst->remcount = r2
ldmpc regs=r4-r11 @ ... and we're out ldmpc regs=r4-r11 @ ... and we're out
.size resample_linear, .-resample_linear .size resample_hermite, .-resample_hermite
/**************************************************************************** /****************************************************************************
* void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p) * void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)

385
lib/rbcodec/dsp/dsp_cf.S
View file

@ -179,145 +179,286 @@ crossfeed_meier_process:
.size crossfeed_meier_process, .-crossfeed_meier_process .size crossfeed_meier_process, .-crossfeed_meier_process
/**************************************************************************** /****************************************************************************
* int resample_linear(struct resample_data *data, struct dsp_buffer *src, * int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
* struct dsp_buffer *dst) * struct dsp_buffer *dst)
*/ */
.section .text .section .text
.align 2 .align 2
.global resample_linear .global resample_hermite
resample_linear: resample_hermite:
| input: 4(sp) = data, 8(sp) = src, 12(sp) = dst | input: 4(sp) = data, 8(sp) = src, 12(sp) = dst
lea.l -44(%sp), %sp | save non-volatiles lea.l -52(%sp), %sp | save non-volatiles, allocate temps
movem.l %d2-%d7/%a2-%a6, (%sp) | movem.l %d2-%d7/%a2-%a6, 8(%sp) |
movem.l 48(%sp), %a0-%a2 | %a0 = data movem.l 56(%sp), %a0-%a2 | %a0 = data
| %a1 = src | %a1 = src
| %a2 = dst | %a2 = dst
clr.l %d1 | %d1 = ch = src->format.num_channels clr.l %d5 | %d5 = ch = src->format.num_channels
move.b 17(%a1), %d1 | move.b 17(%a1), %d5 |
moveq.l #16, %d7 | %d7 = shift lea.l 8(%a0), %a5 | %a5 = h = history[ch]
.lrs_channel_loop: | moveq.l #16, %d7 | %d7 = shift val
movem.l (%a0), %d2-%d3 | %d2 = delta = data->delta, .hrs_channel_loop: |
| %d3 = phase = data->phase movem.l %d5/%a5, (%sp) | store ch, h
move.l (%a1), %d4 | %d4 = srcrem = src->remcount movem.l (%a0), %d1-%d2 | %d1 = delta = data->delta,
move.l 12(%a2), %d5 | %d5 = dstrem = dst->bufcount | %d2 = phase = data->phase
cmp.l #0x8000, %d4 | %d4 = MIN(srcrem, 0x8000) move.l (%a1), %d3 | %d3 = srcrem = src->remcount
ble.b 10f | move.l 12(%a2), %d4 | %d4 = dstrem = dst->bufcount
move.l #0x8000, %d4 |
10: | cmp.l #0x8000, %d3 | %d4 = MIN(srcrem, 0x8000)
move.l (%a1, %d1.l*4), %a3 | %a3 = s = src->p32[ch] ble.b 1f |
move.l (%a2, %d1.l*4), %a4 | %a4 = d = dst->p32[ch] move.l #0x8000, %d3 |
move.l %d3, %d0 | %d0 = pos 1: |
lsr.l %d7, %d0 | ...
beq.b 11f | pos == 0? move.l (%a1, %d5.l*4), %a1 | %a1 = s = src->p32[ch]
cmp.l %d4, %d0 | pos = MIN(pos, srcrem) move.l (%a2, %d5.l*4), %a2 | %a2 = d = dst->p32[ch]
blt.b 12f |
move.l %d4, %d0 | pos = srcrem move.l %d2, %d0 | %d0 = pos = phase >> 16
move.l -4(%a3, %d0.l*4), %d6 | %d6 = last = s[pos - 1] lsr.l %d7, %d0 |
bra.w .lrs_channel_complete | at limit; nothing to do but next
11: | cmp.l %d3, %d0 | pos = MIN(pos, srcrem)
move.l 4(%a0, %d1.l*4), %d6 | %d6 = last = last_sample[ch] ble.b 1f |
tpf.l | trap next move.l (last = s[pos - 1]) move.l %d3, %d0 |
12: | 1:
move.l -4(%a3, %d0.l*4), %d6 | %d6 = last = s[pos - 1]
cmp.l #0x10000, %d2 | delta >= 1.0? lea.l (%a1, %d0.l*4), %a1 | %a1 = &s[pos]
bhs.b .lrs_downsample | yes? downsampling
cmp.l #3, %d0 |
bge.b 1f |
move.l %d0, %a0 |
lea.l (%a0, %a0.l*2), %a0 |
jmp 2(%pc, %a0.l*4) | 4b |
| 0
movem.l (%a5), %a3-%a5 | 4b | x3..x1 = h[0]..h[2]
bra.b 2f | 2b |
.dcb.w 3,0 | 6b | filler
| 1
movem.l 4(%a5), %a3-%a4 | 6b | x3..x2 = h[1]..h[2]
move.l -4(%a1), %a5 | 4b | x1 = s[0]
bra.b 2f | 2b |
| 2
move.l 8(%a5), %a3 | 4b | x3 = h[2]
movem.l -8(%a1), %a4-%a5 | 6b | x2..x1 = s[0]..s[1]
bra.b 2f | 2b |
1: | 3 +
movem.l -12(%a1), %a3-%a5 | x3...x1 = s[pos-3]..s[pos-1]
2:
cmp.l %d3, %d0 | pos past end?
bge.w .hrs_channel_done |
cmp.l #0x10000, %d1 | delta >= 1.0?
bhs.w .hrs_dsstart | yes? downsampling
| |
/** Upsampling **/ | /** Upsampling **/ |
lea.l (%a3, %d0.l*4), %a3 | %a3 = &s[pos] sub.l %d3, %d0 | %d0 = pos - srcrem = -dte
sub.l %d4, %d0 | %d0 = pos - srcrem = -dte lsl.l %d7, %d1 | move delta to bits 30..15
lsl.l %d7, %d2 | move delta to bits 30..15 lsr.l #1, %d1 |
lsl.l %d7, %d2 | move phase to bits 30..15
lsr.l #1, %d2 | lsr.l #1, %d2 |
lsl.l %d7, %d3 | move phase to bits 30..15 |
lsr.l #1, %d3 | | Register usage in loop:
move.l (%a3)+, %a5 | %a5 = s[pos] | r0 = dte, d1 = delta, d2 = phase, d3 = srcrem, d4 = dstrem
move.l %a5, %a6 | %a6 = diff = s[pos] - last | d5 = scratch, d6 = c3, d7 = scratch
sub.l %d6, %a6 | | a0 = c2, a1 = &s[pos], a2 = d,
bra.b 22f | | a3 = x3, a4 = x2, a5 = x1, a6 = x0
/* Funky loop structure is to avoid emac latency stalls */ |
20: | | Try to avoid overflow as much as possible and at the same time preserve
move.l (%a3)+, %a5 | %a5 = s[pos] | accuracy. Same formulas apply to downsampling but registers and
move.l %a5, %a6 | %a6 = diff = s[pos] - last | instruction order differ due to specific constraints.
sub.l %d6, %a6 | | c1 = -0.5*x3 + 0.5*x1
21: | | = 0.5*(x1 - x3) <--
movclr.l %acc0, %d7 | *d++ = %d7 = result |
move.l %d7, (%a4)+ | | v = x1 - x2, -v = x2 - x1
22: | | c2 = x3 - 2.5*x2 + 2*x1 - 0.5*x0
move.l %d6, %acc0 | %acc0 = last | = x3 + 2*(x1 - x2) - 0.5*(x0 + x2)
mac.l %d3, %a6, %acc0 | %acc0 += frac * diff | = x3 + 2*v - 0.5*(x0 + x2) <--
subq.l #1, %d5 | dstrem <= 0? |
ble.b 23f | yes? stop | c3 = -0.5*x3 + 1.5*x2 - 1.5*x1 + 0.5*x0
add.l %d2, %d3 | phase += delta | = 0.5*x0 - 0.5*x3 + 0.5*(x2 - x1) + (x2 - x1)
bpl.b 21b | load next values? | = 0.5*(x0 - x3 - v) - v <--
move.l %a5, %d6 | |
bclr.l #31, %d3 | clear sign bit .hrs_usloop_carry:
move.l (%a1)+, %a6 | %a6 = s[pos]
move.l %a5, %d5 | v
sub.l %a4, %d5 |
move.l %a6, %d6 | c3
sub.l %a3, %d6 |
sub.l %d5, %d6 |
asr.l #1, %d6 |
sub.l %d5, %d6 |
lea.l (%a3, %d5.l*2), %a0 | c2
move.l %a6, %d5 |
add.l %a4, %d5 |
asr.l #1, %d5 |
sub.l %d5, %a0 |
.hrs_usloop_frac:
move.l %a0, %acc0 | %acc0 = frac * c3 + c2
mac.l %d2, %d6, %acc0 |
move.l %a5, %d5 | c1
sub.l %a3, %d5 |
asr.l #1, %d5 |
movclr.l %acc0, %d7 | %acc1 = frac * acc + c1
move.l %d5, %acc1 |
mac.l %d2, %d7, %acc1 |
move.l %a4, %acc0 | %acc0 = frac * acc + x2
movclr.l %acc1, %d5 |
mac.l %d2, %d5, %acc0 |
subq.l #1, %d4 | dstrem <= 0?
ble.b .hrs_usfull | yes? stop
movclr.l %acc0, %d5 | *d++ = d5 = result
move.l %d5, (%a2)+ |
add.l %d1, %d2 | phase += delta
bpl.b .hrs_usloop_frac | load next values?
move.l %a4, %a3 | x3 = x2
move.l %a5, %a4 | x2 = x1
move.l %a6, %a5 | x1 = x0
bclr.l #31, %d2 | clear sign bit
addq.l #1, %d0 | dte > 0? addq.l #1, %d0 | dte > 0?
bmi.b 20b | yes? continue resampling bmi.b .hrs_usloop_carry | yes? continue resampling
tpf.w | trap next add.l (phase += delta) bra.b .hrs_usdone
23: |
add.l %d2, %d3 | phase += delta .hrs_usfull:
lsl.l #1, %d3 | frac -> phase movclr.l %acc0, %d5 | *d++ = d5 = result
bcs.b 24f | was sign bit set? move.l %d5, (%a2) |
tpf.l |
24: | add.l %d1, %d2 | do missed phase increment
move.l %a5, %d6 | yes? was going to move to new s[pos] bpl.b .hrs_usdone | was sign bit set?
addq.l #1, %d0 |
movclr.l %acc0, %d7 | *d = %d7 = result move.l %a4, %a3 | do missed history update
move.l %d7, (%a4) | move.l %a5, %a4 |
add.l %d4, %d0 | %d0 = -dte + srcrem = pos move.l %a6, %a5 |
or.l %d0, %d3 | restore phase
swap.w %d3 | addq.l #1, %d0 | do missed dte decrement
moveq.l #16, %d7 | %d7 = shift
bra.b .lrs_channel_complete | .hrs_usdone:
| moveq.l #16, %d7 | restore shift
/** Downsampling **/ | lsl.l #1, %d2 | frac -> phase
.lrs_downsample: | add.l %d3, %d0 | %d0 = -dte + srcrem = pos
move.l (%a3, %d0.l*4), %a5 | %a5 = s[pos] or.l %d0, %d2 | restore phase
bra.b 31f | swap.w %d2 |
30: |
lea.l -4(%a3, %d0.l*4), %a5 | %d6 = s[pos - 1], %a5 = s[pos] bra.w .hrs_channel_done |
movem.l (%a5), %d6/%a5 |
31: | /** Downsampling **/
move.l %d6, %acc0 | %acc0 = last |
sub.l %d6, %a5 | %a5 = diff = s[pos] - s[pos - 1] | Register usage in loop:
move.l %d3, %d0 | frac = (phase << 16) >> 1 | r0 = pos, d1 = delta, d2 = phase, d3 = srcrem, d4 = dstrem
lsl.l %d7, %d0 | | d5 = scratch, d6 = scratch, d7 = 16 (shift value)
lsr.l #1, %d0 | | a0 = scratch, a1 = &s[pos], a2 = d,
mac.l %d0, %a5, %acc0 | %acc0 += frac * diff | a3 = x3, a4 = x2, a5 = x1, a6 = x0
add.l %d2, %d3 | phase += delta |
move.l %d3, %d0 | pos = phase >> 16 .hrs_dsloop:
movclr.l %acc0, %d5 | *d++ = acc
move.l %d5, (%a2)+ |
sub.l %d0, %a0 | %a0 = -shift = last_pos - pos
move.l %a0, %d5 |
asl.l #2, %d5 | -shift -> -bytes
sub.l %d5, %a1 | %a1 = s = s - -bytes
cmp.l #-4, %a0 | >= 4?
ble.b 1f |
add.l %d5, %a0 | %a0 = 5 * -shift
jmp 40(%pc, %a0.l*2) | 4b |
1: | +4 +
movem.l -12(%a1), %a3-%a5 | 6b | x3..x0 = s[pos-3]..s[pos-1]
bra.b 1f | 2b |
| +3
move.l %a6, %a3 | 2b | x3 = x0
movem.l -8(%a1), %a4-%a5 | 6b | x2..x0 = s[pos-2]..s[pos-1]
bra.b 1f | 2b | 10
| +2
move.l %a5, %a3 | 2b | x3 = x1
move.l %a6, %a4 | 2b | x2 = x0
move.l -4(%a1), %a5 | 4b | x1 = s[pos-1]
bra.b 1f | 2b | 10
| +1
move.l %a4, %a3 | 2b | x3 = x2 | expected loop destination
move.l %a5, %a4 | 2b | x2 = x1
move.l %a6, %a5 | 2b | x1 = x0
1:
subq.l #1, %d4 | 2b | dstrem <= 0?
ble.b .hrs_channel_done | 2b | yes? stop
cmp.l %d3, %d0 |
bge.b .hrs_channel_done |
.hrs_dsstart:
move.l (%a1), %a6 | %a6 = s[pos]
move.l %a5, %d5 | v
sub.l %a4, %d5 |
move.l %a6, %d6 | c3
sub.l %a3, %d6 |
sub.l %d5, %d6 |
asr.l #1, %d6 |
sub.l %d5, %d6 |
lea.l (%a3, %d5.l*2), %a0 | c2
move.l %a6, %d5 |
add.l %a4, %d5 |
asr.l #1, %d5 |
sub.l %d5, %a0 |
move.l %d2, %d5 | phase -> frac
lsl.l %d7, %d5 |
lsr.l #1, %d5 |
move.l %a0, %acc0 | %acc0 = frac * c3 + c2
mac.l %d5, %d6, %acc0 |
move.l %a5, %d6 | c1
sub.l %a3, %d6 |
asr.l #1, %d6 |
movclr.l %acc0, %a0 | %acc1 = frac * acc + c1
move.l %d6, %acc1 |
mac.l %d5, %a0, %acc1 |
move.l %d0, %a0 | %a0 = last_pos
add.l %d1, %d2 | phase += delta
move.l %d2, %d0 | pos = phase >> 16
lsr.l %d7, %d0 | lsr.l %d7, %d0 |
movclr.l %acc0, %a5 |
move.l %a5, (%a4)+ | *d++ = %d0 movclr.l %acc1, %d6 | %acc0 = frac * acc + x2
subq.l #1, %d5 | dst full? move.l %a4, %acc0 |
ble.b 32f | yes? stop mac.l %d5, %d6, %acc0 |
cmp.l %d4, %d0 | pos < srcrem?
blt.b 30b | yes? continue resampling cmp.l %d3, %d0 | %d0 = MIN(pos, srcrem)
tpf.l | trap cmp.l and ble.b ble.w .hrs_dsloop |
32: | move.l %d3, %d0 |
cmp.l %d4, %d0 | pos = MIN(pos, srcrem) bra.w .hrs_dsloop |
ble.b 33f |
move.l %d4, %d0 | .hrs_channel_done: |
33: | movem.l (%sp), %d5/%a0 | restore ch, h
move.l -4(%a3, %d0.l*4), %d6 | %d6 = s[pos - 1] movem.l %a3-%a5, (%a0) | h[0..2] = x3..x1
| lea.l 12(%a0), %a5 | h++
.lrs_channel_complete: | movem.l 56(%sp), %a0-%a2 | load data, src, dst
move.l %d6, 4(%a0, %d1.l*4) | last_sample[ch] = last subq.l #1, %d5 | ch > 0?
subq.l #1, %d1 | ch > 0? bgt.w .hrs_channel_loop | yes? process next channel
bgt.w .lrs_channel_loop | yes? process next channel
|
move.l 12(%a2), %d1 | %d1 = dst->bufcount move.l 12(%a2), %d1 | %d1 = dst->bufcount
sub.l %d5, %d1 | written = dst->bufcount - dstrem sub.l %d4, %d1 | written = dst->bufcount - dstrem
move.l %d1, (%a2) | dst->remcount = written move.l %d1, (%a2) | dst->remcount = written
move.l %d0, %d1 | wrap phase to position in next frame move.l %d0, %d1 | wrap phase to position in next frame
lsl.l %d7, %d1 | data->phase = phase - (pos << 16) lsl.l %d7, %d1 | data->phase = phase - (pos << 16)
sub.l %d1, %d3 | ... sub.l %d1, %d2 |
move.l %d3, 4(%a0) | ... move.l %d2, 4(%a0) |
movem.l (%sp), %d2-%d7/%a2-%a6 | restore non-volatiles movem.l 8(%sp), %d2-%d7/%a2-%a6 | restore non-volatiles
lea.l 44(%sp), %sp | cleanup stack lea.l 52(%sp), %sp | cleanup stack
rts | buh-bye rts | buh-bye
.size resample_linear, .-resample_linear .size resample_hermite, .-resample_hermite
/**************************************************************************** /****************************************************************************
* void channel_mode_proc_mono(struct dsp_proc_entry *this, * void channel_mode_proc_mono(struct dsp_proc_entry *this,

110
lib/rbcodec/dsp/resample.c
View file

@ -9,6 +9,7 @@
* *
* Copyright (C) 2005 Miika Pekkarinen * Copyright (C) 2005 Miika Pekkarinen
* Copyright (C) 2012 Michael Sevakis * Copyright (C) 2012 Michael Sevakis
* Copyright (C) 2013 Michael Giacomelli
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU General Public License
@ -31,9 +32,7 @@
* of our inability to look into the future at the end of a frame. * of our inability to look into the future at the end of a frame.
*/ */
#if 0 /* Set to '1' to enable debug messages */ #if 1 /* Set to '0' to enable debug messages */
#include <debug.h>
#else
#undef DEBUGF #undef DEBUGF
#define DEBUGF(...) #define DEBUGF(...)
#endif #endif
@ -46,24 +45,24 @@ static int32_t resample_out_bufs[3][RESAMPLE_BUF_COUNT] IBSS_ATTR;
/* Data for each resampler on each DSP */ /* Data for each resampler on each DSP */
static struct resample_data static struct resample_data
{ {
uint32_t delta; /* 00h: Phase delta for each step */ uint32_t delta; /* 00h: Phase delta for each step in s15.16*/
uint32_t phase; /* 04h: Current phase [pos16|frac16] */ uint32_t phase; /* 04h: Current phase [pos16|frac16] */
int32_t last_sample[2]; /* 08h: Last samples for interpolation (L+R) */ int32_t history[2][3]; /* 08h: Last samples for interpolation (L+R)
/* 10h */ 0 = oldest, 2 = newest */
int32_t frequency; /* Virtual samplerate */ /* 20h */
int32_t frequency; /* Virtual samplerate */
struct dsp_buffer resample_buf; /* Buffer descriptor for resampled data */ struct dsp_buffer resample_buf; /* Buffer descriptor for resampled data */
int32_t *resample_out_p[2]; /* Actual output buffer pointers */ int32_t *resample_out_p[2]; /* Actual output buffer pointers */
} resample_data[DSP_COUNT] IBSS_ATTR; } resample_data[DSP_COUNT] IBSS_ATTR;
/* Actual worker function. Implemented here or in target assembly code. */ /* Actual worker function. Implemented here or in target assembly code. */
int resample_linear(struct resample_data *data, struct dsp_buffer *src, int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
struct dsp_buffer *dst); struct dsp_buffer *dst);
static void resample_flush_data(struct resample_data *data) static void resample_flush_data(struct resample_data *data)
{ {
data->phase = 0; data->phase = 0;
data->last_sample[0] = 0; memset(&data->history, 0, sizeof (data->history));
data->last_sample[1] = 0;
} }
static void resample_flush(struct dsp_proc_entry *this) static void resample_flush(struct dsp_proc_entry *this)
@ -84,8 +83,8 @@ static bool resample_new_delta(struct resample_data *data,
if (frequency == NATIVE_FREQUENCY) if (frequency == NATIVE_FREQUENCY)
{ {
/* NOTE: If fully glitch-free transistions from no resampling to /* NOTE: If fully glitch-free transistions from no resampling to
resampling are desired, last_sample history should be maintained resampling are desired, history should be maintained even when
even when not resampling. */ not resampling. */
resample_flush_data(data); resample_flush_data(data);
return false; return false;
} }
@ -94,9 +93,8 @@ static bool resample_new_delta(struct resample_data *data,
} }
#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM) #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
/* Where the real work is done */ int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
int resample_linear(struct resample_data *data, struct dsp_buffer *src, struct dsp_buffer *dst)
struct dsp_buffer *dst)
{ {
int ch = src->format.num_channels - 1; int ch = src->format.num_channels - 1;
uint32_t count = MIN(src->remcount, 0x8000); uint32_t count = MIN(src->remcount, 0x8000);
@ -111,35 +109,70 @@ int resample_linear(struct resample_data *data, struct dsp_buffer *src,
d = dst->p32[ch]; d = dst->p32[ch];
int32_t *dmax = d + dst->bufcount; int32_t *dmax = d + dst->bufcount;
/* Restore state */
phase = data->phase; phase = data->phase;
pos = phase >> 16; pos = phase >> 16;
pos = MIN(pos, count); pos = MIN(pos, count);
int32_t last = pos > 0 ? s[pos - 1] : data->last_sample[ch]; while (pos < count && d < dmax)
if (pos < count)
{ {
while (1) int x0, x1, x2, x3;
if (pos < 3)
{ {
*d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last); x3 = data->history[ch][pos+0];
phase += delta; x2 = pos < 2 ? data->history[ch][pos+1] : s[pos-2];
pos = phase >> 16; x1 = pos < 1 ? data->history[ch][pos+2] : s[pos-1];
}
if (pos >= count || d >= dmax) else
break; {
x3 = s[pos-3];
if (pos > 0) x2 = s[pos-2];
last = s[pos - 1]; x1 = s[pos-1];
} }
if (pos > 0) x0 = s[pos];
{
pos = MIN(pos, count); int32_t frac = (phase & 0xffff) << 15;
last = s[pos - 1];
} /* 4-point, 3rd-order Hermite/Catmull-Rom spline (x-form):
* c1 = -0.5*x3 + 0.5*x1
* = 0.5*(x1 - x3) <--
*
* v = x1 - x2, -v = x2 - x1
* c2 = x3 - 2.5*x2 + 2*x1 - 0.5*x0
* = x3 + 2*(x1 - x2) - 0.5*(x0 + x2)
* = x3 + 2*v - 0.5*(x0 + x2) <--
*
* c3 = -0.5*x3 + 1.5*x2 - 1.5*x1 + 0.5*x0
* = 0.5*x0 - 0.5*x3 + 0.5*(x2 - x1) + (x2 - x1)
* = 0.5*(x0 - x3 - v) - v <--
*
* polynomial coefficients */
int32_t c1 = (x1 - x3) >> 1;
int32_t v = x1 - x2;
int32_t c2 = x3 + 2*v - ((x0 + x2) >> 1);
int32_t c3 = ((x0 - x3 - v) >> 1) - v;
/* Evaluate polynomial at time 'frac'; Horner's rule. */
int32_t acc;
acc = FRACMUL(c3, frac) + c2;
acc = FRACMUL(acc, frac) + c1;
acc = FRACMUL(acc, frac) + x2;
*d++ = acc;
phase += delta;
pos = phase >> 16;
} }
data->last_sample[ch] = last; pos = MIN(pos, count);
/* Save delay samples for next time. Must do this even if pos was
* clamped before loop in order to keep record up to date. */
data->history[ch][0] = pos < 3 ? data->history[ch][pos+0] : s[pos-3];
data->history[ch][1] = pos < 2 ? data->history[ch][pos+1] : s[pos-2];
data->history[ch][2] = pos < 1 ? data->history[ch][pos+2] : s[pos-1];
} }
while (--ch >= 0); while (--ch >= 0);
@ -147,7 +180,6 @@ int resample_linear(struct resample_data *data, struct dsp_buffer *src,
data->phase = phase - (pos << 16); data->phase = phase - (pos << 16);
dst->remcount = d - dst->p32[0]; dst->remcount = d - dst->p32[0];
return pos; return pos;
} }
#endif /* CPU */ #endif /* CPU */
@ -175,7 +207,7 @@ static void resample_process(struct dsp_proc_entry *this,
{ {
dst->bufcount = RESAMPLE_BUF_COUNT; dst->bufcount = RESAMPLE_BUF_COUNT;
int consumed = resample_linear(data, src, dst); int consumed = resample_hermite(data, src, dst);
/* Advance src by consumed amount */ /* Advance src by consumed amount */
if (consumed > 0) if (consumed > 0)