From e7503d02686ce5b4bf01bd8b55e2eb2cda1c3ea6 Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 6 Nov 2004 19:38:15 +0000 Subject: Ajout de mul_f824 Test de cos --- n/asserv/src/dsp.c | 86 ++++++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 84 insertions(+), 2 deletions(-) (limited to 'n/asserv/src/dsp.c') 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; } -- cgit v1.2.3