132 lines
4.0 KiB
C
132 lines
4.0 KiB
C
/*
|
|
* Copyright 2018-2022 NXP
|
|
* All rights reserved.
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
|
|
#include "fsl_powerquad.h"
|
|
|
|
/*******************************************************************************
|
|
* Definitions
|
|
******************************************************************************/
|
|
|
|
/* Component ID definition, used by tools. */
|
|
#ifndef FSL_COMPONENT_ID
|
|
#define FSL_COMPONENT_ID "platform.drivers.powerquad_matrix"
|
|
#endif
|
|
|
|
/*******************************************************************************
|
|
* Code
|
|
******************************************************************************/
|
|
void PQ_MatrixAddition(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
|
|
{
|
|
assert(NULL != pAData);
|
|
assert(NULL != pBData);
|
|
assert(NULL != pResult);
|
|
|
|
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
|
|
base->INABASE = (uint32_t)(uint32_t *)pAData;
|
|
base->INBBASE = (uint32_t)(uint32_t *)pBData;
|
|
base->LENGTH = length;
|
|
base->CONTROL = (CP_MTX << 4U) | PQ_MTX_ADD;
|
|
}
|
|
|
|
void PQ_MatrixSubtraction(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
|
|
{
|
|
assert(NULL != pAData);
|
|
assert(NULL != pBData);
|
|
assert(NULL != pResult);
|
|
|
|
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
|
|
base->INABASE = (uint32_t)(uint32_t *)pAData;
|
|
base->INBBASE = (uint32_t)(uint32_t *)pBData;
|
|
base->LENGTH = length;
|
|
base->CONTROL = (CP_MTX << 4U) | PQ_MTX_SUB;
|
|
}
|
|
|
|
void PQ_MatrixMultiplication(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
|
|
{
|
|
assert(NULL != pAData);
|
|
assert(NULL != pBData);
|
|
assert(NULL != pResult);
|
|
|
|
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
|
|
base->INABASE = (uint32_t)(uint32_t *)pAData;
|
|
base->INBBASE = (uint32_t)(uint32_t *)pBData;
|
|
base->LENGTH = length;
|
|
base->CONTROL = (CP_MTX << 4U) | PQ_MTX_MULT;
|
|
}
|
|
|
|
void PQ_MatrixProduct(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
|
|
{
|
|
assert(NULL != pAData);
|
|
assert(NULL != pBData);
|
|
assert(NULL != pResult);
|
|
|
|
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
|
|
base->INABASE = (uint32_t)(uint32_t *)pAData;
|
|
base->INBBASE = (uint32_t)(uint32_t *)pBData;
|
|
base->LENGTH = length;
|
|
base->CONTROL = (CP_MTX << 4U) | PQ_MTX_PROD;
|
|
}
|
|
|
|
void PQ_VectorDotProduct(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
|
|
{
|
|
assert(NULL != pAData);
|
|
assert(NULL != pBData);
|
|
assert(NULL != pResult);
|
|
|
|
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
|
|
base->INABASE = (uint32_t)(uint32_t *)pAData;
|
|
base->INBBASE = (uint32_t)(uint32_t *)pBData;
|
|
base->LENGTH = length;
|
|
base->CONTROL = (CP_MTX << 4U) | PQ_VEC_DOTP;
|
|
}
|
|
|
|
void PQ_MatrixInversion(POWERQUAD_Type *base, uint32_t length, void *pData, void *pTmpData, void *pResult)
|
|
{
|
|
assert(NULL != pData);
|
|
assert(NULL != pTmpData);
|
|
assert(NULL != pResult);
|
|
|
|
/* Workaround:
|
|
*
|
|
* Matrix inv depends on the coproc 1/x function, this puts coproc to right state.
|
|
*/
|
|
_pq_inv0(1.0f);
|
|
|
|
base->INABASE = (uint32_t)(uint32_t *)pData;
|
|
base->TMPBASE = (uint32_t)(uint32_t *)pTmpData;
|
|
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
|
|
base->LENGTH = length;
|
|
base->CONTROL = (CP_MTX << 4U) | PQ_MTX_INV;
|
|
}
|
|
|
|
void PQ_MatrixTranspose(POWERQUAD_Type *base, uint32_t length, void *pData, void *pResult)
|
|
{
|
|
assert(NULL != pData);
|
|
assert(NULL != pResult);
|
|
|
|
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
|
|
base->INABASE = (uint32_t)(uint32_t *)pData;
|
|
base->LENGTH = length;
|
|
base->CONTROL = (CP_MTX << 4U) | PQ_MTX_TRAN;
|
|
}
|
|
|
|
void PQ_MatrixScale(POWERQUAD_Type *base, uint32_t length, float misc, const void *pData, void *pResult)
|
|
{
|
|
assert(NULL != pData);
|
|
assert(NULL != pResult);
|
|
pq_float_t val;
|
|
|
|
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
|
|
base->INABASE = (uint32_t)(const uint32_t *)pData;
|
|
base->LENGTH = length;
|
|
|
|
val.floatX = misc;
|
|
base->MISC = val.integerX;
|
|
|
|
base->CONTROL = (CP_MTX << 4U) | PQ_MTX_SCALE;
|
|
}
|