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 =
239            (src[i] as i64 * scale_fract as i64) >> 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] = 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 =
254            (src[i] as i32 * scale_fract as i32) >> 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] = 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 =
269            (src[i] as i32 * scale_fract as i32) >> 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] = 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] = ((src[i] as i64) << shift_bits).clamp(i32::MIN as i64, i32::MAX as i64) as i32;
286        } else {
287            dst[i] = src[i] >> (-shift_bits);
288        }
289    }
290}
291
292pub fn shift_q15(src: &[q15], shift_bits: i8, dst: &mut [q15]) {
293    let len = src.len().min(dst.len());
294    for i in 0..len {
295        if shift_bits >= 0 {
296            dst[i] = ((src[i] as i32) << shift_bits).clamp(i16::MIN as i32, i16::MAX as i32) as i16;
297        } else {
298            dst[i] = src[i] >> (-shift_bits);
299        }
300    }
301}
302
303pub fn shift_q7(src: &[q7], shift_bits: i8, dst: &mut [q7]) {
304    let len = src.len().min(dst.len());
305    for i in 0..len {
306        if shift_bits >= 0 {
307            dst[i] = ((src[i] as i32) << shift_bits).clamp(i8::MIN as i32, i8::MAX as i32) as i8;
308        } else {
309            dst[i] = src[i] >> (-shift_bits);
310        }
311    }
312}
313
314// --- Dot Product ---
315
316pub fn dot_prod_f32(src_a: &[f32], src_b: &[f32]) -> f32 {
317    let len = src_a.len().min(src_b.len());
318    let mut sum = 0.0f32;
319    for i in 0..len {
320        sum += src_a[i] * src_b[i];
321    }
322    sum
323}
324
325pub fn dot_prod_f64(src_a: &[f64], src_b: &[f64]) -> f64 {
326    let len = src_a.len().min(src_b.len());
327    let mut sum = 0.0f64;
328    for i in 0..len {
329        sum += src_a[i] * src_b[i];
330    }
331    sum
332}
333
334pub fn dot_prod_q31(src_a: &[q31], src_b: &[q31]) -> q63 {
335    let len = src_a.len().min(src_b.len());
336    let mut sum: q63 = 0;
337    for i in 0..len {
338        sum += ((src_a[i] as i64 * src_b[i] as i64) >> 14) as q63;
339    }
340    sum
341}
342
343pub fn dot_prod_q15(src_a: &[q15], src_b: &[q15]) -> q63 {
344    crate::intrinsics::simd_dot_prod_q15(src_a, src_b)
345}
346
347pub fn dot_prod_q7(src_a: &[q7], src_b: &[q7]) -> q31 {
348    let len = src_a.len().min(src_b.len());
349    let mut sum: q31 = 0;
350    for i in 0..len {
351        sum += (src_a[i] as i32 * src_b[i] as i32) as q31;
352    }
353    sum
354}
355
356// --- Clip ---
357
358pub fn clip_f32(src: &[f32], low: f32, high: f32, dst: &mut [f32]) {
359    let len = src.len().min(dst.len());
360    for i in 0..len {
361        dst[i] = src[i].clamp(low, high);
362    }
363}
364
365pub fn clip_q31(src: &[q31], low: q31, high: q31, dst: &mut [q31]) {
366    let len = src.len().min(dst.len());
367    for i in 0..len {
368        dst[i] = src[i].clamp(low, high);
369    }
370}
371
372pub fn clip_q15(src: &[q15], low: q15, high: q15, dst: &mut [q15]) {
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_q7(src: &[q7], low: q7, high: q7, dst: &mut [q7]) {
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
386// --- Logic Operations ---
387
388pub fn and_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
389    let len = src_a.len().min(src_b.len()).min(dst.len());
390    for i in 0..len {
391        dst[i] = src_a[i] & src_b[i];
392    }
393}
394
395pub fn and_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
396    let len = src_a.len().min(src_b.len()).min(dst.len());
397    for i in 0..len {
398        dst[i] = src_a[i] & src_b[i];
399    }
400}
401
402pub fn and_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
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 or_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
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 or_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
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_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
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 not_u32(src: &[u32], dst: &mut [u32]) {
431    let len = src.len().min(dst.len());
432    for i in 0..len {
433        dst[i] = !src[i];
434    }
435}
436
437pub fn not_u16(src: &[u16], dst: &mut [u16]) {
438    let len = src.len().min(dst.len());
439    for i in 0..len {
440        dst[i] = !src[i];
441    }
442}
443
444pub fn not_u8(src: &[u8], dst: &mut [u8]) {
445    let len = src.len().min(dst.len());
446    for i in 0..len {
447        dst[i] = !src[i];
448    }
449}
450
451pub fn xor_u32(src_a: &[u32], src_b: &[u32], dst: &mut [u32]) {
452    let len = src_a.len().min(src_b.len()).min(dst.len());
453    for i in 0..len {
454        dst[i] = src_a[i] ^ src_b[i];
455    }
456}
457
458pub fn xor_u16(src_a: &[u16], src_b: &[u16], dst: &mut [u16]) {
459    let len = src_a.len().min(src_b.len()).min(dst.len());
460    for i in 0..len {
461        dst[i] = src_a[i] ^ src_b[i];
462    }
463}
464
465pub fn xor_u8(src_a: &[u8], src_b: &[u8], dst: &mut [u8]) {
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}
embedded_dsp/basic_math.rs

embedded_dsp/
basic_math.rs
