1 /*
2 * Copyright 2024 NXP
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <stdio.h>
9
10 #include "NatureDSP_Signal.h"
11
12 /**
13 * For fft_real32x32, scaling options are:
14 * 2 - 32-bit dynamic scaling
15 * 3 - fixed scaling before each stage
16 */
17 #define FFT_SCALING_OPTION 3
18 #define RSH 31
19
vec_sum_int16(const int16_t * in_a,const int16_t * in_b,int16_t * out,uint32_t length)20 void vec_sum_int16(const int16_t *in_a, const int16_t *in_b, int16_t *out, uint32_t length)
21 {
22 printk("[Backend] NatureDSP library\n");
23 vec_add16x16(out, in_a, in_b, length);
24 }
25
vec_power_int16(const int16_t * in,int64_t * out,int rsh,uint32_t length)26 void vec_power_int16(const int16_t *in, int64_t *out, int rsh, uint32_t length)
27 {
28 printk("[Backend] NatureDSP library\n");
29 out[0] = vec_power16x16(in, rsh, length);
30 }
31
vec_power_int32(const int32_t * in,int64_t * out,int rsh,uint32_t length)32 void vec_power_int32(const int32_t *in, int64_t *out, int rsh, uint32_t length)
33 {
34 printk("[Backend] NatureDSP library\n");
35 out[0] = vec_power32x32(in, rsh, length);
36 }
37
fft_real32(int32_t * in,int32_t * out,int length)38 void fft_real32(int32_t *in, int32_t *out, int length)
39 {
40 /* FFT handle for 32x32 */
41 extern const fft_handle_t rfft32_32;
42
43 printk("[Backend] NatureDSP library\n");
44
45 /* Apply FFT */
46 fft_real32x32(out, in, rfft32_32, FFT_SCALING_OPTION);
47 }
48
real_block_iir_32(int M,const int32_t * coef_sos,const int16_t * coef_g,const int32_t * in,int32_t * out,int block_size)49 void real_block_iir_32(int M, const int32_t *coef_sos, const int16_t *coef_g,
50 const int32_t *in, int32_t *out, int block_size)
51 {
52 bqriir32x32_df1_handle_t handle;
53 static int32_t objmem[256] = {};
54
55 printk("[Backend] NatureDSP library\n");
56 /*
57 * Initialization routine for IIR filters
58 * value = 0: total gain shift amount applied to output signal
59 *
60 * Returns: handle to the object
61 */
62 handle = bqriir32x32_df1_init(objmem, M, coef_sos, coef_g, 0);
63 /*
64 * Call Bi-quad Real Block IIR
65 * value = NULL: scratch memory area (for fixed-point functions only)
66 *
67 * The filter calculates block_size output samples using
68 * coef_sos and coef_g matrices.
69 */
70 bqriir32x32_df1(handle, NULL, out, in, block_size);
71 }
72
lms_iir_32(int32_t * err,int32_t * coef,int32_t * input,int32_t * ref,int32_t mu,int block_size,int M)73 void lms_iir_32(int32_t *err, int32_t *coef, int32_t *input, int32_t *ref,
74 int32_t mu, int block_size, int M)
75 {
76 int64_t norm64;
77
78 printk("[Backend] NatureDSP library\n");
79
80 /*
81 * Compute the normalization factor,
82 * which is the power of the reference signal times block_size,
83 * where block_size is the number of samples to process
84 */
85 vec_power_int32(ref, &norm64, RSH, block_size);
86 /*
87 * Call Blockwise Adaptive LMS Algorithm for Real Data
88 * value = NULL: scratch memory area (for fixed-point functions only)
89 *
90 * The filter calculates block_size output samples using
91 * coef_sos and coef_g matrices.
92 */
93 fir_blms32x32(err, coef, input, ref, norm64, mu, block_size, M);
94 }
95
