Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

Dependents:   Nucleo-Heart-Rate ejercicioVrms2 PROYECTOFINAL ejercicioVrms ... more

functions/FilteringFunctions/arm_correlate_fast_q31.c@3:4098b9d3d571, 2018-06-21 (annotated)

Committer:
xorjoep
Date:
Thu Jun 21 11:56:27 2018 +0000
Revision:
3:4098b9d3d571
Parent:
1:24714b45cd1b 
headers is a folder not a library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xorjoep 1:24714b45cd1b 1 /* ----------------------------------------------------------------------
xorjoep 1:24714b45cd1b 2 * Project: CMSIS DSP Library
xorjoep 1:24714b45cd1b 3 * Title: arm_correlate_fast_q31.c
xorjoep 1:24714b45cd1b 4 * Description: Fast Q31 Correlation
xorjoep 1:24714b45cd1b 5 *
xorjoep 1:24714b45cd1b 6 * $Date: 27. January 2017
xorjoep 1:24714b45cd1b 7 * $Revision: V.1.5.1
xorjoep 1:24714b45cd1b 8 *
xorjoep 1:24714b45cd1b 9 * Target Processor: Cortex-M cores
xorjoep 1:24714b45cd1b 10 * -------------------------------------------------------------------- */
xorjoep 1:24714b45cd1b 11 /*
xorjoep 1:24714b45cd1b 12 * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
xorjoep 1:24714b45cd1b 13 *
xorjoep 1:24714b45cd1b 14 * SPDX-License-Identifier: Apache-2.0
xorjoep 1:24714b45cd1b 15 *
xorjoep 1:24714b45cd1b 16 * Licensed under the Apache License, Version 2.0 (the License); you may
xorjoep 1:24714b45cd1b 17 * not use this file except in compliance with the License.
xorjoep 1:24714b45cd1b 18 * You may obtain a copy of the License at
xorjoep 1:24714b45cd1b 19 *
xorjoep 1:24714b45cd1b 20 * www.apache.org/licenses/LICENSE-2.0
xorjoep 1:24714b45cd1b 21 *
xorjoep 1:24714b45cd1b 22 * Unless required by applicable law or agreed to in writing, software
xorjoep 1:24714b45cd1b 23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
xorjoep 1:24714b45cd1b 24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
xorjoep 1:24714b45cd1b 25 * See the License for the specific language governing permissions and
xorjoep 1:24714b45cd1b 26 * limitations under the License.
xorjoep 1:24714b45cd1b 27 */
xorjoep 1:24714b45cd1b 28
xorjoep 1:24714b45cd1b 29 #include "arm_math.h"
xorjoep 1:24714b45cd1b 30
xorjoep 1:24714b45cd1b 31 /**
xorjoep 1:24714b45cd1b 32 * @ingroup groupFilters
xorjoep 1:24714b45cd1b 33 */
xorjoep 1:24714b45cd1b 34
xorjoep 1:24714b45cd1b 35 /**
xorjoep 1:24714b45cd1b 36 * @addtogroup Corr
xorjoep 1:24714b45cd1b 37 * @{
xorjoep 1:24714b45cd1b 38 */
xorjoep 1:24714b45cd1b 39
xorjoep 1:24714b45cd1b 40 /**
xorjoep 1:24714b45cd1b 41 * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4.
xorjoep 1:24714b45cd1b 42 * @param[in] *pSrcA points to the first input sequence.
xorjoep 1:24714b45cd1b 43 * @param[in] srcALen length of the first input sequence.
xorjoep 1:24714b45cd1b 44 * @param[in] *pSrcB points to the second input sequence.
xorjoep 1:24714b45cd1b 45 * @param[in] srcBLen length of the second input sequence.
xorjoep 1:24714b45cd1b 46 * @param[out] *pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
xorjoep 1:24714b45cd1b 47 * @return none.
xorjoep 1:24714b45cd1b 48 *
xorjoep 1:24714b45cd1b 49 * @details
xorjoep 1:24714b45cd1b 50 * <b>Scaling and Overflow Behavior:</b>
xorjoep 1:24714b45cd1b 51 *
xorjoep 1:24714b45cd1b 52 * \par
xorjoep 1:24714b45cd1b 53 * This function is optimized for speed at the expense of fixed-point precision and overflow protection.
xorjoep 1:24714b45cd1b 54 * The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
xorjoep 1:24714b45cd1b 55 * These intermediate results are accumulated in a 32-bit register in 2.30 format.
xorjoep 1:24714b45cd1b 56 * Finally, the accumulator is saturated and converted to a 1.31 result.
xorjoep 1:24714b45cd1b 57 *
xorjoep 1:24714b45cd1b 58 * \par
xorjoep 1:24714b45cd1b 59 * The fast version has the same overflow behavior as the standard version but provides less precision since it discards the low 32 bits of each multiplication result.
xorjoep 1:24714b45cd1b 60 * In order to avoid overflows completely the input signals must be scaled down.
xorjoep 1:24714b45cd1b 61 * The input signals should be scaled down to avoid intermediate overflows.
xorjoep 1:24714b45cd1b 62 * Scale down one of the inputs by 1/min(srcALen, srcBLen)to avoid overflows since a
xorjoep 1:24714b45cd1b 63 * maximum of min(srcALen, srcBLen) number of additions is carried internally.
xorjoep 1:24714b45cd1b 64 *
xorjoep 1:24714b45cd1b 65 * \par
xorjoep 1:24714b45cd1b 66 * See <code>arm_correlate_q31()</code> for a slower implementation of this function which uses 64-bit accumulation to provide higher precision.
xorjoep 1:24714b45cd1b 67 */
xorjoep 1:24714b45cd1b 68
xorjoep 1:24714b45cd1b 69 void arm_correlate_fast_q31(
xorjoep 1:24714b45cd1b 70 q31_t * pSrcA,
xorjoep 1:24714b45cd1b 71 uint32_t srcALen,
xorjoep 1:24714b45cd1b 72 q31_t * pSrcB,
xorjoep 1:24714b45cd1b 73 uint32_t srcBLen,
xorjoep 1:24714b45cd1b 74 q31_t * pDst)
xorjoep 1:24714b45cd1b 75 {
xorjoep 1:24714b45cd1b 76 q31_t *pIn1; /* inputA pointer */
xorjoep 1:24714b45cd1b 77 q31_t *pIn2; /* inputB pointer */
xorjoep 1:24714b45cd1b 78 q31_t *pOut = pDst; /* output pointer */
xorjoep 1:24714b45cd1b 79 q31_t *px; /* Intermediate inputA pointer */
xorjoep 1:24714b45cd1b 80 q31_t *py; /* Intermediate inputB pointer */
xorjoep 1:24714b45cd1b 81 q31_t *pSrc1; /* Intermediate pointers */
xorjoep 1:24714b45cd1b 82 q31_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
xorjoep 1:24714b45cd1b 83 q31_t x0, x1, x2, x3, c0; /* temporary variables for holding input and coefficient values */
xorjoep 1:24714b45cd1b 84 uint32_t j, k = 0U, count, blkCnt, outBlockSize, blockSize1, blockSize2, blockSize3; /* loop counter */
xorjoep 1:24714b45cd1b 85 int32_t inc = 1; /* Destination address modifier */
xorjoep 1:24714b45cd1b 86
xorjoep 1:24714b45cd1b 87
xorjoep 1:24714b45cd1b 88 /* The algorithm implementation is based on the lengths of the inputs. */
xorjoep 1:24714b45cd1b 89 /* srcB is always made to slide across srcA. */
xorjoep 1:24714b45cd1b 90 /* So srcBLen is always considered as shorter or equal to srcALen */
xorjoep 1:24714b45cd1b 91 if (srcALen >= srcBLen)
xorjoep 1:24714b45cd1b 92 {
xorjoep 1:24714b45cd1b 93 /* Initialization of inputA pointer */
xorjoep 1:24714b45cd1b 94 pIn1 = (pSrcA);
xorjoep 1:24714b45cd1b 95
xorjoep 1:24714b45cd1b 96 /* Initialization of inputB pointer */
xorjoep 1:24714b45cd1b 97 pIn2 = (pSrcB);
xorjoep 1:24714b45cd1b 98
xorjoep 1:24714b45cd1b 99 /* Number of output samples is calculated */
xorjoep 1:24714b45cd1b 100 outBlockSize = (2U * srcALen) - 1U;
xorjoep 1:24714b45cd1b 101
xorjoep 1:24714b45cd1b 102 /* When srcALen > srcBLen, zero padding is done to srcB
xorjoep 1:24714b45cd1b 103 * to make their lengths equal.
xorjoep 1:24714b45cd1b 104 * Instead, (outBlockSize - (srcALen + srcBLen - 1))
xorjoep 1:24714b45cd1b 105 * number of output samples are made zero */
xorjoep 1:24714b45cd1b 106 j = outBlockSize - (srcALen + (srcBLen - 1U));
xorjoep 1:24714b45cd1b 107
xorjoep 1:24714b45cd1b 108 /* Updating the pointer position to non zero value */
xorjoep 1:24714b45cd1b 109 pOut += j;
xorjoep 1:24714b45cd1b 110
xorjoep 1:24714b45cd1b 111 }
xorjoep 1:24714b45cd1b 112 else
xorjoep 1:24714b45cd1b 113 {
xorjoep 1:24714b45cd1b 114 /* Initialization of inputA pointer */
xorjoep 1:24714b45cd1b 115 pIn1 = (pSrcB);
xorjoep 1:24714b45cd1b 116
xorjoep 1:24714b45cd1b 117 /* Initialization of inputB pointer */
xorjoep 1:24714b45cd1b 118 pIn2 = (pSrcA);
xorjoep 1:24714b45cd1b 119
xorjoep 1:24714b45cd1b 120 /* srcBLen is always considered as shorter or equal to srcALen */
xorjoep 1:24714b45cd1b 121 j = srcBLen;
xorjoep 1:24714b45cd1b 122 srcBLen = srcALen;
xorjoep 1:24714b45cd1b 123 srcALen = j;
xorjoep 1:24714b45cd1b 124
xorjoep 1:24714b45cd1b 125 /* CORR(x, y) = Reverse order(CORR(y, x)) */
xorjoep 1:24714b45cd1b 126 /* Hence set the destination pointer to point to the last output sample */
xorjoep 1:24714b45cd1b 127 pOut = pDst + ((srcALen + srcBLen) - 2U);
xorjoep 1:24714b45cd1b 128
xorjoep 1:24714b45cd1b 129 /* Destination address modifier is set to -1 */
xorjoep 1:24714b45cd1b 130 inc = -1;
xorjoep 1:24714b45cd1b 131
xorjoep 1:24714b45cd1b 132 }
xorjoep 1:24714b45cd1b 133
xorjoep 1:24714b45cd1b 134 /* The function is internally
xorjoep 1:24714b45cd1b 135 * divided into three parts according to the number of multiplications that has to be
xorjoep 1:24714b45cd1b 136 * taken place between inputA samples and inputB samples. In the first part of the
xorjoep 1:24714b45cd1b 137 * algorithm, the multiplications increase by one for every iteration.
xorjoep 1:24714b45cd1b 138 * In the second part of the algorithm, srcBLen number of multiplications are done.
xorjoep 1:24714b45cd1b 139 * In the third part of the algorithm, the multiplications decrease by one
xorjoep 1:24714b45cd1b 140 * for every iteration.*/
xorjoep 1:24714b45cd1b 141 /* The algorithm is implemented in three stages.
xorjoep 1:24714b45cd1b 142 * The loop counters of each stage is initiated here. */
xorjoep 1:24714b45cd1b 143 blockSize1 = srcBLen - 1U;
xorjoep 1:24714b45cd1b 144 blockSize2 = srcALen - (srcBLen - 1U);
xorjoep 1:24714b45cd1b 145 blockSize3 = blockSize1;
xorjoep 1:24714b45cd1b 146
xorjoep 1:24714b45cd1b 147 /* --------------------------
xorjoep 1:24714b45cd1b 148 * Initializations of stage1
xorjoep 1:24714b45cd1b 149 * -------------------------*/
xorjoep 1:24714b45cd1b 150
xorjoep 1:24714b45cd1b 151 /* sum = x[0] * y[srcBlen - 1]
xorjoep 1:24714b45cd1b 152 * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
xorjoep 1:24714b45cd1b 153 * ....
xorjoep 1:24714b45cd1b 154 * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
xorjoep 1:24714b45cd1b 155 */
xorjoep 1:24714b45cd1b 156
xorjoep 1:24714b45cd1b 157 /* In this stage the MAC operations are increased by 1 for every iteration.
xorjoep 1:24714b45cd1b 158 The count variable holds the number of MAC operations performed */
xorjoep 1:24714b45cd1b 159 count = 1U;
xorjoep 1:24714b45cd1b 160
xorjoep 1:24714b45cd1b 161 /* Working pointer of inputA */
xorjoep 1:24714b45cd1b 162 px = pIn1;
xorjoep 1:24714b45cd1b 163
xorjoep 1:24714b45cd1b 164 /* Working pointer of inputB */
xorjoep 1:24714b45cd1b 165 pSrc1 = pIn2 + (srcBLen - 1U);
xorjoep 1:24714b45cd1b 166 py = pSrc1;
xorjoep 1:24714b45cd1b 167
xorjoep 1:24714b45cd1b 168 /* ------------------------
xorjoep 1:24714b45cd1b 169 * Stage1 process
xorjoep 1:24714b45cd1b 170 * ----------------------*/
xorjoep 1:24714b45cd1b 171
xorjoep 1:24714b45cd1b 172 /* The first stage starts here */
xorjoep 1:24714b45cd1b 173 while (blockSize1 > 0U)
xorjoep 1:24714b45cd1b 174 {
xorjoep 1:24714b45cd1b 175 /* Accumulator is made zero for every iteration */
xorjoep 1:24714b45cd1b 176 sum = 0;
xorjoep 1:24714b45cd1b 177
xorjoep 1:24714b45cd1b 178 /* Apply loop unrolling and compute 4 MACs simultaneously. */
xorjoep 1:24714b45cd1b 179 k = count >> 2;
xorjoep 1:24714b45cd1b 180
xorjoep 1:24714b45cd1b 181 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
xorjoep 1:24714b45cd1b 182 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
xorjoep 1:24714b45cd1b 183 while (k > 0U)
xorjoep 1:24714b45cd1b 184 {
xorjoep 1:24714b45cd1b 185 /* x[0] * y[srcBLen - 4] */
xorjoep 1:24714b45cd1b 186 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 187 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 188 /* x[1] * y[srcBLen - 3] */
xorjoep 1:24714b45cd1b 189 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 190 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 191 /* x[2] * y[srcBLen - 2] */
xorjoep 1:24714b45cd1b 192 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 193 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 194 /* x[3] * y[srcBLen - 1] */
xorjoep 1:24714b45cd1b 195 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 196 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 197
xorjoep 1:24714b45cd1b 198 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 199 k--;
xorjoep 1:24714b45cd1b 200 }
xorjoep 1:24714b45cd1b 201
xorjoep 1:24714b45cd1b 202 /* If the count is not a multiple of 4, compute any remaining MACs here.
xorjoep 1:24714b45cd1b 203 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 204 k = count % 0x4U;
xorjoep 1:24714b45cd1b 205
xorjoep 1:24714b45cd1b 206 while (k > 0U)
xorjoep 1:24714b45cd1b 207 {
xorjoep 1:24714b45cd1b 208 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 209 /* x[0] * y[srcBLen - 1] */
xorjoep 1:24714b45cd1b 210 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 211 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 212
xorjoep 1:24714b45cd1b 213 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 214 k--;
xorjoep 1:24714b45cd1b 215 }
xorjoep 1:24714b45cd1b 216
xorjoep 1:24714b45cd1b 217 /* Store the result in the accumulator in the destination buffer. */
xorjoep 1:24714b45cd1b 218 *pOut = sum << 1;
xorjoep 1:24714b45cd1b 219 /* Destination pointer is updated according to the address modifier, inc */
xorjoep 1:24714b45cd1b 220 pOut += inc;
xorjoep 1:24714b45cd1b 221
xorjoep 1:24714b45cd1b 222 /* Update the inputA and inputB pointers for next MAC calculation */
xorjoep 1:24714b45cd1b 223 py = pSrc1 - count;
xorjoep 1:24714b45cd1b 224 px = pIn1;
xorjoep 1:24714b45cd1b 225
xorjoep 1:24714b45cd1b 226 /* Increment the MAC count */
xorjoep 1:24714b45cd1b 227 count++;
xorjoep 1:24714b45cd1b 228
xorjoep 1:24714b45cd1b 229 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 230 blockSize1--;
xorjoep 1:24714b45cd1b 231 }
xorjoep 1:24714b45cd1b 232
xorjoep 1:24714b45cd1b 233 /* --------------------------
xorjoep 1:24714b45cd1b 234 * Initializations of stage2
xorjoep 1:24714b45cd1b 235 * ------------------------*/
xorjoep 1:24714b45cd1b 236
xorjoep 1:24714b45cd1b 237 /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
xorjoep 1:24714b45cd1b 238 * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
xorjoep 1:24714b45cd1b 239 * ....
xorjoep 1:24714b45cd1b 240 * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
xorjoep 1:24714b45cd1b 241 */
xorjoep 1:24714b45cd1b 242
xorjoep 1:24714b45cd1b 243 /* Working pointer of inputA */
xorjoep 1:24714b45cd1b 244 px = pIn1;
xorjoep 1:24714b45cd1b 245
xorjoep 1:24714b45cd1b 246 /* Working pointer of inputB */
xorjoep 1:24714b45cd1b 247 py = pIn2;
xorjoep 1:24714b45cd1b 248
xorjoep 1:24714b45cd1b 249 /* count is index by which the pointer pIn1 to be incremented */
xorjoep 1:24714b45cd1b 250 count = 0U;
xorjoep 1:24714b45cd1b 251
xorjoep 1:24714b45cd1b 252 /* -------------------
xorjoep 1:24714b45cd1b 253 * Stage2 process
xorjoep 1:24714b45cd1b 254 * ------------------*/
xorjoep 1:24714b45cd1b 255
xorjoep 1:24714b45cd1b 256 /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
xorjoep 1:24714b45cd1b 257 * So, to loop unroll over blockSize2,
xorjoep 1:24714b45cd1b 258 * srcBLen should be greater than or equal to 4 */
xorjoep 1:24714b45cd1b 259 if (srcBLen >= 4U)
xorjoep 1:24714b45cd1b 260 {
xorjoep 1:24714b45cd1b 261 /* Loop unroll over blockSize2, by 4 */
xorjoep 1:24714b45cd1b 262 blkCnt = blockSize2 >> 2U;
xorjoep 1:24714b45cd1b 263
xorjoep 1:24714b45cd1b 264 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 265 {
xorjoep 1:24714b45cd1b 266 /* Set all accumulators to zero */
xorjoep 1:24714b45cd1b 267 acc0 = 0;
xorjoep 1:24714b45cd1b 268 acc1 = 0;
xorjoep 1:24714b45cd1b 269 acc2 = 0;
xorjoep 1:24714b45cd1b 270 acc3 = 0;
xorjoep 1:24714b45cd1b 271
xorjoep 1:24714b45cd1b 272 /* read x[0], x[1], x[2] samples */
xorjoep 1:24714b45cd1b 273 x0 = *(px++);
xorjoep 1:24714b45cd1b 274 x1 = *(px++);
xorjoep 1:24714b45cd1b 275 x2 = *(px++);
xorjoep 1:24714b45cd1b 276
xorjoep 1:24714b45cd1b 277 /* Apply loop unrolling and compute 4 MACs simultaneously. */
xorjoep 1:24714b45cd1b 278 k = srcBLen >> 2U;
xorjoep 1:24714b45cd1b 279
xorjoep 1:24714b45cd1b 280 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
xorjoep 1:24714b45cd1b 281 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
xorjoep 1:24714b45cd1b 282 do
xorjoep 1:24714b45cd1b 283 {
xorjoep 1:24714b45cd1b 284 /* Read y[0] sample */
xorjoep 1:24714b45cd1b 285 c0 = *(py++);
xorjoep 1:24714b45cd1b 286
xorjoep 1:24714b45cd1b 287 /* Read x[3] sample */
xorjoep 1:24714b45cd1b 288 x3 = *(px++);
xorjoep 1:24714b45cd1b 289
xorjoep 1:24714b45cd1b 290 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 291 /* acc0 += x[0] * y[0] */
xorjoep 1:24714b45cd1b 292 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
xorjoep 1:24714b45cd1b 293 /* acc1 += x[1] * y[0] */
xorjoep 1:24714b45cd1b 294 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
xorjoep 1:24714b45cd1b 295 /* acc2 += x[2] * y[0] */
xorjoep 1:24714b45cd1b 296 acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
xorjoep 1:24714b45cd1b 297 /* acc3 += x[3] * y[0] */
xorjoep 1:24714b45cd1b 298 acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
xorjoep 1:24714b45cd1b 299
xorjoep 1:24714b45cd1b 300 /* Read y[1] sample */
xorjoep 1:24714b45cd1b 301 c0 = *(py++);
xorjoep 1:24714b45cd1b 302
xorjoep 1:24714b45cd1b 303 /* Read x[4] sample */
xorjoep 1:24714b45cd1b 304 x0 = *(px++);
xorjoep 1:24714b45cd1b 305
xorjoep 1:24714b45cd1b 306 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 307 /* acc0 += x[1] * y[1] */
xorjoep 1:24714b45cd1b 308 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x1 * c0)) >> 32);
xorjoep 1:24714b45cd1b 309 /* acc1 += x[2] * y[1] */
xorjoep 1:24714b45cd1b 310 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x2 * c0)) >> 32);
xorjoep 1:24714b45cd1b 311 /* acc2 += x[3] * y[1] */
xorjoep 1:24714b45cd1b 312 acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x3 * c0)) >> 32);
xorjoep 1:24714b45cd1b 313 /* acc3 += x[4] * y[1] */
xorjoep 1:24714b45cd1b 314 acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x0 * c0)) >> 32);
xorjoep 1:24714b45cd1b 315
xorjoep 1:24714b45cd1b 316 /* Read y[2] sample */
xorjoep 1:24714b45cd1b 317 c0 = *(py++);
xorjoep 1:24714b45cd1b 318
xorjoep 1:24714b45cd1b 319 /* Read x[5] sample */
xorjoep 1:24714b45cd1b 320 x1 = *(px++);
xorjoep 1:24714b45cd1b 321
xorjoep 1:24714b45cd1b 322 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 323 /* acc0 += x[2] * y[2] */
xorjoep 1:24714b45cd1b 324 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x2 * c0)) >> 32);
xorjoep 1:24714b45cd1b 325 /* acc1 += x[3] * y[2] */
xorjoep 1:24714b45cd1b 326 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x3 * c0)) >> 32);
xorjoep 1:24714b45cd1b 327 /* acc2 += x[4] * y[2] */
xorjoep 1:24714b45cd1b 328 acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x0 * c0)) >> 32);
xorjoep 1:24714b45cd1b 329 /* acc3 += x[5] * y[2] */
xorjoep 1:24714b45cd1b 330 acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x1 * c0)) >> 32);
xorjoep 1:24714b45cd1b 331
xorjoep 1:24714b45cd1b 332 /* Read y[3] sample */
xorjoep 1:24714b45cd1b 333 c0 = *(py++);
xorjoep 1:24714b45cd1b 334
xorjoep 1:24714b45cd1b 335 /* Read x[6] sample */
xorjoep 1:24714b45cd1b 336 x2 = *(px++);
xorjoep 1:24714b45cd1b 337
xorjoep 1:24714b45cd1b 338 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 339 /* acc0 += x[3] * y[3] */
xorjoep 1:24714b45cd1b 340 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x3 * c0)) >> 32);
xorjoep 1:24714b45cd1b 341 /* acc1 += x[4] * y[3] */
xorjoep 1:24714b45cd1b 342 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x0 * c0)) >> 32);
xorjoep 1:24714b45cd1b 343 /* acc2 += x[5] * y[3] */
xorjoep 1:24714b45cd1b 344 acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x1 * c0)) >> 32);
xorjoep 1:24714b45cd1b 345 /* acc3 += x[6] * y[3] */
xorjoep 1:24714b45cd1b 346 acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x2 * c0)) >> 32);
xorjoep 1:24714b45cd1b 347
xorjoep 1:24714b45cd1b 348
xorjoep 1:24714b45cd1b 349 } while (--k);
xorjoep 1:24714b45cd1b 350
xorjoep 1:24714b45cd1b 351 /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
xorjoep 1:24714b45cd1b 352 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 353 k = srcBLen % 0x4U;
xorjoep 1:24714b45cd1b 354
xorjoep 1:24714b45cd1b 355 while (k > 0U)
xorjoep 1:24714b45cd1b 356 {
xorjoep 1:24714b45cd1b 357 /* Read y[4] sample */
xorjoep 1:24714b45cd1b 358 c0 = *(py++);
xorjoep 1:24714b45cd1b 359
xorjoep 1:24714b45cd1b 360 /* Read x[7] sample */
xorjoep 1:24714b45cd1b 361 x3 = *(px++);
xorjoep 1:24714b45cd1b 362
xorjoep 1:24714b45cd1b 363 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 364 /* acc0 += x[4] * y[4] */
xorjoep 1:24714b45cd1b 365 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
xorjoep 1:24714b45cd1b 366 /* acc1 += x[5] * y[4] */
xorjoep 1:24714b45cd1b 367 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
xorjoep 1:24714b45cd1b 368 /* acc2 += x[6] * y[4] */
xorjoep 1:24714b45cd1b 369 acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
xorjoep 1:24714b45cd1b 370 /* acc3 += x[7] * y[4] */
xorjoep 1:24714b45cd1b 371 acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
xorjoep 1:24714b45cd1b 372
xorjoep 1:24714b45cd1b 373 /* Reuse the present samples for the next MAC */
xorjoep 1:24714b45cd1b 374 x0 = x1;
xorjoep 1:24714b45cd1b 375 x1 = x2;
xorjoep 1:24714b45cd1b 376 x2 = x3;
xorjoep 1:24714b45cd1b 377
xorjoep 1:24714b45cd1b 378 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 379 k--;
xorjoep 1:24714b45cd1b 380 }
xorjoep 1:24714b45cd1b 381
xorjoep 1:24714b45cd1b 382 /* Store the result in the accumulator in the destination buffer. */
xorjoep 1:24714b45cd1b 383 *pOut = (q31_t) (acc0 << 1);
xorjoep 1:24714b45cd1b 384 /* Destination pointer is updated according to the address modifier, inc */
xorjoep 1:24714b45cd1b 385 pOut += inc;
xorjoep 1:24714b45cd1b 386
xorjoep 1:24714b45cd1b 387 *pOut = (q31_t) (acc1 << 1);
xorjoep 1:24714b45cd1b 388 pOut += inc;
xorjoep 1:24714b45cd1b 389
xorjoep 1:24714b45cd1b 390 *pOut = (q31_t) (acc2 << 1);
xorjoep 1:24714b45cd1b 391 pOut += inc;
xorjoep 1:24714b45cd1b 392
xorjoep 1:24714b45cd1b 393 *pOut = (q31_t) (acc3 << 1);
xorjoep 1:24714b45cd1b 394 pOut += inc;
xorjoep 1:24714b45cd1b 395
xorjoep 1:24714b45cd1b 396 /* Increment the pointer pIn1 index, count by 4 */
xorjoep 1:24714b45cd1b 397 count += 4U;
xorjoep 1:24714b45cd1b 398
xorjoep 1:24714b45cd1b 399 /* Update the inputA and inputB pointers for next MAC calculation */
xorjoep 1:24714b45cd1b 400 px = pIn1 + count;
xorjoep 1:24714b45cd1b 401 py = pIn2;
xorjoep 1:24714b45cd1b 402
xorjoep 1:24714b45cd1b 403
xorjoep 1:24714b45cd1b 404 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 405 blkCnt--;
xorjoep 1:24714b45cd1b 406 }
xorjoep 1:24714b45cd1b 407
xorjoep 1:24714b45cd1b 408 /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
xorjoep 1:24714b45cd1b 409 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 410 blkCnt = blockSize2 % 0x4U;
xorjoep 1:24714b45cd1b 411
xorjoep 1:24714b45cd1b 412 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 413 {
xorjoep 1:24714b45cd1b 414 /* Accumulator is made zero for every iteration */
xorjoep 1:24714b45cd1b 415 sum = 0;
xorjoep 1:24714b45cd1b 416
xorjoep 1:24714b45cd1b 417 /* Apply loop unrolling and compute 4 MACs simultaneously. */
xorjoep 1:24714b45cd1b 418 k = srcBLen >> 2U;
xorjoep 1:24714b45cd1b 419
xorjoep 1:24714b45cd1b 420 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
xorjoep 1:24714b45cd1b 421 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
xorjoep 1:24714b45cd1b 422 while (k > 0U)
xorjoep 1:24714b45cd1b 423 {
xorjoep 1:24714b45cd1b 424 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 425 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 426 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 427 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 428 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 429 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 430 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 431 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 432 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 433
xorjoep 1:24714b45cd1b 434 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 435 k--;
xorjoep 1:24714b45cd1b 436 }
xorjoep 1:24714b45cd1b 437
xorjoep 1:24714b45cd1b 438 /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
xorjoep 1:24714b45cd1b 439 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 440 k = srcBLen % 0x4U;
xorjoep 1:24714b45cd1b 441
xorjoep 1:24714b45cd1b 442 while (k > 0U)
xorjoep 1:24714b45cd1b 443 {
xorjoep 1:24714b45cd1b 444 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 445 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 446 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 447
xorjoep 1:24714b45cd1b 448 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 449 k--;
xorjoep 1:24714b45cd1b 450 }
xorjoep 1:24714b45cd1b 451
xorjoep 1:24714b45cd1b 452 /* Store the result in the accumulator in the destination buffer. */
xorjoep 1:24714b45cd1b 453 *pOut = sum << 1;
xorjoep 1:24714b45cd1b 454 /* Destination pointer is updated according to the address modifier, inc */
xorjoep 1:24714b45cd1b 455 pOut += inc;
xorjoep 1:24714b45cd1b 456
xorjoep 1:24714b45cd1b 457 /* Increment the MAC count */
xorjoep 1:24714b45cd1b 458 count++;
xorjoep 1:24714b45cd1b 459
xorjoep 1:24714b45cd1b 460 /* Update the inputA and inputB pointers for next MAC calculation */
xorjoep 1:24714b45cd1b 461 px = pIn1 + count;
xorjoep 1:24714b45cd1b 462 py = pIn2;
xorjoep 1:24714b45cd1b 463
xorjoep 1:24714b45cd1b 464
xorjoep 1:24714b45cd1b 465 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 466 blkCnt--;
xorjoep 1:24714b45cd1b 467 }
xorjoep 1:24714b45cd1b 468 }
xorjoep 1:24714b45cd1b 469 else
xorjoep 1:24714b45cd1b 470 {
xorjoep 1:24714b45cd1b 471 /* If the srcBLen is not a multiple of 4,
xorjoep 1:24714b45cd1b 472 * the blockSize2 loop cannot be unrolled by 4 */
xorjoep 1:24714b45cd1b 473 blkCnt = blockSize2;
xorjoep 1:24714b45cd1b 474
xorjoep 1:24714b45cd1b 475 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 476 {
xorjoep 1:24714b45cd1b 477 /* Accumulator is made zero for every iteration */
xorjoep 1:24714b45cd1b 478 sum = 0;
xorjoep 1:24714b45cd1b 479
xorjoep 1:24714b45cd1b 480 /* Loop over srcBLen */
xorjoep 1:24714b45cd1b 481 k = srcBLen;
xorjoep 1:24714b45cd1b 482
xorjoep 1:24714b45cd1b 483 while (k > 0U)
xorjoep 1:24714b45cd1b 484 {
xorjoep 1:24714b45cd1b 485 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 486 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 487 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 488
xorjoep 1:24714b45cd1b 489 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 490 k--;
xorjoep 1:24714b45cd1b 491 }
xorjoep 1:24714b45cd1b 492
xorjoep 1:24714b45cd1b 493 /* Store the result in the accumulator in the destination buffer. */
xorjoep 1:24714b45cd1b 494 *pOut = sum << 1;
xorjoep 1:24714b45cd1b 495 /* Destination pointer is updated according to the address modifier, inc */
xorjoep 1:24714b45cd1b 496 pOut += inc;
xorjoep 1:24714b45cd1b 497
xorjoep 1:24714b45cd1b 498 /* Increment the MAC count */
xorjoep 1:24714b45cd1b 499 count++;
xorjoep 1:24714b45cd1b 500
xorjoep 1:24714b45cd1b 501 /* Update the inputA and inputB pointers for next MAC calculation */
xorjoep 1:24714b45cd1b 502 px = pIn1 + count;
xorjoep 1:24714b45cd1b 503 py = pIn2;
xorjoep 1:24714b45cd1b 504
xorjoep 1:24714b45cd1b 505 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 506 blkCnt--;
xorjoep 1:24714b45cd1b 507 }
xorjoep 1:24714b45cd1b 508 }
xorjoep 1:24714b45cd1b 509
xorjoep 1:24714b45cd1b 510 /* --------------------------
xorjoep 1:24714b45cd1b 511 * Initializations of stage3
xorjoep 1:24714b45cd1b 512 * -------------------------*/
xorjoep 1:24714b45cd1b 513
xorjoep 1:24714b45cd1b 514 /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
xorjoep 1:24714b45cd1b 515 * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
xorjoep 1:24714b45cd1b 516 * ....
xorjoep 1:24714b45cd1b 517 * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
xorjoep 1:24714b45cd1b 518 * sum += x[srcALen-1] * y[0]
xorjoep 1:24714b45cd1b 519 */
xorjoep 1:24714b45cd1b 520
xorjoep 1:24714b45cd1b 521 /* In this stage the MAC operations are decreased by 1 for every iteration.
xorjoep 1:24714b45cd1b 522 The count variable holds the number of MAC operations performed */
xorjoep 1:24714b45cd1b 523 count = srcBLen - 1U;
xorjoep 1:24714b45cd1b 524
xorjoep 1:24714b45cd1b 525 /* Working pointer of inputA */
xorjoep 1:24714b45cd1b 526 pSrc1 = ((pIn1 + srcALen) - srcBLen) + 1U;
xorjoep 1:24714b45cd1b 527 px = pSrc1;
xorjoep 1:24714b45cd1b 528
xorjoep 1:24714b45cd1b 529 /* Working pointer of inputB */
xorjoep 1:24714b45cd1b 530 py = pIn2;
xorjoep 1:24714b45cd1b 531
xorjoep 1:24714b45cd1b 532 /* -------------------
xorjoep 1:24714b45cd1b 533 * Stage3 process
xorjoep 1:24714b45cd1b 534 * ------------------*/
xorjoep 1:24714b45cd1b 535
xorjoep 1:24714b45cd1b 536 while (blockSize3 > 0U)
xorjoep 1:24714b45cd1b 537 {
xorjoep 1:24714b45cd1b 538 /* Accumulator is made zero for every iteration */
xorjoep 1:24714b45cd1b 539 sum = 0;
xorjoep 1:24714b45cd1b 540
xorjoep 1:24714b45cd1b 541 /* Apply loop unrolling and compute 4 MACs simultaneously. */
xorjoep 1:24714b45cd1b 542 k = count >> 2U;
xorjoep 1:24714b45cd1b 543
xorjoep 1:24714b45cd1b 544 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
xorjoep 1:24714b45cd1b 545 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
xorjoep 1:24714b45cd1b 546 while (k > 0U)
xorjoep 1:24714b45cd1b 547 {
xorjoep 1:24714b45cd1b 548 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 549 /* sum += x[srcALen - srcBLen + 4] * y[3] */
xorjoep 1:24714b45cd1b 550 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 551 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 552 /* sum += x[srcALen - srcBLen + 3] * y[2] */
xorjoep 1:24714b45cd1b 553 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 554 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 555 /* sum += x[srcALen - srcBLen + 2] * y[1] */
xorjoep 1:24714b45cd1b 556 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 557 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 558 /* sum += x[srcALen - srcBLen + 1] * y[0] */
xorjoep 1:24714b45cd1b 559 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 560 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 561
xorjoep 1:24714b45cd1b 562 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 563 k--;
xorjoep 1:24714b45cd1b 564 }
xorjoep 1:24714b45cd1b 565
xorjoep 1:24714b45cd1b 566 /* If the count is not a multiple of 4, compute any remaining MACs here.
xorjoep 1:24714b45cd1b 567 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 568 k = count % 0x4U;
xorjoep 1:24714b45cd1b 569
xorjoep 1:24714b45cd1b 570 while (k > 0U)
xorjoep 1:24714b45cd1b 571 {
xorjoep 1:24714b45cd1b 572 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 573 sum = (q31_t) ((((q63_t) sum << 32) +
xorjoep 1:24714b45cd1b 574 ((q63_t) * px++ * (*py++))) >> 32);
xorjoep 1:24714b45cd1b 575
xorjoep 1:24714b45cd1b 576 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 577 k--;
xorjoep 1:24714b45cd1b 578 }
xorjoep 1:24714b45cd1b 579
xorjoep 1:24714b45cd1b 580 /* Store the result in the accumulator in the destination buffer. */
xorjoep 1:24714b45cd1b 581 *pOut = sum << 1;
xorjoep 1:24714b45cd1b 582 /* Destination pointer is updated according to the address modifier, inc */
xorjoep 1:24714b45cd1b 583 pOut += inc;
xorjoep 1:24714b45cd1b 584
xorjoep 1:24714b45cd1b 585 /* Update the inputA and inputB pointers for next MAC calculation */
xorjoep 1:24714b45cd1b 586 px = ++pSrc1;
xorjoep 1:24714b45cd1b 587 py = pIn2;
xorjoep 1:24714b45cd1b 588
xorjoep 1:24714b45cd1b 589 /* Decrement the MAC count */
xorjoep 1:24714b45cd1b 590 count--;
xorjoep 1:24714b45cd1b 591
xorjoep 1:24714b45cd1b 592 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 593 blockSize3--;
xorjoep 1:24714b45cd1b 594 }
xorjoep 1:24714b45cd1b 595
xorjoep 1:24714b45cd1b 596 }
xorjoep 1:24714b45cd1b 597
xorjoep 1:24714b45cd1b 598 /**
xorjoep 1:24714b45cd1b 599 * @} end of Corr group
xorjoep 1:24714b45cd1b 600 */