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    let len = src_a.len().min(src_b.len()).min(dst.len());
67    for i in 0..len {
68        dst[i] = src_a[i].saturating_add(src_b[i]);
69    }
70}
71
72pub fn add_q7(src_a: &[q7], src_b: &[q7], dst: &mut [q7]) {
73    let len = src_a.len().min(src_b.len()).min(dst.len());
74    for i in 0..len {
75        dst[i] = src_a[i].saturating_add(src_b[i]);
76    }
77}
78
79// --- Vector Subtraction ---
80
81pub fn sub_f32(src_a: &[f32], src_b: &[f32], dst: &mut [f32]) {
82    let len = src_a.len().min(src_b.len()).min(dst.len());
83    for i in 0..len {
84        dst[i] = src_a[i] - src_b[i];
85    }
86}
87
88pub fn sub_f64(src_a: &[f64], src_b: &[f64], dst: &mut [f64]) {
89    let len = src_a.len().min(src_b.len()).min(dst.len());
90    for i in 0..len {
91        dst[i] = src_a[i] - src_b[i];
92    }
93}
94
95pub fn sub_q31(src_a: &[q31], src_b: &[q31], dst: &mut [q31]) {
96    let len = src_a.len().min(src_b.len()).min(dst.len());
97    for i in 0..len {
98        dst[i] = src_a[i].saturating_sub(src_b[i]);
99    }
100}
101
102pub fn sub_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
103    let len = src_a.len().min(src_b.len()).min(dst.len());
104    for i in 0..len {
105        dst[i] = src_a[i].saturating_sub(src_b[i]);
106    }
107}
108
109pub fn sub_q7(src_a: &[q7], src_b: &[q7], dst: &mut [q7]) {
110    let len = src_a.len().min(src_b.len()).min(dst.len());
111    for i in 0..len {
112        dst[i] = src_a[i].saturating_sub(src_b[i]);
113    }
114}
115
116// --- Vector Multiplication ---
117
118pub fn mult_f32(src_a: &[f32], src_b: &[f32], dst: &mut [f32]) {
119    let len = src_a.len().min(src_b.len()).min(dst.len());
120    for i in 0..len {
121        dst[i] = src_a[i] * src_b[i];
122    }
123}
124
125pub fn mult_f64(src_a: &[f64], src_b: &[f64], dst: &mut [f64]) {
126    let len = src_a.len().min(src_b.len()).min(dst.len());
127    for i in 0..len {
128        dst[i] = src_a[i] * src_b[i];
129    }
130}
131
132pub fn mult_q31(src_a: &[q31], src_b: &[q31], dst: &mut [q31]) {
133    let len = src_a.len().min(src_b.len()).min(dst.len());
134    for i in 0..len {
135        dst[i] = q31_mult(src_a[i], src_b[i]);
136    }
137}
138
139pub fn mult_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
140    let len = src_a.len().min(src_b.len()).min(dst.len());
141    for i in 0..len {
142        dst[i] = q15_mult(src_a[i], src_b[i]);
143    }
144}
145
146pub fn mult_q7(src_a: &[q7], src_b: &[q7], dst: &mut [q7]) {
147    let len = src_a.len().min(src_b.len()).min(dst.len());
148    for i in 0..len {
149        dst[i] = q7_mult(src_a[i], src_b[i]);
150    }
151}
152
153// --- Negate ---
154
155pub fn negate_f32(src: &[f32], dst: &mut [f32]) {
156    let len = src.len().min(dst.len());
157    for i in 0..len {
158        dst[i] = -src[i];
159    }
160}
161
162pub fn negate_f64(src: &[f64], dst: &mut [f64]) {
163    let len = src.len().min(dst.len());
164    for i in 0..len {
165        dst[i] = -src[i];
166    }
167}
168
169pub fn negate_q31(src: &[q31], dst: &mut [q31]) {
170    let len = src.len().min(dst.len());
171    for i in 0..len {
172        dst[i] = src[i].saturating_neg();
173    }
174}
175
176pub fn negate_q15(src: &[q15], dst: &mut [q15]) {
177    let len = src.len().min(dst.len());
178    for i in 0..len {
179        dst[i] = src[i].saturating_neg();
180    }
181}
182
183pub fn negate_q7(src: &[q7], dst: &mut [q7]) {
184    let len = src.len().min(dst.len());
185    for i in 0..len {
186        dst[i] = src[i].saturating_neg();
187    }
188}
189
190// --- Offset ---
191
192pub fn offset_f32(src: &[f32], offset: f32, dst: &mut [f32]) {
193    let len = src.len().min(dst.len());
194    for i in 0..len {
195        dst[i] = src[i] + offset;
196    }
197}
198
199pub fn offset_f64(src: &[f64], offset: f64, dst: &mut [f64]) {
200    let len = src.len().min(dst.len());
201    for i in 0..len {
202        dst[i] = src[i] + offset;
203    }
204}
205
206pub fn offset_q31(src: &[q31], offset: q31, dst: &mut [q31]) {
207    let len = src.len().min(dst.len());
208    for i in 0..len {
209        dst[i] = src[i].saturating_add(offset);
210    }
211}
212
213pub fn offset_q15(src: &[q15], offset: q15, dst: &mut [q15]) {
214    let len = src.len().min(dst.len());
215    for i in 0..len {
216        dst[i] = src[i].saturating_add(offset);
217    }
218}
219
220pub fn offset_q7(src: &[q7], offset: q7, dst: &mut [q7]) {
221    let len = src.len().min(dst.len());
222    for i in 0..len {
223        dst[i] = src[i].saturating_add(offset);
224    }
225}
226
227// --- Scale ---
228
229pub fn scale_f32(src: &[f32], scale: f32, dst: &mut [f32]) {
230    let len = src.len().min(dst.len());
231    for i in 0..len {
232        dst[i] = src[i] * scale;
233    }
234}
235
236pub fn scale_f64(src: &[f64], scale: f64, dst: &mut [f64]) {
237    let len = src.len().min(dst.len());
238    for i in 0..len {
239        dst[i] = src[i] * scale;
240    }
241}
242
243pub fn scale_q31(src: &[q31], scale_fract: q31, shift: i8, dst: &mut [q31]) {
244    let len = src.len().min(dst.len());
245    let k_shift = 31 - shift;
246    for i in 0..len {
247        let mult =
248            (src[i] as i64 * scale_fract as i64) >> if k_shift >= 0 { k_shift as u32 } else { 0 };
249        let val = if k_shift < 0 {
250            mult << (-k_shift as u32)
251        } else {
252            mult
253        };
254        dst[i] = val.clamp(i32::MIN as i64, i32::MAX as i64) as i32;
255    }
256}
257
258pub fn scale_q15(src: &[q15], scale_fract: q15, shift: i8, dst: &mut [q15]) {
259    let len = src.len().min(dst.len());
260    let k_shift = 15 - shift;
261    for i in 0..len {
262        let mult =
263            (src[i] as i32 * scale_fract as i32) >> if k_shift >= 0 { k_shift as u32 } else { 0 };
264        let val = if k_shift < 0 {
265            mult << (-k_shift as u32)
266        } else {
267            mult
268        };
269        dst[i] = val.clamp(i16::MIN as i32, i16::MAX as i32) as i16;
270    }
271}
272
273pub fn scale_q7(src: &[q7], scale_fract: q7, shift: i8, dst: &mut [q7]) {
274    let len = src.len().min(dst.len());
275    let k_shift = 7 - shift;
276    for i in 0..len {
277        let mult =
278            (src[i] as i32 * scale_fract as i32) >> if k_shift >= 0 { k_shift as u32 } else { 0 };
279        let val = if k_shift < 0 {
280            mult << (-k_shift as u32)
281        } else {
282            mult
283        };
284        dst[i] = val.clamp(i8::MIN as i32, i8::MAX as i32) as i8;
285    }
286}
287
288// --- Shift ---
289
290pub fn shift_q31(src: &[q31], shift_bits: i8, dst: &mut [q31]) {
291    let len = src.len().min(dst.len());
292    for i in 0..len {
293        if shift_bits >= 0 {
294            dst[i] = ((src[i] as i64) << shift_bits).clamp(i32::MIN as i64, i32::MAX as i64) as i32;
295        } else {
296            dst[i] = src[i] >> (-shift_bits);
297        }
298    }
299}
300
301pub fn shift_q15(src: &[q15], shift_bits: i8, dst: &mut [q15]) {
302    let len = src.len().min(dst.len());
303    for i in 0..len {
304        if shift_bits >= 0 {
305            dst[i] = ((src[i] as i32) << shift_bits).clamp(i16::MIN as i32, i16::MAX as i32) as i16;
306        } else {
307            dst[i] = src[i] >> (-shift_bits);
308        }
309    }
310}
311
312pub fn shift_q7(src: &[q7], shift_bits: i8, dst: &mut [q7]) {
313    let len = src.len().min(dst.len());
314    for i in 0..len {
315        if shift_bits >= 0 {
316            dst[i] = ((src[i] as i32) << shift_bits).clamp(i8::MIN as i32, i8::MAX as i32) as i8;
317        } else {
318            dst[i] = src[i] >> (-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] as i64 * src_b[i] as i64) >> 14) as q63;
348    }
349    sum
350}
351
352pub fn dot_prod_q15(src_a: &[q15], src_b: &[q15]) -> q63 {
353    let len = src_a.len().min(src_b.len());
354    let mut sum: q63 = 0;
355    for i in 0..len {
356        sum += (src_a[i] as i32 * src_b[i] as i32) as q63;
357    }
358    sum
359}
360
361pub fn dot_prod_q7(src_a: &[q7], src_b: &[q7]) -> q31 {
362    let len = src_a.len().min(src_b.len());
363    let mut sum: q31 = 0;
364    for i in 0..len {
365        sum += (src_a[i] as i32 * src_b[i] as i32) as q31;
366    }
367    sum
368}
369
370// --- Clip ---
371
372pub fn clip_f32(src: &[f32], low: f32, high: f32, dst: &mut [f32]) {
373    let len = src.len().min(dst.len());
374    for i in 0..len {
375        dst[i] = src[i].clamp(low, high);
376    }
377}
378
379pub fn clip_q31(src: &[q31], low: q31, high: q31, dst: &mut [q31]) {
380    let len = src.len().min(dst.len());
381    for i in 0..len {
382        dst[i] = src[i].clamp(low, high);
383    }
384}
385
386pub fn clip_q15(src: &[q15], low: q15, high: q15, dst: &mut [q15]) {
387    let len = src.len().min(dst.len());
388    for i in 0..len {
389        dst[i] = src[i].clamp(low, high);
390    }
391}
392
393pub fn clip_q7(src: &[q7], low: q7, high: q7, dst: &mut [q7]) {
394    let len = src.len().min(dst.len());
395    for i in 0..len {
396        dst[i] = src[i].clamp(low, high);
397    }
398}
399
400// --- Logic Operations ---
401
402pub fn and_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
403    let len = src_a.len().min(src_b.len()).min(dst.len());
404    for i in 0..len {
405        dst[i] = src_a[i] & src_b[i];
406    }
407}
408
409pub fn and_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
410    let len = src_a.len().min(src_b.len()).min(dst.len());
411    for i in 0..len {
412        dst[i] = src_a[i] & src_b[i];
413    }
414}
415
416pub fn and_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
417    let len = src_a.len().min(src_b.len()).min(dst.len());
418    for i in 0..len {
419        dst[i] = src_a[i] & src_b[i];
420    }
421}
422
423pub fn or_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
424    let len = src_a.len().min(src_b.len()).min(dst.len());
425    for i in 0..len {
426        dst[i] = src_a[i] | src_b[i];
427    }
428}
429
430pub fn or_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
431    let len = src_a.len().min(src_b.len()).min(dst.len());
432    for i in 0..len {
433        dst[i] = src_a[i] | src_b[i];
434    }
435}
436
437pub fn or_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
438    let len = src_a.len().min(src_b.len()).min(dst.len());
439    for i in 0..len {
440        dst[i] = src_a[i] | src_b[i];
441    }
442}
443
444pub fn not_u32(src: &[u32], dst: &mut [u32]) {
445    let len = src.len().min(dst.len());
446    for i in 0..len {
447        dst[i] = !src[i];
448    }
449}
450
451pub fn not_u16(src: &[u16], dst: &mut [u16]) {
452    let len = src.len().min(dst.len());
453    for i in 0..len {
454        dst[i] = !src[i];
455    }
456}
457
458pub fn not_u8(src: &[u8], dst: &mut [u8]) {
459    let len = src.len().min(dst.len());
460    for i in 0..len {
461        dst[i] = !src[i];
462    }
463}
464
465pub fn xor_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
466    let len = src_a.len().min(src_b.len()).min(dst.len());
467    for i in 0..len {
468        dst[i] = src_a[i] ^ src_b[i];
469    }
470}
471
472pub fn xor_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
473    let len = src_a.len().min(src_b.len()).min(dst.len());
474    for i in 0..len {
475        dst[i] = src_a[i] ^ src_b[i];
476    }
477}
478
479pub fn xor_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
480    let len = src_a.len().min(src_b.len()).min(dst.len());
481    for i in 0..len {
482        dst[i] = src_a[i] ^ src_b[i];
483    }
484}
embedded_dsp/basic_math.rs

embedded_dsp/
basic_math.rs
