Keyboard firmwares for Atmel AVR and Cortex-M
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

arm_conv_q15.c 22KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734
  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_q15.c
  9. *
  10. * Description: Convolution of Q15 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. #include "arm_math.h"
  41. /**
  42. * @ingroup groupFilters
  43. */
  44. /**
  45. * @addtogroup Conv
  46. * @{
  47. */
  48. /**
  49. * @brief Convolution of Q15 sequences.
  50. * @param[in] *pSrcA points to the first input sequence.
  51. * @param[in] srcALen length of the first input sequence.
  52. * @param[in] *pSrcB points to the second input sequence.
  53. * @param[in] srcBLen length of the second input sequence.
  54. * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
  55. * @return none.
  56. *
  57. * @details
  58. * <b>Scaling and Overflow Behavior:</b>
  59. *
  60. * \par
  61. * The function is implemented using a 64-bit internal accumulator.
  62. * Both inputs are in 1.15 format and multiplications yield a 2.30 result.
  63. * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
  64. * This approach provides 33 guard bits and there is no risk of overflow.
  65. * The 34.30 result is then truncated to 34.15 format by discarding the low 15 bits and then saturated to 1.15 format.
  66. *
  67. * \par
  68. * Refer to <code>arm_conv_fast_q15()</code> for a faster but less precise version of this function for Cortex-M3 and Cortex-M4.
  69. *
  70. * \par
  71. * Refer the function <code>arm_conv_opt_q15()</code> for a faster implementation of this function using scratch buffers.
  72. *
  73. */
  74. void arm_conv_q15(
  75. q15_t * pSrcA,
  76. uint32_t srcALen,
  77. q15_t * pSrcB,
  78. uint32_t srcBLen,
  79. q15_t * pDst)
  80. {
  81. #if (defined(ARM_MATH_CM4) || defined(ARM_MATH_CM3)) && !defined(UNALIGNED_SUPPORT_DISABLE)
  82. /* Run the below code for Cortex-M4 and Cortex-M3 */
  83. q15_t *pIn1; /* inputA pointer */
  84. q15_t *pIn2; /* inputB pointer */
  85. q15_t *pOut = pDst; /* output pointer */
  86. q63_t sum, acc0, acc1, acc2, acc3; /* Accumulator */
  87. q15_t *px; /* Intermediate inputA pointer */
  88. q15_t *py; /* Intermediate inputB pointer */
  89. q15_t *pSrc1, *pSrc2; /* Intermediate pointers */
  90. q31_t x0, x1, x2, x3, c0; /* Temporary variables to hold state and coefficient values */
  91. uint32_t blockSize1, blockSize2, blockSize3, j, k, count, blkCnt; /* loop counter */
  92. /* The algorithm implementation is based on the lengths of the inputs. */
  93. /* srcB is always made to slide across srcA. */
  94. /* So srcBLen is always considered as shorter or equal to srcALen */
  95. if(srcALen >= srcBLen)
  96. {
  97. /* Initialization of inputA pointer */
  98. pIn1 = pSrcA;
  99. /* Initialization of inputB pointer */
  100. pIn2 = pSrcB;
  101. }
  102. else
  103. {
  104. /* Initialization of inputA pointer */
  105. pIn1 = pSrcB;
  106. /* Initialization of inputB pointer */
  107. pIn2 = pSrcA;
  108. /* srcBLen is always considered as shorter or equal to srcALen */
  109. j = srcBLen;
  110. srcBLen = srcALen;
  111. srcALen = j;
  112. }
  113. /* 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] */
  114. /* The function is internally
  115. * divided into three stages according to the number of multiplications that has to be
  116. * taken place between inputA samples and inputB samples. In the first stage of the
  117. * algorithm, the multiplications increase by one for every iteration.
  118. * In the second stage of the algorithm, srcBLen number of multiplications are done.
  119. * In the third stage of the algorithm, the multiplications decrease by one
  120. * for every iteration. */
  121. /* The algorithm is implemented in three stages.
  122. The loop counters of each stage is initiated here. */
  123. blockSize1 = srcBLen - 1u;
  124. blockSize2 = srcALen - (srcBLen - 1u);
  125. /* --------------------------
  126. * Initializations of stage1
  127. * -------------------------*/
  128. /* sum = x[0] * y[0]
  129. * sum = x[0] * y[1] + x[1] * y[0]
  130. * ....
  131. * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
  132. */
  133. /* In this stage the MAC operations are increased by 1 for every iteration.
  134. The count variable holds the number of MAC operations performed */
  135. count = 1u;
  136. /* Working pointer of inputA */
  137. px = pIn1;
  138. /* Working pointer of inputB */
  139. py = pIn2;
  140. /* ------------------------
  141. * Stage1 process
  142. * ----------------------*/
  143. /* For loop unrolling by 4, this stage is divided into two. */
  144. /* First part of this stage computes the MAC operations less than 4 */
  145. /* Second part of this stage computes the MAC operations greater than or equal to 4 */
  146. /* The first part of the stage starts here */
  147. while((count < 4u) && (blockSize1 > 0u))
  148. {
  149. /* Accumulator is made zero for every iteration */
  150. sum = 0;
  151. /* Loop over number of MAC operations between
  152. * inputA samples and inputB samples */
  153. k = count;
  154. while(k > 0u)
  155. {
  156. /* Perform the multiply-accumulates */
  157. sum = __SMLALD(*px++, *py--, sum);
  158. /* Decrement the loop counter */
  159. k--;
  160. }
  161. /* Store the result in the accumulator in the destination buffer. */
  162. *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
  163. /* Update the inputA and inputB pointers for next MAC calculation */
  164. py = pIn2 + count;
  165. px = pIn1;
  166. /* Increment the MAC count */
  167. count++;
  168. /* Decrement the loop counter */
  169. blockSize1--;
  170. }
  171. /* The second part of the stage starts here */
  172. /* The internal loop, over count, is unrolled by 4 */
  173. /* To, read the last two inputB samples using SIMD:
  174. * y[srcBLen] and y[srcBLen-1] coefficients, py is decremented by 1 */
  175. py = py - 1;
  176. while(blockSize1 > 0u)
  177. {
  178. /* Accumulator is made zero for every iteration */
  179. sum = 0;
  180. /* Apply loop unrolling and compute 4 MACs simultaneously. */
  181. k = count >> 2u;
  182. /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
  183. ** a second loop below computes MACs for the remaining 1 to 3 samples. */
  184. while(k > 0u)
  185. {
  186. /* Perform the multiply-accumulates */
  187. /* x[0], x[1] are multiplied with y[srcBLen - 1], y[srcBLen - 2] respectively */
  188. sum = __SMLALDX(*__SIMD32(px)++, *__SIMD32(py)--, sum);
  189. /* x[2], x[3] are multiplied with y[srcBLen - 3], y[srcBLen - 4] respectively */
  190. sum = __SMLALDX(*__SIMD32(px)++, *__SIMD32(py)--, sum);
  191. /* Decrement the loop counter */
  192. k--;
  193. }
  194. /* For the next MAC operations, the pointer py is used without SIMD
  195. * So, py is incremented by 1 */
  196. py = py + 1u;
  197. /* If the count is not a multiple of 4, compute any remaining MACs here.
  198. ** No loop unrolling is used. */
  199. k = count % 0x4u;
  200. while(k > 0u)
  201. {
  202. /* Perform the multiply-accumulates */
  203. sum = __SMLALD(*px++, *py--, sum);
  204. /* Decrement the loop counter */
  205. k--;
  206. }
  207. /* Store the result in the accumulator in the destination buffer. */
  208. *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
  209. /* Update the inputA and inputB pointers for next MAC calculation */
  210. py = pIn2 + (count - 1u);
  211. px = pIn1;
  212. /* Increment the MAC count */
  213. count++;
  214. /* Decrement the loop counter */
  215. blockSize1--;
  216. }
  217. /* --------------------------
  218. * Initializations of stage2
  219. * ------------------------*/
  220. /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
  221. * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
  222. * ....
  223. * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
  224. */
  225. /* Working pointer of inputA */
  226. px = pIn1;
  227. /* Working pointer of inputB */
  228. pSrc2 = pIn2 + (srcBLen - 1u);
  229. py = pSrc2;
  230. /* count is the index by which the pointer pIn1 to be incremented */
  231. count = 0u;
  232. /* --------------------
  233. * Stage2 process
  234. * -------------------*/
  235. /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
  236. * So, to loop unroll over blockSize2,
  237. * srcBLen should be greater than or equal to 4 */
  238. if(srcBLen >= 4u)
  239. {
  240. /* Loop unroll over blockSize2, by 4 */
  241. blkCnt = blockSize2 >> 2u;
  242. while(blkCnt > 0u)
  243. {
  244. py = py - 1u;
  245. /* Set all accumulators to zero */
  246. acc0 = 0;
  247. acc1 = 0;
  248. acc2 = 0;
  249. acc3 = 0;
  250. /* read x[0], x[1] samples */
  251. x0 = *__SIMD32(px);
  252. /* read x[1], x[2] samples */
  253. x1 = _SIMD32_OFFSET(px+1);
  254. px+= 2u;
  255. /* Apply loop unrolling and compute 4 MACs simultaneously. */
  256. k = srcBLen >> 2u;
  257. /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
  258. ** a second loop below computes MACs for the remaining 1 to 3 samples. */
  259. do
  260. {
  261. /* Read the last two inputB samples using SIMD:
  262. * y[srcBLen - 1] and y[srcBLen - 2] */
  263. c0 = *__SIMD32(py)--;
  264. /* acc0 += x[0] * y[srcBLen - 1] + x[1] * y[srcBLen - 2] */
  265. acc0 = __SMLALDX(x0, c0, acc0);
  266. /* acc1 += x[1] * y[srcBLen - 1] + x[2] * y[srcBLen - 2] */
  267. acc1 = __SMLALDX(x1, c0, acc1);
  268. /* Read x[2], x[3] */
  269. x2 = *__SIMD32(px);
  270. /* Read x[3], x[4] */
  271. x3 = _SIMD32_OFFSET(px+1);
  272. /* acc2 += x[2] * y[srcBLen - 1] + x[3] * y[srcBLen - 2] */
  273. acc2 = __SMLALDX(x2, c0, acc2);
  274. /* acc3 += x[3] * y[srcBLen - 1] + x[4] * y[srcBLen - 2] */
  275. acc3 = __SMLALDX(x3, c0, acc3);
  276. /* Read y[srcBLen - 3] and y[srcBLen - 4] */
  277. c0 = *__SIMD32(py)--;
  278. /* acc0 += x[2] * y[srcBLen - 3] + x[3] * y[srcBLen - 4] */
  279. acc0 = __SMLALDX(x2, c0, acc0);
  280. /* acc1 += x[3] * y[srcBLen - 3] + x[4] * y[srcBLen - 4] */
  281. acc1 = __SMLALDX(x3, c0, acc1);
  282. /* Read x[4], x[5] */
  283. x0 = _SIMD32_OFFSET(px+2);
  284. /* Read x[5], x[6] */
  285. x1 = _SIMD32_OFFSET(px+3);
  286. px += 4u;
  287. /* acc2 += x[4] * y[srcBLen - 3] + x[5] * y[srcBLen - 4] */
  288. acc2 = __SMLALDX(x0, c0, acc2);
  289. /* acc3 += x[5] * y[srcBLen - 3] + x[6] * y[srcBLen - 4] */
  290. acc3 = __SMLALDX(x1, c0, acc3);
  291. } while(--k);
  292. /* For the next MAC operations, SIMD is not used
  293. * So, the 16 bit pointer if inputB, py is updated */
  294. /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
  295. ** No loop unrolling is used. */
  296. k = srcBLen % 0x4u;
  297. if(k == 1u)
  298. {
  299. /* Read y[srcBLen - 5] */
  300. c0 = *(py+1);
  301. #ifdef ARM_MATH_BIG_ENDIAN
  302. c0 = c0 << 16u;
  303. #else
  304. c0 = c0 & 0x0000FFFF;
  305. #endif /* #ifdef ARM_MATH_BIG_ENDIAN */
  306. /* Read x[7] */
  307. x3 = *__SIMD32(px);
  308. px++;
  309. /* Perform the multiply-accumulates */
  310. acc0 = __SMLALD(x0, c0, acc0);
  311. acc1 = __SMLALD(x1, c0, acc1);
  312. acc2 = __SMLALDX(x1, c0, acc2);
  313. acc3 = __SMLALDX(x3, c0, acc3);
  314. }
  315. if(k == 2u)
  316. {
  317. /* Read y[srcBLen - 5], y[srcBLen - 6] */
  318. c0 = _SIMD32_OFFSET(py);
  319. /* Read x[7], x[8] */
  320. x3 = *__SIMD32(px);
  321. /* Read x[9] */
  322. x2 = _SIMD32_OFFSET(px+1);
  323. px += 2u;
  324. /* Perform the multiply-accumulates */
  325. acc0 = __SMLALDX(x0, c0, acc0);
  326. acc1 = __SMLALDX(x1, c0, acc1);
  327. acc2 = __SMLALDX(x3, c0, acc2);
  328. acc3 = __SMLALDX(x2, c0, acc3);
  329. }
  330. if(k == 3u)
  331. {
  332. /* Read y[srcBLen - 5], y[srcBLen - 6] */
  333. c0 = _SIMD32_OFFSET(py);
  334. /* Read x[7], x[8] */
  335. x3 = *__SIMD32(px);
  336. /* Read x[9] */
  337. x2 = _SIMD32_OFFSET(px+1);
  338. /* Perform the multiply-accumulates */
  339. acc0 = __SMLALDX(x0, c0, acc0);
  340. acc1 = __SMLALDX(x1, c0, acc1);
  341. acc2 = __SMLALDX(x3, c0, acc2);
  342. acc3 = __SMLALDX(x2, c0, acc3);
  343. c0 = *(py-1);
  344. #ifdef ARM_MATH_BIG_ENDIAN
  345. c0 = c0 << 16u;
  346. #else
  347. c0 = c0 & 0x0000FFFF;
  348. #endif /* #ifdef ARM_MATH_BIG_ENDIAN */
  349. /* Read x[10] */
  350. x3 = _SIMD32_OFFSET(px+2);
  351. px += 3u;
  352. /* Perform the multiply-accumulates */
  353. acc0 = __SMLALDX(x1, c0, acc0);
  354. acc1 = __SMLALD(x2, c0, acc1);
  355. acc2 = __SMLALDX(x2, c0, acc2);
  356. acc3 = __SMLALDX(x3, c0, acc3);
  357. }
  358. /* Store the results in the accumulators in the destination buffer. */
  359. #ifndef ARM_MATH_BIG_ENDIAN
  360. *__SIMD32(pOut)++ =
  361. __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16);
  362. *__SIMD32(pOut)++ =
  363. __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16);
  364. #else
  365. *__SIMD32(pOut)++ =
  366. __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16);
  367. *__SIMD32(pOut)++ =
  368. __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16);
  369. #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
  370. /* Increment the pointer pIn1 index, count by 4 */
  371. count += 4u;
  372. /* Update the inputA and inputB pointers for next MAC calculation */
  373. px = pIn1 + count;
  374. py = pSrc2;
  375. /* Decrement the loop counter */
  376. blkCnt--;
  377. }
  378. /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
  379. ** No loop unrolling is used. */
  380. blkCnt = blockSize2 % 0x4u;
  381. while(blkCnt > 0u)
  382. {
  383. /* Accumulator is made zero for every iteration */
  384. sum = 0;
  385. /* Apply loop unrolling and compute 4 MACs simultaneously. */
  386. k = srcBLen >> 2u;
  387. /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
  388. ** a second loop below computes MACs for the remaining 1 to 3 samples. */
  389. while(k > 0u)
  390. {
  391. /* Perform the multiply-accumulates */
  392. sum += (q63_t) ((q31_t) * px++ * *py--);
  393. sum += (q63_t) ((q31_t) * px++ * *py--);
  394. sum += (q63_t) ((q31_t) * px++ * *py--);
  395. sum += (q63_t) ((q31_t) * px++ * *py--);
  396. /* Decrement the loop counter */
  397. k--;
  398. }
  399. /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
  400. ** No loop unrolling is used. */
  401. k = srcBLen % 0x4u;
  402. while(k > 0u)
  403. {
  404. /* Perform the multiply-accumulates */
  405. sum += (q63_t) ((q31_t) * px++ * *py--);
  406. /* Decrement the loop counter */
  407. k--;
  408. }
  409. /* Store the result in the accumulator in the destination buffer. */
  410. *pOut++ = (q15_t) (__SSAT(sum >> 15, 16));
  411. /* Increment the pointer pIn1 index, count by 1 */
  412. count++;
  413. /* Update the inputA and inputB pointers for next MAC calculation */
  414. px = pIn1 + count;
  415. py = pSrc2;
  416. /* Decrement the loop counter */
  417. blkCnt--;
  418. }
  419. }
  420. else
  421. {
  422. /* If the srcBLen is not a multiple of 4,
  423. * the blockSize2 loop cannot be unrolled by 4 */
  424. blkCnt = blockSize2;
  425. while(blkCnt > 0u)
  426. {
  427. /* Accumulator is made zero for every iteration */
  428. sum = 0;
  429. /* srcBLen number of MACS should be performed */
  430. k = srcBLen;
  431. while(k > 0u)
  432. {
  433. /* Perform the multiply-accumulate */
  434. sum += (q63_t) ((q31_t) * px++ * *py--);
  435. /* Decrement the loop counter */
  436. k--;
  437. }
  438. /* Store the result in the accumulator in the destination buffer. */
  439. *pOut++ = (q15_t) (__SSAT(sum >> 15, 16));
  440. /* Increment the MAC count */
  441. count++;
  442. /* Update the inputA and inputB pointers for next MAC calculation */
  443. px = pIn1 + count;
  444. py = pSrc2;
  445. /* Decrement the loop counter */
  446. blkCnt--;
  447. }
  448. }
  449. /* --------------------------
  450. * Initializations of stage3
  451. * -------------------------*/
  452. /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
  453. * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
  454. * ....
  455. * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
  456. * sum += x[srcALen-1] * y[srcBLen-1]
  457. */
  458. /* In this stage the MAC operations are decreased by 1 for every iteration.
  459. The blockSize3 variable holds the number of MAC operations performed */
  460. blockSize3 = srcBLen - 1u;
  461. /* Working pointer of inputA */
  462. pSrc1 = (pIn1 + srcALen) - (srcBLen - 1u);
  463. px = pSrc1;
  464. /* Working pointer of inputB */
  465. pSrc2 = pIn2 + (srcBLen - 1u);
  466. pIn2 = pSrc2 - 1u;
  467. py = pIn2;
  468. /* -------------------
  469. * Stage3 process
  470. * ------------------*/
  471. /* For loop unrolling by 4, this stage is divided into two. */
  472. /* First part of this stage computes the MAC operations greater than 4 */
  473. /* Second part of this stage computes the MAC operations less than or equal to 4 */
  474. /* The first part of the stage starts here */
  475. j = blockSize3 >> 2u;
  476. while((j > 0u) && (blockSize3 > 0u))
  477. {
  478. /* Accumulator is made zero for every iteration */
  479. sum = 0;
  480. /* Apply loop unrolling and compute 4 MACs simultaneously. */
  481. k = blockSize3 >> 2u;
  482. /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
  483. ** a second loop below computes MACs for the remaining 1 to 3 samples. */
  484. while(k > 0u)
  485. {
  486. /* x[srcALen - srcBLen + 1], x[srcALen - srcBLen + 2] are multiplied
  487. * with y[srcBLen - 1], y[srcBLen - 2] respectively */
  488. sum = __SMLALDX(*__SIMD32(px)++, *__SIMD32(py)--, sum);
  489. /* x[srcALen - srcBLen + 3], x[srcALen - srcBLen + 4] are multiplied
  490. * with y[srcBLen - 3], y[srcBLen - 4] respectively */
  491. sum = __SMLALDX(*__SIMD32(px)++, *__SIMD32(py)--, sum);
  492. /* Decrement the loop counter */
  493. k--;
  494. }
  495. /* For the next MAC operations, the pointer py is used without SIMD
  496. * So, py is incremented by 1 */
  497. py = py + 1u;
  498. /* If the blockSize3 is not a multiple of 4, compute any remaining MACs here.
  499. ** No loop unrolling is used. */
  500. k = blockSize3 % 0x4u;
  501. while(k > 0u)
  502. {
  503. /* sum += x[srcALen - srcBLen + 5] * y[srcBLen - 5] */
  504. sum = __SMLALD(*px++, *py--, sum);
  505. /* Decrement the loop counter */
  506. k--;
  507. }
  508. /* Store the result in the accumulator in the destination buffer. */
  509. *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
  510. /* Update the inputA and inputB pointers for next MAC calculation */
  511. px = ++pSrc1;
  512. py = pIn2;
  513. /* Decrement the loop counter */
  514. blockSize3--;
  515. j--;
  516. }
  517. /* The second part of the stage starts here */
  518. /* SIMD is not used for the next MAC operations,
  519. * so pointer py is updated to read only one sample at a time */
  520. py = py + 1u;
  521. while(blockSize3 > 0u)
  522. {
  523. /* Accumulator is made zero for every iteration */
  524. sum = 0;
  525. /* Apply loop unrolling and compute 4 MACs simultaneously. */
  526. k = blockSize3;
  527. while(k > 0u)
  528. {
  529. /* Perform the multiply-accumulates */
  530. /* sum += x[srcALen-1] * y[srcBLen-1] */
  531. sum = __SMLALD(*px++, *py--, sum);
  532. /* Decrement the loop counter */
  533. k--;
  534. }
  535. /* Store the result in the accumulator in the destination buffer. */
  536. *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
  537. /* Update the inputA and inputB pointers for next MAC calculation */
  538. px = ++pSrc1;
  539. py = pSrc2;
  540. /* Decrement the loop counter */
  541. blockSize3--;
  542. }
  543. #else
  544. /* Run the below code for Cortex-M0 */
  545. q15_t *pIn1 = pSrcA; /* input pointer */
  546. q15_t *pIn2 = pSrcB; /* coefficient pointer */
  547. q63_t sum; /* Accumulator */
  548. uint32_t i, j; /* loop counter */
  549. /* Loop to calculate output of convolution for output length number of times */
  550. for (i = 0; i < (srcALen + srcBLen - 1); i++)
  551. {
  552. /* Initialize sum with zero to carry on MAC operations */
  553. sum = 0;
  554. /* Loop to perform MAC operations according to convolution equation */
  555. for (j = 0; j <= i; j++)
  556. {
  557. /* Check the array limitations */
  558. if(((i - j) < srcBLen) && (j < srcALen))
  559. {
  560. /* z[i] += x[i-j] * y[j] */
  561. sum += (q31_t) pIn1[j] * (pIn2[i - j]);
  562. }
  563. }
  564. /* Store the output in the destination buffer */
  565. pDst[i] = (q15_t) __SSAT((sum >> 15u), 16u);
  566. }
  567. #endif /* #if (defined(ARM_MATH_CM4) || defined(ARM_MATH_CM3)) && !defined(UNALIGNED_SUPPORT_DISABLE)*/
  568. }
  569. /**
  570. * @} end of Conv group
  571. */