diff options
-rw-r--r-- | apps/fracmul.h | 105 | ||||
-rw-r--r-- | firmware/include/gcc_extensions.h | 7 |
2 files changed, 64 insertions, 48 deletions
diff --git a/apps/fracmul.h b/apps/fracmul.h index 5cc83af624..5dbe4f9ef5 100644 --- a/apps/fracmul.h +++ b/apps/fracmul.h | |||
@@ -1,6 +1,9 @@ | |||
1 | #ifndef _FRACMUL_H | 1 | #ifndef _FRACMUL_H |
2 | #define _FRACMUL_H | 2 | #define _FRACMUL_H |
3 | 3 | ||
4 | #include <stdint.h> | ||
5 | #include "gcc_extensions.h" | ||
6 | |||
4 | /** FRACTIONAL MULTIPLICATION - TAKEN FROM apps/dsp.h | 7 | /** FRACTIONAL MULTIPLICATION - TAKEN FROM apps/dsp.h |
5 | * Multiply two fixed point numbers with 31 fractional bits: | 8 | * Multiply two fixed point numbers with 31 fractional bits: |
6 | * FRACMUL(x, y) | 9 | * FRACMUL(x, y) |
@@ -21,72 +24,78 @@ | |||
21 | /* Multiply two S.31 fractional integers and return the sign bit and the | 24 | /* Multiply two S.31 fractional integers and return the sign bit and the |
22 | * 31 most significant bits of the result. | 25 | * 31 most significant bits of the result. |
23 | */ | 26 | */ |
24 | #define FRACMUL(x, y) \ | 27 | static inline int32_t FRACMUL(int32_t x, int32_t y) |
25 | ({ \ | 28 | { |
26 | long t; \ | 29 | int32_t t; |
27 | asm ("mac.l %[a], %[b], %%acc0\n\t" \ | 30 | asm ("mac.l %[a], %[b], %%acc0\n\t" |
28 | "movclr.l %%acc0, %[t]\n\t" \ | 31 | "movclr.l %%acc0, %[t]\n\t" |
29 | : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); \ | 32 | : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); |
30 | t; \ | 33 | return t; |
31 | }) | 34 | } |
32 | 35 | ||
33 | /* Multiply two S.31 fractional integers, and return the 32 most significant | 36 | /* Multiply two S.31 fractional integers, and return the 32 most significant |
34 | * bits after a shift left by the constant z. NOTE: Only works for shifts of | 37 | * bits after a shift left by the constant z. NOTE: Only works for shifts of |
35 | * 1 to 8 on Coldfire! | 38 | * 1 to 8 on Coldfire! |
36 | */ | 39 | */ |
37 | #define FRACMUL_SHL(x, y, z) \ | 40 | static inline FORCE_INLINE int32_t FRACMUL_SHL(int32_t x, int32_t y, int z) |
38 | ({ \ | 41 | { |
39 | long t, t2; \ | 42 | int32_t t, t2; |
40 | asm ("mac.l %[a], %[b], %%acc0\n\t" \ | 43 | asm ("mac.l %[a], %[b], %%acc0\n\t" |
41 | "moveq.l %[d], %[t]\n\t" \ | 44 | "move.l %[d], %[t]\n\t" |
42 | "move.l %%accext01, %[t2]\n\t" \ | 45 | "move.l %%accext01, %[t2]\n\t" |
43 | "and.l %[mask], %[t2]\n\t" \ | 46 | "and.l %[mask], %[t2]\n\t" |
44 | "lsr.l %[t], %[t2]\n\t" \ | 47 | "lsr.l %[t], %[t2]\n\t" |
45 | "movclr.l %%acc0, %[t]\n\t" \ | 48 | "movclr.l %%acc0, %[t]\n\t" |
46 | "asl.l %[c], %[t]\n\t" \ | 49 | "asl.l %[c], %[t]\n\t" |
47 | "or.l %[t2], %[t]\n\t" \ | 50 | "or.l %[t2], %[t]\n\t" |
48 | : [t] "=&d" (t), [t2] "=&d" (t2) \ | 51 | : [t] "=&d" (t), [t2] "=&d" (t2) |
49 | : [a] "r" (x), [b] "r" (y), [mask] "d" (0xff), \ | 52 | : [a] "r" (x), [b] "r" (y), [mask] "d" (0xff), |
50 | [c] "i" ((z)), [d] "i" (8 - (z))); \ | 53 | [c] "id" ((z)), [d] "id" (8 - (z))); |
51 | t; \ | 54 | return t; |
52 | }) | 55 | } |
53 | 56 | ||
54 | #elif defined(CPU_ARM) | 57 | #elif defined(CPU_ARM) |
55 | 58 | ||
56 | /* Multiply two S.31 fractional integers and return the sign bit and the | 59 | /* Multiply two S.31 fractional integers and return the sign bit and the |
57 | * 31 most significant bits of the result. | 60 | * 31 most significant bits of the result. |
58 | */ | 61 | */ |
59 | #define FRACMUL(x, y) \ | 62 | static inline int32_t FRACMUL(int32_t x, int32_t y) |
60 | ({ \ | 63 | { |
61 | long t, t2; \ | 64 | int32_t t, t2; |
62 | asm ("smull %[t], %[t2], %[a], %[b]\n\t" \ | 65 | asm ("smull %[t], %[t2], %[a], %[b]\n\t" |
63 | "mov %[t2], %[t2], asl #1\n\t" \ | 66 | "mov %[t2], %[t2], asl #1\n\t" |
64 | "orr %[t], %[t2], %[t], lsr #31\n\t" \ | 67 | "orr %[t], %[t2], %[t], lsr #31\n\t" |
65 | : [t] "=&r" (t), [t2] "=&r" (t2) \ | 68 | : [t] "=&r" (t), [t2] "=&r" (t2) |
66 | : [a] "r" (x), [b] "r" (y)); \ | 69 | : [a] "r" (x), [b] "r" (y)); |
67 | t; \ | 70 | return t; |
68 | }) | 71 | } |
69 | 72 | ||
70 | /* Multiply two S.31 fractional integers, and return the 32 most significant | 73 | /* Multiply two S.31 fractional integers, and return the 32 most significant |
71 | * bits after a shift left by the constant z. | 74 | * bits after a shift left by the constant z. |
72 | */ | 75 | */ |
73 | #define FRACMUL_SHL(x, y, z) \ | 76 | static inline int32_t FRACMUL_SHL(int32_t x, int32_t y, int z) |
74 | ({ \ | 77 | { |
75 | long t, t2; \ | 78 | int32_t t, t2; \ |
76 | asm ("smull %[t], %[t2], %[a], %[b]\n\t" \ | 79 | asm ("smull %[t], %[t2], %[a], %[b]\n\t" |
77 | "mov %[t2], %[t2], asl %[c]\n\t" \ | 80 | "mov %[t2], %[t2], asl %[c]\n\t" |
78 | "orr %[t], %[t2], %[t], lsr %[d]\n\t" \ | 81 | "orr %[t], %[t2], %[t], lsr %[d]\n\t" |
79 | : [t] "=&r" (t), [t2] "=&r" (t2) \ | 82 | : [t] "=&r" (t), [t2] "=&r" (t2) |
80 | : [a] "r" (x), [b] "r" (y), \ | 83 | : [a] "r" (x), [b] "r" (y), |
81 | [c] "M" ((z) + 1), [d] "M" (31 - (z))); \ | 84 | [c] "M" ((z) + 1), [d] "M" (31 - (z))); |
82 | t; \ | 85 | return t; |
83 | }) | 86 | } |
84 | 87 | ||
85 | #else | 88 | #else |
86 | 89 | ||
87 | #define FRACMUL(x, y) (long) (((((long long) (x)) * ((long long) (y))) >> 31)) | 90 | static inline int32_t FRACMUL(int32_t x, int32_t y) |
88 | #define FRACMUL_SHL(x, y, z) \ | 91 | { |
89 | ((long)(((((long long) (x)) * ((long long) (y))) >> (31 - (z))))) | 92 | return (int32_t) (((int64_t)x * y) >> 31); |
93 | } | ||
94 | |||
95 | static inline int32_t FRACMUL_SHL(int32_t x, int32_t y, int z) | ||
96 | { | ||
97 | return (int32_t) (((int64_t)x * y) >> (31 - z)); | ||
98 | } | ||
90 | 99 | ||
91 | #endif | 100 | #endif |
92 | 101 | ||
diff --git a/firmware/include/gcc_extensions.h b/firmware/include/gcc_extensions.h index f7580f6ddc..7109edaaf6 100644 --- a/firmware/include/gcc_extensions.h +++ b/firmware/include/gcc_extensions.h | |||
@@ -50,4 +50,11 @@ | |||
50 | #define NORETURN_ATTR | 50 | #define NORETURN_ATTR |
51 | #endif | 51 | #endif |
52 | 52 | ||
53 | |||
54 | #if defined(__GNUC__) | ||
55 | #define FORCE_INLINE inline __attribute__((always_inline)) | ||
56 | #else | ||
57 | #define FORCE_INLINE inline | ||
58 | #endif | ||
59 | |||
53 | #endif /* _GCC_EXTENSIONS_H_ */ | 60 | #endif /* _GCC_EXTENSIONS_H_ */ |