arm_biquad_cascade_stereo_df2T_f32.c

00001 /* ----------------------------------------------------------------------    
00002 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.    
00003 *    
00004 * $Date:        19. March 2015 
00005 * $Revision:    V.1.4.5
00006 *    
00007 * Project:      CMSIS DSP Library    
00008 * Title:        arm_biquad_cascade_stereo_df2T_f32.c    
00009 *    
00010 * Description:  Processing function for the floating-point transposed    
00011 *               direct form II Biquad cascade filter. 2 channels  
00012 *    
00013 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
00014 *  
00015 * Redistribution and use in source and binary forms, with or without 
00016 * modification, are permitted provided that the following conditions
00017 * are met:
00018 *   - Redistributions of source code must retain the above copyright
00019 *     notice, this list of conditions and the following disclaimer.
00020 *   - Redistributions in binary form must reproduce the above copyright
00021 *     notice, this list of conditions and the following disclaimer in
00022 *     the documentation and/or other materials provided with the 
00023 *     distribution.
00024 *   - Neither the name of ARM LIMITED nor the names of its contributors
00025 *     may be used to endorse or promote products derived from this
00026 *     software without specific prior written permission.
00027 *
00028 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00029 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00030 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00031 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00032 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00033 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00034 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00036 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00037 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00038 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00039 * POSSIBILITY OF SUCH DAMAGE.   
00040 * -------------------------------------------------------------------- */
00041 
00042 #include "arm_math.h"
00043 
00044 /**       
00045 * @ingroup groupFilters       
00046 */
00047 
00048 /**       
00049 * @defgroup BiquadCascadeDF2T Biquad Cascade IIR Filters Using a Direct Form II Transposed Structure       
00050 *       
00051 * This set of functions implements arbitrary order recursive (IIR) filters using a transposed direct form II structure.       
00052 * The filters are implemented as a cascade of second order Biquad sections.       
00053 * These functions provide a slight memory savings as compared to the direct form I Biquad filter functions.      
00054 * Only floating-point data is supported.       
00055 *       
00056 * This function operate on blocks of input and output data and each call to the function       
00057 * processes <code>blockSize</code> samples through the filter.       
00058 * <code>pSrc</code> points to the array of input data and       
00059 * <code>pDst</code> points to the array of output data.       
00060 * Both arrays contain <code>blockSize</code> values.       
00061 *       
00062 * \par Algorithm       
00063 * Each Biquad stage implements a second order filter using the difference equation:       
00064 * <pre>       
00065 *    y[n] = b0 * x[n] + d1       
00066 *    d1 = b1 * x[n] + a1 * y[n] + d2       
00067 *    d2 = b2 * x[n] + a2 * y[n]       
00068 * </pre>       
00069 * where d1 and d2 represent the two state values.       
00070 *       
00071 * \par       
00072 * A Biquad filter using a transposed Direct Form II structure is shown below.       
00073 * \image html BiquadDF2Transposed.gif "Single transposed Direct Form II Biquad"       
00074 * Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.       
00075 * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.       
00076 * Pay careful attention to the sign of the feedback coefficients.       
00077 * Some design tools flip the sign of the feedback coefficients:       
00078 * <pre>       
00079 *    y[n] = b0 * x[n] + d1;       
00080 *    d1 = b1 * x[n] - a1 * y[n] + d2;       
00081 *    d2 = b2 * x[n] - a2 * y[n];       
00082 * </pre>       
00083 * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.       
00084 *       
00085 * \par       
00086 * Higher order filters are realized as a cascade of second order sections.       
00087 * <code>numStages</code> refers to the number of second order stages used.       
00088 * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.       
00089 * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the       
00090 * coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).       
00091 *       
00092 * \par       
00093 * <code>pState</code> points to the state variable array.       
00094 * Each Biquad stage has 2 state variables <code>d1</code> and <code>d2</code>.       
00095 * The state variables are arranged in the <code>pState</code> array as:       
00096 * <pre>       
00097 *     {d11, d12, d21, d22, ...}       
00098 * </pre>       
00099 * where <code>d1x</code> refers to the state variables for the first Biquad and       
00100 * <code>d2x</code> refers to the state variables for the second Biquad.       
00101 * The state array has a total length of <code>2*numStages</code> values.       
00102 * The state variables are updated after each block of data is processed; the coefficients are untouched.       
00103 *       
00104 * \par       
00105 * The CMSIS library contains Biquad filters in both Direct Form I and transposed Direct Form II.    
00106 * The advantage of the Direct Form I structure is that it is numerically more robust for fixed-point data types.    
00107 * That is why the Direct Form I structure supports Q15 and Q31 data types.    
00108 * 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>.    
00109 * Because of this, the CMSIS library only has a floating-point version of the Direct Form II Biquad.    
00110 * 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.    
00111 *       
00112 * \par Instance Structure       
00113 * The coefficients and state variables for a filter are stored together in an instance data structure.       
00114 * A separate instance structure must be defined for each filter.       
00115 * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.       
00116 *       
00117 * \par Init Functions       
00118 * There is also an associated initialization function.      
00119 * The initialization function performs following operations:       
00120 * - Sets the values of the internal structure fields.       
00121 * - Zeros out the values in the state buffer.       
00122 * To do this manually without calling the init function, assign the follow subfields of the instance structure:
00123 * numStages, pCoeffs, pState. Also set all of the values in pState to zero. 
00124 *       
00125 * \par       
00126 * Use of the initialization function is optional.       
00127 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.       
00128 * To place an instance structure into a const data section, the instance structure must be manually initialized.       
00129 * Set the values in the state buffer to zeros before static initialization.       
00130 * For example, to statically initialize the instance structure use       
00131 * <pre>       
00132 *     arm_biquad_cascade_df2T_instance_f32 S1 = {numStages, pState, pCoeffs};       
00133 * </pre>       
00134 * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer.       
00135 * <code>pCoeffs</code> is the address of the coefficient buffer;        
00136 *       
00137 */
00138 
00139 /**       
00140 * @addtogroup BiquadCascadeDF2T       
00141 * @{       
00142 */
00143 
00144 /**      
00145 * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.      
00146 * @param[in]  *S        points to an instance of the filter data structure.      
00147 * @param[in]  *pSrc     points to the block of input data.      
00148 * @param[out] *pDst     points to the block of output data      
00149 * @param[in]  blockSize number of samples to process.      
00150 * @return none.      
00151 */
00152 
00153 
00154 LOW_OPTIMIZATION_ENTER
00155 void arm_biquad_cascade_stereo_df2T_f32(
00156 const arm_biquad_cascade_stereo_df2T_instance_f32 * S,
00157 float32_t * pSrc,
00158 float32_t * pDst,
00159 uint32_t blockSize)
00160 {
00161 
00162     float32_t *pIn = pSrc;                         /*  source pointer            */
00163     float32_t *pOut = pDst;                        /*  destination pointer       */
00164     float32_t *pState = S->pState;                 /*  State pointer             */
00165     float32_t *pCoeffs = S->pCoeffs;               /*  coefficient pointer       */
00166     float32_t acc1a, acc1b;                        /*  accumulator               */
00167     float32_t b0, b1, b2, a1, a2;                  /*  Filter coefficients       */
00168     float32_t Xn1a, Xn1b;                          /*  temporary input           */
00169     float32_t d1a, d2a, d1b, d2b;                  /*  state variables           */
00170     uint32_t sample, stage = S->numStages;         /*  loop counters             */
00171 
00172 #if defined(ARM_MATH_CM7)
00173     
00174     float32_t Xn2a, Xn3a, Xn4a, Xn5a, Xn6a, Xn7a, Xn8a;         /*  Input State variables     */
00175     float32_t Xn2b, Xn3b, Xn4b, Xn5b, Xn6b, Xn7b, Xn8b;         /*  Input State variables     */
00176     float32_t acc2a, acc3a, acc4a, acc5a, acc6a, acc7a, acc8a;  /*  Simulates the accumulator */
00177     float32_t acc2b, acc3b, acc4b, acc5b, acc6b, acc7b, acc8b;  /*  Simulates the accumulator */
00178 
00179     do
00180     {
00181         /* Reading the coefficients */ 
00182         b0 = pCoeffs[0]; 
00183         b1 = pCoeffs[1]; 
00184         b2 = pCoeffs[2]; 
00185         a1 = pCoeffs[3]; 
00186         /* Apply loop unrolling and compute 8 output values simultaneously. */ 
00187         sample = blockSize >> 3u; 
00188         a2 = pCoeffs[4]; 
00189 
00190         /*Reading the state values */ 
00191         d1a = pState[0]; 
00192         d2a = pState[1]; 
00193         d1b = pState[2]; 
00194         d2b = pState[3]; 
00195 
00196         pCoeffs += 5u;
00197 
00198         /* First part of the processing with loop unrolling.  Compute 8 outputs at a time.       
00199         ** a second loop below computes the remaining 1 to 7 samples. */
00200         while(sample > 0u) {
00201 
00202             /* y[n] = b0 * x[n] + d1 */
00203             /* d1 = b1 * x[n] + a1 * y[n] + d2 */
00204             /* d2 = b2 * x[n] + a2 * y[n] */
00205 
00206             /* Read the first 2 inputs. 2 cycles */
00207             Xn1a  = pIn[0 ];
00208             Xn1b  = pIn[1 ];
00209 
00210             /* Sample 1. 5 cycles */
00211             Xn2a  = pIn[2 ];
00212             acc1a = b0 * Xn1a + d1a;
00213 
00214             Xn2b  = pIn[3 ];
00215             d1a = b1 * Xn1a + d2a;
00216 
00217             Xn3a  = pIn[4 ];
00218             d2a = b2 * Xn1a;
00219 
00220             Xn3b  = pIn[5 ];
00221             d1a += a1 * acc1a;
00222 
00223             Xn4a  = pIn[6 ];
00224             d2a += a2 * acc1a;
00225 
00226             /* Sample 2. 5 cycles */
00227             Xn4b  = pIn[7 ];
00228             acc1b = b0 * Xn1b + d1b;
00229 
00230             Xn5a  = pIn[8 ];
00231             d1b = b1 * Xn1b + d2b;
00232 
00233             Xn5b = pIn[9 ];
00234             d2b = b2 * Xn1b;
00235 
00236             Xn6a = pIn[10];
00237             d1b += a1 * acc1b;
00238 
00239             Xn6b = pIn[11];
00240             d2b += a2 * acc1b;
00241 
00242             /* Sample 3. 5 cycles */
00243             Xn7a = pIn[12];
00244             acc2a = b0 * Xn2a + d1a;
00245 
00246             Xn7b = pIn[13];
00247             d1a = b1 * Xn2a + d2a;
00248 
00249             Xn8a = pIn[14];
00250             d2a = b2 * Xn2a;
00251 
00252             Xn8b = pIn[15];
00253             d1a += a1 * acc2a;
00254 
00255             pIn += 16;
00256             d2a += a2 * acc2a;
00257 
00258             /* Sample 4. 5 cycles */
00259             acc2b = b0 * Xn2b + d1b;
00260             d1b = b1 * Xn2b + d2b;
00261             d2b = b2 * Xn2b;
00262             d1b += a1 * acc2b;
00263             d2b += a2 * acc2b;
00264 
00265             /* Sample 5. 5 cycles */
00266             acc3a = b0 * Xn3a + d1a;
00267             d1a = b1 * Xn3a + d2a;
00268             d2a = b2 * Xn3a;
00269             d1a += a1 * acc3a;
00270             d2a += a2 * acc3a;
00271 
00272             /* Sample 6. 5 cycles */
00273             acc3b = b0 * Xn3b + d1b;
00274             d1b = b1 * Xn3b + d2b;
00275             d2b = b2 * Xn3b;
00276             d1b += a1 * acc3b;
00277             d2b += a2 * acc3b;
00278 
00279             /* Sample 7. 5 cycles */
00280             acc4a = b0 * Xn4a + d1a;
00281             d1a = b1 * Xn4a + d2a;
00282             d2a = b2 * Xn4a;
00283             d1a += a1 * acc4a;
00284             d2a += a2 * acc4a;
00285 
00286             /* Sample 8. 5 cycles */
00287             acc4b = b0 * Xn4b + d1b;
00288             d1b = b1 * Xn4b + d2b;
00289             d2b = b2 * Xn4b;
00290             d1b += a1 * acc4b;
00291             d2b += a2 * acc4b;
00292 
00293             /* Sample 9. 5 cycles */
00294             acc5a = b0 * Xn5a + d1a;
00295             d1a = b1 * Xn5a + d2a;
00296             d2a = b2 * Xn5a;
00297             d1a += a1 * acc5a;
00298             d2a += a2 * acc5a;
00299 
00300             /* Sample 10. 5 cycles */
00301             acc5b = b0 * Xn5b + d1b;
00302             d1b = b1 * Xn5b + d2b;
00303             d2b = b2 * Xn5b;
00304             d1b += a1 * acc5b;
00305             d2b += a2 * acc5b;
00306 
00307             /* Sample 11. 5 cycles */
00308             acc6a = b0 * Xn6a + d1a;
00309             d1a = b1 * Xn6a + d2a;
00310             d2a = b2 * Xn6a;
00311             d1a += a1 * acc6a;
00312             d2a += a2 * acc6a;
00313 
00314             /* Sample 12. 5 cycles */
00315             acc6b = b0 * Xn6b + d1b;
00316             d1b = b1 * Xn6b + d2b;
00317             d2b = b2 * Xn6b;
00318             d1b += a1 * acc6b;
00319             d2b += a2 * acc6b;
00320 
00321             /* Sample 13. 5 cycles */
00322             acc7a = b0 * Xn7a + d1a;         
00323             d1a = b1 * Xn7a + d2a;   
00324             
00325             pOut[0 ] = acc1a ;      
00326             d2a = b2 * Xn7a;
00327 
00328             pOut[1 ] = acc1b ;  
00329             d1a += a1 * acc7a;
00330 
00331             pOut[2 ] = acc2a ;  
00332             d2a += a2 * acc7a;
00333 
00334             /* Sample 14. 5 cycles */
00335             pOut[3 ] = acc2b ;
00336             acc7b = b0 * Xn7b + d1b;
00337 
00338             pOut[4 ] = acc3a ; 
00339             d1b = b1 * Xn7b + d2b;
00340 
00341             pOut[5 ] = acc3b ;  
00342             d2b = b2 * Xn7b;
00343 
00344             pOut[6 ] = acc4a ;    
00345             d1b += a1 * acc7b;
00346 
00347             pOut[7 ] = acc4b ;
00348             d2b += a2 * acc7b;
00349 
00350             /* Sample 15. 5 cycles */
00351             pOut[8 ] = acc5a ;  
00352             acc8a = b0 * Xn8a + d1a;
00353 
00354             pOut[9 ] = acc5b;   
00355             d1a = b1 * Xn8a + d2a;
00356 
00357             pOut[10] = acc6a;   
00358             d2a = b2 * Xn8a;
00359 
00360             pOut[11] = acc6b;
00361             d1a += a1 * acc8a;
00362 
00363             pOut[12] = acc7a;
00364             d2a += a2 * acc8a;
00365 
00366             /* Sample 16. 5 cycles */
00367             pOut[13] = acc7b;   
00368             acc8b = b0 * Xn8b + d1b;
00369 
00370             pOut[14] = acc8a;   
00371             d1b = b1 * Xn8b + d2b;
00372 
00373             pOut[15] = acc8b;
00374             d2b = b2 * Xn8b;
00375 
00376             sample--;    
00377             d1b += a1 * acc8b;
00378 
00379             pOut += 16;
00380             d2b += a2 * acc8b;
00381         }
00382 
00383         sample = blockSize & 0x7u;
00384         while(sample > 0u) {
00385             /* Read the input */
00386             Xn1a = *pIn++; //Channel a
00387             Xn1b = *pIn++; //Channel b
00388 
00389             /* y[n] = b0 * x[n] + d1 */
00390             acc1a = (b0 * Xn1a) + d1a;
00391             acc1b = (b0 * Xn1b) + d1b;
00392 
00393             /* Store the result in the accumulator in the destination buffer. */
00394             *pOut++ = acc1a;
00395             *pOut++ = acc1b;
00396 
00397             /* Every time after the output is computed state should be updated. */
00398             /* d1 = b1 * x[n] + a1 * y[n] + d2 */
00399             d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
00400             d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
00401 
00402             /* d2 = b2 * x[n] + a2 * y[n] */
00403             d2a = (b2 * Xn1a) + (a2 * acc1a);
00404             d2b = (b2 * Xn1b) + (a2 * acc1b);
00405 
00406             sample--;   
00407         }
00408 
00409         /* Store the updated state variables back into the state array */ 
00410         pState[0] = d1a; 
00411         pState[1] = d2a;         
00412 
00413         pState[2] = d1b; 
00414         pState[3] = d2b; 
00415         
00416         /* The current stage input is given as the output to the next stage */ 
00417         pIn = pDst; 
00418         /* decrement the loop counter */ 
00419         stage--; 
00420 
00421         pState += 4u;
00422         /*Reset the output working pointer */ 
00423         pOut = pDst; 
00424 
00425     } while(stage > 0u);
00426     
00427 #elif defined(ARM_MATH_CM0_FAMILY)
00428 
00429     /* Run the below code for Cortex-M0 */
00430 
00431     do
00432     {
00433         /* Reading the coefficients */
00434         b0 = *pCoeffs++;
00435         b1 = *pCoeffs++;
00436         b2 = *pCoeffs++;
00437         a1 = *pCoeffs++;
00438         a2 = *pCoeffs++;
00439 
00440         /*Reading the state values */
00441         d1a = pState[0];
00442         d2a = pState[1];
00443         d1b = pState[2];
00444         d2b = pState[3];
00445 
00446 
00447         sample = blockSize;
00448 
00449         while(sample > 0u)
00450         {
00451             /* Read the input */
00452             Xn1a = *pIn++; //Channel a
00453             Xn1b = *pIn++; //Channel b
00454 
00455             /* y[n] = b0 * x[n] + d1 */
00456             acc1a = (b0 * Xn1a) + d1a;
00457             acc1b = (b0 * Xn1b) + d1b;
00458 
00459             /* Store the result in the accumulator in the destination buffer. */
00460             *pOut++ = acc1a;
00461             *pOut++ = acc1b;
00462 
00463             /* Every time after the output is computed state should be updated. */
00464             /* d1 = b1 * x[n] + a1 * y[n] + d2 */
00465             d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
00466             d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
00467 
00468             /* d2 = b2 * x[n] + a2 * y[n] */
00469             d2a = (b2 * Xn1a) + (a2 * acc1a);
00470             d2b = (b2 * Xn1b) + (a2 * acc1b);
00471 
00472             /* decrement the loop counter */
00473             sample--;
00474         }
00475 
00476         /* Store the updated state variables back into the state array */
00477         *pState++ = d1a;
00478         *pState++ = d2a;
00479         *pState++ = d1b;
00480         *pState++ = d2b;
00481 
00482         /* The current stage input is given as the output to the next stage */
00483         pIn = pDst;
00484 
00485         /*Reset the output working pointer */
00486         pOut = pDst;
00487 
00488         /* decrement the loop counter */
00489         stage--;
00490 
00491     } while(stage > 0u);
00492      
00493 #else
00494 
00495     float32_t Xn2a, Xn3a, Xn4a;                          /*  Input State variables     */
00496     float32_t Xn2b, Xn3b, Xn4b;                          /*  Input State variables     */
00497     float32_t acc2a, acc3a, acc4a;                       /*  accumulator               */
00498     float32_t acc2b, acc3b, acc4b;                       /*  accumulator               */
00499     float32_t p0a, p1a, p2a, p3a, p4a, A1a;
00500     float32_t p0b, p1b, p2b, p3b, p4b, A1b;
00501 
00502     /* Run the below code for Cortex-M4 and Cortex-M3 */
00503     do
00504     {
00505         /* Reading the coefficients */     
00506         b0 = *pCoeffs++;
00507         b1 = *pCoeffs++;
00508         b2 = *pCoeffs++;
00509         a1 = *pCoeffs++;
00510         a2 = *pCoeffs++;      
00511 
00512         /*Reading the state values */
00513         d1a = pState[0];
00514         d2a = pState[1];
00515         d1b = pState[2];
00516         d2b = pState[3];
00517 
00518         /* Apply loop unrolling and compute 4 output values simultaneously. */
00519         sample = blockSize >> 2u;
00520 
00521         /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.       
00522         ** a second loop below computes the remaining 1 to 3 samples. */
00523         while(sample > 0u) {
00524 
00525             /* y[n] = b0 * x[n] + d1 */
00526             /* d1 = b1 * x[n] + a1 * y[n] + d2 */
00527             /* d2 = b2 * x[n] + a2 * y[n] */
00528 
00529             /* Read the four inputs */
00530             Xn1a = pIn[0];
00531             Xn1b = pIn[1];
00532             Xn2a = pIn[2];
00533             Xn2b = pIn[3];
00534             Xn3a = pIn[4];
00535             Xn3b = pIn[5];
00536             Xn4a = pIn[6];
00537             Xn4b = pIn[7];
00538             pIn += 8;     
00539             
00540             p0a = b0 * Xn1a; 
00541             p0b = b0 * Xn1b; 
00542             p1a = b1 * Xn1a;
00543             p1b = b1 * Xn1b;
00544             acc1a = p0a + d1a;
00545             acc1b = p0b + d1b;
00546             p0a = b0 * Xn2a; 
00547             p0b = b0 * Xn2b; 
00548             p3a = a1 * acc1a;
00549             p3b = a1 * acc1b;
00550             p2a = b2 * Xn1a;
00551             p2b = b2 * Xn1b;
00552             A1a = p1a + p3a;
00553             A1b = p1b + p3b;
00554             p4a = a2 * acc1a;
00555             p4b = a2 * acc1b;
00556             d1a = A1a + d2a;
00557             d1b = A1b + d2b;
00558             d2a = p2a + p4a;
00559             d2b = p2b + p4b;
00560             
00561             p1a = b1 * Xn2a;
00562             p1b = b1 * Xn2b;
00563             acc2a = p0a + d1a;
00564             acc2b = p0b + d1b;
00565             p0a = b0 * Xn3a; 
00566             p0b = b0 * Xn3b; 
00567             p3a = a1 * acc2a;
00568             p3b = a1 * acc2b;
00569             p2a = b2 * Xn2a;
00570             p2b = b2 * Xn2b;
00571             A1a = p1a + p3a;
00572             A1b = p1b + p3b;
00573             p4a = a2 * acc2a;
00574             p4b = a2 * acc2b;
00575             d1a = A1a + d2a;
00576             d1b = A1b + d2b;
00577             d2a = p2a + p4a;
00578             d2b = p2b + p4b;
00579             
00580             p1a = b1 * Xn3a;
00581             p1b = b1 * Xn3b;
00582             acc3a = p0a + d1a;
00583             acc3b = p0b + d1b;
00584             p0a = b0 * Xn4a; 
00585             p0b = b0 * Xn4b; 
00586             p3a = a1 * acc3a;
00587             p3b = a1 * acc3b;
00588             p2a = b2 * Xn3a;
00589             p2b = b2 * Xn3b;
00590             A1a = p1a + p3a;
00591             A1b = p1b + p3b;
00592             p4a = a2 * acc3a;
00593             p4b = a2 * acc3b;
00594             d1a = A1a + d2a;
00595             d1b = A1b + d2b;
00596             d2a = p2a + p4a;
00597             d2b = p2b + p4b;
00598             
00599             acc4a = p0a + d1a;
00600             acc4b = p0b + d1b;
00601             p1a = b1 * Xn4a;
00602             p1b = b1 * Xn4b;
00603             p3a = a1 * acc4a;
00604             p3b = a1 * acc4b;
00605             p2a = b2 * Xn4a;
00606             p2b = b2 * Xn4b;
00607             A1a = p1a + p3a;
00608             A1b = p1b + p3b;
00609             p4a = a2 * acc4a;
00610             p4b = a2 * acc4b;
00611             d1a = A1a + d2a;
00612             d1b = A1b + d2b;
00613             d2a = p2a + p4a;
00614             d2b = p2b + p4b;
00615 
00616             pOut[0] = acc1a;    
00617             pOut[1] = acc1b;    
00618             pOut[2] = acc2a;    
00619             pOut[3] = acc2b;
00620             pOut[4] = acc3a;    
00621             pOut[5] = acc3b;    
00622             pOut[6] = acc4a;    
00623             pOut[7] = acc4b;
00624             pOut += 8;
00625              
00626             sample--;          
00627         }
00628 
00629         sample = blockSize & 0x3u;
00630         while(sample > 0u) {
00631             Xn1a = *pIn++;
00632             Xn1b = *pIn++;
00633 
00634             p0a = b0 * Xn1a; 
00635             p0b = b0 * Xn1b; 
00636             p1a = b1 * Xn1a;
00637             p1b = b1 * Xn1b;
00638             acc1a = p0a + d1a;
00639             acc1b = p0b + d1b;
00640             p3a = a1 * acc1a;
00641             p3b = a1 * acc1b;
00642             p2a = b2 * Xn1a;
00643             p2b = b2 * Xn1b;
00644             A1a = p1a + p3a;
00645             A1b = p1b + p3b;
00646             p4a = a2 * acc1a;
00647             p4b = a2 * acc1b;
00648             d1a = A1a + d2a;
00649             d1b = A1b + d2b;
00650             d2a = p2a + p4a;
00651             d2b = p2b + p4b;
00652 
00653             *pOut++ = acc1a;
00654             *pOut++ = acc1b;
00655 
00656             sample--;          
00657         }
00658 
00659         /* Store the updated state variables back into the state array */
00660         *pState++ = d1a;
00661         *pState++ = d2a;
00662         *pState++ = d1b;
00663         *pState++ = d2b;
00664 
00665         /* The current stage input is given as the output to the next stage */
00666         pIn = pDst;
00667 
00668         /*Reset the output working pointer */
00669         pOut = pDst;
00670 
00671         /* decrement the loop counter */
00672         stage--;
00673 
00674     } while(stage > 0u);
00675 
00676 #endif 
00677 
00678 }
00679 LOW_OPTIMIZATION_EXIT
00680 
00681 /**       
00682    * @} end of BiquadCascadeDF2T group       
00683    */