1 /**
2 *******************************************************************************
3 * @file OpenPDMFilter.c
4 * @author CL
5 * @version V1.0.0
6 * @date 9-September-2015
7 * @brief Open PDM audio software decoding Library.
8 * This Library is used to decode and reconstruct the audio signal
9 * produced by ST MEMS microphone (MP45Dxxx, MP34Dxxx).
10 *******************************************************************************
11 * @attention
12 *
13 * <h2><center>© COPYRIGHT 2018 STMicroelectronics</center></h2>
14 *
15 * Licensed under the Apache License, Version 2.0 (the "License");
16 * you may not use this file except in compliance with the License.
17 * You may obtain a copy of the License at
18 *
19 * http://www.apache.org/licenses/LICENSE-2.0
20 *
21 * Unless required by applicable law or agreed to in writing, software
22 * distributed under the License is distributed on an "AS IS" BASIS,
23 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
24 * See the License for the specific language governing permissions and
25 * limitations under the License.
26 *******************************************************************************
27 */
28
29
30 /* Includes ------------------------------------------------------------------*/
31
32 #include "OpenPDMFilter.h"
33
34
35 /* Variables -----------------------------------------------------------------*/
36
37 uint32_t div_const = 0;
38 int64_t sub_const = 0;
39 uint32_t sinc[DECIMATION_MAX * SINCN];
40 uint32_t sinc1[DECIMATION_MAX];
41 uint32_t sinc2[DECIMATION_MAX * 2];
42 uint32_t coef[SINCN][DECIMATION_MAX];
43 #ifdef USE_LUT
44 int32_t lut[256][DECIMATION_MAX / 8][SINCN];
45 #endif
46
47
48 /* Functions -----------------------------------------------------------------*/
49
50 #ifdef USE_LUT
filter_table_mono_64(uint8_t * data,uint8_t sincn)51 int32_t filter_table_mono_64(uint8_t *data, uint8_t sincn)
52 {
53 return (int32_t)
54 lut[data[0]][0][sincn] +
55 lut[data[1]][1][sincn] +
56 lut[data[2]][2][sincn] +
57 lut[data[3]][3][sincn] +
58 lut[data[4]][4][sincn] +
59 lut[data[5]][5][sincn] +
60 lut[data[6]][6][sincn] +
61 lut[data[7]][7][sincn];
62 }
filter_table_stereo_64(uint8_t * data,uint8_t sincn)63 int32_t filter_table_stereo_64(uint8_t *data, uint8_t sincn)
64 {
65 return (int32_t)
66 lut[data[0]][0][sincn] +
67 lut[data[2]][1][sincn] +
68 lut[data[4]][2][sincn] +
69 lut[data[6]][3][sincn] +
70 lut[data[8]][4][sincn] +
71 lut[data[10]][5][sincn] +
72 lut[data[12]][6][sincn] +
73 lut[data[14]][7][sincn];
74 }
filter_table_mono_128(uint8_t * data,uint8_t sincn)75 int32_t filter_table_mono_128(uint8_t *data, uint8_t sincn)
76 {
77 return (int32_t)
78 lut[data[0]][0][sincn] +
79 lut[data[1]][1][sincn] +
80 lut[data[2]][2][sincn] +
81 lut[data[3]][3][sincn] +
82 lut[data[4]][4][sincn] +
83 lut[data[5]][5][sincn] +
84 lut[data[6]][6][sincn] +
85 lut[data[7]][7][sincn] +
86 lut[data[8]][8][sincn] +
87 lut[data[9]][9][sincn] +
88 lut[data[10]][10][sincn] +
89 lut[data[11]][11][sincn] +
90 lut[data[12]][12][sincn] +
91 lut[data[13]][13][sincn] +
92 lut[data[14]][14][sincn] +
93 lut[data[15]][15][sincn];
94 }
filter_table_stereo_128(uint8_t * data,uint8_t sincn)95 int32_t filter_table_stereo_128(uint8_t *data, uint8_t sincn)
96 {
97 return (int32_t)
98 lut[data[0]][0][sincn] +
99 lut[data[2]][1][sincn] +
100 lut[data[4]][2][sincn] +
101 lut[data[6]][3][sincn] +
102 lut[data[8]][4][sincn] +
103 lut[data[10]][5][sincn] +
104 lut[data[12]][6][sincn] +
105 lut[data[14]][7][sincn] +
106 lut[data[16]][8][sincn] +
107 lut[data[18]][9][sincn] +
108 lut[data[20]][10][sincn] +
109 lut[data[22]][11][sincn] +
110 lut[data[24]][12][sincn] +
111 lut[data[26]][13][sincn] +
112 lut[data[28]][14][sincn] +
113 lut[data[30]][15][sincn];
114 }
115 int32_t (* filter_tables_64[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono_64, filter_table_stereo_64};
116 int32_t (* filter_tables_128[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono_128, filter_table_stereo_128};
117 #else
filter_table(uint8_t * data,uint8_t sincn,TPDMFilter_InitStruct * param)118 int32_t filter_table(uint8_t *data, uint8_t sincn, TPDMFilter_InitStruct *param)
119 {
120 uint8_t c, i;
121 uint16_t data_index = 0;
122 uint32_t *coef_p = &coef[sincn][0];
123 int32_t F = 0;
124 uint8_t decimation = param->Decimation;
125 uint8_t channels = param->In_MicChannels;
126
127 for (i = 0; i < decimation; i += 8) {
128 c = data[data_index];
129 F += ((c >> 7) ) * coef_p[i ] +
130 ((c >> 6) & 0x01) * coef_p[i + 1] +
131 ((c >> 5) & 0x01) * coef_p[i + 2] +
132 ((c >> 4) & 0x01) * coef_p[i + 3] +
133 ((c >> 3) & 0x01) * coef_p[i + 4] +
134 ((c >> 2) & 0x01) * coef_p[i + 5] +
135 ((c >> 1) & 0x01) * coef_p[i + 6] +
136 ((c ) & 0x01) * coef_p[i + 7];
137 data_index += channels;
138 }
139 return F;
140 }
141 #endif
142
convolve(uint32_t Signal[],unsigned short SignalLen,uint32_t Kernel[],unsigned short KernelLen,uint32_t Result[])143 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
144 uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
145 uint32_t Result[/* SignalLen + KernelLen - 1 */])
146 {
147 uint16_t n;
148
149 for (n = 0; n < SignalLen + KernelLen - 1; n++)
150 {
151 unsigned short kmin, kmax, k;
152
153 Result[n] = 0;
154
155 kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
156 kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
157
158 for (k = kmin; k <= kmax; k++) {
159 Result[n] += Signal[k] * Kernel[n - k];
160 }
161 }
162 }
163
Open_PDM_Filter_Init(TPDMFilter_InitStruct * Param)164 void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param)
165 {
166 uint16_t i, j;
167 int64_t sum = 0;
168
169 uint8_t decimation = Param->Decimation;
170
171 for (i = 0; i < SINCN; i++) {
172 Param->Coef[i] = 0;
173 Param->bit[i] = 0;
174 }
175 for (i = 0; i < decimation; i++) {
176 sinc1[i] = 1;
177 }
178
179 Param->OldOut = Param->OldIn = Param->OldZ = 0;
180 Param->LP_ALFA = (Param->LP_HZ != 0 ? (uint16_t) (Param->LP_HZ * 256 / (Param->LP_HZ + Param->Fs / (2 * 3.14159))) : 0);
181 Param->HP_ALFA = (Param->HP_HZ != 0 ? (uint16_t) (Param->Fs * 256 / (2 * 3.14159 * Param->HP_HZ + Param->Fs)) : 0);
182
183 Param->FilterLen = decimation * SINCN;
184 sinc[0] = 0;
185 sinc[decimation * SINCN - 1] = 0;
186 convolve(sinc1, decimation, sinc1, decimation, sinc2);
187 convolve(sinc2, decimation * 2 - 1, sinc1, decimation, &sinc[1]);
188 for(j = 0; j < SINCN; j++) {
189 for (i = 0; i < decimation; i++) {
190 coef[j][i] = sinc[j * decimation + i];
191 sum += sinc[j * decimation + i];
192 }
193 }
194
195 sub_const = sum >> 1;
196 div_const = sub_const * Param->MaxVolume / 32768 / FILTER_GAIN;
197 div_const = (div_const == 0 ? 1 : div_const);
198
199 #ifdef USE_LUT
200 /* Look-Up Table. */
201 uint16_t c, d, s;
202 for (s = 0; s < SINCN; s++)
203 {
204 uint32_t *coef_p = &coef[s][0];
205 for (c = 0; c < 256; c++)
206 for (d = 0; d < decimation / 8; d++)
207 lut[c][d][s] = ((c >> 7) ) * coef_p[d * 8 ] +
208 ((c >> 6) & 0x01) * coef_p[d * 8 + 1] +
209 ((c >> 5) & 0x01) * coef_p[d * 8 + 2] +
210 ((c >> 4) & 0x01) * coef_p[d * 8 + 3] +
211 ((c >> 3) & 0x01) * coef_p[d * 8 + 4] +
212 ((c >> 2) & 0x01) * coef_p[d * 8 + 5] +
213 ((c >> 1) & 0x01) * coef_p[d * 8 + 6] +
214 ((c ) & 0x01) * coef_p[d * 8 + 7];
215 }
216 #endif
217 }
218
Open_PDM_Filter_64(uint8_t * data,uint16_t * dataOut,uint16_t volume,TPDMFilter_InitStruct * Param)219 void Open_PDM_Filter_64(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param)
220 {
221 uint8_t i, data_out_index;
222 uint8_t channels = Param->In_MicChannels;
223 uint8_t data_inc = ((DECIMATION_MAX >> 4) * channels);
224 int64_t Z, Z0, Z1, Z2;
225 int64_t OldOut, OldIn, OldZ;
226
227 OldOut = Param->OldOut;
228 OldIn = Param->OldIn;
229 OldZ = Param->OldZ;
230
231 #ifdef USE_LUT
232 uint8_t j = channels - 1;
233 #endif
234
235 for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) {
236 #ifdef USE_LUT
237 Z0 = filter_tables_64[j](data, 0);
238 Z1 = filter_tables_64[j](data, 1);
239 Z2 = filter_tables_64[j](data, 2);
240 #else
241 Z0 = filter_table(data, 0, Param);
242 Z1 = filter_table(data, 1, Param);
243 Z2 = filter_table(data, 2, Param);
244 #endif
245
246 Z = Param->Coef[1] + Z2 - sub_const;
247 Param->Coef[1] = Param->Coef[0] + Z1;
248 Param->Coef[0] = Z0;
249
250 OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8;
251 OldIn = Z;
252 OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * OldOut) >> 8;
253
254 Z = OldZ * volume;
255 Z = RoundDiv(Z, div_const);
256 Z = SaturaLH(Z, -32700, 32700);
257
258 dataOut[data_out_index] = Z;
259 data += data_inc;
260 }
261
262 Param->OldOut = OldOut;
263 Param->OldIn = OldIn;
264 Param->OldZ = OldZ;
265 }
266
Open_PDM_Filter_128(uint8_t * data,uint16_t * dataOut,uint16_t volume,TPDMFilter_InitStruct * Param)267 void Open_PDM_Filter_128(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param)
268 {
269 uint8_t i, data_out_index;
270 uint8_t channels = Param->In_MicChannels;
271 uint8_t data_inc = ((DECIMATION_MAX >> 3) * channels);
272 int64_t Z, Z0, Z1, Z2;
273 int64_t OldOut, OldIn, OldZ;
274
275 OldOut = Param->OldOut;
276 OldIn = Param->OldIn;
277 OldZ = Param->OldZ;
278
279 #ifdef USE_LUT
280 uint8_t j = channels - 1;
281 #endif
282
283 for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) {
284 #ifdef USE_LUT
285 Z0 = filter_tables_128[j](data, 0);
286 Z1 = filter_tables_128[j](data, 1);
287 Z2 = filter_tables_128[j](data, 2);
288 #else
289 Z0 = filter_table(data, 0, Param);
290 Z1 = filter_table(data, 1, Param);
291 Z2 = filter_table(data, 2, Param);
292 #endif
293
294 Z = Param->Coef[1] + Z2 - sub_const;
295 Param->Coef[1] = Param->Coef[0] + Z1;
296 Param->Coef[0] = Z0;
297
298 OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8;
299 OldIn = Z;
300 OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * OldOut) >> 8;
301
302 Z = OldZ * volume;
303 Z = RoundDiv(Z, div_const);
304 Z = SaturaLH(Z, -32700, 32700);
305
306 dataOut[data_out_index] = Z;
307 data += data_inc;
308 }
309
310 Param->OldOut = OldOut;
311 Param->OldIn = OldIn;
312 Param->OldZ = OldZ;
313 }
314
