void arm_fill_q15(
q15_t value,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q31_t packedValue; /* value packed to 32 bits */
/*loop Unrolling */
blkCnt = blockSize >> 3u;
/* Packing two 16 bit values to 32 bit value in order to use SIMD */
packedValue = __PKHBT(value, value, 16u);
/* First part of the processing with loop unrolling. Compute 8 outputs at a time.
** a second loop below computes the remaining 1 to 7 samples. */
while(blkCnt > 0u)
{
/* C = value */
/* Fill the value in the destination buffer */
_SIMD32_OFFSET(pDst) = packedValue;
_SIMD32_OFFSET(pDst + 2) = packedValue;
_SIMD32_OFFSET(pDst + 4) = packedValue;
_SIMD32_OFFSET(pDst + 6) = packedValue;
pDst += 8u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 8, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x8u;
while(blkCnt > 0u)
{
/* C = value */
/* Fill the value in the destination buffer */
*pDst++ = value;
/* Decrement the loop counter */
blkCnt--;
}
}
void arm_negate_q15(
q15_t * pSrc,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q15_t in;
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2; /* Temporary variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = -A */
/* Read two inputs at a time */
in1 = _SIMD32_OFFSET(pSrc);
in2 = _SIMD32_OFFSET(pSrc + 2);
/* negate two samples at a time */
in1 = __QSUB16(0, in1);
/* negate two samples at a time */
in2 = __QSUB16(0, in2);
/* store the result to destination 2 samples at a time */
_SIMD32_OFFSET(pDst) = in1;
/* store the result to destination 2 samples at a time */
_SIMD32_OFFSET(pDst + 2) = in2;
/* update pointers to process next samples */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = -A */
/* Negate and then store the result in the destination buffer. */
in = *pSrc++;
*pDst++ = (in == (q15_t) 0x8000) ? 0x7fff : -in;
/* Decrement the loop counter */
blkCnt--;
}
}
void arm_sub_q7(
q7_t * pSrcA,
q7_t * pSrcB,
q7_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q31_t inA1, inB1, inA2, inB2; /* temporary input variabels */
q7_t inA, inB; /* temporary variables */
q31_t out1, out2, out3, out4; /* temporary output variables */
/*loop Unrolling */
blkCnt = blockSize >> 4u;
/* First part of the processing with loop unrolling. Compute 16 outputs at a time.
** a second loop below computes the remaining 1 to 15 samples. */
while(blkCnt > 0u)
{
/* C = A - B */
/* Subtract and then store the results in the destination buffer 4 samples at a time. */
/* read 4 samples at a time from sourceA */
inA1 = _SIMD32_OFFSET(pSrcA);
/* read 4 samples at a time from sourceB */
inB1 = _SIMD32_OFFSET(pSrcB);
/* read 4 samples at a time from sourceA */
inA2 = _SIMD32_OFFSET(pSrcA + 4);
/* out = saturate(sourceA - sourceB) four samples at a time */
out1 = __QSUB8(inA1, inB1);
/* read 4 samples at a time from sourceB */
inB2 = _SIMD32_OFFSET(pSrcB + 4);
/* store result to destination four samples at a time */
_SIMD32_OFFSET(pDst) = out1;
/* out = saturate(sourceA - sourceB) four samples at a time */
out2 = __QSUB8(inA2, inB2);
/* read 4 samples at a time from sourceA */
inA1 = _SIMD32_OFFSET(pSrcA + 8);
/* read 4 samples at a time from sourceB */
inB1 = _SIMD32_OFFSET(pSrcB + 8);
/* read 4 samples at a time from sourceA */
inA2 = _SIMD32_OFFSET(pSrcA + 12);
/* out = saturate(sourceA - sourceB) four samples at a time */
out3 = __QSUB8(inA1, inB1);
/* read 4 samples at a time from sourceB */
inB2 = _SIMD32_OFFSET(pSrcB + 12);
/* increment sourceA pointer by 16 to process next samples */
pSrcA += 16u;
/* store result to destination four samples at a time */
_SIMD32_OFFSET(pDst + 4) = out2;
/* out = saturate(sourceA - sourceB) four samples at a time */
out4 = __QSUB8(inA2, inB2);
/* store result to destination four samples at a time */
_SIMD32_OFFSET(pDst + 8) = out3;
/* Update source pointer to process next sampels */
pSrcB += 16u;
/* store result to destination four samples at a time */
_SIMD32_OFFSET(pDst + 12) = out4;
/* Update destination pointer to process next sampels */
pDst += 16u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 16, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x10u;
while(blkCnt > 0u)
{
/* C = A - B */
/* Subtract and then store the result in the destination buffer. */
inA = *pSrcA++;
inB = *pSrcB++;
#ifdef CCS
*pDst++ = __SSATA(inA - inB, 0, 8);
#else
*pDst++ = __SSAT(inA - inB, 8);
#endif //#ifdef CCS
/* Decrement the loop counter */
blkCnt--;
}
}
void arm_dot_prod_q15(
q15_t * pSrcA,
q15_t * pSrcB,
uint32_t blockSize,
q63_t * result)
{
q63_t sum = 0; /* Temporary result storage */
uint32_t blkCnt; /* loop counter */
q31_t inA1, inA2, inB1, inB2; /* Temporary variables to store input data */
q31_t inA3, inA4, inB3, inB4;
/*loop Unrolling */
blkCnt = blockSize >> 3u;
/* First part of the processing with loop unrolling. Compute 8 outputs at a time.
** a second loop below computes the remaining 1 to 7 samples. */
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the result in a temporary buffer. */
/* read two samples at a time from soruceA buffer */
inA1 = _SIMD32_OFFSET(pSrcA);
/* read two samples at a time from soruceB buffer */
inB1 = _SIMD32_OFFSET(pSrcB);
/* read two samples at a time from soruceA buffer */
inA2 = _SIMD32_OFFSET(pSrcA+2);
/* multiply and accumulate two samples at a time */
sum = __SMLALD(inA1, inB1, sum);
/* read two samples at a time from soruceB buffer */
inB2 = _SIMD32_OFFSET(pSrcB+2);
/* read two samples at a time from soruceA buffer */
inA3 = _SIMD32_OFFSET(pSrcA+4);
/* read two samples at a time from soruceB buffer */
inB3 = _SIMD32_OFFSET(pSrcB+4);
/* multiply and accumulate two samples at a time */
sum = __SMLALD(inA2, inB2, sum);
/* read two samples at a time from soruceA buffer */
inA4 = _SIMD32_OFFSET(pSrcA+6);
/* read two samples at a time from soruceB buffer */
inB4 = _SIMD32_OFFSET(pSrcB+6);
/* increment source A buffer by 8 */
pSrcA += 8u;
/* increment sourceB buffer by 8 */
pSrcB += 8u;
/* multiply and accumulate two samples at a time */
sum = __SMLALD(inA3, inB3, sum);
sum = __SMLALD(inA4, inB4, sum);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 8, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x8u;
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the results in a temporary buffer. */
sum = __SMLALD(*pSrcA++, *pSrcB++, sum);
/* Decrement the loop counter */
blkCnt--;
}
/* Store the result in the destination buffer in 34.30 format */
*result = sum;
}
void arm_q7_to_q15(
q7_t * pSrc,
q15_t * pDst,
uint32_t blockSize)
{
q7_t *pIn = pSrc; /* Src pointer */
uint32_t blkCnt; /* loop counter */
q31_t in;
q31_t in1, in2;
q31_t out1, out2;
q31_t and = 0xFF00FF00;
/*loop Unrolling */
blkCnt = blockSize >> 3u;
/* First part of the processing with loop unrolling. Compute 8 outputs at a time.
** a second loop below computes the remaining 1 to 7 samples. */
while(blkCnt > 0u)
{
/* C = (q15_t) A << 8 */
/* convert from q7 to q15 and then store the results in the destination buffer */
/* read 4 samples at a time */
in = *__SIMD32(pIn)++;
#ifdef CCS
/* rotatate in by 8 and extend two q7_t values to q15_t values */
in1 = __SXTB16(in, 8);
/* extend remainig two q7_t values to q15_t values */
in2 = __SXTB16(in, 0);
#else
/* rotatate in by 8 and extend two q7_t values to q15_t values */
in1 = __SXTB16(__ROR(in, 8));
/* extend remainig two q7_t values to q15_t values */
in2 = __SXTB16(in);
#endif /* shift in1 by 8 to convert q7_t value to q15_t value (ex: 0x00ff00ff ==> 0xff00ff00*/
in1 = in1 << 8u;
in2 = in2 << 8u;
/* read next 4 sampels */
in = *__SIMD32(pIn)++;
/* anding with 0xff00ff00 */
in1 = in1 & and;
out2 = in2 & and;
/* pack two 16 bit values */
out1 = __PKHTB(in1, out2, 16);
out2 = __PKHBT(out2, in1, 16);
#ifndef ARM_MATH_BIG_ENDIAN
/* store two q15_t samples at a time to destination */
_SIMD32_OFFSET(pDst + 2) = out1;
#ifdef CCS
/* rotatate in by 8 and extend two q7_t values to q15_t values */
in1 = __SXTB16(in, 8);
#else
/* rotatate in by 8 and extend two q7_t values to q15_t values */
in1 = __SXTB16(__ROR(in, 8));
#endif
/* store two q15_t samples at a time to destination */
_SIMD32_OFFSET(pDst) = out2;
#else
/* store two q15_t samples at a time to destination */
_SIMD32_OFFSET(pDst) = out1;
#ifdef CCS
/* rotatate in by 8 and extend two q7_t values to q15_t values */
in1 = __SXTB16(in, 8);
#else
/* rotatate in by 8 and extend two q7_t values to q15_t values */
in1 = __SXTB16(__ROR(in, 8));
#endif
/* store two q15_t samples at a time to destination */
_SIMD32_OFFSET(pDst + 2) = out2;
#endif // #ifndef ARM_MATH_BIG_ENDIAN
#ifdef CCS
/* rotatate in by 8 and extend two q7_t values to q15_t values */
in2 = __SXTB16(in, 0);
#else
/* rotatate in by 8 and extend two q7_t values to q15_t values */
in2 = __SXTB16(in);
#endif
/* shift in1 by 8 to convert q7_t value to q15_t value (ex: 0x00ff00ff ==> 0xff00ff00*/
in1 = in1 << 8u;
in2 = in2 << 8u;
/* anding with 0xff00ff00 */
out1 = in1 & and;
out2 = in2 & and;
/* pack two 16 bit values */
out1 = __PKHTB(in1, out2, 16);
out2 = __PKHBT(out2, in1, 16);
/* store two q15_t samples at a time to destination */
#ifndef ARM_MATH_BIG_ENDIAN
_SIMD32_OFFSET(pDst + 6) = out1;
_SIMD32_OFFSET(pDst + 4) = out2;
#else
_SIMD32_OFFSET(pDst + 4) = out1;
_SIMD32_OFFSET(pDst + 6) = out2;
#endif // #ifndef ARM_MATH_BIG_ENDIAN
/* incremnet destination pointer */
pDst += 8u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 8, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x8u;
while(blkCnt > 0u)
{
/* C = (q15_t) A << 8 */
/* convert from q7 to q15 and then store the results in the destination buffer */
*pDst++ = (q15_t) * pIn++ << 8;
/* Decrement the loop counter */
blkCnt--;
}
}
void arm_power_q7(
q7_t * pSrc,
uint32_t blockSize,
q31_t * pResult)
{
q31_t acc = 0; /* Temporary result storage */
q31_t input1; /* Temporary variable to store packed input */
q7_t in; /* Temporary variable to store input */
uint32_t blkCnt; /* loop counter */
q31_t inA1, inA2; /* Temporary variables to hold intermiediate data */
q31_t acc1 = 0;
/*loop Unrolling */
blkCnt = blockSize >> 3u;
/* First part of the processing with loop unrolling. Compute 8 outputs at a time.
** a second loop below computes the remaining 1 to 7 samples. */
while(blkCnt > 0u)
{
/* read four samples at a time from soruce buffer */
input1 = _SIMD32_OFFSET(pSrc);
/* extend two q7_t values to q15_t values */
#ifdef CCS
inA1 = __SXTB16(input1, 8);
inA2 = __SXTB16(input1, 0);
#else
inA1 = __SXTB16(__ROR(input1, 8));
inA2 = __SXTB16(input1);
#endif // #ifdef CCS
/* calculate power and accumulate to accumulator */
acc = __SMLAD(inA1, inA1, acc);
/* read four samples at a time from soruce buffer */
input1 = _SIMD32_OFFSET(pSrc + 4);
#ifdef CCS
/* extend two q7_t values to q15_t values */
inA1 = __SXTB16(input1, 8);
/* calculate power and accumulate to accumulator */
acc1 = __SMLAD(inA2, inA2, acc1);
/* extend two q7_t values to q15_t values */
inA2 = __SXTB16(input1, 0);
#else
/* extend two q7_t values to q15_t values */
inA1 = __SXTB16(__ROR(input1, 8));
/* calculate power and accumulate to accumulator */
acc1 = __SMLAD(inA2, inA2, acc1);
/* extend two q7_t values to q15_t values */
inA2 = __SXTB16(input1);
#endif // #ifdef CCS
/* calculate power and accumulate to accumulator */
acc = __SMLAD(inA1, inA1, acc);
acc1 = __SMLAD(inA2, inA2, acc1);
/* update source buffer to process next samples */
pSrc += 8u;
/* Decrement the loop counter */
blkCnt--;
}
/* add accumulators */
acc = acc + acc1;
/* If the blockSize is not a multiple of 8, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x8u;
while(blkCnt > 0u)
{
/* C = A[0] * A[0] + A[1] * A[1] + A[2] * A[2] + ... + A[blockSize-1] * A[blockSize-1] */
/* Compute Power and then store the result in a temporary variable, acc. */
in = *pSrc++;
acc += ((q15_t) in * in);
/* Decrement the loop counter */
blkCnt--;
}
/* Store the result in 18.14 format */
*pResult = acc;
}
