/**
\brief Test case: TC_CoreSimd_SatAddSub
\details
- Check Saturating addition and subtraction:
__QADD
__QSUB
*/
void TC_CoreSimd_SatAddSub (void) {
#if ((defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
(defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1)) )
volatile int32_t op1_s32, op2_s32;
volatile int32_t res_s32;
/* --- __QADD Test ---------------------------------------------- */
op1_s32 = (int32_t)0x80000003;
op2_s32 = (int32_t)0x00000004;
res_s32 = __QADD(op1_s32, op2_s32);
ASSERT_TRUE(res_s32 == (int32_t)0x80000007);
op1_s32 = (int32_t)0x80000000;
op2_s32 = (int32_t)0x80000002;
res_s32 = __QADD(op1_s32, op2_s32);
ASSERT_TRUE(res_s32 == (int32_t)0x80000000);
/* --- __QSUB Test ---------------------------------------------- */
op1_s32 = (int32_t)0x80000003;
op2_s32 = (int32_t)0x00000004;
res_s32 = __QSUB(op1_s32, op2_s32);
ASSERT_TRUE(res_s32 == (int32_t)0x80000000);
op1_s32 = (int32_t)0x80000003;
op2_s32 = (int32_t)0x00000002;
res_s32 = __QSUB(op1_s32, op2_s32);
ASSERT_TRUE(res_s32 == (int32_t)0x80000001);
#endif
}
void arm_pid_init_q31(
arm_pid_instance_q31 * S,
int32_t resetStateFlag)
{
#ifndef ARM_MATH_CM0_FAMILY
/* Run the below code for Cortex-M4 and Cortex-M3 */
/* Derived coefficient A0 */
S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd);
/* Derived coefficient A1 */
S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp);
#else
/* Run the below code for Cortex-M0 */
q31_t temp;
/* Derived coefficient A0 */
temp = clip_q63_to_q31((q63_t) S->Kp + S->Ki);
S->A0 = clip_q63_to_q31((q63_t) temp + S->Kd);
/* Derived coefficient A1 */
temp = clip_q63_to_q31((q63_t) S->Kd + S->Kd);
S->A1 = -clip_q63_to_q31((q63_t) temp + S->Kp);
#endif /* #ifndef ARM_MATH_CM0_FAMILY */
/* Derived coefficient A2 */
S->A2 = S->Kd;
/* Check whether state needs reset or not */
if(resetStateFlag)
{
/* Clear the state buffer. The size will be always 3 samples */
memset(S->state, 0, 3u * sizeof(q31_t));
}
}
void arm_add_q31(
q31_t * pSrcA,
q31_t * pSrcB,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
/*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 + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
/* 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;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
}
void arm_pid_init_q31(
arm_pid_instance_q31 * S,
int32_t resetStateFlag)
{
/* Derived coefficient A0 */
S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd);
/* Derived coefficient A1 */
S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp);
/* Derived coefficient A2 */
S->A2 = S->Kd;
/* Check whether state needs reset or not */
if(resetStateFlag)
{
/* Clear the state buffer. The size will be always 3 samples */
memset(S->state, 0, 3u * sizeof(q31_t));
}
}
void arm_pid_init_q31(
arm_pid_instance_q31 * S,
int32_t resetStateFlag)
{
#if defined (ARM_MATH_DSP)
/* Derived coefficient A0 */
S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd);
/* Derived coefficient A1 */
S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp);
#else
q31_t temp; /* to store the sum */
/* Derived coefficient A0 */
temp = clip_q63_to_q31((q63_t) S->Kp + S->Ki);
S->A0 = clip_q63_to_q31((q63_t) temp + S->Kd);
/* Derived coefficient A1 */
temp = clip_q63_to_q31((q63_t) S->Kd + S->Kd);
S->A1 = -clip_q63_to_q31((q63_t) temp + S->Kp);
#endif /* #if defined (ARM_MATH_DSP) */
/* Derived coefficient A2 */
S->A2 = S->Kd;
/* Check whether state needs reset or not */
if (resetStateFlag)
{
/* Reset state to zero, The size will be always 3 samples */
memset(S->state, 0, 3U * sizeof(q31_t));
}
}
arm_status arm_mat_add_q31(
const arm_matrix_instance_q31 * pSrcA,
const arm_matrix_instance_q31 * pSrcB,
arm_matrix_instance_q31 * pDst)
{
q31_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */
q31_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */
q31_t *pOut = pDst->pData; /* output data matrix pointer */
q31_t inA1, inB1; /* temporary variables */
#ifndef ARM_MATH_CM0_FAMILY
q31_t inA2, inB2; /* temporary variables */
q31_t out1, out2; /* temporary variables */
#endif // #ifndef ARM_MATH_CM0_FAMILY
uint32_t numSamples; /* total number of elements in the matrix */
uint32_t blkCnt; /* loop counters */
arm_status status; /* status of matrix addition */
#ifdef ARM_MATH_MATRIX_CHECK
/* Check for matrix mismatch condition */
if((pSrcA->numRows != pSrcB->numRows) ||
(pSrcA->numCols != pSrcB->numCols) ||
(pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols))
{
/* Set status as ARM_MATH_SIZE_MISMATCH */
status = ARM_MATH_SIZE_MISMATCH;
}
else
#endif
{
/* Total number of samples in the input matrix */
numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols;
#ifndef ARM_MATH_CM0_FAMILY
/* Run the below code for Cortex-M4 and Cortex-M3 */
/* Loop Unrolling */
blkCnt = numSamples >> 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(m,n) = A(m,n) + B(m,n) */
/* Add, saturate and then store the results in the destination buffer. */
/* Read values from source A */
inA1 = pIn1[0];
/* Read values from source B */
inB1 = pIn2[0];
/* Read values from source A */
inA2 = pIn1[1];
/* Add and saturate */
out1 = __QADD(inA1, inB1);
/* Read values from source B */
inB2 = pIn2[1];
/* Read values from source A */
inA1 = pIn1[2];
/* Add and saturate */
out2 = __QADD(inA2, inB2);
/* Read values from source B */
inB1 = pIn2[2];
/* Store result in destination */
pOut[0] = out1;
pOut[1] = out2;
/* Read values from source A */
inA2 = pIn1[3];
/* Read values from source B */
inB2 = pIn2[3];
/* Add and saturate */
out1 = __QADD(inA1, inB1);
out2 = __QADD(inA2, inB2);
/* Store result in destination */
pOut[2] = out1;
pOut[3] = out2;
/* update pointers to process next sampels */
pIn1 += 4u;
pIn2 += 4u;
pOut += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the numSamples is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = numSamples % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = numSamples;
#endif /* #ifndef ARM_MATH_CM0_FAMILY */
while(blkCnt > 0u)
{
/* C(m,n) = A(m,n) + B(m,n) */
/* Add, saturate and then store the results in the destination buffer. */
inA1 = *pIn1++;
inB1 = *pIn2++;
inA1 = __QADD(inA1, inB1);
/* Decrement the loop counter */
blkCnt--;
*pOut++ = inA1;
}
/* set status as ARM_MATH_SUCCESS */
status = ARM_MATH_SUCCESS;
}
/* Return to application */
return (status);
}
void arm_add_q31(
q31_t * pSrcA,
q31_t * pSrcB,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t inA1, inA2, inA3, inA4;
q31_t inB1, inB2, inB3, inB4;
/*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 + B */
/* Add and then store the results in the destination buffer. */
inA1 = *pSrcA++;
inA2 = *pSrcA++;
inB1 = *pSrcB++;
inB2 = *pSrcB++;
inA3 = *pSrcA++;
inA4 = *pSrcA++;
inB3 = *pSrcB++;
inB4 = *pSrcB++;
*pDst++ = __QADD(inA1, inB1);
*pDst++ = __QADD(inA2, inB2);
*pDst++ = __QADD(inA3, inB3);
*pDst++ = __QADD(inA4, inB4);
/* 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;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrcA++ + *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
void arm_offset_q31(
q31_t * pSrc,
q31_t offset,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0_FAMILY
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2, in3, in4;
/*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 + offset */
/* Add offset and then store the results in the destination buffer. */
in1 = *pSrc++;
in2 = *pSrc++;
in3 = *pSrc++;
in4 = *pSrc++;
*pDst++ = __QADD(in1, offset);
*pDst++ = __QADD(in2, offset);
*pDst++ = __QADD(in3, offset);
*pDst++ = __QADD(in4, offset);
/* 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;
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the result in the destination buffer. */
*pDst++ = __QADD(*pSrc++, offset);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the result in the destination buffer. */
*pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrc++ + offset);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0_FAMILY */
}
void arm_add_q31(
q31_t * pSrcA,
q31_t * pSrcB,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q31_t inA1, inA2, inA3, inA4; /* temporary input variables */
q31_t inB1, inB2, inB3, inB4; /* temporary input variables */
/*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 + B */
/* Add and then store the results in the destination buffer. */
/* read input from soruceA */
inA1 = *pSrcA;
/* read input from soruceB */
inB1 = *pSrcB;
/* read input from soruceA */
inA2 = *(pSrcA + 1);
/* read input from soruceB */
inB2 = *(pSrcB + 1);
/* add, saturate and store result to destination */
*pDst = __QADD(inA1, inB1);
/* read input from soruceA */
inA3 = *(pSrcA + 2);
/* read input from soruceB */
inB3 = *(pSrcB + 2);
/* add, saturate and store result to destination */
*(pDst + 1) = __QADD(inA2, inB2);
/* read input from soruceA */
inA4 = *(pSrcA + 3);
/* read input from soruceB */
inB4 = *(pSrcB + 3);
/* add, saturate and store result to destination */
*(pDst + 2) = __QADD(inA3, inB3);
/* read input from soruceA */
inA1 = *(pSrcA + 4);
/* read input from soruceB */
inB1 = *(pSrcB + 4);
/* add, saturate and store result to destination */
*(pDst + 3) = __QADD(inA4, inB4);
/* read input from soruceA */
inA2 = *(pSrcA + 5);
/* read input from soruceB */
inB2 = *(pSrcB + 5);
/* add, saturate and store result to destination */
*(pDst + 4) = __QADD(inA1, inB1);
/* read input from soruceA */
inA3 = *(pSrcA + 6);
/* read input from soruceB */
inB3 = *(pSrcB + 6);
/* add, saturate and store result to destination */
*(pDst + 5) = __QADD(inA2, inB2);
/* read input from soruceA */
inA4 = *(pSrcA + 7);
/* add, saturate and store result to destination */
*(pDst + 6) = __QADD(inA3, inB3);
/* increment sourceA pointer by 8 */
pSrcA += 8u;
/* read input from soruceB */
inB4 = *(pSrcB + 7);
/* increment sourceB pointer by 8 */
pSrcB += 8u;
/* add, saturate and store result to destination */
*(pDst + 7) = __QADD(inA4, inB4);
/* increment destination pointer by 8 */
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 = A + B */
/* Add and then store the results in the destination buffer. */
inA1 = *pSrcA++;
inB1 = *pSrcB++;
*pDst++ = __QADD(inA1, inB1);
/* Decrement the loop counter */
blkCnt--;
}
}
void arm_offset_q31(
q31_t * pSrc,
q31_t offset,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q31_t in1, in2, in3, in4; /* temporary variables */
/*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 + offset */
/* Add offset and then store the results in the destination buffer. */
/* read samples from the source */
in1 = *pSrc;
in2 = *(pSrc + 1);
/* add offset to the input */
in1 = __QADD(in1, offset);
/* read sample from the source */
in3 = *(pSrc + 2);
/* add offset to the input */
in2 = __QADD(in2, offset);
/* read sample from the source */
in4 = *(pSrc + 3);
/* add offset to the input */
in3 = __QADD(in3, offset);
/* store result to destination buffer */
*pDst = in1;
/* add offset to the input */
in4 = __QADD(in4, offset);
/* store result to destination buffer */
*(pDst + 1) = in2;
*(pDst + 2) = in3;
*(pDst + 3) = in4;
/* read samples from the source */
in1 = *(pSrc + 4);
in2 = *(pSrc + 5);
/* add offset to the input */
in1 = __QADD(in1, offset);
/* read sample from the source */
in3 = *(pSrc + 6);
/* add offset to the input */
in2 = __QADD(in2, offset);
/* read sample from the source */
in4 = *(pSrc + 7);
/* store result to destination buffer */
*(pDst + 4) = in1;
/* add offset to the input */
in3 = __QADD(in3, offset);
/* store result to destination buffer */
*(pDst + 5) = in2;
/* add offset to the input */
in4 = __QADD(in4, offset);
/* store result to destination buffer */
*(pDst + 6) = in3;
/* increment source pointer by 8 to process next samples */
pSrc += 8u;
/* store result to destination buffer */
*(pDst + 7) = in4;
/* increment destination pointer by 8 */
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 = A + offset */
/* Add offset and then store the result in the destination buffer. */
in1 = *pSrc++;
*pDst++ = __QADD(in1, offset);
/* Decrement the loop counter */
blkCnt--;
}
}
#define OP_NAME iadd
#include <ngl_opcode_begin.c>
#ifdef OPCODE_BODY
#ifdef NGL_ARM
{
ngl_val right = ngl_stack_pop(&stack);
ngl_val left = ngl_stack_pop(&stack);
ngl_stack_push(&stack, ngl_val_uint(__QADD(left.uinteger, right.uinteger)));
}
#else
{
ngl_val right = ngl_stack_pop(&stack);
ngl_val left = ngl_stack_pop(&stack);
int64_t sum = (int64_t)(left.integer) + (int64_t)(right.integer);
if (sum > INT_MAX) {
sum = INT_MAX;
}
if (sum < INT_MIN) {
sum = INT_MIN;
}
ngl_stack_push(&stack, ngl_val_int(sum));
}
#endif
#endif
#include <ngl_opcode_end.c>