| 1 |
/* { dg-do compile } */ |
| 2 |
/* { dg-options "-mrvv-vector-bits=scalable -march=rv64gcv -mabi=lp64d -O3" } */ |
| 3 |
|
| 4 |
#include "riscv_vector.h" |
| 5 |
|
| 6 |
#define RISCV_MATH_LOOPUNROLL |
| 7 |
#define RISCV_MATH_VECTOR |
| 8 |
typedef float float32_t; |
| 9 |
|
| 10 |
typedef struct |
| 11 |
{ |
| 12 |
uint16_t numTaps; /**< number of coefficients in the filter. */ |
| 13 |
float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ |
| 14 |
float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ |
| 15 |
float32_t mu; /**< step size that controls filter coefficient updates. */ |
| 16 |
} riscv_lms_instance_f32; |
| 17 |
|
| 18 |
|
| 19 |
void riscv_lms_f32( |
| 20 |
const riscv_lms_instance_f32 * S, |
| 21 |
const float32_t * pSrc, |
| 22 |
float32_t * pRef, |
| 23 |
float32_t * pOut, |
| 24 |
float32_t * pErr, |
| 25 |
uint32_t blockSize) |
| 26 |
{ |
| 27 |
float32_t *pState = S->pState; /* State pointer */ |
| 28 |
float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ |
| 29 |
float32_t *pStateCurnt; /* Points to the current sample of the state */ |
| 30 |
float32_t *px, *pb; /* Temporary pointers for state and coefficient buffers */ |
| 31 |
float32_t mu = S->mu; /* Adaptive factor */ |
| 32 |
float32_t acc, e; /* Accumulator, error */ |
| 33 |
float32_t w; /* Weight factor */ |
| 34 |
uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */ |
| 35 |
uint32_t tapCnt, blkCnt; /* Loop counters */ |
| 36 |
|
| 37 |
/* Initializations of error, difference, Coefficient update */ |
| 38 |
e = 0.0f; |
| 39 |
w = 0.0f; |
| 40 |
|
| 41 |
/* S->pState points to state array which contains previous frame (numTaps - 1) samples */ |
| 42 |
/* pStateCurnt points to the location where the new input data should be written */ |
| 43 |
pStateCurnt = &(S->pState[(numTaps - 1U)]); |
| 44 |
|
| 45 |
/* initialise loop count */ |
| 46 |
blkCnt = blockSize; |
| 47 |
|
| 48 |
while (blkCnt > 0U) |
| 49 |
{ |
| 50 |
/* Copy the new input sample into the state buffer */ |
| 51 |
*pStateCurnt++ = *pSrc++; |
| 52 |
|
| 53 |
/* Initialize pState pointer */ |
| 54 |
px = pState; |
| 55 |
|
| 56 |
/* Initialize coefficient pointer */ |
| 57 |
pb = pCoeffs; |
| 58 |
|
| 59 |
/* Set the accumulator to zero */ |
| 60 |
acc = 0.0f; |
| 61 |
uint32_t vblkCnt = numTaps; /* Loop counter */ |
| 62 |
size_t l; |
| 63 |
vfloat32m8_t vx, vy; |
| 64 |
vfloat32m1_t temp00m1; |
| 65 |
l = __riscv_vsetvl_e32m1(1); |
| 66 |
temp00m1 = __riscv_vfmv_v_f_f32m1(0, l); |
| 67 |
for (; (l = __riscv_vsetvl_e32m8(vblkCnt)) > 0; vblkCnt -= l) { |
| 68 |
vx = __riscv_vle32_v_f32m8(px, l); |
| 69 |
px += l; |
| 70 |
vy = __riscv_vle32_v_f32m8(pb, l); |
| 71 |
pb += l; |
| 72 |
temp00m1 = __riscv_vfredusum_vs_f32m8_f32m1(__riscv_vfmul_vv_f32m8(vx, vy, l), temp00m1, l); |
| 73 |
} |
| 74 |
acc += __riscv_vfmv_f_s_f32m1_f32(temp00m1); |
| 75 |
|
| 76 |
while (tapCnt > 0U) |
| 77 |
{ |
| 78 |
/* Perform the multiply-accumulate */ |
| 79 |
acc += (*px++) * (*pb++); |
| 80 |
|
| 81 |
/* Decrement the loop counter */ |
| 82 |
tapCnt--; |
| 83 |
} |
| 84 |
/* Store the result from accumulator into the destination buffer. */ |
| 85 |
*pOut++ = acc; |
| 86 |
|
| 87 |
/* Compute and store error */ |
| 88 |
e = (float32_t) *pRef++ - acc; |
| 89 |
*pErr++ = e; |
| 90 |
|
| 91 |
/* Calculation of Weighting factor for updating filter coefficients */ |
| 92 |
w = e * mu; |
| 93 |
|
| 94 |
/* Initialize pState pointer */ |
| 95 |
/* Advance state pointer by 1 for the next sample */ |
| 96 |
px = pState++; |
| 97 |
|
| 98 |
/* Initialize coefficient pointer */ |
| 99 |
pb = pCoeffs; |
| 100 |
|
| 101 |
vblkCnt = numTaps; |
| 102 |
for (; (l = __riscv_vsetvl_e32m8(vblkCnt)) > 0; vblkCnt -= l) { |
| 103 |
vx = __riscv_vle32_v_f32m8(px, l); |
| 104 |
px += l; |
| 105 |
__riscv_vse32_v_f32m8(pb, __riscv_vfadd_vv_f32m8(__riscv_vfmul_vf_f32m8(vx, w, l), __riscv_vle32_v_f32m8(pb, l), l) , l); |
| 106 |
pb += l; |
| 107 |
} |
| 108 |
while (tapCnt > 0U) |
| 109 |
{ |
| 110 |
/* Perform the multiply-accumulate */ |
| 111 |
*pb += w * (*px++); |
| 112 |
pb++; |
| 113 |
|
| 114 |
/* Decrement loop counter */ |
| 115 |
tapCnt--; |
| 116 |
} |
| 117 |
/* Decrement loop counter */ |
| 118 |
blkCnt--; |
| 119 |
} |
| 120 |
|
| 121 |
/* Processing is complete. |
| 122 |
Now copy the last numTaps - 1 samples to the start of the state buffer. |
| 123 |
This prepares the state buffer for the next function call. */ |
| 124 |
|
| 125 |
/* Points to the start of the pState buffer */ |
| 126 |
pStateCurnt = S->pState; |
| 127 |
|
| 128 |
/* copy data */ |
| 129 |
|
| 130 |
uint32_t vblkCnt = (numTaps - 1U); /* Loop counter */ |
| 131 |
size_t l; |
| 132 |
for (; (l = __riscv_vsetvl_e32m8(vblkCnt)) > 0; vblkCnt -= l) { |
| 133 |
__riscv_vse32_v_f32m8(pStateCurnt, __riscv_vle32_v_f32m8(pState, l) , l); |
| 134 |
pState += l; |
| 135 |
pStateCurnt += l; |
| 136 |
} |
| 137 |
|
| 138 |
|
| 139 |
/* Loop unrolling: Compute 4 taps at a time. */ |
| 140 |
tapCnt = (numTaps - 1U) >> 2U; |
| 141 |
|
| 142 |
while (tapCnt > 0U) |
| 143 |
{ |
| 144 |
*pStateCurnt++ = *pState++; |
| 145 |
*pStateCurnt++ = *pState++; |
| 146 |
*pStateCurnt++ = *pState++; |
| 147 |
*pStateCurnt++ = *pState++; |
| 148 |
|
| 149 |
/* Decrement loop counter */ |
| 150 |
tapCnt--; |
| 151 |
} |
| 152 |
|
| 153 |
/* Loop unrolling: Compute remaining taps */ |
| 154 |
tapCnt = (numTaps - 1U) & 0x3U; |
| 155 |
|
| 156 |
|
| 157 |
|
| 158 |
/* Initialize tapCnt with number of samples */ |
| 159 |
tapCnt = (numTaps - 1U); |
| 160 |
|
| 161 |
|
| 162 |
|
| 163 |
while (tapCnt > 0U) |
| 164 |
{ |
| 165 |
*pStateCurnt++ = *pState++; |
| 166 |
|
| 167 |
/* Decrement loop counter */ |
| 168 |
tapCnt--; |
| 169 |
} |
| 170 |
} |