embedded_dsp/
basic_math.rs

1//! Basic math operations (absolute value, addition, subtraction, multiplication, dot product, negation, offset, scale, shift, clip, logic ops).
2
3use crate::types::*;
4
5// --- Absolute Value ---
6
7pub fn abs_f32(src: &[f32], dst: &mut [f32]) {
8    let len = src.len().min(dst.len());
9    for i in 0..len {
10        dst[i] = src[i].abs();
11    }
12}
13
14pub fn abs_f64(src: &[f64], dst: &mut [f64]) {
15    let len = src.len().min(dst.len());
16    for i in 0..len {
17        dst[i] = src[i].abs();
18    }
19}
20
21pub fn abs_q31(src: &[q31], dst: &mut [q31]) {
22    let len = src.len().min(dst.len());
23    for i in 0..len {
24        dst[i] = src[i].saturating_abs();
25    }
26}
27
28pub fn abs_q15(src: &[q15], dst: &mut [q15]) {
29    let len = src.len().min(dst.len());
30    for i in 0..len {
31        dst[i] = src[i].saturating_abs();
32    }
33}
34
35pub fn abs_q7(src: &[q7], dst: &mut [q7]) {
36    let len = src.len().min(dst.len());
37    for i in 0..len {
38        dst[i] = src[i].saturating_abs();
39    }
40}
41
42// --- Vector Addition ---
43
44pub fn add_f32(src_a: &[f32], src_b: &[f32], dst: &mut [f32]) {
45    let len = src_a.len().min(src_b.len()).min(dst.len());
46    for i in 0..len {
47        dst[i] = src_a[i] + src_b[i];
48    }
49}
50
51pub fn add_f64(src_a: &[f64], src_b: &[f64], dst: &mut [f64]) {
52    let len = src_a.len().min(src_b.len()).min(dst.len());
53    for i in 0..len {
54        dst[i] = src_a[i] + src_b[i];
55    }
56}
57
58pub fn add_q31(src_a: &[q31], src_b: &[q31], dst: &mut [q31]) {
59    let len = src_a.len().min(src_b.len()).min(dst.len());
60    for i in 0..len {
61        dst[i] = src_a[i].saturating_add(src_b[i]);
62    }
63}
64
65pub fn add_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
66    crate::intrinsics::simd_add_q15(src_a, src_b, dst);
67}
68
69pub fn add_q7(src_a: &[q7], src_b: &[q7], dst: &mut [q7]) {
70    let len = src_a.len().min(src_b.len()).min(dst.len());
71    for i in 0..len {
72        dst[i] = src_a[i].saturating_add(src_b[i]);
73    }
74}
75
76// --- Vector Subtraction ---
77
78pub fn sub_f32(src_a: &[f32], src_b: &[f32], dst: &mut [f32]) {
79    let len = src_a.len().min(src_b.len()).min(dst.len());
80    for i in 0..len {
81        dst[i] = src_a[i] - src_b[i];
82    }
83}
84
85pub fn sub_f64(src_a: &[f64], src_b: &[f64], dst: &mut [f64]) {
86    let len = src_a.len().min(src_b.len()).min(dst.len());
87    for i in 0..len {
88        dst[i] = src_a[i] - src_b[i];
89    }
90}
91
92pub fn sub_q31(src_a: &[q31], src_b: &[q31], dst: &mut [q31]) {
93    let len = src_a.len().min(src_b.len()).min(dst.len());
94    for i in 0..len {
95        dst[i] = src_a[i].saturating_sub(src_b[i]);
96    }
97}
98
99pub fn sub_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
100    crate::intrinsics::simd_sub_q15(src_a, src_b, dst);
101}
102
103pub fn sub_q7(src_a: &[q7], src_b: &[q7], dst: &mut [q7]) {
104    let len = src_a.len().min(src_b.len()).min(dst.len());
105    for i in 0..len {
106        dst[i] = src_a[i].saturating_sub(src_b[i]);
107    }
108}
109
110// --- Vector Multiplication ---
111
112pub fn mult_f32(src_a: &[f32], src_b: &[f32], dst: &mut [f32]) {
113    let len = src_a.len().min(src_b.len()).min(dst.len());
114    for i in 0..len {
115        dst[i] = src_a[i] * src_b[i];
116    }
117}
118
119pub fn mult_f64(src_a: &[f64], src_b: &[f64], dst: &mut [f64]) {
120    let len = src_a.len().min(src_b.len()).min(dst.len());
121    for i in 0..len {
122        dst[i] = src_a[i] * src_b[i];
123    }
124}
125
126pub fn mult_q31(src_a: &[q31], src_b: &[q31], dst: &mut [q31]) {
127    let len = src_a.len().min(src_b.len()).min(dst.len());
128    for i in 0..len {
129        dst[i] = q31_mult(src_a[i], src_b[i]);
130    }
131}
132
133pub fn mult_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
134    crate::intrinsics::simd_mult_q15(src_a, src_b, dst);
135}
136
137pub fn mult_q7(src_a: &[q7], src_b: &[q7], dst: &mut [q7]) {
138    let len = src_a.len().min(src_b.len()).min(dst.len());
139    for i in 0..len {
140        dst[i] = q7_mult(src_a[i], src_b[i]);
141    }
142}
143
144// --- Negate ---
145
146pub fn negate_f32(src: &[f32], dst: &mut [f32]) {
147    let len = src.len().min(dst.len());
148    for i in 0..len {
149        dst[i] = -src[i];
150    }
151}
152
153pub fn negate_f64(src: &[f64], dst: &mut [f64]) {
154    let len = src.len().min(dst.len());
155    for i in 0..len {
156        dst[i] = -src[i];
157    }
158}
159
160pub fn negate_q31(src: &[q31], dst: &mut [q31]) {
161    let len = src.len().min(dst.len());
162    for i in 0..len {
163        dst[i] = src[i].saturating_neg();
164    }
165}
166
167pub fn negate_q15(src: &[q15], dst: &mut [q15]) {
168    let len = src.len().min(dst.len());
169    for i in 0..len {
170        dst[i] = src[i].saturating_neg();
171    }
172}
173
174pub fn negate_q7(src: &[q7], dst: &mut [q7]) {
175    let len = src.len().min(dst.len());
176    for i in 0..len {
177        dst[i] = src[i].saturating_neg();
178    }
179}
180
181// --- Offset ---
182
183pub fn offset_f32(src: &[f32], offset: f32, dst: &mut [f32]) {
184    let len = src.len().min(dst.len());
185    for i in 0..len {
186        dst[i] = src[i] + offset;
187    }
188}
189
190pub fn offset_f64(src: &[f64], offset: f64, dst: &mut [f64]) {
191    let len = src.len().min(dst.len());
192    for i in 0..len {
193        dst[i] = src[i] + offset;
194    }
195}
196
197pub fn offset_q31(src: &[q31], offset: q31, dst: &mut [q31]) {
198    let len = src.len().min(dst.len());
199    for i in 0..len {
200        dst[i] = src[i].saturating_add(offset);
201    }
202}
203
204pub fn offset_q15(src: &[q15], offset: q15, dst: &mut [q15]) {
205    let len = src.len().min(dst.len());
206    for i in 0..len {
207        dst[i] = src[i].saturating_add(offset);
208    }
209}
210
211pub fn offset_q7(src: &[q7], offset: q7, dst: &mut [q7]) {
212    let len = src.len().min(dst.len());
213    for i in 0..len {
214        dst[i] = src[i].saturating_add(offset);
215    }
216}
217
218// --- Scale ---
219
220pub fn scale_f32(src: &[f32], scale: f32, dst: &mut [f32]) {
221    let len = src.len().min(dst.len());
222    for i in 0..len {
223        dst[i] = src[i] * scale;
224    }
225}
226
227pub fn scale_f64(src: &[f64], scale: f64, dst: &mut [f64]) {
228    let len = src.len().min(dst.len());
229    for i in 0..len {
230        dst[i] = src[i] * scale;
231    }
232}
233
234pub fn scale_q31(src: &[q31], scale_fract: q31, shift: i8, dst: &mut [q31]) {
235    let len = src.len().min(dst.len());
236    let k_shift = 31 - shift;
237    for i in 0..len {
238        let mult = (src[i].to_bits() as i64 * scale_fract.to_bits() as i64)
239            >> if k_shift >= 0 { k_shift as u32 } else { 0 };
240        let val = if k_shift < 0 {
241            mult << (-k_shift as u32)
242        } else {
243            mult
244        };
245        dst[i] = q31::from_bits(val.clamp(i32::MIN as i64, i32::MAX as i64) as i32);
246    }
247}
248
249pub fn scale_q15(src: &[q15], scale_fract: q15, shift: i8, dst: &mut [q15]) {
250    let len = src.len().min(dst.len());
251    let k_shift = 15 - shift;
252    for i in 0..len {
253        let mult = (src[i].to_bits() as i32 * scale_fract.to_bits() as i32)
254            >> if k_shift >= 0 { k_shift as u32 } else { 0 };
255        let val = if k_shift < 0 {
256            mult << (-k_shift as u32)
257        } else {
258            mult
259        };
260        dst[i] = q15::from_bits(val.clamp(i16::MIN as i32, i16::MAX as i32) as i16);
261    }
262}
263
264pub fn scale_q7(src: &[q7], scale_fract: q7, shift: i8, dst: &mut [q7]) {
265    let len = src.len().min(dst.len());
266    let k_shift = 7 - shift;
267    for i in 0..len {
268        let mult = (src[i].to_bits() as i32 * scale_fract.to_bits() as i32)
269            >> if k_shift >= 0 { k_shift as u32 } else { 0 };
270        let val = if k_shift < 0 {
271            mult << (-k_shift as u32)
272        } else {
273            mult
274        };
275        dst[i] = q7::from_bits(val.clamp(i8::MIN as i32, i8::MAX as i32) as i8);
276    }
277}
278
279// --- Shift ---
280
281pub fn shift_q31(src: &[q31], shift_bits: i8, dst: &mut [q31]) {
282    let len = src.len().min(dst.len());
283    for i in 0..len {
284        if shift_bits >= 0 {
285            dst[i] = q31::from_bits(
286                ((src[i].to_bits() as i64) << shift_bits).clamp(i32::MIN as i64, i32::MAX as i64)
287                    as i32,
288            );
289        } else {
290            dst[i] = q31::from_bits(src[i].to_bits() >> (-shift_bits));
291        }
292    }
293}
294
295pub fn shift_q15(src: &[q15], shift_bits: i8, dst: &mut [q15]) {
296    let len = src.len().min(dst.len());
297    for i in 0..len {
298        if shift_bits >= 0 {
299            dst[i] = q15::from_bits(
300                ((src[i].to_bits() as i32) << shift_bits).clamp(i16::MIN as i32, i16::MAX as i32)
301                    as i16,
302            );
303        } else {
304            dst[i] = q15::from_bits(src[i].to_bits() >> (-shift_bits));
305        }
306    }
307}
308
309pub fn shift_q7(src: &[q7], shift_bits: i8, dst: &mut [q7]) {
310    let len = src.len().min(dst.len());
311    for i in 0..len {
312        if shift_bits >= 0 {
313            dst[i] = q7::from_bits(
314                ((src[i].to_bits() as i32) << shift_bits).clamp(i8::MIN as i32, i8::MAX as i32)
315                    as i8,
316            );
317        } else {
318            dst[i] = q7::from_bits(src[i].to_bits() >> (-shift_bits));
319        }
320    }
321}
322
323// --- Dot Product ---
324
325pub fn dot_prod_f32(src_a: &[f32], src_b: &[f32]) -> f32 {
326    let len = src_a.len().min(src_b.len());
327    let mut sum = 0.0f32;
328    for i in 0..len {
329        sum += src_a[i] * src_b[i];
330    }
331    sum
332}
333
334pub fn dot_prod_f64(src_a: &[f64], src_b: &[f64]) -> f64 {
335    let len = src_a.len().min(src_b.len());
336    let mut sum = 0.0f64;
337    for i in 0..len {
338        sum += src_a[i] * src_b[i];
339    }
340    sum
341}
342
343pub fn dot_prod_q31(src_a: &[q31], src_b: &[q31]) -> q63 {
344    let len = src_a.len().min(src_b.len());
345    let mut sum: q63 = 0;
346    for i in 0..len {
347        sum += (src_a[i].to_bits() as i64 * src_b[i].to_bits() as i64) >> 14;
348    }
349    sum
350}
351
352pub fn dot_prod_q15(src_a: &[q15], src_b: &[q15]) -> q63 {
353    crate::intrinsics::simd_dot_prod_q15(src_a, src_b)
354}
355
356pub fn dot_prod_q7(src_a: &[q7], src_b: &[q7]) -> q31 {
357    let len = src_a.len().min(src_b.len());
358    let mut sum = q31::ZERO;
359    for i in 0..len {
360        sum += q31::from_bits(src_a[i].to_bits() as i32 * src_b[i].to_bits() as i32);
361    }
362    sum
363}
364
365// --- Clip ---
366
367pub fn clip_f32(src: &[f32], low: f32, high: f32, dst: &mut [f32]) {
368    let len = src.len().min(dst.len());
369    for i in 0..len {
370        dst[i] = src[i].clamp(low, high);
371    }
372}
373
374pub fn clip_q31(src: &[q31], low: q31, high: q31, dst: &mut [q31]) {
375    let len = src.len().min(dst.len());
376    for i in 0..len {
377        dst[i] = src[i].clamp(low, high);
378    }
379}
380
381pub fn clip_q15(src: &[q15], low: q15, high: q15, dst: &mut [q15]) {
382    let len = src.len().min(dst.len());
383    for i in 0..len {
384        dst[i] = src[i].clamp(low, high);
385    }
386}
387
388pub fn clip_q7(src: &[q7], low: q7, high: q7, dst: &mut [q7]) {
389    let len = src.len().min(dst.len());
390    for i in 0..len {
391        dst[i] = src[i].clamp(low, high);
392    }
393}
394
395// --- Logic Operations ---
396
397pub fn and_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
398    let len = src_a.len().min(src_b.len()).min(dst.len());
399    for i in 0..len {
400        dst[i] = src_a[i] & src_b[i];
401    }
402}
403
404pub fn and_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
405    let len = src_a.len().min(src_b.len()).min(dst.len());
406    for i in 0..len {
407        dst[i] = src_a[i] & src_b[i];
408    }
409}
410
411pub fn and_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
412    let len = src_a.len().min(src_b.len()).min(dst.len());
413    for i in 0..len {
414        dst[i] = src_a[i] & src_b[i];
415    }
416}
417
418pub fn or_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
419    let len = src_a.len().min(src_b.len()).min(dst.len());
420    for i in 0..len {
421        dst[i] = src_a[i] | src_b[i];
422    }
423}
424
425pub fn or_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
426    let len = src_a.len().min(src_b.len()).min(dst.len());
427    for i in 0..len {
428        dst[i] = src_a[i] | src_b[i];
429    }
430}
431
432pub fn or_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
433    let len = src_a.len().min(src_b.len()).min(dst.len());
434    for i in 0..len {
435        dst[i] = src_a[i] | src_b[i];
436    }
437}
438
439pub fn not_u32(src: &[u32], dst: &mut [u32]) {
440    let len = src.len().min(dst.len());
441    for i in 0..len {
442        dst[i] = !src[i];
443    }
444}
445
446pub fn not_u16(src: &[u16], dst: &mut [u16]) {
447    let len = src.len().min(dst.len());
448    for i in 0..len {
449        dst[i] = !src[i];
450    }
451}
452
453pub fn not_u8(src: &[u8], dst: &mut [u8]) {
454    let len = src.len().min(dst.len());
455    for i in 0..len {
456        dst[i] = !src[i];
457    }
458}
459
460pub fn xor_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
461    let len = src_a.len().min(src_b.len()).min(dst.len());
462    for i in 0..len {
463        dst[i] = src_a[i] ^ src_b[i];
464    }
465}
466
467pub fn xor_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
468    let len = src_a.len().min(src_b.len()).min(dst.len());
469    for i in 0..len {
470        dst[i] = src_a[i] ^ src_b[i];
471    }
472}
473
474pub fn xor_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
475    let len = src_a.len().min(src_b.len()).min(dst.len());
476    for i in 0..len {
477        dst[i] = src_a[i] ^ src_b[i];
478    }
479}
embedded_dsp/basic_math.rs

embedded_dsp/
basic_math.rs
