openmv/lib/cmsis/include/cmsis_extension.h
2025-08-18 13:33:25 -07:00

296 lines
7.7 KiB
C

/*
* This file is part of the OpenMV project.
*
* Copyright (c) 2013-2024 Ibrahim Abdelkader <iabdalkader@openmv.io>
* Copyright (c) 2013-2024 Kwabena W. Agyeman <kwagyeman@openmv.io>
*
* This work is licensed under the MIT license, see the file LICENSE for details.
*
* CMSIS Extension
*/
#ifndef __CMSIS_EXTENSION_H
#define __CMSIS_EXTENSION_H
#include "cmsis_compiler.h"
#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
(defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) )
/**
\brief Signed Saturate
\details Saturates a signed value.
\param [in] ARG1 Value to be saturated
\param [in] ARG2 Bit position to saturate to (1..32)
\param [in] ARG3 Right shift (0..31)
\return Saturated value
*/
#define __SSAT_ASR(ARG1, ARG2, ARG3) \
__extension__ \
({ \
int32_t __RES, __ARG1 = (ARG1); \
__ASM volatile ("ssat %0, %1, %2, asr %3" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1), "I" (ARG3) : "cc" ); \
__RES; \
})
/**
\brief Unsigned Saturate
\details Saturates an unsigned value.
\param [in] ARG1 Value to be saturated
\param [in] ARG2 Bit position to saturate to (0..31)
\param [in] ARG3 Right shift (0..31)
\return Saturated value
*/
#define __USAT_ASR(ARG1, ARG2, ARG3) \
__extension__ \
({ \
uint32_t __RES, __ARG1 = (ARG1); \
__ASM volatile ("usat %0, %1, %2, asr %3" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1), "I" (ARG3) : "cc" ); \
__RES; \
})
#else /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
(defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) ) */
/**
\brief Signed Saturate
\details Saturates a signed value.
\param [in] value Value to be saturated
\param [in] sat Bit position to saturate to (1..32)
\param [in] shift Right shift (0..31)
\return Saturated value
*/
__STATIC_FORCEINLINE int32_t __SSAT_ASR(int32_t val, uint32_t sat, uint32_t shift)
{
val >>= shift & 0x1F;
if ((sat >= 1U) && (sat <= 32U))
{
const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
const int32_t min = -1 - max ;
if (val > max)
{
return max;
}
else if (val < min)
{
return min;
}
}
return val;
}
/**
\brief Signed Saturate
\details Saturates two signed values.
\param [in] value Values to be saturated
\param [in] sat Bit position to saturate to (1..16)
\return Saturated value
*/
__STATIC_FORCEINLINE int32_t __SSAT16(int32_t val, uint32_t sat)
{
if ((sat >= 1U) && (sat <= 32U))
{
const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
const int32_t min = -1 - max ;
int32_t valHi = val >> 16;
if (valHi > max)
{
valHi = max;
}
else if (valHi < min)
{
valHi = min;
}
int32_t valLo = (val << 16) >> 16;
if (valLo > max)
{
valLo = max;
}
else if (valLo < min)
{
valLo = min;
}
return (valHi << 16) | (valLo & 0xFFFF);
}
return val;
}
/**
\brief Unsigned Saturate
\details Saturates an unsigned value.
\param [in] value Value to be saturated
\param [in] sat Bit position to saturate to (0..31)
\param [in] shift Right shift (0..31)
\return Saturated value
*/
__STATIC_FORCEINLINE uint32_t __USAT_ASR(int32_t val, uint32_t sat, uint32_t shift)
{
val >>= shift & 0x1F;
if (sat <= 31U)
{
const uint32_t max = ((1U << sat) - 1U);
if (val > (int32_t)max)
{
return max;
}
else if (val < 0)
{
return 0U;
}
}
return (uint32_t)val;
}
/**
\brief Unsigned Saturate
\details Saturates two unsigned values.
\param [in] value Values to be saturated
\param [in] sat Bit position to saturate to (0..15)
\return Saturated value
*/
__STATIC_FORCEINLINE uint32_t __USAT16(int32_t val, uint32_t sat)
{
if (sat <= 15U)
{
const uint32_t max = ((1U << sat) - 1U);
int32_t valHi = val >> 16;
if (valHi > (int32_t)max)
{
valHi = max;
}
else if (valHi < 0)
{
valHi = 0U;
}
int32_t valLo = (val << 16) >> 16;
if (valLo > (int32_t)max)
{
valLo = max;
}
else if (valLo < 0)
{
valLo = 0U;
}
return (valHi << 16) | valLo;
}
return (uint32_t)val;
}
#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__ == 1)) || \
(defined (__ARM_ARCH_7EM__ ) && (__ARM_ARCH_7EM__ == 1)) || \
(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) ) */
#if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
__STATIC_FORCEINLINE uint32_t __UXTB(uint32_t op1)
{
uint32_t result;
__ASM volatile ("uxtb %0, %1" : "=r" (result) : "r" (op1));
return(result);
}
__STATIC_FORCEINLINE uint32_t __UXTAB(uint32_t op1, uint32_t op2)
{
uint32_t result;
__ASM volatile ("uxtab %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2));
return(result);
}
__STATIC_FORCEINLINE uint32_t __UXTB_RORn(uint32_t op1, uint32_t rotate)
{
uint32_t result;
if (__builtin_constant_p(rotate) && ((rotate == 8U) || (rotate == 16U) || (rotate == 24U))) {
__ASM volatile ("uxtb %0, %1, ROR %2" : "=r" (result) : "r" (op1), "i" (rotate) );
} else {
result = __UXTB(__ROR(op1, rotate));
}
return result;
}
__STATIC_FORCEINLINE uint32_t __UXTB16_RORn(uint32_t op1, uint32_t rotate)
{
uint32_t result;
if (__builtin_constant_p(rotate) && ((rotate == 8U) || (rotate == 16U) || (rotate == 24U))) {
__ASM volatile ("uxtb16 %0, %1, ROR %2" : "=r" (result) : "r" (op1), "i" (rotate) );
} else {
result = __UXTB16(__ROR(op1, rotate));
}
return result;
}
__STATIC_FORCEINLINE uint32_t __UXTAB_RORn(uint32_t op1, uint32_t op2, uint32_t rotate)
{
uint32_t result;
if (__builtin_constant_p(rotate) && ((rotate == 8U) || (rotate == 16U) || (rotate == 24U))) {
__ASM volatile ("uxtab %0, %1, %2, ROR %3" : "=r" (result) : "r" (op1), "r" (op2), "i" (rotate) );
} else {
result = __UXTAB(op1, __ROR(op2, rotate));
}
return result;
}
__STATIC_FORCEINLINE uint32_t __SXTB(uint32_t op1)
{
uint32_t result;
__ASM volatile ("sxtb %0, %1" : "=r" (result) : "r" (op1));
return(result);
}
__STATIC_FORCEINLINE uint32_t __SXTB_RORn(uint32_t op1, uint32_t rotate)
{
uint32_t result;
if (__builtin_constant_p(rotate) && ((rotate == 8U) || (rotate == 16U) || (rotate == 24U))) {
__ASM volatile ("sxtb %0, %1, ROR %2" : "=r" (result) : "r" (op1), "i" (rotate) );
} else {
result = __SXTB(__ROR(op1, rotate));
}
return result;
}
#else
__STATIC_FORCEINLINE uint32_t __UXTB(uint32_t op1)
{
return op1 & 0xFF;
}
__STATIC_FORCEINLINE uint32_t __UXTB_RORn(uint32_t op1, uint32_t rotate)
{
return (op1 >> rotate) & 0xFF;
}
__STATIC_FORCEINLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)
{
return ((op1 & 0xFFFF0000) - (op2 & 0xFFFF0000)) | ((op1 - op2) & 0xFFFF);
}
__STATIC_FORCEINLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)
{
uint32_t result = abs((op1 & 0xFF) - (op2 & 0xFF));
result += abs(((op1 >> 8) & 0xFF) - ((op2 >> 8) & 0xFF));
result += abs(((op1 >> 16) & 0xFF) - ((op2 >> 16) & 0xFF));
result += abs(((op1 >> 24) & 0xFF) - ((op2 >> 24) & 0xFF));
return result;
}
__STATIC_FORCEINLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)
{
op3 += abs((op1 & 0xFF) - (op2 & 0xFF));
op3 += abs(((op1 >> 8) & 0xFF) - ((op2 >> 8) & 0xFF));
op3 += abs(((op1 >> 16) & 0xFF) - ((op2 >> 16) & 0xFF));
op3 += abs(((op1 >> 24) & 0xFF) - ((op2 >> 24) & 0xFF));
return op3;
}
#endif /* (__ARM_FEATURE_DSP == 1) */
#endif /* __CMSIS_EXTENSIONS_H */