From 6ab94e0b318884bbcb95e2ea3835f951502e1d99 Mon Sep 17 00:00:00 2001 From: jaseg Date: Wed, 14 Oct 2020 12:47:28 +0200 Subject: Move firmware into subdirectory --- .../RefLibs/src/MatrixFunctions/mat_add.c | 58 ++++++++++ .../RefLibs/src/MatrixFunctions/mat_cmplx_mult.c | 118 +++++++++++++++++++++ .../RefLibs/src/MatrixFunctions/mat_inverse.c | 57 ++++++++++ .../RefLibs/src/MatrixFunctions/mat_mult.c | 91 ++++++++++++++++ .../RefLibs/src/MatrixFunctions/mat_scale.c | 64 +++++++++++ .../RefLibs/src/MatrixFunctions/mat_sub.c | 58 ++++++++++ .../RefLibs/src/MatrixFunctions/mat_trans.c | 77 ++++++++++++++ 7 files changed, 523 insertions(+) create mode 100644 fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_add.c create mode 100644 fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_cmplx_mult.c create mode 100644 fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_inverse.c create mode 100644 fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_mult.c create mode 100644 fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_scale.c create mode 100644 fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_sub.c create mode 100644 fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_trans.c (limited to 'fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions') diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_add.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_add.c new file mode 100644 index 0000000..a6e0067 --- /dev/null +++ b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_add.c @@ -0,0 +1,58 @@ +#include "ref.h" + +arm_status ref_mat_add_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + + for(i=0;ipData[i] = pSrcA->pData[i] + pSrcB->pData[i]; + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_add_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + + for(i=0;ipData[i] = ref_sat_q31( (q63_t)pSrcA->pData[i] + pSrcB->pData[i]); + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_add_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + + for(i=0;ipData[i] = ref_sat_q15( (q31_t)pSrcA->pData[i] + pSrcB->pData[i]); + } + + return ARM_MATH_SUCCESS; +} diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_cmplx_mult.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_cmplx_mult.c new file mode 100644 index 0000000..9364619 --- /dev/null +++ b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_cmplx_mult.c @@ -0,0 +1,118 @@ +#include "ref.h" + +arm_status ref_mat_cmplx_mult_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst) +{ + uint32_t r,c,i,outR,outC,innerSize; + float32_t sumR,sumI; + float32_t a0,b0,c0,d0; + + outR = pSrcA->numRows; + outC = pSrcB->numCols; + innerSize = pSrcA->numCols; + + for(r=0;rpData[2*(r*innerSize + i) + 0]; + b0 = pSrcA->pData[2*(r*innerSize + i) + 1]; + c0 = pSrcB->pData[2*(i*outC + c) + 0]; + d0 = pSrcB->pData[2*(i*outC + c) + 1]; + + sumR += a0 * c0 - b0 * d0; + sumI += b0 * c0 + a0 * d0; + } + + pDst->pData[2*(r*outC + c) + 0] = sumR; + pDst->pData[2*(r*outC + c) + 1] = sumI; + } + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_cmplx_mult_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + uint32_t r,c,i,outR,outC,innerSize; + q63_t sumR,sumI; + q31_t a0,b0,c0,d0; + + outR = pSrcA->numRows; + outC = pSrcB->numCols; + innerSize = pSrcA->numCols; + + for(r=0;rpData[2*(r*innerSize + i) + 0]; + b0 = pSrcA->pData[2*(r*innerSize + i) + 1]; + c0 = pSrcB->pData[2*(i*outC + c) + 0]; + d0 = pSrcB->pData[2*(i*outC + c) + 1]; + + sumR += (q63_t)a0 * c0 - (q63_t)b0 * d0; + sumI += (q63_t)b0 * c0 + (q63_t)a0 * d0; + } + + pDst->pData[2*(r*outC + c) + 0] = ref_sat_q31(sumR >> 31); + pDst->pData[2*(r*outC + c) + 1] = ref_sat_q31(sumI >> 31); + } + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_cmplx_mult_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst) +{ + uint32_t r,c,i,outR,outC,innerSize; + q63_t sumR,sumI; + q15_t a0,b0,c0,d0; + + outR = pSrcA->numRows; + outC = pSrcB->numCols; + innerSize = pSrcA->numCols; + + for(r=0;rpData[2*(r*innerSize + i) + 0]; + b0 = pSrcA->pData[2*(r*innerSize + i) + 1]; + c0 = pSrcB->pData[2*(i*outC + c) + 0]; + d0 = pSrcB->pData[2*(i*outC + c) + 1]; + + sumR += (q31_t)a0 * c0 - (q31_t)b0 * d0; + sumI += (q31_t)b0 * c0 + (q31_t)a0 * d0; + } + + pDst->pData[2*(r*outC + c) + 0] = ref_sat_q15(sumR >> 15); + pDst->pData[2*(r*outC + c) + 1] = ref_sat_q15(sumI >> 15); + } + } + + return ARM_MATH_SUCCESS; +} diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_inverse.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_inverse.c new file mode 100644 index 0000000..74d3ccc --- /dev/null +++ b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_inverse.c @@ -0,0 +1,57 @@ +#include "ref.h" + +arm_status ref_mat_inverse_f32( + const arm_matrix_instance_f32 * pSrc, + arm_matrix_instance_f32 * pDst) +{ + float32_t det; + uint32_t i, size; + arm_matrix_instance_f32 tmp; + + tmp.numCols = pSrc->numCols; + tmp.numRows = pSrc->numRows; + tmp.pData = scratchArray; + + det = ref_detrm(pSrc->pData,scratchArray,pSrc->numCols); + + size = pSrc->numCols * pSrc->numCols; + + ref_cofact(pSrc->pData,scratchArray,scratchArray + size,pSrc->numCols); + + ref_mat_trans_f32(&tmp,pDst); + + for(i=0;ipData[i] /= det; + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_inverse_f64( + const arm_matrix_instance_f64 * pSrc, + arm_matrix_instance_f64 * pDst) +{ + float64_t det; + uint32_t i, size; + arm_matrix_instance_f64 tmp; + + tmp.numCols = pSrc->numCols; + tmp.numRows = pSrc->numRows; + tmp.pData = (float64_t*)scratchArray; + + det = ref_detrm64(pSrc->pData,(float64_t*)scratchArray,pSrc->numCols); + + size = pSrc->numCols * pSrc->numCols; + + ref_cofact64(pSrc->pData,(float64_t*)scratchArray,(float64_t*)scratchArray + size,pSrc->numCols); + + ref_mat_trans_f64(&tmp,pDst); + + for(i=0;ipData[i] /= det; + } + + return ARM_MATH_SUCCESS; +} diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_mult.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_mult.c new file mode 100644 index 0000000..e9ef432 --- /dev/null +++ b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_mult.c @@ -0,0 +1,91 @@ +#include "ref.h" + +arm_status ref_mat_mult_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst) +{ + uint32_t r,c,i,outR,outC,innerSize; + float32_t sum; + + outR = pSrcA->numRows; + outC = pSrcB->numCols; + innerSize = pSrcA->numCols; + + for(r=0;rpData[r*innerSize + i] * pSrcB->pData[i*outC + c]; + } + + pDst->pData[r*outC + c] = sum; + } + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_mult_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + uint32_t r,c,i,outR,outC,innerSize; + q63_t sum; + + outR = pSrcA->numRows; + outC = pSrcB->numCols; + innerSize = pSrcA->numCols; + + for(r=0;rpData[r*innerSize + i]) * pSrcB->pData[i*outC + c]; + } + + pDst->pData[r*outC + c] = ref_sat_q31(sum >> 31); + } + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_mult_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst) +{ + uint32_t r,c,i,outR,outC,innerSize; + q63_t sum; + + outR = pSrcA->numRows; + outC = pSrcB->numCols; + innerSize = pSrcA->numCols; + + for(r=0;rpData[r*innerSize + i]) * pSrcB->pData[i*outC + c]; + } + + pDst->pData[r*outC + c] = ref_sat_q15(sum >> 15); + } + } + + return ARM_MATH_SUCCESS; +} diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_scale.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_scale.c new file mode 100644 index 0000000..d426ad6 --- /dev/null +++ b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_scale.c @@ -0,0 +1,64 @@ +#include "ref.h" + +arm_status ref_mat_scale_f32( + const arm_matrix_instance_f32 * pSrc, + float32_t scale, + arm_matrix_instance_f32 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; + + for(i=0;ipData[i] = pSrc->pData[i] * scale; + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_scale_q31( + const arm_matrix_instance_q31 * pSrc, + q31_t scale, + int32_t shift, + arm_matrix_instance_q31 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + int32_t totShift = shift + 1; + q31_t tmp; + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; + + for(i=0;ipData[i] * scale) >> 32; + pDst->pData[i] = ref_sat_q31((q63_t)tmp << totShift ); + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_scale_q15( + const arm_matrix_instance_q15 * pSrc, + q15_t scale, + int32_t shift, + arm_matrix_instance_q15 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + int32_t totShift = 15 - shift; + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; + + for(i=0;ipData[i] = ref_sat_q15( ((q31_t)pSrc->pData[i] * scale) >> totShift); + } + + return ARM_MATH_SUCCESS; +} diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_sub.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_sub.c new file mode 100644 index 0000000..bbd23f0 --- /dev/null +++ b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_sub.c @@ -0,0 +1,58 @@ +#include "ref.h" + +arm_status ref_mat_sub_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + + for(i=0;ipData[i] = pSrcA->pData[i] - pSrcB->pData[i]; + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_sub_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + + for(i=0;ipData[i] = ref_sat_q31( (q63_t)pSrcA->pData[i] - pSrcB->pData[i]); + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_sub_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst) +{ + uint32_t i; + uint32_t numSamples; /* total number of elements in the matrix */ + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + + for(i=0;ipData[i] = ref_sat_q15( (q31_t)pSrcA->pData[i] - pSrcB->pData[i]); + } + + return ARM_MATH_SUCCESS; +} diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_trans.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_trans.c new file mode 100644 index 0000000..8cb9a8d --- /dev/null +++ b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/MatrixFunctions/mat_trans.c @@ -0,0 +1,77 @@ +#include "ref.h" + +arm_status ref_mat_trans_f64( + const arm_matrix_instance_f64 * pSrc, + arm_matrix_instance_f64 * pDst) +{ + uint64_t r,c; + uint64_t numR = pSrc->numRows; + uint64_t numC = pSrc->numCols; + + for(r=0;rpData[c*numR + r] = pSrc->pData[r*numC + c]; + } + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_trans_f32( + const arm_matrix_instance_f32 * pSrc, + arm_matrix_instance_f32 * pDst) +{ + uint32_t r,c; + uint32_t numR = pSrc->numRows; + uint32_t numC = pSrc->numCols; + + for(r=0;rpData[c*numR + r] = pSrc->pData[r*numC + c]; + } + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_trans_q31( + const arm_matrix_instance_q31 * pSrc, + arm_matrix_instance_q31 * pDst) +{ + uint32_t r,c; + uint32_t numR = pSrc->numRows; + uint32_t numC = pSrc->numCols; + + for(r=0;rpData[c*numR + r] = pSrc->pData[r*numC + c]; + } + } + + return ARM_MATH_SUCCESS; +} + +arm_status ref_mat_trans_q15( + const arm_matrix_instance_q15 * pSrc, + arm_matrix_instance_q15 * pDst) +{ + uint32_t r,c; + uint32_t numR = pSrc->numRows; + uint32_t numC = pSrc->numCols; + + for(r=0;rpData[c*numR + r] = pSrc->pData[r*numC + c]; + } + } + + return ARM_MATH_SUCCESS; +} -- cgit