Laxmi Kant Tiwari
/
mbed-dsp
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
mbed-dsp
by 
mbed official
cmsis_dsp/FilteringFunctions/arm_correlate_fast_opt_q15.c@1:fdd22bb7aa52, 2012-11-28 (annotated)
- Committer:
- emilmont
- Date:
- Wed Nov 28 12:30:09 2012 +0000
- Revision:
- 1:fdd22bb7aa52
- Child:
- 2:da51fb522205
DSP library code
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| emilmont | 1:fdd22bb7aa52 | 1 | /* ---------------------------------------------------------------------- |
| emilmont | 1:fdd22bb7aa52 | 2 | * Copyright (C) 2010 ARM Limited. All rights reserved. |
| emilmont | 1:fdd22bb7aa52 | 3 | * |
| emilmont | 1:fdd22bb7aa52 | 4 | * $Date: 15. February 2012 |
| emilmont | 1:fdd22bb7aa52 | 5 | * $Revision: V1.1.0 |
| emilmont | 1:fdd22bb7aa52 | 6 | * |
| emilmont | 1:fdd22bb7aa52 | 7 | * Project: CMSIS DSP Library |
| emilmont | 1:fdd22bb7aa52 | 8 | * Title: arm_correlate_fast_opt_q15.c |
| emilmont | 1:fdd22bb7aa52 | 9 | * |
| emilmont | 1:fdd22bb7aa52 | 10 | * Description: Fast Q15 Correlation. |
| emilmont | 1:fdd22bb7aa52 | 11 | * |
| emilmont | 1:fdd22bb7aa52 | 12 | * Target Processor: Cortex-M4/Cortex-M3 |
| emilmont | 1:fdd22bb7aa52 | 13 | * |
| emilmont | 1:fdd22bb7aa52 | 14 | * Version 1.1.0 2012/02/15 |
| emilmont | 1:fdd22bb7aa52 | 15 | * Updated with more optimizations, bug fixes and minor API changes. |
| emilmont | 1:fdd22bb7aa52 | 16 | * |
| emilmont | 1:fdd22bb7aa52 | 17 | * Version 1.0.11 2011/10/18 |
| emilmont | 1:fdd22bb7aa52 | 18 | * Bug Fix in conv, correlation, partial convolution. |
| emilmont | 1:fdd22bb7aa52 | 19 | * |
| emilmont | 1:fdd22bb7aa52 | 20 | * Version 1.0.10 2011/7/15 |
| emilmont | 1:fdd22bb7aa52 | 21 | * Big Endian support added and Merged M0 and M3/M4 Source code. |
| emilmont | 1:fdd22bb7aa52 | 22 | * |
| emilmont | 1:fdd22bb7aa52 | 23 | * Version 1.0.3 2010/11/29 |
| emilmont | 1:fdd22bb7aa52 | 24 | * Re-organized the CMSIS folders and updated documentation. |
| emilmont | 1:fdd22bb7aa52 | 25 | * |
| emilmont | 1:fdd22bb7aa52 | 26 | * Version 1.0.2 2010/11/11 |
| emilmont | 1:fdd22bb7aa52 | 27 | * Documentation updated. |
| emilmont | 1:fdd22bb7aa52 | 28 | * |
| emilmont | 1:fdd22bb7aa52 | 29 | * Version 1.0.1 2010/10/05 |
| emilmont | 1:fdd22bb7aa52 | 30 | * Production release and review comments incorporated. |
| emilmont | 1:fdd22bb7aa52 | 31 | * |
| emilmont | 1:fdd22bb7aa52 | 32 | * Version 1.0.0 2010/09/20 |
| emilmont | 1:fdd22bb7aa52 | 33 | * Production release and review comments incorporated. |
| emilmont | 1:fdd22bb7aa52 | 34 | * -------------------------------------------------------------------- */ |
| emilmont | 1:fdd22bb7aa52 | 35 | |
| emilmont | 1:fdd22bb7aa52 | 36 | #include "arm_math.h" |
| emilmont | 1:fdd22bb7aa52 | 37 | |
| emilmont | 1:fdd22bb7aa52 | 38 | /** |
| emilmont | 1:fdd22bb7aa52 | 39 | * @ingroup groupFilters |
| emilmont | 1:fdd22bb7aa52 | 40 | */ |
| emilmont | 1:fdd22bb7aa52 | 41 | |
| emilmont | 1:fdd22bb7aa52 | 42 | /** |
| emilmont | 1:fdd22bb7aa52 | 43 | * @addtogroup Corr |
| emilmont | 1:fdd22bb7aa52 | 44 | * @{ |
| emilmont | 1:fdd22bb7aa52 | 45 | */ |
| emilmont | 1:fdd22bb7aa52 | 46 | |
| emilmont | 1:fdd22bb7aa52 | 47 | /** |
| emilmont | 1:fdd22bb7aa52 | 48 | * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4. |
| emilmont | 1:fdd22bb7aa52 | 49 | * @param[in] *pSrcA points to the first input sequence. |
| emilmont | 1:fdd22bb7aa52 | 50 | * @param[in] srcALen length of the first input sequence. |
| emilmont | 1:fdd22bb7aa52 | 51 | * @param[in] *pSrcB points to the second input sequence. |
| emilmont | 1:fdd22bb7aa52 | 52 | * @param[in] srcBLen length of the second input sequence. |
| emilmont | 1:fdd22bb7aa52 | 53 | * @param[out] *pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1. |
| emilmont | 1:fdd22bb7aa52 | 54 | * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. |
| emilmont | 1:fdd22bb7aa52 | 55 | * @return none. |
| emilmont | 1:fdd22bb7aa52 | 56 | * |
| emilmont | 1:fdd22bb7aa52 | 57 | * |
| emilmont | 1:fdd22bb7aa52 | 58 | * \par Restrictions |
| emilmont | 1:fdd22bb7aa52 | 59 | * If the silicon does not support unaligned memory access enable the macro UNALIGNED_SUPPORT_DISABLE |
| emilmont | 1:fdd22bb7aa52 | 60 | * In this case input, output, scratch buffers should be aligned by 32-bit |
| emilmont | 1:fdd22bb7aa52 | 61 | * |
| emilmont | 1:fdd22bb7aa52 | 62 | * |
| emilmont | 1:fdd22bb7aa52 | 63 | * <b>Scaling and Overflow Behavior:</b> |
| emilmont | 1:fdd22bb7aa52 | 64 | * |
| emilmont | 1:fdd22bb7aa52 | 65 | * \par |
| emilmont | 1:fdd22bb7aa52 | 66 | * This fast version uses a 32-bit accumulator with 2.30 format. |
| emilmont | 1:fdd22bb7aa52 | 67 | * The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit. |
| emilmont | 1:fdd22bb7aa52 | 68 | * There is no saturation on intermediate additions. |
| emilmont | 1:fdd22bb7aa52 | 69 | * Thus, if the accumulator overflows it wraps around and distorts the result. |
| emilmont | 1:fdd22bb7aa52 | 70 | * The input signals should be scaled down to avoid intermediate overflows. |
| emilmont | 1:fdd22bb7aa52 | 71 | * Scale down one of the inputs by 1/min(srcALen, srcBLen) to avoid overflow since a |
| emilmont | 1:fdd22bb7aa52 | 72 | * maximum of min(srcALen, srcBLen) number of additions is carried internally. |
| emilmont | 1:fdd22bb7aa52 | 73 | * The 2.30 accumulator is right shifted by 15 bits and then saturated to 1.15 format to yield the final result. |
| emilmont | 1:fdd22bb7aa52 | 74 | * |
| emilmont | 1:fdd22bb7aa52 | 75 | * \par |
| emilmont | 1:fdd22bb7aa52 | 76 | * See <code>arm_correlate_q15()</code> for a slower implementation of this function which uses a 64-bit accumulator to avoid wrap around distortion. |
| emilmont | 1:fdd22bb7aa52 | 77 | */ |
| emilmont | 1:fdd22bb7aa52 | 78 | |
| emilmont | 1:fdd22bb7aa52 | 79 | void arm_correlate_fast_opt_q15( |
| emilmont | 1:fdd22bb7aa52 | 80 | q15_t * pSrcA, |
| emilmont | 1:fdd22bb7aa52 | 81 | uint32_t srcALen, |
| emilmont | 1:fdd22bb7aa52 | 82 | q15_t * pSrcB, |
| emilmont | 1:fdd22bb7aa52 | 83 | uint32_t srcBLen, |
| emilmont | 1:fdd22bb7aa52 | 84 | q15_t * pDst, |
| emilmont | 1:fdd22bb7aa52 | 85 | q15_t * pScratch) |
| emilmont | 1:fdd22bb7aa52 | 86 | { |
| emilmont | 1:fdd22bb7aa52 | 87 | q15_t *pIn1; /* inputA pointer */ |
| emilmont | 1:fdd22bb7aa52 | 88 | q15_t *pIn2; /* inputB pointer */ |
| emilmont | 1:fdd22bb7aa52 | 89 | q31_t acc0, acc1, acc2, acc3; /* Accumulators */ |
| emilmont | 1:fdd22bb7aa52 | 90 | q15_t *py; /* Intermediate inputB pointer */ |
| emilmont | 1:fdd22bb7aa52 | 91 | q31_t x1, x2, x3; /* temporary variables for holding input and coefficient values */ |
| emilmont | 1:fdd22bb7aa52 | 92 | uint32_t j, blkCnt, outBlockSize; /* loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 93 | int32_t inc = 1; /* Destination address modifier */ |
| emilmont | 1:fdd22bb7aa52 | 94 | uint32_t tapCnt; |
| emilmont | 1:fdd22bb7aa52 | 95 | q31_t y1, y2; |
| emilmont | 1:fdd22bb7aa52 | 96 | q15_t *pScr; /* Intermediate pointers */ |
| emilmont | 1:fdd22bb7aa52 | 97 | q15_t *pOut = pDst; /* output pointer */ |
| emilmont | 1:fdd22bb7aa52 | 98 | #ifdef UNALIGNED_SUPPORT_DISABLE |
| emilmont | 1:fdd22bb7aa52 | 99 | |
| emilmont | 1:fdd22bb7aa52 | 100 | q15_t a, b; |
| emilmont | 1:fdd22bb7aa52 | 101 | |
| emilmont | 1:fdd22bb7aa52 | 102 | #endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ |
| emilmont | 1:fdd22bb7aa52 | 103 | |
| emilmont | 1:fdd22bb7aa52 | 104 | /* The algorithm implementation is based on the lengths of the inputs. */ |
| emilmont | 1:fdd22bb7aa52 | 105 | /* srcB is always made to slide across srcA. */ |
| emilmont | 1:fdd22bb7aa52 | 106 | /* So srcBLen is always considered as shorter or equal to srcALen */ |
| emilmont | 1:fdd22bb7aa52 | 107 | /* But CORR(x, y) is reverse of CORR(y, x) */ |
| emilmont | 1:fdd22bb7aa52 | 108 | /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */ |
| emilmont | 1:fdd22bb7aa52 | 109 | /* and the destination pointer modifier, inc is set to -1 */ |
| emilmont | 1:fdd22bb7aa52 | 110 | /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */ |
| emilmont | 1:fdd22bb7aa52 | 111 | /* But to improve the performance, |
| emilmont | 1:fdd22bb7aa52 | 112 | * we include zeroes in the output instead of zero padding either of the the inputs*/ |
| emilmont | 1:fdd22bb7aa52 | 113 | /* If srcALen > srcBLen, |
| emilmont | 1:fdd22bb7aa52 | 114 | * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */ |
| emilmont | 1:fdd22bb7aa52 | 115 | /* If srcALen < srcBLen, |
| emilmont | 1:fdd22bb7aa52 | 116 | * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */ |
| emilmont | 1:fdd22bb7aa52 | 117 | if(srcALen >= srcBLen) |
| emilmont | 1:fdd22bb7aa52 | 118 | { |
| emilmont | 1:fdd22bb7aa52 | 119 | /* Initialization of inputA pointer */ |
| emilmont | 1:fdd22bb7aa52 | 120 | pIn1 = (pSrcA); |
| emilmont | 1:fdd22bb7aa52 | 121 | |
| emilmont | 1:fdd22bb7aa52 | 122 | /* Initialization of inputB pointer */ |
| emilmont | 1:fdd22bb7aa52 | 123 | pIn2 = (pSrcB); |
| emilmont | 1:fdd22bb7aa52 | 124 | |
| emilmont | 1:fdd22bb7aa52 | 125 | /* Number of output samples is calculated */ |
| emilmont | 1:fdd22bb7aa52 | 126 | outBlockSize = (2u * srcALen) - 1u; |
| emilmont | 1:fdd22bb7aa52 | 127 | |
| emilmont | 1:fdd22bb7aa52 | 128 | /* When srcALen > srcBLen, zero padding is done to srcB |
| emilmont | 1:fdd22bb7aa52 | 129 | * to make their lengths equal. |
| emilmont | 1:fdd22bb7aa52 | 130 | * Instead, (outBlockSize - (srcALen + srcBLen - 1)) |
| emilmont | 1:fdd22bb7aa52 | 131 | * number of output samples are made zero */ |
| emilmont | 1:fdd22bb7aa52 | 132 | j = outBlockSize - (srcALen + (srcBLen - 1u)); |
| emilmont | 1:fdd22bb7aa52 | 133 | |
| emilmont | 1:fdd22bb7aa52 | 134 | /* Updating the pointer position to non zero value */ |
| emilmont | 1:fdd22bb7aa52 | 135 | pOut += j; |
| emilmont | 1:fdd22bb7aa52 | 136 | |
| emilmont | 1:fdd22bb7aa52 | 137 | } |
| emilmont | 1:fdd22bb7aa52 | 138 | else |
| emilmont | 1:fdd22bb7aa52 | 139 | { |
| emilmont | 1:fdd22bb7aa52 | 140 | /* Initialization of inputA pointer */ |
| emilmont | 1:fdd22bb7aa52 | 141 | pIn1 = (pSrcB); |
| emilmont | 1:fdd22bb7aa52 | 142 | |
| emilmont | 1:fdd22bb7aa52 | 143 | /* Initialization of inputB pointer */ |
| emilmont | 1:fdd22bb7aa52 | 144 | pIn2 = (pSrcA); |
| emilmont | 1:fdd22bb7aa52 | 145 | |
| emilmont | 1:fdd22bb7aa52 | 146 | /* srcBLen is always considered as shorter or equal to srcALen */ |
| emilmont | 1:fdd22bb7aa52 | 147 | j = srcBLen; |
| emilmont | 1:fdd22bb7aa52 | 148 | srcBLen = srcALen; |
| emilmont | 1:fdd22bb7aa52 | 149 | srcALen = j; |
| emilmont | 1:fdd22bb7aa52 | 150 | |
| emilmont | 1:fdd22bb7aa52 | 151 | /* CORR(x, y) = Reverse order(CORR(y, x)) */ |
| emilmont | 1:fdd22bb7aa52 | 152 | /* Hence set the destination pointer to point to the last output sample */ |
| emilmont | 1:fdd22bb7aa52 | 153 | pOut = pDst + ((srcALen + srcBLen) - 2u); |
| emilmont | 1:fdd22bb7aa52 | 154 | |
| emilmont | 1:fdd22bb7aa52 | 155 | /* Destination address modifier is set to -1 */ |
| emilmont | 1:fdd22bb7aa52 | 156 | inc = -1; |
| emilmont | 1:fdd22bb7aa52 | 157 | |
| emilmont | 1:fdd22bb7aa52 | 158 | } |
| emilmont | 1:fdd22bb7aa52 | 159 | |
| emilmont | 1:fdd22bb7aa52 | 160 | pScr = pScratch; |
| emilmont | 1:fdd22bb7aa52 | 161 | |
| emilmont | 1:fdd22bb7aa52 | 162 | /* Fill (srcBLen - 1u) zeros in scratch buffer */ |
| emilmont | 1:fdd22bb7aa52 | 163 | arm_fill_q15(0, pScr, (srcBLen - 1u)); |
| emilmont | 1:fdd22bb7aa52 | 164 | |
| emilmont | 1:fdd22bb7aa52 | 165 | /* Update temporary scratch pointer */ |
| emilmont | 1:fdd22bb7aa52 | 166 | pScr += (srcBLen - 1u); |
| emilmont | 1:fdd22bb7aa52 | 167 | |
| emilmont | 1:fdd22bb7aa52 | 168 | #ifndef UNALIGNED_SUPPORT_DISABLE |
| emilmont | 1:fdd22bb7aa52 | 169 | |
| emilmont | 1:fdd22bb7aa52 | 170 | /* Copy (srcALen) samples in scratch buffer */ |
| emilmont | 1:fdd22bb7aa52 | 171 | arm_copy_q15(pIn1, pScr, srcALen); |
| emilmont | 1:fdd22bb7aa52 | 172 | |
| emilmont | 1:fdd22bb7aa52 | 173 | /* Update pointers */ |
| emilmont | 1:fdd22bb7aa52 | 174 | pScr += srcALen; |
| emilmont | 1:fdd22bb7aa52 | 175 | |
| emilmont | 1:fdd22bb7aa52 | 176 | #else |
| emilmont | 1:fdd22bb7aa52 | 177 | |
| emilmont | 1:fdd22bb7aa52 | 178 | /* Apply loop unrolling and do 4 Copies simultaneously. */ |
| emilmont | 1:fdd22bb7aa52 | 179 | j = srcALen >> 2u; |
| emilmont | 1:fdd22bb7aa52 | 180 | |
| emilmont | 1:fdd22bb7aa52 | 181 | /* First part of the processing with loop unrolling copies 4 data points at a time. |
| emilmont | 1:fdd22bb7aa52 | 182 | ** a second loop below copies for the remaining 1 to 3 samples. */ |
| emilmont | 1:fdd22bb7aa52 | 183 | while(j > 0u) |
| emilmont | 1:fdd22bb7aa52 | 184 | { |
| emilmont | 1:fdd22bb7aa52 | 185 | /* copy second buffer in reversal manner */ |
| emilmont | 1:fdd22bb7aa52 | 186 | *pScr++ = *pIn1++; |
| emilmont | 1:fdd22bb7aa52 | 187 | *pScr++ = *pIn1++; |
| emilmont | 1:fdd22bb7aa52 | 188 | *pScr++ = *pIn1++; |
| emilmont | 1:fdd22bb7aa52 | 189 | *pScr++ = *pIn1++; |
| emilmont | 1:fdd22bb7aa52 | 190 | |
| emilmont | 1:fdd22bb7aa52 | 191 | /* Decrement the loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 192 | j--; |
| emilmont | 1:fdd22bb7aa52 | 193 | } |
| emilmont | 1:fdd22bb7aa52 | 194 | |
| emilmont | 1:fdd22bb7aa52 | 195 | /* If the count is not a multiple of 4, copy remaining samples here. |
| emilmont | 1:fdd22bb7aa52 | 196 | ** No loop unrolling is used. */ |
| emilmont | 1:fdd22bb7aa52 | 197 | j = srcALen % 0x4u; |
| emilmont | 1:fdd22bb7aa52 | 198 | |
| emilmont | 1:fdd22bb7aa52 | 199 | while(j > 0u) |
| emilmont | 1:fdd22bb7aa52 | 200 | { |
| emilmont | 1:fdd22bb7aa52 | 201 | /* copy second buffer in reversal manner for remaining samples */ |
| emilmont | 1:fdd22bb7aa52 | 202 | *pScr++ = *pIn1++; |
| emilmont | 1:fdd22bb7aa52 | 203 | |
| emilmont | 1:fdd22bb7aa52 | 204 | /* Decrement the loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 205 | j--; |
| emilmont | 1:fdd22bb7aa52 | 206 | } |
| emilmont | 1:fdd22bb7aa52 | 207 | |
| emilmont | 1:fdd22bb7aa52 | 208 | #endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ |
| emilmont | 1:fdd22bb7aa52 | 209 | |
| emilmont | 1:fdd22bb7aa52 | 210 | #ifndef UNALIGNED_SUPPORT_DISABLE |
| emilmont | 1:fdd22bb7aa52 | 211 | |
| emilmont | 1:fdd22bb7aa52 | 212 | /* Fill (srcBLen - 1u) zeros at end of scratch buffer */ |
| emilmont | 1:fdd22bb7aa52 | 213 | arm_fill_q15(0, pScr, (srcBLen - 1u)); |
| emilmont | 1:fdd22bb7aa52 | 214 | |
| emilmont | 1:fdd22bb7aa52 | 215 | /* Update pointer */ |
| emilmont | 1:fdd22bb7aa52 | 216 | pScr += (srcBLen - 1u); |
| emilmont | 1:fdd22bb7aa52 | 217 | |
| emilmont | 1:fdd22bb7aa52 | 218 | #else |
| emilmont | 1:fdd22bb7aa52 | 219 | |
| emilmont | 1:fdd22bb7aa52 | 220 | /* Apply loop unrolling and do 4 Copies simultaneously. */ |
| emilmont | 1:fdd22bb7aa52 | 221 | j = (srcBLen - 1u) >> 2u; |
| emilmont | 1:fdd22bb7aa52 | 222 | |
| emilmont | 1:fdd22bb7aa52 | 223 | /* First part of the processing with loop unrolling copies 4 data points at a time. |
| emilmont | 1:fdd22bb7aa52 | 224 | ** a second loop below copies for the remaining 1 to 3 samples. */ |
| emilmont | 1:fdd22bb7aa52 | 225 | while(j > 0u) |
| emilmont | 1:fdd22bb7aa52 | 226 | { |
| emilmont | 1:fdd22bb7aa52 | 227 | /* copy second buffer in reversal manner */ |
| emilmont | 1:fdd22bb7aa52 | 228 | *pScr++ = 0; |
| emilmont | 1:fdd22bb7aa52 | 229 | *pScr++ = 0; |
| emilmont | 1:fdd22bb7aa52 | 230 | *pScr++ = 0; |
| emilmont | 1:fdd22bb7aa52 | 231 | *pScr++ = 0; |
| emilmont | 1:fdd22bb7aa52 | 232 | |
| emilmont | 1:fdd22bb7aa52 | 233 | /* Decrement the loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 234 | j--; |
| emilmont | 1:fdd22bb7aa52 | 235 | } |
| emilmont | 1:fdd22bb7aa52 | 236 | |
| emilmont | 1:fdd22bb7aa52 | 237 | /* If the count is not a multiple of 4, copy remaining samples here. |
| emilmont | 1:fdd22bb7aa52 | 238 | ** No loop unrolling is used. */ |
| emilmont | 1:fdd22bb7aa52 | 239 | j = (srcBLen - 1u) % 0x4u; |
| emilmont | 1:fdd22bb7aa52 | 240 | |
| emilmont | 1:fdd22bb7aa52 | 241 | while(j > 0u) |
| emilmont | 1:fdd22bb7aa52 | 242 | { |
| emilmont | 1:fdd22bb7aa52 | 243 | /* copy second buffer in reversal manner for remaining samples */ |
| emilmont | 1:fdd22bb7aa52 | 244 | *pScr++ = 0; |
| emilmont | 1:fdd22bb7aa52 | 245 | |
| emilmont | 1:fdd22bb7aa52 | 246 | /* Decrement the loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 247 | j--; |
| emilmont | 1:fdd22bb7aa52 | 248 | } |
| emilmont | 1:fdd22bb7aa52 | 249 | |
| emilmont | 1:fdd22bb7aa52 | 250 | #endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ |
| emilmont | 1:fdd22bb7aa52 | 251 | |
| emilmont | 1:fdd22bb7aa52 | 252 | /* Temporary pointer for scratch2 */ |
| emilmont | 1:fdd22bb7aa52 | 253 | py = pIn2; |
| emilmont | 1:fdd22bb7aa52 | 254 | |
| emilmont | 1:fdd22bb7aa52 | 255 | |
| emilmont | 1:fdd22bb7aa52 | 256 | /* Actual correlation process starts here */ |
| emilmont | 1:fdd22bb7aa52 | 257 | blkCnt = (srcALen + srcBLen - 1u) >> 2; |
| emilmont | 1:fdd22bb7aa52 | 258 | |
| emilmont | 1:fdd22bb7aa52 | 259 | while(blkCnt > 0) |
| emilmont | 1:fdd22bb7aa52 | 260 | { |
| emilmont | 1:fdd22bb7aa52 | 261 | /* Initialze temporary scratch pointer as scratch1 */ |
| emilmont | 1:fdd22bb7aa52 | 262 | pScr = pScratch; |
| emilmont | 1:fdd22bb7aa52 | 263 | |
| emilmont | 1:fdd22bb7aa52 | 264 | /* Clear Accumlators */ |
| emilmont | 1:fdd22bb7aa52 | 265 | acc0 = 0; |
| emilmont | 1:fdd22bb7aa52 | 266 | acc1 = 0; |
| emilmont | 1:fdd22bb7aa52 | 267 | acc2 = 0; |
| emilmont | 1:fdd22bb7aa52 | 268 | acc3 = 0; |
| emilmont | 1:fdd22bb7aa52 | 269 | |
| emilmont | 1:fdd22bb7aa52 | 270 | /* Read four samples from scratch1 buffer */ |
| emilmont | 1:fdd22bb7aa52 | 271 | x1 = *__SIMD32(pScr)++; |
| emilmont | 1:fdd22bb7aa52 | 272 | |
| emilmont | 1:fdd22bb7aa52 | 273 | /* Read next four samples from scratch1 buffer */ |
| emilmont | 1:fdd22bb7aa52 | 274 | x2 = *__SIMD32(pScr)++; |
| emilmont | 1:fdd22bb7aa52 | 275 | |
| emilmont | 1:fdd22bb7aa52 | 276 | tapCnt = (srcBLen) >> 2u; |
| emilmont | 1:fdd22bb7aa52 | 277 | |
| emilmont | 1:fdd22bb7aa52 | 278 | while(tapCnt > 0u) |
| emilmont | 1:fdd22bb7aa52 | 279 | { |
| emilmont | 1:fdd22bb7aa52 | 280 | |
| emilmont | 1:fdd22bb7aa52 | 281 | #ifndef UNALIGNED_SUPPORT_DISABLE |
| emilmont | 1:fdd22bb7aa52 | 282 | |
| emilmont | 1:fdd22bb7aa52 | 283 | /* Read four samples from smaller buffer */ |
| emilmont | 1:fdd22bb7aa52 | 284 | y1 = _SIMD32_OFFSET(pIn2); |
| emilmont | 1:fdd22bb7aa52 | 285 | y2 = _SIMD32_OFFSET(pIn2 + 2u); |
| emilmont | 1:fdd22bb7aa52 | 286 | |
| emilmont | 1:fdd22bb7aa52 | 287 | acc0 = __SMLAD(x1, y1, acc0); |
| emilmont | 1:fdd22bb7aa52 | 288 | |
| emilmont | 1:fdd22bb7aa52 | 289 | acc2 = __SMLAD(x2, y1, acc2); |
| emilmont | 1:fdd22bb7aa52 | 290 | |
| emilmont | 1:fdd22bb7aa52 | 291 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 292 | x3 = __PKHBT(x2, x1, 0); |
| emilmont | 1:fdd22bb7aa52 | 293 | #else |
| emilmont | 1:fdd22bb7aa52 | 294 | x3 = __PKHBT(x1, x2, 0); |
| emilmont | 1:fdd22bb7aa52 | 295 | #endif |
| emilmont | 1:fdd22bb7aa52 | 296 | |
| emilmont | 1:fdd22bb7aa52 | 297 | acc1 = __SMLADX(x3, y1, acc1); |
| emilmont | 1:fdd22bb7aa52 | 298 | |
| emilmont | 1:fdd22bb7aa52 | 299 | x1 = _SIMD32_OFFSET(pScr); |
| emilmont | 1:fdd22bb7aa52 | 300 | |
| emilmont | 1:fdd22bb7aa52 | 301 | acc0 = __SMLAD(x2, y2, acc0); |
| emilmont | 1:fdd22bb7aa52 | 302 | |
| emilmont | 1:fdd22bb7aa52 | 303 | acc2 = __SMLAD(x1, y2, acc2); |
| emilmont | 1:fdd22bb7aa52 | 304 | |
| emilmont | 1:fdd22bb7aa52 | 305 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 306 | x3 = __PKHBT(x1, x2, 0); |
| emilmont | 1:fdd22bb7aa52 | 307 | #else |
| emilmont | 1:fdd22bb7aa52 | 308 | x3 = __PKHBT(x2, x1, 0); |
| emilmont | 1:fdd22bb7aa52 | 309 | #endif |
| emilmont | 1:fdd22bb7aa52 | 310 | |
| emilmont | 1:fdd22bb7aa52 | 311 | acc3 = __SMLADX(x3, y1, acc3); |
| emilmont | 1:fdd22bb7aa52 | 312 | |
| emilmont | 1:fdd22bb7aa52 | 313 | acc1 = __SMLADX(x3, y2, acc1); |
| emilmont | 1:fdd22bb7aa52 | 314 | |
| emilmont | 1:fdd22bb7aa52 | 315 | x2 = _SIMD32_OFFSET(pScr + 2u); |
| emilmont | 1:fdd22bb7aa52 | 316 | |
| emilmont | 1:fdd22bb7aa52 | 317 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 318 | x3 = __PKHBT(x2, x1, 0); |
| emilmont | 1:fdd22bb7aa52 | 319 | #else |
| emilmont | 1:fdd22bb7aa52 | 320 | x3 = __PKHBT(x1, x2, 0); |
| emilmont | 1:fdd22bb7aa52 | 321 | #endif |
| emilmont | 1:fdd22bb7aa52 | 322 | |
| emilmont | 1:fdd22bb7aa52 | 323 | acc3 = __SMLADX(x3, y2, acc3); |
| emilmont | 1:fdd22bb7aa52 | 324 | #else |
| emilmont | 1:fdd22bb7aa52 | 325 | |
| emilmont | 1:fdd22bb7aa52 | 326 | /* Read four samples from smaller buffer */ |
| emilmont | 1:fdd22bb7aa52 | 327 | a = *pIn2; |
| emilmont | 1:fdd22bb7aa52 | 328 | b = *(pIn2 + 1); |
| emilmont | 1:fdd22bb7aa52 | 329 | |
| emilmont | 1:fdd22bb7aa52 | 330 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 331 | y1 = __PKHBT(a, b, 16); |
| emilmont | 1:fdd22bb7aa52 | 332 | #else |
| emilmont | 1:fdd22bb7aa52 | 333 | y1 = __PKHBT(b, a, 16); |
| emilmont | 1:fdd22bb7aa52 | 334 | #endif |
| emilmont | 1:fdd22bb7aa52 | 335 | |
| emilmont | 1:fdd22bb7aa52 | 336 | a = *(pIn2 + 2); |
| emilmont | 1:fdd22bb7aa52 | 337 | b = *(pIn2 + 3); |
| emilmont | 1:fdd22bb7aa52 | 338 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 339 | y2 = __PKHBT(a, b, 16); |
| emilmont | 1:fdd22bb7aa52 | 340 | #else |
| emilmont | 1:fdd22bb7aa52 | 341 | y2 = __PKHBT(b, a, 16); |
| emilmont | 1:fdd22bb7aa52 | 342 | #endif |
| emilmont | 1:fdd22bb7aa52 | 343 | |
| emilmont | 1:fdd22bb7aa52 | 344 | acc0 = __SMLAD(x1, y1, acc0); |
| emilmont | 1:fdd22bb7aa52 | 345 | |
| emilmont | 1:fdd22bb7aa52 | 346 | acc2 = __SMLAD(x2, y1, acc2); |
| emilmont | 1:fdd22bb7aa52 | 347 | |
| emilmont | 1:fdd22bb7aa52 | 348 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 349 | x3 = __PKHBT(x2, x1, 0); |
| emilmont | 1:fdd22bb7aa52 | 350 | #else |
| emilmont | 1:fdd22bb7aa52 | 351 | x3 = __PKHBT(x1, x2, 0); |
| emilmont | 1:fdd22bb7aa52 | 352 | #endif |
| emilmont | 1:fdd22bb7aa52 | 353 | |
| emilmont | 1:fdd22bb7aa52 | 354 | acc1 = __SMLADX(x3, y1, acc1); |
| emilmont | 1:fdd22bb7aa52 | 355 | |
| emilmont | 1:fdd22bb7aa52 | 356 | a = *pScr; |
| emilmont | 1:fdd22bb7aa52 | 357 | b = *(pScr + 1); |
| emilmont | 1:fdd22bb7aa52 | 358 | |
| emilmont | 1:fdd22bb7aa52 | 359 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 360 | x1 = __PKHBT(a, b, 16); |
| emilmont | 1:fdd22bb7aa52 | 361 | #else |
| emilmont | 1:fdd22bb7aa52 | 362 | x1 = __PKHBT(b, a, 16); |
| emilmont | 1:fdd22bb7aa52 | 363 | #endif |
| emilmont | 1:fdd22bb7aa52 | 364 | |
| emilmont | 1:fdd22bb7aa52 | 365 | acc0 = __SMLAD(x2, y2, acc0); |
| emilmont | 1:fdd22bb7aa52 | 366 | |
| emilmont | 1:fdd22bb7aa52 | 367 | acc2 = __SMLAD(x1, y2, acc2); |
| emilmont | 1:fdd22bb7aa52 | 368 | |
| emilmont | 1:fdd22bb7aa52 | 369 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 370 | x3 = __PKHBT(x1, x2, 0); |
| emilmont | 1:fdd22bb7aa52 | 371 | #else |
| emilmont | 1:fdd22bb7aa52 | 372 | x3 = __PKHBT(x2, x1, 0); |
| emilmont | 1:fdd22bb7aa52 | 373 | #endif |
| emilmont | 1:fdd22bb7aa52 | 374 | |
| emilmont | 1:fdd22bb7aa52 | 375 | acc3 = __SMLADX(x3, y1, acc3); |
| emilmont | 1:fdd22bb7aa52 | 376 | |
| emilmont | 1:fdd22bb7aa52 | 377 | acc1 = __SMLADX(x3, y2, acc1); |
| emilmont | 1:fdd22bb7aa52 | 378 | |
| emilmont | 1:fdd22bb7aa52 | 379 | a = *(pScr + 2); |
| emilmont | 1:fdd22bb7aa52 | 380 | b = *(pScr + 3); |
| emilmont | 1:fdd22bb7aa52 | 381 | |
| emilmont | 1:fdd22bb7aa52 | 382 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 383 | x2 = __PKHBT(a, b, 16); |
| emilmont | 1:fdd22bb7aa52 | 384 | #else |
| emilmont | 1:fdd22bb7aa52 | 385 | x2 = __PKHBT(b, a, 16); |
| emilmont | 1:fdd22bb7aa52 | 386 | #endif |
| emilmont | 1:fdd22bb7aa52 | 387 | |
| emilmont | 1:fdd22bb7aa52 | 388 | #ifndef ARM_MATH_BIG_ENDIAN |
| emilmont | 1:fdd22bb7aa52 | 389 | x3 = __PKHBT(x2, x1, 0); |
| emilmont | 1:fdd22bb7aa52 | 390 | #else |
| emilmont | 1:fdd22bb7aa52 | 391 | x3 = __PKHBT(x1, x2, 0); |
| emilmont | 1:fdd22bb7aa52 | 392 | #endif |
| emilmont | 1:fdd22bb7aa52 | 393 | |
| emilmont | 1:fdd22bb7aa52 | 394 | acc3 = __SMLADX(x3, y2, acc3); |
| emilmont | 1:fdd22bb7aa52 | 395 | |
| emilmont | 1:fdd22bb7aa52 | 396 | #endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ |
| emilmont | 1:fdd22bb7aa52 | 397 | |
| emilmont | 1:fdd22bb7aa52 | 398 | pIn2 += 4u; |
| emilmont | 1:fdd22bb7aa52 | 399 | |
| emilmont | 1:fdd22bb7aa52 | 400 | pScr += 4u; |
| emilmont | 1:fdd22bb7aa52 | 401 | |
| emilmont | 1:fdd22bb7aa52 | 402 | |
| emilmont | 1:fdd22bb7aa52 | 403 | /* Decrement the loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 404 | tapCnt--; |
| emilmont | 1:fdd22bb7aa52 | 405 | } |
| emilmont | 1:fdd22bb7aa52 | 406 | |
| emilmont | 1:fdd22bb7aa52 | 407 | |
| emilmont | 1:fdd22bb7aa52 | 408 | |
| emilmont | 1:fdd22bb7aa52 | 409 | /* Update scratch pointer for remaining samples of smaller length sequence */ |
| emilmont | 1:fdd22bb7aa52 | 410 | pScr -= 4u; |
| emilmont | 1:fdd22bb7aa52 | 411 | |
| emilmont | 1:fdd22bb7aa52 | 412 | |
| emilmont | 1:fdd22bb7aa52 | 413 | /* apply same above for remaining samples of smaller length sequence */ |
| emilmont | 1:fdd22bb7aa52 | 414 | tapCnt = (srcBLen) & 3u; |
| emilmont | 1:fdd22bb7aa52 | 415 | |
| emilmont | 1:fdd22bb7aa52 | 416 | while(tapCnt > 0u) |
| emilmont | 1:fdd22bb7aa52 | 417 | { |
| emilmont | 1:fdd22bb7aa52 | 418 | |
| emilmont | 1:fdd22bb7aa52 | 419 | /* accumlate the results */ |
| emilmont | 1:fdd22bb7aa52 | 420 | acc0 += (*pScr++ * *pIn2); |
| emilmont | 1:fdd22bb7aa52 | 421 | acc1 += (*pScr++ * *pIn2); |
| emilmont | 1:fdd22bb7aa52 | 422 | acc2 += (*pScr++ * *pIn2); |
| emilmont | 1:fdd22bb7aa52 | 423 | acc3 += (*pScr++ * *pIn2++); |
| emilmont | 1:fdd22bb7aa52 | 424 | |
| emilmont | 1:fdd22bb7aa52 | 425 | pScr -= 3u; |
| emilmont | 1:fdd22bb7aa52 | 426 | |
| emilmont | 1:fdd22bb7aa52 | 427 | /* Decrement the loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 428 | tapCnt--; |
| emilmont | 1:fdd22bb7aa52 | 429 | } |
| emilmont | 1:fdd22bb7aa52 | 430 | |
| emilmont | 1:fdd22bb7aa52 | 431 | blkCnt--; |
| emilmont | 1:fdd22bb7aa52 | 432 | |
| emilmont | 1:fdd22bb7aa52 | 433 | |
| emilmont | 1:fdd22bb7aa52 | 434 | /* Store the results in the accumulators in the destination buffer. */ |
| emilmont | 1:fdd22bb7aa52 | 435 | *pOut = (__SSAT(acc0 >> 15u, 16)); |
| emilmont | 1:fdd22bb7aa52 | 436 | pOut += inc; |
| emilmont | 1:fdd22bb7aa52 | 437 | *pOut = (__SSAT(acc1 >> 15u, 16)); |
| emilmont | 1:fdd22bb7aa52 | 438 | pOut += inc; |
| emilmont | 1:fdd22bb7aa52 | 439 | *pOut = (__SSAT(acc2 >> 15u, 16)); |
| emilmont | 1:fdd22bb7aa52 | 440 | pOut += inc; |
| emilmont | 1:fdd22bb7aa52 | 441 | *pOut = (__SSAT(acc3 >> 15u, 16)); |
| emilmont | 1:fdd22bb7aa52 | 442 | pOut += inc; |
| emilmont | 1:fdd22bb7aa52 | 443 | |
| emilmont | 1:fdd22bb7aa52 | 444 | |
| emilmont | 1:fdd22bb7aa52 | 445 | /* Initialization of inputB pointer */ |
| emilmont | 1:fdd22bb7aa52 | 446 | pIn2 = py; |
| emilmont | 1:fdd22bb7aa52 | 447 | |
| emilmont | 1:fdd22bb7aa52 | 448 | pScratch += 4u; |
| emilmont | 1:fdd22bb7aa52 | 449 | |
| emilmont | 1:fdd22bb7aa52 | 450 | } |
| emilmont | 1:fdd22bb7aa52 | 451 | |
| emilmont | 1:fdd22bb7aa52 | 452 | |
| emilmont | 1:fdd22bb7aa52 | 453 | blkCnt = (srcALen + srcBLen - 1u) & 0x3; |
| emilmont | 1:fdd22bb7aa52 | 454 | |
| emilmont | 1:fdd22bb7aa52 | 455 | /* Calculate correlation for remaining samples of Bigger length sequence */ |
| emilmont | 1:fdd22bb7aa52 | 456 | while(blkCnt > 0) |
| emilmont | 1:fdd22bb7aa52 | 457 | { |
| emilmont | 1:fdd22bb7aa52 | 458 | /* Initialze temporary scratch pointer as scratch1 */ |
| emilmont | 1:fdd22bb7aa52 | 459 | pScr = pScratch; |
| emilmont | 1:fdd22bb7aa52 | 460 | |
| emilmont | 1:fdd22bb7aa52 | 461 | /* Clear Accumlators */ |
| emilmont | 1:fdd22bb7aa52 | 462 | acc0 = 0; |
| emilmont | 1:fdd22bb7aa52 | 463 | |
| emilmont | 1:fdd22bb7aa52 | 464 | tapCnt = (srcBLen) >> 1u; |
| emilmont | 1:fdd22bb7aa52 | 465 | |
| emilmont | 1:fdd22bb7aa52 | 466 | while(tapCnt > 0u) |
| emilmont | 1:fdd22bb7aa52 | 467 | { |
| emilmont | 1:fdd22bb7aa52 | 468 | |
| emilmont | 1:fdd22bb7aa52 | 469 | acc0 += (*pScr++ * *pIn2++); |
| emilmont | 1:fdd22bb7aa52 | 470 | acc0 += (*pScr++ * *pIn2++); |
| emilmont | 1:fdd22bb7aa52 | 471 | |
| emilmont | 1:fdd22bb7aa52 | 472 | /* Decrement the loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 473 | tapCnt--; |
| emilmont | 1:fdd22bb7aa52 | 474 | } |
| emilmont | 1:fdd22bb7aa52 | 475 | |
| emilmont | 1:fdd22bb7aa52 | 476 | tapCnt = (srcBLen) & 1u; |
| emilmont | 1:fdd22bb7aa52 | 477 | |
| emilmont | 1:fdd22bb7aa52 | 478 | /* apply same above for remaining samples of smaller length sequence */ |
| emilmont | 1:fdd22bb7aa52 | 479 | while(tapCnt > 0u) |
| emilmont | 1:fdd22bb7aa52 | 480 | { |
| emilmont | 1:fdd22bb7aa52 | 481 | |
| emilmont | 1:fdd22bb7aa52 | 482 | /* accumlate the results */ |
| emilmont | 1:fdd22bb7aa52 | 483 | acc0 += (*pScr++ * *pIn2++); |
| emilmont | 1:fdd22bb7aa52 | 484 | |
| emilmont | 1:fdd22bb7aa52 | 485 | /* Decrement the loop counter */ |
| emilmont | 1:fdd22bb7aa52 | 486 | tapCnt--; |
| emilmont | 1:fdd22bb7aa52 | 487 | } |
| emilmont | 1:fdd22bb7aa52 | 488 | |
| emilmont | 1:fdd22bb7aa52 | 489 | blkCnt--; |
| emilmont | 1:fdd22bb7aa52 | 490 | |
| emilmont | 1:fdd22bb7aa52 | 491 | /* Store the result in the accumulator in the destination buffer. */ |
| emilmont | 1:fdd22bb7aa52 | 492 | |
| emilmont | 1:fdd22bb7aa52 | 493 | *pOut = (q15_t) (__SSAT((acc0 >> 15), 16)); |
| emilmont | 1:fdd22bb7aa52 | 494 | |
| emilmont | 1:fdd22bb7aa52 | 495 | pOut += inc; |
| emilmont | 1:fdd22bb7aa52 | 496 | |
| emilmont | 1:fdd22bb7aa52 | 497 | /* Initialization of inputB pointer */ |
| emilmont | 1:fdd22bb7aa52 | 498 | pIn2 = py; |
| emilmont | 1:fdd22bb7aa52 | 499 | |
| emilmont | 1:fdd22bb7aa52 | 500 | pScratch += 1u; |
| emilmont | 1:fdd22bb7aa52 | 501 | |
| emilmont | 1:fdd22bb7aa52 | 502 | } |
| emilmont | 1:fdd22bb7aa52 | 503 | } |
| emilmont | 1:fdd22bb7aa52 | 504 | |
| emilmont | 1:fdd22bb7aa52 | 505 | /** |
| emilmont | 1:fdd22bb7aa52 | 506 | * @} end of Corr group |
| emilmont | 1:fdd22bb7aa52 | 507 | */ |