A bundled STM32F10x Std Periph and CMSIS library
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.
 
 
 
 
 

684 lignes
23 KiB

  1. /* ----------------------------------------------------------------------
  2. * Copyright (C) 2010-2014 ARM Limited. All rights reserved.
  3. *
  4. * $Date: 31. July 2014
  5. * $Revision: V1.4.4
  6. *
  7. * Project: CMSIS DSP Library
  8. * Title: arm_biquad_cascade_stereo_df2T_f32.c
  9. *
  10. * Description: Processing function for the floating-point transposed
  11. * direct form II Biquad cascade filter. 2 channels
  12. *
  13. * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  14. *
  15. * Redistribution and use in source and binary forms, with or without
  16. * modification, are permitted provided that the following conditions
  17. * are met:
  18. * - Redistributions of source code must retain the above copyright
  19. * notice, this list of conditions and the following disclaimer.
  20. * - Redistributions in binary form must reproduce the above copyright
  21. * notice, this list of conditions and the following disclaimer in
  22. * the documentation and/or other materials provided with the
  23. * distribution.
  24. * - Neither the name of ARM LIMITED nor the names of its contributors
  25. * may be used to endorse or promote products derived from this
  26. * software without specific prior written permission.
  27. *
  28. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  29. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  30. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  31. * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  32. * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  33. * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  34. * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  35. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  36. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  37. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  38. * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  39. * POSSIBILITY OF SUCH DAMAGE.
  40. * -------------------------------------------------------------------- */
  41. #include "arm_math.h"
  42. /**
  43. * @ingroup groupFilters
  44. */
  45. /**
  46. * @defgroup BiquadCascadeDF2T Biquad Cascade IIR Filters Using a Direct Form II Transposed Structure
  47. *
  48. * This set of functions implements arbitrary order recursive (IIR) filters using a transposed direct form II structure.
  49. * The filters are implemented as a cascade of second order Biquad sections.
  50. * These functions provide a slight memory savings as compared to the direct form I Biquad filter functions.
  51. * Only floating-point data is supported.
  52. *
  53. * This function operate on blocks of input and output data and each call to the function
  54. * processes <code>blockSize</code> samples through the filter.
  55. * <code>pSrc</code> points to the array of input data and
  56. * <code>pDst</code> points to the array of output data.
  57. * Both arrays contain <code>blockSize</code> values.
  58. *
  59. * \par Algorithm
  60. * Each Biquad stage implements a second order filter using the difference equation:
  61. * <pre>
  62. * y[n] = b0 * x[n] + d1
  63. * d1 = b1 * x[n] + a1 * y[n] + d2
  64. * d2 = b2 * x[n] + a2 * y[n]
  65. * </pre>
  66. * where d1 and d2 represent the two state values.
  67. *
  68. * \par
  69. * A Biquad filter using a transposed Direct Form II structure is shown below.
  70. * \image html BiquadDF2Transposed.gif "Single transposed Direct Form II Biquad"
  71. * Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
  72. * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
  73. * Pay careful attention to the sign of the feedback coefficients.
  74. * Some design tools flip the sign of the feedback coefficients:
  75. * <pre>
  76. * y[n] = b0 * x[n] + d1;
  77. * d1 = b1 * x[n] - a1 * y[n] + d2;
  78. * d2 = b2 * x[n] - a2 * y[n];
  79. * </pre>
  80. * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.
  81. *
  82. * \par
  83. * Higher order filters are realized as a cascade of second order sections.
  84. * <code>numStages</code> refers to the number of second order stages used.
  85. * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
  86. * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the
  87. * coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).
  88. *
  89. * \par
  90. * <code>pState</code> points to the state variable array.
  91. * Each Biquad stage has 2 state variables <code>d1</code> and <code>d2</code>.
  92. * The state variables are arranged in the <code>pState</code> array as:
  93. * <pre>
  94. * {d11, d12, d21, d22, ...}
  95. * </pre>
  96. * where <code>d1x</code> refers to the state variables for the first Biquad and
  97. * <code>d2x</code> refers to the state variables for the second Biquad.
  98. * The state array has a total length of <code>2*numStages</code> values.
  99. * The state variables are updated after each block of data is processed; the coefficients are untouched.
  100. *
  101. * \par
  102. * The CMSIS library contains Biquad filters in both Direct Form I and transposed Direct Form II.
  103. * The advantage of the Direct Form I structure is that it is numerically more robust for fixed-point data types.
  104. * That is why the Direct Form I structure supports Q15 and Q31 data types.
  105. * The transposed Direct Form II structure, on the other hand, requires a wide dynamic range for the state variables <code>d1</code> and <code>d2</code>.
  106. * Because of this, the CMSIS library only has a floating-point version of the Direct Form II Biquad.
  107. * The advantage of the Direct Form II Biquad is that it requires half the number of state variables, 2 rather than 4, per Biquad stage.
  108. *
  109. * \par Instance Structure
  110. * The coefficients and state variables for a filter are stored together in an instance data structure.
  111. * A separate instance structure must be defined for each filter.
  112. * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
  113. *
  114. * \par Init Functions
  115. * There is also an associated initialization function.
  116. * The initialization function performs following operations:
  117. * - Sets the values of the internal structure fields.
  118. * - Zeros out the values in the state buffer.
  119. * To do this manually without calling the init function, assign the follow subfields of the instance structure:
  120. * numStages, pCoeffs, pState. Also set all of the values in pState to zero.
  121. *
  122. * \par
  123. * Use of the initialization function is optional.
  124. * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
  125. * To place an instance structure into a const data section, the instance structure must be manually initialized.
  126. * Set the values in the state buffer to zeros before static initialization.
  127. * For example, to statically initialize the instance structure use
  128. * <pre>
  129. * arm_biquad_cascade_df2T_instance_f32 S1 = {numStages, pState, pCoeffs};
  130. * </pre>
  131. * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer.
  132. * <code>pCoeffs</code> is the address of the coefficient buffer;
  133. *
  134. */
  135. /**
  136. * @addtogroup BiquadCascadeDF2T
  137. * @{
  138. */
  139. /**
  140. * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
  141. * @param[in] *S points to an instance of the filter data structure.
  142. * @param[in] *pSrc points to the block of input data.
  143. * @param[out] *pDst points to the block of output data
  144. * @param[in] blockSize number of samples to process.
  145. * @return none.
  146. */
  147. LOW_OPTIMIZATION_ENTER
  148. void arm_biquad_cascade_stereo_df2T_f32(
  149. const arm_biquad_cascade_stereo_df2T_instance_f32 * S,
  150. float32_t * pSrc,
  151. float32_t * pDst,
  152. uint32_t blockSize)
  153. {
  154. float32_t *pIn = pSrc; /* source pointer */
  155. float32_t *pOut = pDst; /* destination pointer */
  156. float32_t *pState = S->pState; /* State pointer */
  157. float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
  158. float32_t acc1a, acc1b; /* accumulator */
  159. float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
  160. float32_t Xn1a, Xn1b; /* temporary input */
  161. float32_t d1a, d2a, d1b, d2b; /* state variables */
  162. uint32_t sample, stage = S->numStages; /* loop counters */
  163. #if defined(ARM_MATH_CM7)
  164. float32_t Xn2a, Xn3a, Xn4a, Xn5a, Xn6a, Xn7a, Xn8a; /* Input State variables */
  165. float32_t Xn2b, Xn3b, Xn4b, Xn5b, Xn6b, Xn7b, Xn8b; /* Input State variables */
  166. float32_t acc2a, acc3a, acc4a, acc5a, acc6a, acc7a, acc8a; /* Simulates the accumulator */
  167. float32_t acc2b, acc3b, acc4b, acc5b, acc6b, acc7b, acc8b; /* Simulates the accumulator */
  168. do
  169. {
  170. /* Reading the coefficients */
  171. b0 = pCoeffs[0];
  172. b1 = pCoeffs[1];
  173. b2 = pCoeffs[2];
  174. a1 = pCoeffs[3];
  175. /* Apply loop unrolling and compute 8 output values simultaneously. */
  176. sample = blockSize >> 3u;
  177. a2 = pCoeffs[4];
  178. /*Reading the state values */
  179. d1a = pState[0];
  180. d2a = pState[1];
  181. d1b = pState[2];
  182. d2b = pState[3];
  183. pCoeffs += 5u;
  184. /* First part of the processing with loop unrolling. Compute 8 outputs at a time.
  185. ** a second loop below computes the remaining 1 to 7 samples. */
  186. while(sample > 0u) {
  187. /* y[n] = b0 * x[n] + d1 */
  188. /* d1 = b1 * x[n] + a1 * y[n] + d2 */
  189. /* d2 = b2 * x[n] + a2 * y[n] */
  190. /* Read the first 2 inputs. 2 cycles */
  191. Xn1a = pIn[0 ];
  192. Xn1b = pIn[1 ];
  193. /* Sample 1. 5 cycles */
  194. Xn2a = pIn[2 ];
  195. acc1a = b0 * Xn1a + d1a;
  196. Xn2b = pIn[3 ];
  197. d1a = b1 * Xn1a + d2a;
  198. Xn3a = pIn[4 ];
  199. d2a = b2 * Xn1a;
  200. Xn3b = pIn[5 ];
  201. d1a += a1 * acc1a;
  202. Xn4a = pIn[6 ];
  203. d2a += a2 * acc1a;
  204. /* Sample 2. 5 cycles */
  205. Xn4b = pIn[7 ];
  206. acc1b = b0 * Xn1b + d1b;
  207. Xn5a = pIn[8 ];
  208. d1b = b1 * Xn1b + d2b;
  209. Xn5b = pIn[9 ];
  210. d2b = b2 * Xn1b;
  211. Xn6a = pIn[10];
  212. d1b += a1 * acc1b;
  213. Xn6b = pIn[11];
  214. d2b += a2 * acc1b;
  215. /* Sample 3. 5 cycles */
  216. Xn7a = pIn[12];
  217. acc2a = b0 * Xn2a + d1a;
  218. Xn7b = pIn[13];
  219. d1a = b1 * Xn2a + d2a;
  220. Xn8a = pIn[14];
  221. d2a = b2 * Xn2a;
  222. Xn8b = pIn[15];
  223. d1a += a1 * acc2a;
  224. pIn += 16;
  225. d2a += a2 * acc2a;
  226. /* Sample 4. 5 cycles */
  227. acc2b = b0 * Xn2b + d1b;
  228. d1b = b1 * Xn2b + d2b;
  229. d2b = b2 * Xn2b;
  230. d1b += a1 * acc2b;
  231. d2b += a2 * acc2b;
  232. /* Sample 5. 5 cycles */
  233. acc3a = b0 * Xn3a + d1a;
  234. d1a = b1 * Xn3a + d2a;
  235. d2a = b2 * Xn3a;
  236. d1a += a1 * acc3a;
  237. d2a += a2 * acc3a;
  238. /* Sample 6. 5 cycles */
  239. acc3b = b0 * Xn3b + d1b;
  240. d1b = b1 * Xn3b + d2b;
  241. d2b = b2 * Xn3b;
  242. d1b += a1 * acc3b;
  243. d2b += a2 * acc3b;
  244. /* Sample 7. 5 cycles */
  245. acc4a = b0 * Xn4a + d1a;
  246. d1a = b1 * Xn4a + d2a;
  247. d2a = b2 * Xn4a;
  248. d1a += a1 * acc4a;
  249. d2a += a2 * acc4a;
  250. /* Sample 8. 5 cycles */
  251. acc4b = b0 * Xn4b + d1b;
  252. d1b = b1 * Xn4b + d2b;
  253. d2b = b2 * Xn4b;
  254. d1b += a1 * acc4b;
  255. d2b += a2 * acc4b;
  256. /* Sample 9. 5 cycles */
  257. acc5a = b0 * Xn5a + d1a;
  258. d1a = b1 * Xn5a + d2a;
  259. d2a = b2 * Xn5a;
  260. d1a += a1 * acc5a;
  261. d2a += a2 * acc5a;
  262. /* Sample 10. 5 cycles */
  263. acc5b = b0 * Xn5b + d1b;
  264. d1b = b1 * Xn5b + d2b;
  265. d2b = b2 * Xn5b;
  266. d1b += a1 * acc5b;
  267. d2b += a2 * acc5b;
  268. /* Sample 11. 5 cycles */
  269. acc6a = b0 * Xn6a + d1a;
  270. d1a = b1 * Xn6a + d2a;
  271. d2a = b2 * Xn6a;
  272. d1a += a1 * acc6a;
  273. d2a += a2 * acc6a;
  274. /* Sample 12. 5 cycles */
  275. acc6b = b0 * Xn6b + d1b;
  276. d1b = b1 * Xn6b + d2b;
  277. d2b = b2 * Xn6b;
  278. d1b += a1 * acc6b;
  279. d2b += a2 * acc6b;
  280. /* Sample 13. 5 cycles */
  281. acc7a = b0 * Xn7a + d1a;
  282. d1a = b1 * Xn7a + d2a;
  283. pOut[0 ] = acc1a ;
  284. d2a = b2 * Xn7a;
  285. pOut[1 ] = acc1b ;
  286. d1a += a1 * acc7a;
  287. pOut[2 ] = acc2a ;
  288. d2a += a2 * acc7a;
  289. /* Sample 14. 5 cycles */
  290. pOut[3 ] = acc2b ;
  291. acc7b = b0 * Xn7b + d1b;
  292. pOut[4 ] = acc3a ;
  293. d1b = b1 * Xn7b + d2b;
  294. pOut[5 ] = acc3b ;
  295. d2b = b2 * Xn7b;
  296. pOut[6 ] = acc4a ;
  297. d1b += a1 * acc7b;
  298. pOut[7 ] = acc4b ;
  299. d2b += a2 * acc7b;
  300. /* Sample 15. 5 cycles */
  301. pOut[8 ] = acc5a ;
  302. acc8a = b0 * Xn8a + d1a;
  303. pOut[9 ] = acc5b;
  304. d1a = b1 * Xn8a + d2a;
  305. pOut[10] = acc6a;
  306. d2a = b2 * Xn8a;
  307. pOut[11] = acc6b;
  308. d1a += a1 * acc8a;
  309. pOut[12] = acc7a;
  310. d2a += a2 * acc8a;
  311. /* Sample 16. 5 cycles */
  312. pOut[13] = acc7b;
  313. acc8b = b0 * Xn8b + d1b;
  314. pOut[14] = acc8a;
  315. d1b = b1 * Xn8b + d2b;
  316. pOut[15] = acc8b;
  317. d2b = b2 * Xn8b;
  318. sample--;
  319. d1b += a1 * acc8b;
  320. pOut += 16;
  321. d2b += a2 * acc8b;
  322. }
  323. sample = blockSize & 0x7u;
  324. while(sample > 0u) {
  325. /* Read the input */
  326. Xn1a = *pIn++; //Channel a
  327. Xn1b = *pIn++; //Channel b
  328. /* y[n] = b0 * x[n] + d1 */
  329. acc1a = (b0 * Xn1a) + d1a;
  330. acc1b = (b0 * Xn1b) + d1b;
  331. /* Store the result in the accumulator in the destination buffer. */
  332. *pOut++ = acc1a;
  333. *pOut++ = acc1b;
  334. /* Every time after the output is computed state should be updated. */
  335. /* d1 = b1 * x[n] + a1 * y[n] + d2 */
  336. d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
  337. d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
  338. /* d2 = b2 * x[n] + a2 * y[n] */
  339. d2a = (b2 * Xn1a) + (a2 * acc1a);
  340. d2b = (b2 * Xn1b) + (a2 * acc1b);
  341. sample--;
  342. }
  343. /* Store the updated state variables back into the state array */
  344. pState[0] = d1a;
  345. pState[1] = d2a;
  346. pState[2] = d1b;
  347. pState[3] = d2b;
  348. /* The current stage input is given as the output to the next stage */
  349. pIn = pDst;
  350. /* decrement the loop counter */
  351. stage--;
  352. pState += 4u;
  353. /*Reset the output working pointer */
  354. pOut = pDst;
  355. } while(stage > 0u);
  356. #elif defined(ARM_MATH_CM0_FAMILY)
  357. /* Run the below code for Cortex-M0 */
  358. do
  359. {
  360. /* Reading the coefficients */
  361. b0 = *pCoeffs++;
  362. b1 = *pCoeffs++;
  363. b2 = *pCoeffs++;
  364. a1 = *pCoeffs++;
  365. a2 = *pCoeffs++;
  366. /*Reading the state values */
  367. d1a = pState[0];
  368. d2a = pState[1];
  369. d1b = pState[2];
  370. d2b = pState[3];
  371. sample = blockSize;
  372. while(sample > 0u)
  373. {
  374. /* Read the input */
  375. Xn1a = *pIn++; //Channel a
  376. Xn1b = *pIn++; //Channel b
  377. /* y[n] = b0 * x[n] + d1 */
  378. acc1a = (b0 * Xn1a) + d1a;
  379. acc1b = (b0 * Xn1b) + d1b;
  380. /* Store the result in the accumulator in the destination buffer. */
  381. *pOut++ = acc1a;
  382. *pOut++ = acc1b;
  383. /* Every time after the output is computed state should be updated. */
  384. /* d1 = b1 * x[n] + a1 * y[n] + d2 */
  385. d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
  386. d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
  387. /* d2 = b2 * x[n] + a2 * y[n] */
  388. d2a = (b2 * Xn1a) + (a2 * acc1a);
  389. d2b = (b2 * Xn1b) + (a2 * acc1b);
  390. /* decrement the loop counter */
  391. sample--;
  392. }
  393. /* Store the updated state variables back into the state array */
  394. *pState++ = d1a;
  395. *pState++ = d2a;
  396. *pState++ = d1b;
  397. *pState++ = d2b;
  398. /* The current stage input is given as the output to the next stage */
  399. pIn = pDst;
  400. /*Reset the output working pointer */
  401. pOut = pDst;
  402. /* decrement the loop counter */
  403. stage--;
  404. } while(stage > 0u);
  405. #else
  406. float32_t Xn2a, Xn3a, Xn4a; /* Input State variables */
  407. float32_t Xn2b, Xn3b, Xn4b; /* Input State variables */
  408. float32_t acc2a, acc3a, acc4a; /* accumulator */
  409. float32_t acc2b, acc3b, acc4b; /* accumulator */
  410. float32_t p0a, p1a, p2a, p3a, p4a, A1a;
  411. float32_t p0b, p1b, p2b, p3b, p4b, A1b;
  412. /* Run the below code for Cortex-M4 and Cortex-M3 */
  413. do
  414. {
  415. /* Reading the coefficients */
  416. b0 = *pCoeffs++;
  417. b1 = *pCoeffs++;
  418. b2 = *pCoeffs++;
  419. a1 = *pCoeffs++;
  420. a2 = *pCoeffs++;
  421. /*Reading the state values */
  422. d1a = pState[0];
  423. d2a = pState[1];
  424. d1b = pState[2];
  425. d2b = pState[3];
  426. /* Apply loop unrolling and compute 4 output values simultaneously. */
  427. sample = blockSize >> 2u;
  428. /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
  429. ** a second loop below computes the remaining 1 to 3 samples. */
  430. while(sample > 0u) {
  431. /* y[n] = b0 * x[n] + d1 */
  432. /* d1 = b1 * x[n] + a1 * y[n] + d2 */
  433. /* d2 = b2 * x[n] + a2 * y[n] */
  434. /* Read the four inputs */
  435. Xn1a = pIn[0];
  436. Xn1b = pIn[1];
  437. Xn2a = pIn[2];
  438. Xn2b = pIn[3];
  439. Xn3a = pIn[4];
  440. Xn3b = pIn[5];
  441. Xn4a = pIn[6];
  442. Xn4b = pIn[7];
  443. pIn += 8;
  444. p0a = b0 * Xn1a;
  445. p0b = b0 * Xn1b;
  446. p1a = b1 * Xn1a;
  447. p1b = b1 * Xn1b;
  448. acc1a = p0a + d1a;
  449. acc1b = p0b + d1b;
  450. p0a = b0 * Xn2a;
  451. p0b = b0 * Xn2b;
  452. p3a = a1 * acc1a;
  453. p3b = a1 * acc1b;
  454. p2a = b2 * Xn1a;
  455. p2b = b2 * Xn1b;
  456. A1a = p1a + p3a;
  457. A1b = p1b + p3b;
  458. p4a = a2 * acc1a;
  459. p4b = a2 * acc1b;
  460. d1a = A1a + d2a;
  461. d1b = A1b + d2b;
  462. d2a = p2a + p4a;
  463. d2b = p2b + p4b;
  464. p1a = b1 * Xn2a;
  465. p1b = b1 * Xn2b;
  466. acc2a = p0a + d1a;
  467. acc2b = p0b + d1b;
  468. p0a = b0 * Xn3a;
  469. p0b = b0 * Xn3b;
  470. p3a = a1 * acc2a;
  471. p3b = a1 * acc2b;
  472. p2a = b2 * Xn2a;
  473. p2b = b2 * Xn2b;
  474. A1a = p1a + p3a;
  475. A1b = p1b + p3b;
  476. p4a = a2 * acc2a;
  477. p4b = a2 * acc2b;
  478. d1a = A1a + d2a;
  479. d1b = A1b + d2b;
  480. d2a = p2a + p4a;
  481. d2b = p2b + p4b;
  482. p1a = b1 * Xn3a;
  483. p1b = b1 * Xn3b;
  484. acc3a = p0a + d1a;
  485. acc3b = p0b + d1b;
  486. p0a = b0 * Xn4a;
  487. p0b = b0 * Xn4b;
  488. p3a = a1 * acc3a;
  489. p3b = a1 * acc3b;
  490. p2a = b2 * Xn3a;
  491. p2b = b2 * Xn3b;
  492. A1a = p1a + p3a;
  493. A1b = p1b + p3b;
  494. p4a = a2 * acc3a;
  495. p4b = a2 * acc3b;
  496. d1a = A1a + d2a;
  497. d1b = A1b + d2b;
  498. d2a = p2a + p4a;
  499. d2b = p2b + p4b;
  500. acc4a = p0a + d1a;
  501. acc4b = p0b + d1b;
  502. p1a = b1 * Xn4a;
  503. p1b = b1 * Xn4b;
  504. p3a = a1 * acc4a;
  505. p3b = a1 * acc4b;
  506. p2a = b2 * Xn4a;
  507. p2b = b2 * Xn4b;
  508. A1a = p1a + p3a;
  509. A1b = p1b + p3b;
  510. p4a = a2 * acc4a;
  511. p4b = a2 * acc4b;
  512. d1a = A1a + d2a;
  513. d1b = A1b + d2b;
  514. d2a = p2a + p4a;
  515. d2b = p2b + p4b;
  516. pOut[0] = acc1a;
  517. pOut[1] = acc1b;
  518. pOut[2] = acc2a;
  519. pOut[3] = acc2b;
  520. pOut[4] = acc3a;
  521. pOut[5] = acc3b;
  522. pOut[6] = acc4a;
  523. pOut[7] = acc4b;
  524. pOut += 8;
  525. sample--;
  526. }
  527. sample = blockSize & 0x3u;
  528. while(sample > 0u) {
  529. Xn1a = *pIn++;
  530. Xn1b = *pIn++;
  531. p0a = b0 * Xn1a;
  532. p0b = b0 * Xn1b;
  533. p1a = b1 * Xn1a;
  534. p1b = b1 * Xn1b;
  535. acc1a = p0a + d1a;
  536. acc1b = p0b + d1b;
  537. p3a = a1 * acc1a;
  538. p3b = a1 * acc1b;
  539. p2a = b2 * Xn1a;
  540. p2b = b2 * Xn1b;
  541. A1a = p1a + p3a;
  542. A1b = p1b + p3b;
  543. p4a = a2 * acc1a;
  544. p4b = a2 * acc1b;
  545. d1a = A1a + d2a;
  546. d1b = A1b + d2b;
  547. d2a = p2a + p4a;
  548. d2b = p2b + p4b;
  549. *pOut++ = acc1a;
  550. *pOut++ = acc1b;
  551. sample--;
  552. }
  553. /* Store the updated state variables back into the state array */
  554. *pState++ = d1a;
  555. *pState++ = d2a;
  556. *pState++ = d1b;
  557. *pState++ = d2b;
  558. /* The current stage input is given as the output to the next stage */
  559. pIn = pDst;
  560. /*Reset the output working pointer */
  561. pOut = pDst;
  562. /* decrement the loop counter */
  563. stage--;
  564. } while(stage > 0u);
  565. #endif
  566. }
  567. LOW_OPTIMIZATION_EXIT
  568. /**
  569. * @} end of BiquadCascadeDF2T group
  570. */