tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_conv_partial_q31.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        17. January 2013
   5 * $Revision:    V1.4.1
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:                arm_conv_partial_q31.c
   9 *
  10 * Description:  Partial convolution of Q31 sequences.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Redistribution and use in source and binary forms, with or without
  15 * modification, are permitted provided that the following conditions
  16 * are met:
  17 *   - Redistributions of source code must retain the above copyright
  18 *     notice, this list of conditions and the following disclaimer.
  19 *   - Redistributions in binary form must reproduce the above copyright
  20 *     notice, this list of conditions and the following disclaimer in
  21 *     the documentation and/or other materials provided with the
  22 *     distribution.
  23 *   - Neither the name of ARM LIMITED nor the names of its contributors
  24 *     may be used to endorse or promote products derived from this
  25 *     software without specific prior written permission.
  26 *
  27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  38 * POSSIBILITY OF SUCH DAMAGE.
  39 * -------------------------------------------------------------------- */
  40
  41 #include "arm_math.h"
  42
  43 /**
  44  * @ingroup groupFilters
  45  */
  46
  47 /**
  48  * @addtogroup PartialConv
  49  * @{
  50  */
  51
  52 /**
  53  * @brief Partial convolution of Q31 sequences.
  54  * @param[in]       *pSrcA points to the first input sequence.
  55  * @param[in]       srcALen length of the first input sequence.
  56  * @param[in]       *pSrcB points to the second input sequence.
  57  * @param[in]       srcBLen length of the second input sequence.
  58  * @param[out]      *pDst points to the location where the output result is written.
  59  * @param[in]       firstIndex is the first output sample to start with.
  60  * @param[in]       numPoints is the number of output points to be computed.
  61  * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
  62  *
  63  * See <code>arm_conv_partial_fast_q31()</code> for a faster but less precise implementation of this function for Cortex-M3 and Cortex-M4.
  64  */
  65
  66 arm_status arm_conv_partial_q31(
  67   q31_t * pSrcA,
  68   uint32_t srcALen,
  69   q31_t * pSrcB,
  70   uint32_t srcBLen,
  71   q31_t * pDst,
  72   uint32_t firstIndex,
  73   uint32_t numPoints)
  74 {
  75
  76
  77 #ifndef ARM_MATH_CM0_FAMILY
  78
  79   /* Run the below code for Cortex-M4 and Cortex-M3 */
  80
  81   q31_t *pIn1;                                   /* inputA pointer               */
  82   q31_t *pIn2;                                   /* inputB pointer               */
  83   q31_t *pOut = pDst;                            /* output pointer               */
  84   q31_t *px;                                     /* Intermediate inputA pointer  */
  85   q31_t *py;                                     /* Intermediate inputB pointer  */
  86   q31_t *pSrc1, *pSrc2;                          /* Intermediate pointers        */
  87   q63_t sum, acc0, acc1, acc2;                   /* Accumulator                  */
  88   q31_t x0, x1, x2, c0;
  89   uint32_t j, k, count, check, blkCnt;
  90   int32_t blockSize1, blockSize2, blockSize3;    /* loop counter                 */
  91   arm_status status;                             /* status of Partial convolution */
  92
  93
  94   /* Check for range of output samples to be calculated */
  95   if((firstIndex + numPoints) > ((srcALen + (srcBLen - 1u))))
  96   {
  97     /* Set status as ARM_MATH_ARGUMENT_ERROR */
  98     status = ARM_MATH_ARGUMENT_ERROR;
  99   }
 100   else
 101   {
 102
 103     /* The algorithm implementation is based on the lengths of the inputs. */
 104     /* srcB is always made to slide across srcA. */
 105     /* So srcBLen is always considered as shorter or equal to srcALen */
 106     if(srcALen >= srcBLen)
 107     {
 108       /* Initialization of inputA pointer */
 109       pIn1 = pSrcA;
 110
 111       /* Initialization of inputB pointer */
 112       pIn2 = pSrcB;
 113     }
 114     else
 115     {
 116       /* Initialization of inputA pointer */
 117       pIn1 = pSrcB;
 118
 119       /* Initialization of inputB pointer */
 120       pIn2 = pSrcA;
 121
 122       /* srcBLen is always considered as shorter or equal to srcALen */
 123       j = srcBLen;
 124       srcBLen = srcALen;
 125       srcALen = j;
 126     }
 127
 128     /* Conditions to check which loopCounter holds
 129      * the first and last indices of the output samples to be calculated. */
 130     check = firstIndex + numPoints;
 131     blockSize3 = ((int32_t) check - (int32_t) srcALen);
 132     blockSize3 = (blockSize3 > 0) ? blockSize3 : 0;
 133     blockSize1 = (((int32_t) srcBLen - 1) - (int32_t) firstIndex);
 134     blockSize1 = (blockSize1 > 0) ? ((check > (srcBLen - 1u)) ? blockSize1 :
 135                                      (int32_t) numPoints) : 0;
 136     blockSize2 = (int32_t) check - ((blockSize3 + blockSize1) +
 137                                     (int32_t) firstIndex);
 138     blockSize2 = (blockSize2 > 0) ? blockSize2 : 0;
 139
 140     /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
 141     /* The function is internally
 142      * divided into three stages according to the number of multiplications that has to be
 143      * taken place between inputA samples and inputB samples. In the first stage of the
 144      * algorithm, the multiplications increase by one for every iteration.
 145      * In the second stage of the algorithm, srcBLen number of multiplications are done.
 146      * In the third stage of the algorithm, the multiplications decrease by one
 147      * for every iteration. */
 148
 149     /* Set the output pointer to point to the firstIndex
 150      * of the output sample to be calculated. */
 151     pOut = pDst + firstIndex;
 152
 153     /* --------------------------
 154      * Initializations of stage1
 155      * -------------------------*/
 156
 157     /* sum = x[0] * y[0]
 158      * sum = x[0] * y[1] + x[1] * y[0]
 159      * ....
 160      * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
 161      */
 162
 163     /* In this stage the MAC operations are increased by 1 for every iteration.
 164        The count variable holds the number of MAC operations performed.
 165        Since the partial convolution starts from firstIndex
 166        Number of Macs to be performed is firstIndex + 1 */
 167     count = 1u + firstIndex;
 168
 169     /* Working pointer of inputA */
 170     px = pIn1;
 171
 172     /* Working pointer of inputB */
 173     pSrc2 = pIn2 + firstIndex;
 174     py = pSrc2;
 175
 176     /* ------------------------
 177      * Stage1 process
 178      * ----------------------*/
 179
 180     /* The first loop starts here */
 181     while(blockSize1 > 0)
 182     {
 183       /* Accumulator is made zero for every iteration */
 184       sum = 0;
 185
 186       /* Apply loop unrolling and compute 4 MACs simultaneously. */
 187       k = count >> 2u;
 188
 189       /* First part of the processing with loop unrolling.  Compute 4 MACs at a time.
 190        ** a second loop below computes MACs for the remaining 1 to 3 samples. */
 191       while(k > 0u)
 192       {
 193         /* x[0] * y[srcBLen - 1] */
 194         sum += (q63_t) * px++ * (*py--);
 195         /* x[1] * y[srcBLen - 2] */
 196         sum += (q63_t) * px++ * (*py--);
 197         /* x[2] * y[srcBLen - 3] */
 198         sum += (q63_t) * px++ * (*py--);
 199         /* x[3] * y[srcBLen - 4] */
 200         sum += (q63_t) * px++ * (*py--);
 201
 202         /* Decrement the loop counter */
 203         k--;
 204       }
 205
 206       /* If the count is not a multiple of 4, compute any remaining MACs here.
 207        ** No loop unrolling is used. */
 208       k = count % 0x4u;
 209
 210       while(k > 0u)
 211       {
 212         /* Perform the multiply-accumulate */
 213         sum += (q63_t) * px++ * (*py--);
 214
 215         /* Decrement the loop counter */
 216         k--;
 217       }
 218
 219       /* Store the result in the accumulator in the destination buffer. */
 220       *pOut++ = (q31_t) (sum >> 31);
 221
 222       /* Update the inputA and inputB pointers for next MAC calculation */
 223       py = ++pSrc2;
 224       px = pIn1;
 225
 226       /* Increment the MAC count */
 227       count++;
 228
 229       /* Decrement the loop counter */
 230       blockSize1--;
 231     }
 232
 233     /* --------------------------
 234      * Initializations of stage2
 235      * ------------------------*/
 236
 237     /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
 238      * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
 239      * ....
 240      * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
 241      */
 242
 243     /* Working pointer of inputA */
 244     px = pIn1;
 245
 246     /* Working pointer of inputB */
 247     pSrc2 = pIn2 + (srcBLen - 1u);
 248     py = pSrc2;
 249
 250     /* count is index by which the pointer pIn1 to be incremented */
 251     count = 0u;
 252
 253     /* -------------------
 254      * Stage2 process
 255      * ------------------*/
 256
 257     /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
 258      * So, to loop unroll over blockSize2,
 259      * srcBLen should be greater than or equal to 4 */
 260     if(srcBLen >= 4u)
 261     {
 262       /* Loop unroll over blkCnt */
 263
 264       blkCnt = blockSize2 / 3;
 265       while(blkCnt > 0u)
 266       {
 267         /* Set all accumulators to zero */
 268         acc0 = 0;
 269         acc1 = 0;
 270         acc2 = 0;
 271
 272         /* read x[0], x[1] samples */
 273         x0 = *(px++);
 274         x1 = *(px++);
 275
 276         /* Apply loop unrolling and compute 3 MACs simultaneously. */
 277         k = srcBLen / 3;
 278
 279         /* First part of the processing with loop unrolling.  Compute 3 MACs at a time.
 280          ** a second loop below computes MACs for the remaining 1 to 2 samples. */
 281         do
 282         {
 283           /* Read y[srcBLen - 1] sample */
 284           c0 = *(py);
 285
 286           /* Read x[2] sample */
 287           x2 = *(px);
 288
 289           /* Perform the multiply-accumulates */
 290           /* acc0 +=  x[0] * y[srcBLen - 1] */
 291           acc0 += (q63_t) x0 *c0;
 292           /* acc1 +=  x[1] * y[srcBLen - 1] */
 293           acc1 += (q63_t) x1 *c0;
 294           /* acc2 +=  x[2] * y[srcBLen - 1] */
 295           acc2 += (q63_t) x2 *c0;
 296
 297           /* Read y[srcBLen - 2] sample */
 298           c0 = *(py - 1u);
 299
 300           /* Read x[3] sample */
 301           x0 = *(px + 1u);
 302
 303           /* Perform the multiply-accumulate */
 304           /* acc0 +=  x[1] * y[srcBLen - 2] */
 305           acc0 += (q63_t) x1 *c0;
 306           /* acc1 +=  x[2] * y[srcBLen - 2] */
 307           acc1 += (q63_t) x2 *c0;
 308           /* acc2 +=  x[3] * y[srcBLen - 2] */
 309           acc2 += (q63_t) x0 *c0;
 310
 311           /* Read y[srcBLen - 3] sample */
 312           c0 = *(py - 2u);
 313
 314           /* Read x[4] sample */
 315           x1 = *(px + 2u);
 316
 317           /* Perform the multiply-accumulates */
 318           /* acc0 +=  x[2] * y[srcBLen - 3] */
 319           acc0 += (q63_t) x2 *c0;
 320           /* acc1 +=  x[3] * y[srcBLen - 2] */
 321           acc1 += (q63_t) x0 *c0;
 322           /* acc2 +=  x[4] * y[srcBLen - 2] */
 323           acc2 += (q63_t) x1 *c0;
 324
 325
 326           px += 3u;
 327
 328           py -= 3u;
 329
 330         } while(--k);
 331
 332         /* If the srcBLen is not a multiple of 3, compute any remaining MACs here.
 333          ** No loop unrolling is used. */
 334         k = srcBLen - (3 * (srcBLen / 3));
 335
 336         while(k > 0u)
 337         {
 338           /* Read y[srcBLen - 5] sample */
 339           c0 = *(py--);
 340
 341           /* Read x[7] sample */
 342           x2 = *(px++);
 343
 344           /* Perform the multiply-accumulates */
 345           /* acc0 +=  x[4] * y[srcBLen - 5] */
 346           acc0 += (q63_t) x0 *c0;
 347           /* acc1 +=  x[5] * y[srcBLen - 5] */
 348           acc1 += (q63_t) x1 *c0;
 349           /* acc2 +=  x[6] * y[srcBLen - 5] */
 350           acc2 += (q63_t) x2 *c0;
 351
 352           /* Reuse the present samples for the next MAC */
 353           x0 = x1;
 354           x1 = x2;
 355
 356           /* Decrement the loop counter */
 357           k--;
 358         }
 359
 360         /* Store the result in the accumulator in the destination buffer. */
 361         *pOut++ = (q31_t) (acc0 >> 31);
 362         *pOut++ = (q31_t) (acc1 >> 31);
 363         *pOut++ = (q31_t) (acc2 >> 31);
 364
 365         /* Increment the pointer pIn1 index, count by 3 */
 366         count += 3u;
 367
 368         /* Update the inputA and inputB pointers for next MAC calculation */
 369         px = pIn1 + count;
 370         py = pSrc2;
 371
 372         /* Decrement the loop counter */
 373         blkCnt--;
 374       }
 375
 376       /* If the blockSize2 is not a multiple of 3, compute any remaining output samples here.
 377        ** No loop unrolling is used. */
 378       blkCnt = blockSize2 - 3 * (blockSize2 / 3);
 379
 380       while(blkCnt > 0u)
 381       {
 382         /* Accumulator is made zero for every iteration */
 383         sum = 0;
 384
 385         /* Apply loop unrolling and compute 4 MACs simultaneously. */
 386         k = srcBLen >> 2u;
 387
 388         /* First part of the processing with loop unrolling.  Compute 4 MACs at a time.
 389          ** a second loop below computes MACs for the remaining 1 to 3 samples. */
 390         while(k > 0u)
 391         {
 392           /* Perform the multiply-accumulates */
 393           sum += (q63_t) * px++ * (*py--);
 394           sum += (q63_t) * px++ * (*py--);
 395           sum += (q63_t) * px++ * (*py--);
 396           sum += (q63_t) * px++ * (*py--);
 397
 398           /* Decrement the loop counter */
 399           k--;
 400         }
 401
 402         /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
 403          ** No loop unrolling is used. */
 404         k = srcBLen % 0x4u;
 405
 406         while(k > 0u)
 407         {
 408           /* Perform the multiply-accumulate */
 409           sum += (q63_t) * px++ * (*py--);
 410
 411           /* Decrement the loop counter */
 412           k--;
 413         }
 414
 415         /* Store the result in the accumulator in the destination buffer. */
 416         *pOut++ = (q31_t) (sum >> 31);
 417
 418         /* Increment the MAC count */
 419         count++;
 420
 421         /* Update the inputA and inputB pointers for next MAC calculation */
 422         px = pIn1 + count;
 423         py = pSrc2;
 424
 425         /* Decrement the loop counter */
 426         blkCnt--;
 427       }
 428     }
 429     else
 430     {
 431       /* If the srcBLen is not a multiple of 4,
 432        * the blockSize2 loop cannot be unrolled by 4 */
 433       blkCnt = (uint32_t) blockSize2;
 434
 435       while(blkCnt > 0u)
 436       {
 437         /* Accumulator is made zero for every iteration */
 438         sum = 0;
 439
 440         /* srcBLen number of MACS should be performed */
 441         k = srcBLen;
 442
 443         while(k > 0u)
 444         {
 445           /* Perform the multiply-accumulate */
 446           sum += (q63_t) * px++ * (*py--);
 447
 448           /* Decrement the loop counter */
 449           k--;
 450         }
 451
 452         /* Store the result in the accumulator in the destination buffer. */
 453         *pOut++ = (q31_t) (sum >> 31);
 454
 455         /* Increment the MAC count */
 456         count++;
 457
 458         /* Update the inputA and inputB pointers for next MAC calculation */
 459         px = pIn1 + count;
 460         py = pSrc2;
 461
 462         /* Decrement the loop counter */
 463         blkCnt--;
 464       }
 465     }
 466
 467
 468     /* --------------------------
 469      * Initializations of stage3
 470      * -------------------------*/
 471
 472     /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
 473      * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
 474      * ....
 475      * sum +=  x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
 476      * sum +=  x[srcALen-1] * y[srcBLen-1]
 477      */
 478
 479     /* In this stage the MAC operations are decreased by 1 for every iteration.
 480        The blockSize3 variable holds the number of MAC operations performed */
 481     count = srcBLen - 1u;
 482
 483     /* Working pointer of inputA */
 484     pSrc1 = (pIn1 + srcALen) - (srcBLen - 1u);
 485     px = pSrc1;
 486
 487     /* Working pointer of inputB */
 488     pSrc2 = pIn2 + (srcBLen - 1u);
 489     py = pSrc2;
 490
 491     /* -------------------
 492      * Stage3 process
 493      * ------------------*/
 494
 495     while(blockSize3 > 0)
 496     {
 497       /* Accumulator is made zero for every iteration */
 498       sum = 0;
 499
 500       /* Apply loop unrolling and compute 4 MACs simultaneously. */
 501       k = count >> 2u;
 502
 503       /* First part of the processing with loop unrolling.  Compute 4 MACs at a time.
 504        ** a second loop below computes MACs for the remaining 1 to 3 samples. */
 505       while(k > 0u)
 506       {
 507         sum += (q63_t) * px++ * (*py--);
 508         sum += (q63_t) * px++ * (*py--);
 509         sum += (q63_t) * px++ * (*py--);
 510         sum += (q63_t) * px++ * (*py--);
 511
 512         /* Decrement the loop counter */
 513         k--;
 514       }
 515
 516       /* If the blockSize3 is not a multiple of 4, compute any remaining MACs here.
 517        ** No loop unrolling is used. */
 518       k = count % 0x4u;
 519
 520       while(k > 0u)
 521       {
 522         /* Perform the multiply-accumulate */
 523         sum += (q63_t) * px++ * (*py--);
 524
 525         /* Decrement the loop counter */
 526         k--;
 527       }
 528
 529       /* Store the result in the accumulator in the destination buffer. */
 530       *pOut++ = (q31_t) (sum >> 31);
 531
 532       /* Update the inputA and inputB pointers for next MAC calculation */
 533       px = ++pSrc1;
 534       py = pSrc2;
 535
 536       /* Decrement the MAC count */
 537       count--;
 538
 539       /* Decrement the loop counter */
 540       blockSize3--;
 541
 542     }
 543
 544     /* set status as ARM_MATH_SUCCESS */
 545     status = ARM_MATH_SUCCESS;
 546   }
 547
 548   /* Return to application */
 549   return (status);
 550
 551 #else
 552
 553   /* Run the below code for Cortex-M0 */
 554
 555   q31_t *pIn1 = pSrcA;                           /* inputA pointer */
 556   q31_t *pIn2 = pSrcB;                           /* inputB pointer */
 557   q63_t sum;                                     /* Accumulator */
 558   uint32_t i, j;                                 /* loop counters */
 559   arm_status status;                             /* status of Partial convolution */
 560
 561   /* Check for range of output samples to be calculated */
 562   if((firstIndex + numPoints) > ((srcALen + (srcBLen - 1u))))
 563   {
 564     /* Set status as ARM_ARGUMENT_ERROR */
 565     status = ARM_MATH_ARGUMENT_ERROR;
 566   }
 567   else
 568   {
 569     /* Loop to calculate convolution for output length number of values */
 570     for (i = firstIndex; i <= (firstIndex + numPoints - 1); i++)
 571     {
 572       /* Initialize sum with zero to carry on MAC operations */
 573       sum = 0;
 574
 575       /* Loop to perform MAC operations according to convolution equation */
 576       for (j = 0; j <= i; j++)
 577       {
 578         /* Check the array limitations */
 579         if(((i - j) < srcBLen) && (j < srcALen))
 580         {
 581           /* z[i] += x[i-j] * y[j] */
 582           sum += ((q63_t) pIn1[j] * (pIn2[i - j]));
 583         }
 584       }
 585
 586       /* Store the output in the destination buffer */
 587       pDst[i] = (q31_t) (sum >> 31u);
 588     }
 589     /* set status as ARM_SUCCESS as there are no argument errors */
 590     status = ARM_MATH_SUCCESS;
 591   }
 592   return (status);
 593
 594 #endif /*    #ifndef ARM_MATH_CM0_FAMILY      */
 595
 596 }
 597
 598 /**
 599  * @} end of PartialConv group
 600  */