12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319 |
- /* ----------------------------------------------------------------------
- * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
- *
- * $Date: 17. January 2013
- * $Revision: V1.4.1
- *
- * Project: CMSIS DSP Library
- * Title: arm_correlate_fast_q15.c
- *
- * Description: Fast Q15 Correlation.
- *
- * Target Processor: Cortex-M4/Cortex-M3
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * - Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * - Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * - Neither the name of ARM LIMITED nor the names of its contributors
- * may be used to endorse or promote products derived from this
- * software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- * -------------------------------------------------------------------- */
-
- #include "arm_math.h"
-
- /**
- * @ingroup groupFilters
- */
-
- /**
- * @addtogroup Corr
- * @{
- */
-
- /**
- * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- *
- * <b>Scaling and Overflow Behavior:</b>
- *
- * \par
- * This fast version uses a 32-bit accumulator with 2.30 format.
- * The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
- * There is no saturation on intermediate additions.
- * Thus, if the accumulator overflows it wraps around and distorts the result.
- * The input signals should be scaled down to avoid intermediate overflows.
- * Scale down one of the inputs by 1/min(srcALen, srcBLen) to avoid overflow since a
- * maximum of min(srcALen, srcBLen) number of additions is carried internally.
- * The 2.30 accumulator is right shifted by 15 bits and then saturated to 1.15 format to yield the final result.
- *
- * \par
- * See <code>arm_correlate_q15()</code> for a slower implementation of this function which uses a 64-bit accumulator to avoid wrap around distortion.
- */
-
- void arm_correlate_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst)
- {
- #ifndef UNALIGNED_SUPPORT_DISABLE
-
- q15_t *pIn1; /* inputA pointer */
- q15_t *pIn2; /* inputB pointer */
- q15_t *pOut = pDst; /* output pointer */
- q31_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
- q15_t *px; /* Intermediate inputA pointer */
- q15_t *py; /* Intermediate inputB pointer */
- q15_t *pSrc1; /* Intermediate pointers */
- q31_t x0, x1, x2, x3, c0; /* temporary variables for holding input and coefficient values */
- uint32_t j, k = 0u, count, blkCnt, outBlockSize, blockSize1, blockSize2, blockSize3; /* loop counter */
- int32_t inc = 1; /* Destination address modifier */
-
-
- /* The algorithm implementation is based on the lengths of the inputs. */
- /* srcB is always made to slide across srcA. */
- /* So srcBLen is always considered as shorter or equal to srcALen */
- /* But CORR(x, y) is reverse of CORR(y, x) */
- /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
- /* and the destination pointer modifier, inc is set to -1 */
- /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
- /* But to improve the performance,
- * we include zeroes in the output instead of zero padding either of the the inputs*/
- /* If srcALen > srcBLen,
- * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
- /* If srcALen < srcBLen,
- * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
- if(srcALen >= srcBLen)
- {
- /* Initialization of inputA pointer */
- pIn1 = (pSrcA);
-
- /* Initialization of inputB pointer */
- pIn2 = (pSrcB);
-
- /* Number of output samples is calculated */
- outBlockSize = (2u * srcALen) - 1u;
-
- /* When srcALen > srcBLen, zero padding is done to srcB
- * to make their lengths equal.
- * Instead, (outBlockSize - (srcALen + srcBLen - 1))
- * number of output samples are made zero */
- j = outBlockSize - (srcALen + (srcBLen - 1u));
-
- /* Updating the pointer position to non zero value */
- pOut += j;
-
- }
- else
- {
- /* Initialization of inputA pointer */
- pIn1 = (pSrcB);
-
- /* Initialization of inputB pointer */
- pIn2 = (pSrcA);
-
- /* srcBLen is always considered as shorter or equal to srcALen */
- j = srcBLen;
- srcBLen = srcALen;
- srcALen = j;
-
- /* CORR(x, y) = Reverse order(CORR(y, x)) */
- /* Hence set the destination pointer to point to the last output sample */
- pOut = pDst + ((srcALen + srcBLen) - 2u);
-
- /* Destination address modifier is set to -1 */
- inc = -1;
-
- }
-
- /* The function is internally
- * divided into three parts according to the number of multiplications that has to be
- * taken place between inputA samples and inputB samples. In the first part of the
- * algorithm, the multiplications increase by one for every iteration.
- * In the second part of the algorithm, srcBLen number of multiplications are done.
- * In the third part of the algorithm, the multiplications decrease by one
- * for every iteration.*/
- /* The algorithm is implemented in three stages.
- * The loop counters of each stage is initiated here. */
- blockSize1 = srcBLen - 1u;
- blockSize2 = srcALen - (srcBLen - 1u);
- blockSize3 = blockSize1;
-
- /* --------------------------
- * Initializations of stage1
- * -------------------------*/
-
- /* sum = x[0] * y[srcBlen - 1]
- * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
- * ....
- * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
- */
-
- /* In this stage the MAC operations are increased by 1 for every iteration.
- The count variable holds the number of MAC operations performed */
- count = 1u;
-
- /* Working pointer of inputA */
- px = pIn1;
-
- /* Working pointer of inputB */
- pSrc1 = pIn2 + (srcBLen - 1u);
- py = pSrc1;
-
- /* ------------------------
- * Stage1 process
- * ----------------------*/
-
- /* The first loop starts here */
- while(blockSize1 > 0u)
- {
- /* Accumulator is made zero for every iteration */
- sum = 0;
-
- /* Apply loop unrolling and compute 4 MACs simultaneously. */
- k = count >> 2;
-
- /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
- ** a second loop below computes MACs for the remaining 1 to 3 samples. */
- while(k > 0u)
- {
- /* x[0] * y[srcBLen - 4] , x[1] * y[srcBLen - 3] */
- sum = __SMLAD(*__SIMD32(px)++, *__SIMD32(py)++, sum);
- /* x[3] * y[srcBLen - 1] , x[2] * y[srcBLen - 2] */
- sum = __SMLAD(*__SIMD32(px)++, *__SIMD32(py)++, sum);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* If the count is not a multiple of 4, compute any remaining MACs here.
- ** No loop unrolling is used. */
- k = count % 0x4u;
-
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- /* x[0] * y[srcBLen - 1] */
- sum = __SMLAD(*px++, *py++, sum);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (sum >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- py = pSrc1 - count;
- px = pIn1;
-
- /* Increment the MAC count */
- count++;
-
- /* Decrement the loop counter */
- blockSize1--;
- }
-
- /* --------------------------
- * Initializations of stage2
- * ------------------------*/
-
- /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
- * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
- * ....
- * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
- */
-
- /* Working pointer of inputA */
- px = pIn1;
-
- /* Working pointer of inputB */
- py = pIn2;
-
- /* count is index by which the pointer pIn1 to be incremented */
- count = 0u;
-
- /* -------------------
- * Stage2 process
- * ------------------*/
-
- /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
- * So, to loop unroll over blockSize2,
- * srcBLen should be greater than or equal to 4, to loop unroll the srcBLen loop */
- if(srcBLen >= 4u)
- {
- /* Loop unroll over blockSize2, by 4 */
- blkCnt = blockSize2 >> 2u;
-
- while(blkCnt > 0u)
- {
- /* Set all accumulators to zero */
- acc0 = 0;
- acc1 = 0;
- acc2 = 0;
- acc3 = 0;
-
- /* read x[0], x[1] samples */
- x0 = *__SIMD32(px);
- /* read x[1], x[2] samples */
- x1 = _SIMD32_OFFSET(px + 1);
- px += 2u;
-
- /* Apply loop unrolling and compute 4 MACs simultaneously. */
- k = srcBLen >> 2u;
-
- /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
- ** a second loop below computes MACs for the remaining 1 to 3 samples. */
- do
- {
- /* Read the first two inputB samples using SIMD:
- * y[0] and y[1] */
- c0 = *__SIMD32(py)++;
-
- /* acc0 += x[0] * y[0] + x[1] * y[1] */
- acc0 = __SMLAD(x0, c0, acc0);
-
- /* acc1 += x[1] * y[0] + x[2] * y[1] */
- acc1 = __SMLAD(x1, c0, acc1);
-
- /* Read x[2], x[3] */
- x2 = *__SIMD32(px);
-
- /* Read x[3], x[4] */
- x3 = _SIMD32_OFFSET(px + 1);
-
- /* acc2 += x[2] * y[0] + x[3] * y[1] */
- acc2 = __SMLAD(x2, c0, acc2);
-
- /* acc3 += x[3] * y[0] + x[4] * y[1] */
- acc3 = __SMLAD(x3, c0, acc3);
-
- /* Read y[2] and y[3] */
- c0 = *__SIMD32(py)++;
-
- /* acc0 += x[2] * y[2] + x[3] * y[3] */
- acc0 = __SMLAD(x2, c0, acc0);
-
- /* acc1 += x[3] * y[2] + x[4] * y[3] */
- acc1 = __SMLAD(x3, c0, acc1);
-
- /* Read x[4], x[5] */
- x0 = _SIMD32_OFFSET(px + 2);
-
- /* Read x[5], x[6] */
- x1 = _SIMD32_OFFSET(px + 3);
- px += 4u;
-
- /* acc2 += x[4] * y[2] + x[5] * y[3] */
- acc2 = __SMLAD(x0, c0, acc2);
-
- /* acc3 += x[5] * y[2] + x[6] * y[3] */
- acc3 = __SMLAD(x1, c0, acc3);
-
- } while(--k);
-
- /* For the next MAC operations, SIMD is not used
- * So, the 16 bit pointer if inputB, py is updated */
-
- /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
- ** No loop unrolling is used. */
- k = srcBLen % 0x4u;
-
- if(k == 1u)
- {
- /* Read y[4] */
- c0 = *py;
- #ifdef ARM_MATH_BIG_ENDIAN
-
- c0 = c0 << 16u;
-
- #else
-
- c0 = c0 & 0x0000FFFF;
-
- #endif /* #ifdef ARM_MATH_BIG_ENDIAN */
-
- /* Read x[7] */
- x3 = *__SIMD32(px);
- px++;
-
- /* Perform the multiply-accumulates */
- acc0 = __SMLAD(x0, c0, acc0);
- acc1 = __SMLAD(x1, c0, acc1);
- acc2 = __SMLADX(x1, c0, acc2);
- acc3 = __SMLADX(x3, c0, acc3);
- }
-
- if(k == 2u)
- {
- /* Read y[4], y[5] */
- c0 = *__SIMD32(py);
-
- /* Read x[7], x[8] */
- x3 = *__SIMD32(px);
-
- /* Read x[9] */
- x2 = _SIMD32_OFFSET(px + 1);
- px += 2u;
-
- /* Perform the multiply-accumulates */
- acc0 = __SMLAD(x0, c0, acc0);
- acc1 = __SMLAD(x1, c0, acc1);
- acc2 = __SMLAD(x3, c0, acc2);
- acc3 = __SMLAD(x2, c0, acc3);
- }
-
- if(k == 3u)
- {
- /* Read y[4], y[5] */
- c0 = *__SIMD32(py)++;
-
- /* Read x[7], x[8] */
- x3 = *__SIMD32(px);
-
- /* Read x[9] */
- x2 = _SIMD32_OFFSET(px + 1);
-
- /* Perform the multiply-accumulates */
- acc0 = __SMLAD(x0, c0, acc0);
- acc1 = __SMLAD(x1, c0, acc1);
- acc2 = __SMLAD(x3, c0, acc2);
- acc3 = __SMLAD(x2, c0, acc3);
-
- c0 = (*py);
- /* Read y[6] */
- #ifdef ARM_MATH_BIG_ENDIAN
-
- c0 = c0 << 16u;
- #else
-
- c0 = c0 & 0x0000FFFF;
- #endif /* #ifdef ARM_MATH_BIG_ENDIAN */
-
- /* Read x[10] */
- x3 = _SIMD32_OFFSET(px + 2);
- px += 3u;
-
- /* Perform the multiply-accumulates */
- acc0 = __SMLADX(x1, c0, acc0);
- acc1 = __SMLAD(x2, c0, acc1);
- acc2 = __SMLADX(x2, c0, acc2);
- acc3 = __SMLADX(x3, c0, acc3);
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (acc0 >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- *pOut = (q15_t) (acc1 >> 15);
- pOut += inc;
-
- *pOut = (q15_t) (acc2 >> 15);
- pOut += inc;
-
- *pOut = (q15_t) (acc3 >> 15);
- pOut += inc;
-
- /* Increment the pointer pIn1 index, count by 1 */
- count += 4u;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- px = pIn1 + count;
- py = pIn2;
-
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
- ** No loop unrolling is used. */
- blkCnt = blockSize2 % 0x4u;
-
- while(blkCnt > 0u)
- {
- /* Accumulator is made zero for every iteration */
- sum = 0;
-
- /* Apply loop unrolling and compute 4 MACs simultaneously. */
- k = srcBLen >> 2u;
-
- /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
- ** a second loop below computes MACs for the remaining 1 to 3 samples. */
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
- ** No loop unrolling is used. */
- k = srcBLen % 0x4u;
-
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (sum >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- /* Increment the pointer pIn1 index, count by 1 */
- count++;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- px = pIn1 + count;
- py = pIn2;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
- }
- else
- {
- /* If the srcBLen is not a multiple of 4,
- * the blockSize2 loop cannot be unrolled by 4 */
- blkCnt = blockSize2;
-
- while(blkCnt > 0u)
- {
- /* Accumulator is made zero for every iteration */
- sum = 0;
-
- /* Loop over srcBLen */
- k = srcBLen;
-
- while(k > 0u)
- {
- /* Perform the multiply-accumulate */
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (sum >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- /* Increment the MAC count */
- count++;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- px = pIn1 + count;
- py = pIn2;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
- }
-
- /* --------------------------
- * Initializations of stage3
- * -------------------------*/
-
- /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
- * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
- * ....
- * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
- * sum += x[srcALen-1] * y[0]
- */
-
- /* In this stage the MAC operations are decreased by 1 for every iteration.
- The count variable holds the number of MAC operations performed */
- count = srcBLen - 1u;
-
- /* Working pointer of inputA */
- pSrc1 = (pIn1 + srcALen) - (srcBLen - 1u);
- px = pSrc1;
-
- /* Working pointer of inputB */
- py = pIn2;
-
- /* -------------------
- * Stage3 process
- * ------------------*/
-
- while(blockSize3 > 0u)
- {
- /* Accumulator is made zero for every iteration */
- sum = 0;
-
- /* Apply loop unrolling and compute 4 MACs simultaneously. */
- k = count >> 2u;
-
- /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
- ** a second loop below computes MACs for the remaining 1 to 3 samples. */
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- /* sum += x[srcALen - srcBLen + 4] * y[3] , sum += x[srcALen - srcBLen + 3] * y[2] */
- sum = __SMLAD(*__SIMD32(px)++, *__SIMD32(py)++, sum);
- /* sum += x[srcALen - srcBLen + 2] * y[1] , sum += x[srcALen - srcBLen + 1] * y[0] */
- sum = __SMLAD(*__SIMD32(px)++, *__SIMD32(py)++, sum);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* If the count is not a multiple of 4, compute any remaining MACs here.
- ** No loop unrolling is used. */
- k = count % 0x4u;
-
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- sum = __SMLAD(*px++, *py++, sum);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (sum >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- px = ++pSrc1;
- py = pIn2;
-
- /* Decrement the MAC count */
- count--;
-
- /* Decrement the loop counter */
- blockSize3--;
- }
-
- #else
-
- q15_t *pIn1; /* inputA pointer */
- q15_t *pIn2; /* inputB pointer */
- q15_t *pOut = pDst; /* output pointer */
- q31_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
- q15_t *px; /* Intermediate inputA pointer */
- q15_t *py; /* Intermediate inputB pointer */
- q15_t *pSrc1; /* Intermediate pointers */
- q31_t x0, x1, x2, x3, c0; /* temporary variables for holding input and coefficient values */
- uint32_t j, k = 0u, count, blkCnt, outBlockSize, blockSize1, blockSize2, blockSize3; /* loop counter */
- int32_t inc = 1; /* Destination address modifier */
- q15_t a, b;
-
-
- /* The algorithm implementation is based on the lengths of the inputs. */
- /* srcB is always made to slide across srcA. */
- /* So srcBLen is always considered as shorter or equal to srcALen */
- /* But CORR(x, y) is reverse of CORR(y, x) */
- /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
- /* and the destination pointer modifier, inc is set to -1 */
- /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
- /* But to improve the performance,
- * we include zeroes in the output instead of zero padding either of the the inputs*/
- /* If srcALen > srcBLen,
- * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
- /* If srcALen < srcBLen,
- * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
- if(srcALen >= srcBLen)
- {
- /* Initialization of inputA pointer */
- pIn1 = (pSrcA);
-
- /* Initialization of inputB pointer */
- pIn2 = (pSrcB);
-
- /* Number of output samples is calculated */
- outBlockSize = (2u * srcALen) - 1u;
-
- /* When srcALen > srcBLen, zero padding is done to srcB
- * to make their lengths equal.
- * Instead, (outBlockSize - (srcALen + srcBLen - 1))
- * number of output samples are made zero */
- j = outBlockSize - (srcALen + (srcBLen - 1u));
-
- /* Updating the pointer position to non zero value */
- pOut += j;
-
- }
- else
- {
- /* Initialization of inputA pointer */
- pIn1 = (pSrcB);
-
- /* Initialization of inputB pointer */
- pIn2 = (pSrcA);
-
- /* srcBLen is always considered as shorter or equal to srcALen */
- j = srcBLen;
- srcBLen = srcALen;
- srcALen = j;
-
- /* CORR(x, y) = Reverse order(CORR(y, x)) */
- /* Hence set the destination pointer to point to the last output sample */
- pOut = pDst + ((srcALen + srcBLen) - 2u);
-
- /* Destination address modifier is set to -1 */
- inc = -1;
-
- }
-
- /* The function is internally
- * divided into three parts according to the number of multiplications that has to be
- * taken place between inputA samples and inputB samples. In the first part of the
- * algorithm, the multiplications increase by one for every iteration.
- * In the second part of the algorithm, srcBLen number of multiplications are done.
- * In the third part of the algorithm, the multiplications decrease by one
- * for every iteration.*/
- /* The algorithm is implemented in three stages.
- * The loop counters of each stage is initiated here. */
- blockSize1 = srcBLen - 1u;
- blockSize2 = srcALen - (srcBLen - 1u);
- blockSize3 = blockSize1;
-
- /* --------------------------
- * Initializations of stage1
- * -------------------------*/
-
- /* sum = x[0] * y[srcBlen - 1]
- * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
- * ....
- * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
- */
-
- /* In this stage the MAC operations are increased by 1 for every iteration.
- The count variable holds the number of MAC operations performed */
- count = 1u;
-
- /* Working pointer of inputA */
- px = pIn1;
-
- /* Working pointer of inputB */
- pSrc1 = pIn2 + (srcBLen - 1u);
- py = pSrc1;
-
- /* ------------------------
- * Stage1 process
- * ----------------------*/
-
- /* The first loop starts here */
- while(blockSize1 > 0u)
- {
- /* Accumulator is made zero for every iteration */
- sum = 0;
-
- /* Apply loop unrolling and compute 4 MACs simultaneously. */
- k = count >> 2;
-
- /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
- ** a second loop below computes MACs for the remaining 1 to 3 samples. */
- while(k > 0u)
- {
- /* x[0] * y[srcBLen - 4] , x[1] * y[srcBLen - 3] */
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* If the count is not a multiple of 4, compute any remaining MACs here.
- ** No loop unrolling is used. */
- k = count % 0x4u;
-
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- /* x[0] * y[srcBLen - 1] */
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (sum >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- py = pSrc1 - count;
- px = pIn1;
-
- /* Increment the MAC count */
- count++;
-
- /* Decrement the loop counter */
- blockSize1--;
- }
-
- /* --------------------------
- * Initializations of stage2
- * ------------------------*/
-
- /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
- * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
- * ....
- * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
- */
-
- /* Working pointer of inputA */
- px = pIn1;
-
- /* Working pointer of inputB */
- py = pIn2;
-
- /* count is index by which the pointer pIn1 to be incremented */
- count = 0u;
-
- /* -------------------
- * Stage2 process
- * ------------------*/
-
- /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
- * So, to loop unroll over blockSize2,
- * srcBLen should be greater than or equal to 4, to loop unroll the srcBLen loop */
- if(srcBLen >= 4u)
- {
- /* Loop unroll over blockSize2, by 4 */
- blkCnt = blockSize2 >> 2u;
-
- while(blkCnt > 0u)
- {
- /* Set all accumulators to zero */
- acc0 = 0;
- acc1 = 0;
- acc2 = 0;
- acc3 = 0;
-
- /* read x[0], x[1], x[2] samples */
- a = *px;
- b = *(px + 1);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- x0 = __PKHBT(a, b, 16);
- a = *(px + 2);
- x1 = __PKHBT(b, a, 16);
-
- #else
-
- x0 = __PKHBT(b, a, 16);
- a = *(px + 2);
- x1 = __PKHBT(a, b, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- px += 2u;
-
- /* Apply loop unrolling and compute 4 MACs simultaneously. */
- k = srcBLen >> 2u;
-
- /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
- ** a second loop below computes MACs for the remaining 1 to 3 samples. */
- do
- {
- /* Read the first two inputB samples using SIMD:
- * y[0] and y[1] */
- a = *py;
- b = *(py + 1);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- c0 = __PKHBT(a, b, 16);
-
- #else
-
- c0 = __PKHBT(b, a, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- /* acc0 += x[0] * y[0] + x[1] * y[1] */
- acc0 = __SMLAD(x0, c0, acc0);
-
- /* acc1 += x[1] * y[0] + x[2] * y[1] */
- acc1 = __SMLAD(x1, c0, acc1);
-
- /* Read x[2], x[3], x[4] */
- a = *px;
- b = *(px + 1);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- x2 = __PKHBT(a, b, 16);
- a = *(px + 2);
- x3 = __PKHBT(b, a, 16);
-
- #else
-
- x2 = __PKHBT(b, a, 16);
- a = *(px + 2);
- x3 = __PKHBT(a, b, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- /* acc2 += x[2] * y[0] + x[3] * y[1] */
- acc2 = __SMLAD(x2, c0, acc2);
-
- /* acc3 += x[3] * y[0] + x[4] * y[1] */
- acc3 = __SMLAD(x3, c0, acc3);
-
- /* Read y[2] and y[3] */
- a = *(py + 2);
- b = *(py + 3);
-
- py += 4u;
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- c0 = __PKHBT(a, b, 16);
-
- #else
-
- c0 = __PKHBT(b, a, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- /* acc0 += x[2] * y[2] + x[3] * y[3] */
- acc0 = __SMLAD(x2, c0, acc0);
-
- /* acc1 += x[3] * y[2] + x[4] * y[3] */
- acc1 = __SMLAD(x3, c0, acc1);
-
- /* Read x[4], x[5], x[6] */
- a = *(px + 2);
- b = *(px + 3);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- x0 = __PKHBT(a, b, 16);
- a = *(px + 4);
- x1 = __PKHBT(b, a, 16);
-
- #else
-
- x0 = __PKHBT(b, a, 16);
- a = *(px + 4);
- x1 = __PKHBT(a, b, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- px += 4u;
-
- /* acc2 += x[4] * y[2] + x[5] * y[3] */
- acc2 = __SMLAD(x0, c0, acc2);
-
- /* acc3 += x[5] * y[2] + x[6] * y[3] */
- acc3 = __SMLAD(x1, c0, acc3);
-
- } while(--k);
-
- /* For the next MAC operations, SIMD is not used
- * So, the 16 bit pointer if inputB, py is updated */
-
- /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
- ** No loop unrolling is used. */
- k = srcBLen % 0x4u;
-
- if(k == 1u)
- {
- /* Read y[4] */
- c0 = *py;
- #ifdef ARM_MATH_BIG_ENDIAN
-
- c0 = c0 << 16u;
-
- #else
-
- c0 = c0 & 0x0000FFFF;
-
- #endif /* #ifdef ARM_MATH_BIG_ENDIAN */
-
- /* Read x[7] */
- a = *px;
- b = *(px + 1);
-
- px++;;
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- x3 = __PKHBT(a, b, 16);
-
- #else
-
- x3 = __PKHBT(b, a, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- px++;
-
- /* Perform the multiply-accumulates */
- acc0 = __SMLAD(x0, c0, acc0);
- acc1 = __SMLAD(x1, c0, acc1);
- acc2 = __SMLADX(x1, c0, acc2);
- acc3 = __SMLADX(x3, c0, acc3);
- }
-
- if(k == 2u)
- {
- /* Read y[4], y[5] */
- a = *py;
- b = *(py + 1);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- c0 = __PKHBT(a, b, 16);
-
- #else
-
- c0 = __PKHBT(b, a, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- /* Read x[7], x[8], x[9] */
- a = *px;
- b = *(px + 1);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- x3 = __PKHBT(a, b, 16);
- a = *(px + 2);
- x2 = __PKHBT(b, a, 16);
-
- #else
-
- x3 = __PKHBT(b, a, 16);
- a = *(px + 2);
- x2 = __PKHBT(a, b, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- px += 2u;
-
- /* Perform the multiply-accumulates */
- acc0 = __SMLAD(x0, c0, acc0);
- acc1 = __SMLAD(x1, c0, acc1);
- acc2 = __SMLAD(x3, c0, acc2);
- acc3 = __SMLAD(x2, c0, acc3);
- }
-
- if(k == 3u)
- {
- /* Read y[4], y[5] */
- a = *py;
- b = *(py + 1);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- c0 = __PKHBT(a, b, 16);
-
- #else
-
- c0 = __PKHBT(b, a, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- py += 2u;
-
- /* Read x[7], x[8], x[9] */
- a = *px;
- b = *(px + 1);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- x3 = __PKHBT(a, b, 16);
- a = *(px + 2);
- x2 = __PKHBT(b, a, 16);
-
- #else
-
- x3 = __PKHBT(b, a, 16);
- a = *(px + 2);
- x2 = __PKHBT(a, b, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- /* Perform the multiply-accumulates */
- acc0 = __SMLAD(x0, c0, acc0);
- acc1 = __SMLAD(x1, c0, acc1);
- acc2 = __SMLAD(x3, c0, acc2);
- acc3 = __SMLAD(x2, c0, acc3);
-
- c0 = (*py);
- /* Read y[6] */
- #ifdef ARM_MATH_BIG_ENDIAN
-
- c0 = c0 << 16u;
- #else
-
- c0 = c0 & 0x0000FFFF;
- #endif /* #ifdef ARM_MATH_BIG_ENDIAN */
-
- /* Read x[10] */
- b = *(px + 3);
-
- #ifndef ARM_MATH_BIG_ENDIAN
-
- x3 = __PKHBT(a, b, 16);
-
- #else
-
- x3 = __PKHBT(b, a, 16);
-
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
-
- px += 3u;
-
- /* Perform the multiply-accumulates */
- acc0 = __SMLADX(x1, c0, acc0);
- acc1 = __SMLAD(x2, c0, acc1);
- acc2 = __SMLADX(x2, c0, acc2);
- acc3 = __SMLADX(x3, c0, acc3);
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (acc0 >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- *pOut = (q15_t) (acc1 >> 15);
- pOut += inc;
-
- *pOut = (q15_t) (acc2 >> 15);
- pOut += inc;
-
- *pOut = (q15_t) (acc3 >> 15);
- pOut += inc;
-
- /* Increment the pointer pIn1 index, count by 1 */
- count += 4u;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- px = pIn1 + count;
- py = pIn2;
-
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
- ** No loop unrolling is used. */
- blkCnt = blockSize2 % 0x4u;
-
- while(blkCnt > 0u)
- {
- /* Accumulator is made zero for every iteration */
- sum = 0;
-
- /* Apply loop unrolling and compute 4 MACs simultaneously. */
- k = srcBLen >> 2u;
-
- /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
- ** a second loop below computes MACs for the remaining 1 to 3 samples. */
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
- ** No loop unrolling is used. */
- k = srcBLen % 0x4u;
-
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (sum >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- /* Increment the pointer pIn1 index, count by 1 */
- count++;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- px = pIn1 + count;
- py = pIn2;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
- }
- else
- {
- /* If the srcBLen is not a multiple of 4,
- * the blockSize2 loop cannot be unrolled by 4 */
- blkCnt = blockSize2;
-
- while(blkCnt > 0u)
- {
- /* Accumulator is made zero for every iteration */
- sum = 0;
-
- /* Loop over srcBLen */
- k = srcBLen;
-
- while(k > 0u)
- {
- /* Perform the multiply-accumulate */
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (sum >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- /* Increment the MAC count */
- count++;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- px = pIn1 + count;
- py = pIn2;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
- }
-
- /* --------------------------
- * Initializations of stage3
- * -------------------------*/
-
- /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
- * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
- * ....
- * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
- * sum += x[srcALen-1] * y[0]
- */
-
- /* In this stage the MAC operations are decreased by 1 for every iteration.
- The count variable holds the number of MAC operations performed */
- count = srcBLen - 1u;
-
- /* Working pointer of inputA */
- pSrc1 = (pIn1 + srcALen) - (srcBLen - 1u);
- px = pSrc1;
-
- /* Working pointer of inputB */
- py = pIn2;
-
- /* -------------------
- * Stage3 process
- * ------------------*/
-
- while(blockSize3 > 0u)
- {
- /* Accumulator is made zero for every iteration */
- sum = 0;
-
- /* Apply loop unrolling and compute 4 MACs simultaneously. */
- k = count >> 2u;
-
- /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
- ** a second loop below computes MACs for the remaining 1 to 3 samples. */
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* If the count is not a multiple of 4, compute any remaining MACs here.
- ** No loop unrolling is used. */
- k = count % 0x4u;
-
- while(k > 0u)
- {
- /* Perform the multiply-accumulates */
- sum += ((q31_t) * px++ * *py++);
-
- /* Decrement the loop counter */
- k--;
- }
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut = (q15_t) (sum >> 15);
- /* Destination pointer is updated according to the address modifier, inc */
- pOut += inc;
-
- /* Update the inputA and inputB pointers for next MAC calculation */
- px = ++pSrc1;
- py = pIn2;
-
- /* Decrement the MAC count */
- count--;
-
- /* Decrement the loop counter */
- blockSize3--;
- }
-
- #endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */
-
- }
-
- /**
- * @} end of Corr group
- */
|