Plaquette
 
Loading...
Searching...
No Matches
pq_fixed32_math.h
1/*
2 * pq_fixed32_math.h
3 *
4 * Fixed32-point mathfunctions.
5 *
6 * (c) 2025 Sofian Audry :: info(@)sofianaudry(.)com
7 *
8 * Code imported from Audio Library for Teensy 3.X
9 * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com
10 *
11 * This program is free software: you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation, either version 3 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU General Public License for more details.
20 *
21 * You should have received a copy of the GNU General Public License
22 * along with this program. If not, see <http://www.gnu.org/licenses/>.
23 */
24
25#ifndef PQ_FIXED_MATH_H
26#define PQ_FIXED_MATH_H
27
28#include "pq_globals.h"
29#include <stdint.h>
30
31namespace pq {
32
33// computes limit((val >> rshift), 2**bits)
34static inline int32_t signed_saturate_rshift(int32_t val, int bits, int rshift) __attribute__((always_inline, unused));
35static inline int32_t signed_saturate_rshift(int32_t val, int bits, int rshift)
36{
37#if defined (__ARM_ARCH_7EM__)
38 int32_t out;
39 asm volatile("ssat %0, %1, %2, asr %3" : "=r" (out) : "I" (bits), "r" (val), "I" (rshift));
40 return out;
41#else
42 int32_t out, max;
43 out = val >> rshift;
44 max = 1 << (bits - 1);
45 if (out >= 0) {
46 if (out > max - 1) out = max - 1;
47 } else {
48 if (out < -max) out = -max;
49 }
50 return out;
51#endif
52}
53
54// computes limit(val, 2**bits)
55static inline int16_t saturate16(int32_t val) __attribute__((always_inline, unused));
56static inline int16_t saturate16(int32_t val)
57{
58#if defined (__ARM_ARCH_7EM__)
59 int16_t out;
60 int32_t tmp;
61 asm volatile("ssat %0, %1, %2" : "=r" (tmp) : "I" (16), "r" (val) );
62 out = (int16_t) (tmp);
63 return out;
64#else
65 if (val > 32767) val = 32767;
66 else if (val < -32768) val = -32768;
67 return val;
68#endif
69}
70
71// computes ((a[31:0] * b[15:0]) >> 16)
72static inline int32_t signed_multiply_32x16b(int32_t a, uint32_t b) __attribute__((always_inline, unused));
73static inline int32_t signed_multiply_32x16b(int32_t a, uint32_t b)
74{
75#if defined (__ARM_ARCH_7EM__)
76 int32_t out;
77 asm volatile("smulwb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
78 return out;
79#else
80 return ((int64_t)a * (int16_t)(b & 0xFFFF)) >> 16;
81#endif
82}
83
84// computes ((a[31:0] * b[31:16]) >> 16)
85static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b) __attribute__((always_inline, unused));
86static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b)
87{
88#if defined (__ARM_ARCH_7EM__)
89 int32_t out;
90 asm volatile("smulwt %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
91 return out;
92#else
93 return ((int64_t)a * (int16_t)(b >> 16)) >> 16;
94#endif
95}
96
97// computes (((int64_t)a[31:0] * (int64_t)b[31:0]) >> 32)
98static inline int32_t multiply_32x32_rshift32(int32_t a, int32_t b) __attribute__((always_inline, unused));
99static inline int32_t multiply_32x32_rshift32(int32_t a, int32_t b)
100{
101#if defined (__ARM_ARCH_7EM__)
102 int32_t out;
103 asm volatile("smmul %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
104 return out;
105#else
106 return ((int64_t)a * (int64_t)b) >> 32;
107#endif
108}
109
110// computes (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x80000000) >> 32)
111static inline int32_t multiply_32x32_rshift32_rounded(int32_t a, int32_t b) __attribute__((always_inline, unused));
112static inline int32_t multiply_32x32_rshift32_rounded(int32_t a, int32_t b)
113{
114#if defined (__ARM_ARCH_7EM__)
115 int32_t out;
116 asm volatile("smmulr %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
117 return out;
118#else
119 return (((int64_t)a * (int64_t)b) + 0x80000000) >> 32;
120#endif
121}
122
123// computes sum + (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x80000000) >> 32)
124static inline int32_t multiply_accumulate_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) __attribute__((always_inline, unused));
125static inline int32_t multiply_accumulate_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b)
126{
127#if defined (__ARM_ARCH_7EM__)
128 int32_t out;
129 asm volatile("smmlar %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));
130 return out;
131#else
132 return sum + ((((int64_t)a * (int64_t)b) + 0x80000000) >> 32);
133#endif
134}
135
136// computes sum - (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x80000000) >> 32)
137static inline int32_t multiply_subtract_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) __attribute__((always_inline, unused));
138static inline int32_t multiply_subtract_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b)
139{
140#if defined (__ARM_ARCH_7EM__)
141 int32_t out;
142 asm volatile("smmlsr %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));
143 return out;
144#else
145 return sum - ((((int64_t)a * (int64_t)b) + 0x80000000) >> 32);
146#endif
147}
148
149
150// computes (a[31:16] | (b[31:16] >> 16))
151static inline uint32_t pack_16t_16t(int32_t a, int32_t b) __attribute__((always_inline, unused));
152static inline uint32_t pack_16t_16t(int32_t a, int32_t b)
153{
154#if defined (__ARM_ARCH_7EM__)
155 int32_t out;
156 asm volatile("pkhtb %0, %1, %2, asr #16" : "=r" (out) : "r" (a), "r" (b));
157 return out;
158#else
159 return (a & 0xFFFF0000) | ((uint32_t)b >> 16);
160#endif
161}
162
163// computes (a[31:16] | b[15:0])
164static inline uint32_t pack_16t_16b(int32_t a, int32_t b) __attribute__((always_inline, unused));
165static inline uint32_t pack_16t_16b(int32_t a, int32_t b)
166{
167#if defined (__ARM_ARCH_7EM__)
168 int32_t out;
169 asm volatile("pkhtb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
170 return out;
171#else
172 return (a & 0xFFFF0000) | (b & 0x0000FFFF);
173#endif
174}
175
176// computes ((a[15:0] << 16) | b[15:0])
177static inline uint32_t pack_16b_16b(int32_t a, int32_t b) __attribute__((always_inline, unused));
178static inline uint32_t pack_16b_16b(int32_t a, int32_t b)
179{
180#if defined (__ARM_ARCH_7EM__)
181 int32_t out;
182 asm volatile("pkhbt %0, %1, %2, lsl #16" : "=r" (out) : "r" (b), "r" (a));
183 return out;
184#else
185 return (a << 16) | (b & 0x0000FFFF);
186#endif
187}
188
189// computes ((a[31:0] << 32) / b[31:0])
190static inline uint32_t divide_32div32(uint32_t a, uint32_t b) __attribute__((always_inline, unused));
191static inline uint32_t divide_32div32(uint32_t a, uint32_t b) {
192 return b ? (uint64_t(a) << 32) / b : 0xFFFFFFFF /* 2^32-1 UINT32_MAX */;
193}
194
195} // namespace pq
196
197
198#endif