summaryrefslogtreecommitdiff
path: root/n/asserv/src/dsp.c
diff options
context:
space:
mode:
authorschodet2004-11-07 12:49:10 +0000
committerschodet2004-11-07 12:49:10 +0000
commit5b4e5290c4c840f79722691c584cafc5c4223db0 (patch)
treed775d29bd540a8cd0b9823bbddc17e359cc29076 /n/asserv/src/dsp.c
parente7503d02686ce5b4bf01bd8b55e2eb2cda1c3ea6 (diff)
Fixed and tested mul_f824.
Diffstat (limited to 'n/asserv/src/dsp.c')
-rw-r--r--n/asserv/src/dsp.c83
1 files changed, 46 insertions, 37 deletions
diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c
index 2e940db..bb81fdb 100644
--- a/n/asserv/src/dsp.c
+++ b/n/asserv/src/dsp.c
@@ -48,7 +48,7 @@ dsp_add_sat_i16i16 (int16_t a, int16_t b)
return a;
}
-/** Multiply signed 16 by fixed unsigned 8.8. */
+/** Multiply signed 16 by unsigned 8.8, return signed 16. */
extern inline int16_t
dsp_mul_i16f88 (int16_t a, uint16_t b)
{
@@ -68,7 +68,7 @@ dsp_mul_i16f88 (int16_t a, uint16_t b)
return r;
}
-/** Multiply f8.24 by f8.24. */
+/** Multiply signed 8.24 by signed 8.24, return signed 8.24. */
int32_t
dsp_mul_f824 (int32_t a, int32_t b)
{
@@ -76,77 +76,86 @@ dsp_mul_f824 (int32_t a, int32_t b)
ar = a;
br = b;
int32_t r;
+ int8_t z;
asm (""
- "clr r2\n\t"
+ "clr %1\n\t"
/* Low dword (>> 8, with 8 guards). */
- "mul %A1, %B2\n\t"
+ "mul %A2, %B3\n\t"
"movw %A0, r0\n\t"
"clr %C0\n\t"
"clr %D0\n\t"
- "mul %A1, %A2\n\t"
+ "mul %A2, %A3\n\t"
"add %A0, r1\n\t"
- "adc %B0, r2\n\t"
- "adc %C0, r2\n\t"
- "mul %B1, %A2\n\t"
+ "adc %B0, %1\n\t"
+ "adc %C0, %1\n\t"
+ "mul %B2, %A3\n\t"
"add %A0, r0\n\t"
"adc %B0, r1\n\t"
- "adc %C0, r2\n\t"
- "mul %A1, %C2\n\t"
+ "adc %C0, %1\n\t"
+ "mul %A2, %C3\n\t"
"add %B0, r0\n\t"
"adc %C0, r1\n\t"
- "adc %D0, r2\n\t"
- "mul %B1, %B2\n\t"
+ "adc %D0, %1\n\t"
+ "mul %B2, %B3\n\t"
"add %B0, r0\n\t"
"adc %C0, r1\n\t"
- "adc %D0, r2\n\t"
- "mul %C1, %A2\n\t"
+ "adc %D0, %1\n\t"
+ "mul %C2, %A3\n\t"
"add %B0, r0\n\t"
"adc %C0, r1\n\t"
- "adc %D0, r2\n\t"
+ "adc %D0, %1\n\t"
/* Shift. */
"movw %A0, %C0\n\t"
/* Upper word. */
- "mulsu %D2, %C1\n\t"
+ "mulsu %D3, %C2\n\t"
"movw %C0, r0\n\t"
- "mulsu %D2, %A1\n\t"
+ "mulsu %D3, %A2\n\t"
+ "sbc %C0, %1\n\t"
+ "sbc %D0, %1\n\t"
"add %A0, r0\n\t"
"adc %B0, r1\n\t"
- "adc %C0, r2\n\t"
- "adc %D0, r2\n\t"
- "mul %B1, %C2\n\t"
+ "adc %C0, %1\n\t"
+ "adc %D0, %1\n\t"
+ "mul %B2, %C3\n\t"
"add %A0, r0\n\t"
"adc %B0, r1\n\t"
- "adc %C0, r2\n\t"
- "adc %D0, r2\n\t"
- "mul %C1, %B2\n\t"
+ "adc %C0, %1\n\t"
+ "adc %D0, %1\n\t"
+ "mul %C2, %B3\n\t"
"add %A0, r0\n\t"
"adc %B0, r1\n\t"
- "adc %C0, r2\n\t"
- "adc %D0, r2\n\t"
- "mulsu %D1, %A2\n\t"
+ "adc %C0, %1\n\t"
+ "adc %D0, %1\n\t"
+ "mulsu %D2, %A3\n\t"
+ "sbc %C0, %1\n\t"
+ "sbc %D0, %1\n\t"
"add %A0, r0\n\t"
"adc %B0, r1\n\t"
- "adc %C0, r2\n\t"
- "adc %D0, r2\n\t"
- "mulsu %D2, %B1\n\t"
+ "adc %C0, %1\n\t"
+ "adc %D0, %1\n\t"
+ "mulsu %D3, %B2\n\t"
+ "sbc %D0, %1\n\t"
"add %B0, r0\n\t"
"adc %C0, r1\n\t"
- "adc %D0, r2\n\t"
- "mul %C1, %C2\n\t"
+ "adc %D0, %1\n\t"
+ "mul %C2, %C3\n\t"
"add %B0, r0\n\t"
"adc %C0, r1\n\t"
- "adc %D0, r2\n\t"
- "mulsu %D1, %B2\n\t"
+ "adc %D0, %1\n\t"
+ "mulsu %D2, %B3\n\t"
+ "sbc %D0, %1\n\t"
"add %B0, r0\n\t"
"adc %C0, r1\n\t"
- "adc %D0, r2\n\t"
- "mulsu %D1, %C2\n\t"
+ "adc %D0, %1\n\t"
+ "mulsu %D2, %C3\n\t"
"add %C0, r0\n\t"
"adc %D0, r1\n\t"
- "muls %D1, %D2\n\t"
+ "muls %D2, %D3\n\t"
"add %D0, r0\n\t"
"clr r1\n\t"
- : "=&r" (r) : "a" (ar), "a" (br) : "r0", "r2");
+ : "=&r" (r), "=r" (z) : "a" (ar), "a" (br) : "r0");
return r;
}
+/** Compute cosinus. */
+