[an error occurred while processing this directive] [an error occurred while processing this directive]
|
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;Scales a dword value in r1r0 as r1r0=r1r0*r2r3/2^32
;Equivalent to r1r0*K where 0<=K<=[(2^32)-1]/2^32, but much faster
;than the appropriate floating point routine
scale:
push r2,r3,r4,r5
mov r4,r0
mov r5,r1
mulu.w r0,r2
xch r0,r1
xch r0,r4
mulu.w r0,r3
add r4,r0
xch r0,r1
xch r0,r5
addc r5,#0
mov r1,r2
mov r2,r0
mulu.w r0,r1
add r4,r0
addc r1,r5
movs r0,#0
addc r0,#0
mulu.w r2,r3
add r1,r2
addc r0,r3
xch r1,r0
pop r2,r3,r4,r5
ret
;
;&&&&&&&&&&&&&&&&&&&&&&&&&&&
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;Puts a dword r1r0 to a 4-entry circular buffer and manages circular pointer.
;The array must be aligned by 16, i.e. LSB of array must be at xxx0 address.
;At input r6 must contain the address of the corresponding pointer.
ring32:
xch r4,[r6] ;Read current pointer position and save r4
rl r4l,#4 ;Synthetic modulo-16 addition affecting only 4 LSbs
add r4l,#60h;(add 60h instead of 6h to the swapped lower reg half)
rl r4l,#4 ;Only 4 LSbs of r4 are incremented by 6 (by 4 at the end)
xch r1,[r4] ;Put the data to new buffer position and
adds r4,#-2 ;read the oldest data to r2r0. Reverse (MSW-LSW) order
xch r0,[r4] ;of ops used to save time.
xch r4,[r6] ;Save new pointer value and restore r4
ret
;
;&&&&&&&&&&&&&&&&&&&&&&&&&&&
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;Applies a 2-3-2-1 filter to a circular array of 4 signed dwords.
;At input r6 must contain the ADDRESS of the array pointer (#xxxptr)
;result in r1r0 (normalized, outscale=inscale)
filt32:
push r2,r3,r6
mov r6,[r6] ;Read current array pointer position
mov r0,[r6]
mov r1,[r6+2]
sext r2
sext r3
add r0,r0
addc r1,r1
addc r2,r3
rl r6l,#4 ;Synthetic modulo-16 decrement by 4 affecting only 4 LSbs
sub r6l,#40h;(sub 40h instead of 4h from the swapped lower reg half)
rl r6l,#4 ;Only 4 LSbs of r6 have been decremented by 4
adds.w [r6+2],#0 ;Get 2nd operand's sign to N
sext r3
add r0,[r6]
addc r1,[r6+2]
addc r2,r3
add r0,[r6]
addc r1,[r6+2]
addc r2,r3
add r0,[r6]
addc r1,[r6+2]
addc r2,r3
rl r6l,#4 ;Point to previous sample
sub r6l,#40h
rl r6l,#4
adds.w [r6+2],#0
sext r3
add r0,[r6]
addc r1,[r6+2]
addc r2,r3
add r0,[r6]
addc r1,[r6+2]
addc r2,r3
rl r6l,#4 ;Point to previous sample
sub r6l,#40h
rl r6l,#4
adds.w [r6+2],#0
sext r3
add r0,[r6]
addc r1,[r6+2]
addc r2,r3
lsr.d r0,#3 ;Normalize to keep input scale intact
rr r2l,#3 ;3LSbs->3MSbs
and r2l,#0e0h ;Strip 5 LSbs
or r1h,r2l ;Insert 3 MSbs
pop r2,r3,r6
ret
;
;&&&&&&&&&&&&&&&&&&&&&&&&&&&
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;Calculates a Fourth Digital Difference of 5 last values in circular array
;[[r6]] and r1r0 (oldest) using FDD=(m0-4m1+6m2-4m3+m4)/16 standard formula,
;where m0=newest ([[r6]]), m4=oldest (r1r0 on entry) sample.
;Result=r1r0
fdd:
push r4,r5,r6
mov r6,[r6] ;Read current array pointer position
add r0,[r6] ;m4+m0
addc r1,[r6+2]
rl r6l,#4 ;Point to next sample
add r6l,#40h
rl r6l,#4
mov r4,[r6] ;4m1
mov r5,[r6+2]
asl.d r4,#2
sub r0,r4 ;-4m1
subb r1,r5
rl r6l,#4 ;Point to next sample
add r6l,#40h
rl r6l,#4
mov r4,[r6] ;6m2:
mov r5,[r6+2]
asl.d r4,#1 ;2m2
add r4,[r6] ;+m2=3m2
addc r5,[r6+2]
asl.d r4,#1 ;3m2*2=6m2
add r0,r4 ;+6m2
addc r1,r5
rl r6l,#4 ;Point to next sample
add r6l,#40h
rl r6l,#4
mov r4,[r6] ;4m3
mov r5,[r6+2]
asl.d r4,#2
sub r0,r4 ;-4m3
subb r1,r5
asr.d r0,#4 ;/16, keep sign
pop r4,r5,r6
ret
;
;&&&&&&&&&&&&&&&&&&&&&&&&&&&
E-mail: 
info@telesys.ru
