1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2017 Intel Corporation. All rights reserved.
4 //
5 // Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
6 // Liam Girdwood <liam.r.girdwood@linux.intel.com>
7 // Keyon Jie <yang.jie@linux.intel.com>
8
9 #include <sof/math/fir_config.h>
10
11 #if FIR_GENERIC
12
13 #include <sof/common.h>
14 #include <sof/audio/buffer.h>
15 #include <sof/audio/format.h>
16 #include <sof/math/fir_generic.h>
17 #include <user/fir.h>
18 #include <errno.h>
19 #include <stddef.h>
20 #include <stdint.h>
21
22 /*
23 * EQ FIR algorithm code
24 */
25
fir_reset(struct fir_state_32x16 * fir)26 void fir_reset(struct fir_state_32x16 *fir)
27 {
28 fir->rwi = 0;
29 fir->length = 0;
30 fir->out_shift = 0;
31 fir->coef = NULL;
32 /* There may need to know the beginning of dynamic allocation after
33 * reset so omitting setting also fir->delay to NULL.
34 */
35 }
36
fir_delay_size(struct sof_fir_coef_data * config)37 int fir_delay_size(struct sof_fir_coef_data *config)
38 {
39 /* Check for sane FIR length. The generic version does not
40 * have other constraints.
41 */
42 if (config->length > SOF_FIR_MAX_LENGTH || config->length < 1)
43 return -EINVAL;
44
45 return config->length * sizeof(int32_t);
46 }
47
fir_init_coef(struct fir_state_32x16 * fir,struct sof_fir_coef_data * config)48 int fir_init_coef(struct fir_state_32x16 *fir,
49 struct sof_fir_coef_data *config)
50 {
51 fir->rwi = 0;
52 fir->length = (int)config->length;
53 fir->taps = fir->length; /* The same for generic C version */
54 fir->out_shift = (int)config->out_shift;
55 fir->coef = ASSUME_ALIGNED(&config->coef[0], 4);
56 return 0;
57 }
58
fir_init_delay(struct fir_state_32x16 * fir,int32_t ** data)59 void fir_init_delay(struct fir_state_32x16 *fir, int32_t **data)
60 {
61 fir->delay = *data;
62 *data += fir->length; /* Point to next delay line start */
63 }
64
fir_32x16(struct fir_state_32x16 * fir,int32_t x)65 int32_t fir_32x16(struct fir_state_32x16 *fir, int32_t x)
66 {
67 int64_t y = 0;
68 int32_t *data = &fir->delay[fir->rwi];
69 int16_t *coef = &fir->coef[0];
70 int n1;
71 int n2;
72 int n;
73
74 /* Bypass is set with length set to zero. */
75 if (!fir->length)
76 return x;
77
78 /* Write sample to delay */
79 *data = x;
80
81 /* Advance write pointer and calculate into n1 max. number of taps
82 * to process before circular wrap.
83 */
84 n1 = ++fir->rwi;
85 if (fir->rwi == fir->length)
86 fir->rwi = 0;
87
88 /* Check if no need to un-wrap FIR data. */
89 if (n1 > fir->length) {
90 /* Data is Q1.31, coef is Q1.15, product is Q2.46 */
91 for (n = 0; n < fir->length; n++) {
92 y += (int64_t)(*coef) * (*data);
93 coef++;
94 data--;
95 }
96
97 /* Q2.46 -> Q2.31, saturate to Q1.31 */
98 return sat_int32(y >> (15 + fir->out_shift));
99 }
100
101 /* Part 1, loop n1 times */
102 for (n = 0; n < n1; n++) {
103 y += (int64_t)(*coef) * (*data);
104 coef++;
105 data--;
106 }
107
108 /* Part 2, un-wrap data, continue n2 times */
109 n2 = fir->length - n1;
110 data = &fir->delay[fir->length - 1];
111 for (n = 0; n < n2; n++) {
112 y += (int64_t)(*coef) * (*data);
113 coef++;
114 data--;
115 }
116
117 /* Q2.46 -> Q2.31, saturate to Q1.31 */
118 return sat_int32(y >> (15 + fir->out_shift));
119 }
120
121 #endif
122
