summaryrefslogtreecommitdiff
path: root/n/asserv/src/dsp.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/dsp.c')
-rw-r--r--n/asserv/src/dsp.c86
1 files changed, 84 insertions, 2 deletions
diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c
index 9520513..2e940db 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 8.8. */
+/** Multiply signed 16 by fixed unsigned 8.8. */
extern inline int16_t
dsp_mul_i16f88 (int16_t a, uint16_t b)
{
@@ -64,7 +64,89 @@ dsp_mul_i16f88 (int16_t a, uint16_t b)
"add %A0, r0\n\t"
"adc %B0, r1\n\t"
"clr r1\n\t"
- : "=&r" (r) : "a" (a), "a" (b) : "r0", "r1");
+ : "=&r" (r) : "a" (a), "a" (b) : "r0");
+ return r;
+}
+
+/** Multiply f8.24 by f8.24. */
+int32_t
+dsp_mul_f824 (int32_t a, int32_t b)
+{
+ register int32_t ar asm ("r16"), br asm ("r20");
+ ar = a;
+ br = b;
+ int32_t r;
+ asm (""
+ "clr r2\n\t"
+ /* Low dword (>> 8, with 8 guards). */
+ "mul %A1, %B2\n\t"
+ "movw %A0, r0\n\t"
+ "clr %C0\n\t"
+ "clr %D0\n\t"
+ "mul %A1, %A2\n\t"
+ "add %A0, r1\n\t"
+ "adc %B0, r2\n\t"
+ "adc %C0, r2\n\t"
+ "mul %B1, %A2\n\t"
+ "add %A0, r0\n\t"
+ "adc %B0, r1\n\t"
+ "adc %C0, r2\n\t"
+ "mul %A1, %C2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mul %B1, %B2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mul %C1, %A2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ /* Shift. */
+ "movw %A0, %C0\n\t"
+ /* Upper word. */
+ "mulsu %D2, %C1\n\t"
+ "movw %C0, r0\n\t"
+ "mulsu %D2, %A1\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"
+ "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"
+ "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"
+ "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"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mul %C1, %C2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mulsu %D1, %B2\n\t"
+ "add %B0, r0\n\t"
+ "adc %C0, r1\n\t"
+ "adc %D0, r2\n\t"
+ "mulsu %D1, %C2\n\t"
+ "add %C0, r0\n\t"
+ "adc %D0, r1\n\t"
+ "muls %D1, %D2\n\t"
+ "add %D0, r0\n\t"
+ "clr r1\n\t"
+ : "=&r" (r) : "a" (ar), "a" (br) : "r0", "r2");
return r;
}