diff --git a/components/hal/hal_utils.c b/components/hal/hal_utils.c index 68ea4bb60a..9e4aa794ce 100644 --- a/components/hal/hal_utils.c +++ b/components/hal/hal_utils.c @@ -7,6 +7,14 @@ #include "hal/hal_utils.h" #include "hal/assert.h" +#ifndef BIT +#define BIT(n) (1UL << (n)) +#endif + +#ifndef BIT_MASK +#define BIT_MASK(n) (BIT(n) - 1) +#endif + __attribute__((always_inline)) static inline uint32_t _sub_abs(uint32_t a, uint32_t b) { @@ -131,3 +139,56 @@ uint32_t hal_utils_calc_clk_div_integer(const hal_utils_clk_info_t *clk_info, ui // Return the actual frequency return clk_info->src_freq_hz / div_integ; } + +typedef union { + struct { + uint32_t mantissa: 23; + uint32_t exponent: 8; + uint32_t sign: 1; + }; + uint32_t val; +} hal_utils_ieee754_float_t; + +int hal_utils_float_to_fixed_point_32b(float flt, const hal_utils_fixed_point_t *fp_cfg, uint32_t *fp_out) +{ + int ret = 0; + uint32_t output = 0; + const hal_utils_ieee754_float_t *f = (const hal_utils_ieee754_float_t *)&flt; + if (fp_cfg->int_bit + fp_cfg->frac_bit > 31) { + // Not supported + return -3; + } + + if (f->val == 0) { // Zero case + *fp_out = 0; + return 0; + } + if (f->exponent != 0xFF) { // Normal case + int real_exp = (int)f->exponent - 127; + uint32_t real_mant = f->mantissa | BIT(23); // Add the hidden bit + // Overflow check + if (real_exp >= (int)fp_cfg->int_bit) { + ret = -1; + } + // Determine sign + output |= f->sign << (fp_cfg->int_bit + fp_cfg->frac_bit); + // Determine integer and fraction part + int shift = 23 - fp_cfg->frac_bit - real_exp; + output |= shift >= 0 ? real_mant >> shift : real_mant << -shift; + } else { + if (f->mantissa && f->mantissa < BIT(23) - 1) { // NaN (Not-a-Number) case + return -2; + } else { // Infinity or Largest Number case + output = f->sign ? ~(uint32_t)0 : BIT(31) - 1; + ret = -1; + } + } + + if (ret != 0 && fp_cfg->saturation) { + *fp_out = (f->sign << (fp_cfg->int_bit + fp_cfg->frac_bit)) | + (BIT_MASK(fp_cfg->int_bit + fp_cfg->frac_bit)); + } else { + *fp_out = output; + } + return ret; +} diff --git a/components/hal/include/hal/hal_utils.h b/components/hal/include/hal/hal_utils.h index b95931c252..f5860da573 100644 --- a/components/hal/include/hal/hal_utils.h +++ b/components/hal/include/hal/hal_utils.h @@ -7,6 +7,7 @@ #pragma once #include +#include #ifdef __cplusplus extern "C" { @@ -148,6 +149,38 @@ static inline uint32_t hal_utils_calc_lcm(uint32_t a, uint32_t b) return (a * b / hal_utils_gcd(a, b)); } +/** + * @brief Fixed-point data configuration + * + */ +typedef struct { + uint32_t int_bit; /*!< Integer bit of the fixed point */ + uint32_t frac_bit; /*!< Fractional bit of the fixed point */ + bool saturation; /*!< Whether to limit the value to the maximum when fixed-point data overflow. + * When set true, the value will be limited to the maximum when the float type data is out of range. + * When set false, the function will return false when the float type data is out of range. + */ +} hal_utils_fixed_point_t; + +/** + * @brief Convert the float type to fixed point type + * @note The supported data format: + * - [input] float (IEEE 754): + * sign(1bit) + exponent(8bit) + mantissa(23bit) (32 bit in total) + * - [output] fixed-point: + * sign(1bit) + integer(int_bit) + fraction(frac_bit) (less or equal to 32 bit) + * + * @param[in] flt IEEE 754 float type data + * @param[in] fp_cfg Fixed-point data configuration + * @param[out] fp_out The output fixed-point data + * @return + * 0: Success + * -1: Fixed point data overflow, `fp_out` will still be assigned + * -2: Float is NaN + * -3: Invalid configuration + */ +int hal_utils_float_to_fixed_point_32b(float flt, const hal_utils_fixed_point_t *fp_cfg, uint32_t *fp_out); + #ifdef __cplusplus } #endif diff --git a/components/hal/test_apps/hal_utils/main/CMakeLists.txt b/components/hal/test_apps/hal_utils/main/CMakeLists.txt index 09293d7c0e..927a360197 100644 --- a/components/hal/test_apps/hal_utils/main/CMakeLists.txt +++ b/components/hal/test_apps/hal_utils/main/CMakeLists.txt @@ -1,4 +1,5 @@ idf_component_register(SRCS "test_app_main.c" + "test_fmt_convert.c" "test_calc_clk_div.c" "test_hal_utils_misc.c" INCLUDE_DIRS "." diff --git a/components/hal/test_apps/hal_utils/main/test_fmt_convert.c b/components/hal/test_apps/hal_utils/main/test_fmt_convert.c new file mode 100644 index 0000000000..04670cb6d2 --- /dev/null +++ b/components/hal/test_apps/hal_utils/main/test_fmt_convert.c @@ -0,0 +1,94 @@ +/* + * SPDX-FileCopyrightText: 2023-2024 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include "unity.h" +#include "hal/hal_utils.h" + +#ifndef BIT +#define BIT(n) (1UL << (n)) +#endif + +TEST_CASE("test_float_to_fixed_point", "[fmt_convert]") +{ + float f_nan = NAN; + float f_inf = INFINITY; + float f0 = 100.9f; + float f_zero = 0; + float f_precise = 0.5f; + float f_int = 2.0f; + float f_frac = 1.0f / 3.0f; + float f_dec = 1.453f; + float f_neg = -1.25f; + uint32_t out = 0; + hal_utils_fixed_point_t fp_cfg = { + .int_bit = 24, + .frac_bit = 8, + .saturation = false, + }; + + // Invalid arguments case + TEST_ASSERT_EQUAL_INT(-3, hal_utils_float_to_fixed_point_32b(f_dec, &fp_cfg, &out)); + printf("Invalid arguments case passed!\n"); + fp_cfg.int_bit = 2; + + // Overflow case + TEST_ASSERT_EQUAL_INT(-1, hal_utils_float_to_fixed_point_32b(f0, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(0x64E6, out); // integ: 0x64 = 100, frac: 0xE6 / 0x100 = 0.8984375 + printf("Overflow case passed!\n"); + + // Not-a-Number case + TEST_ASSERT_EQUAL_INT(-2, hal_utils_float_to_fixed_point_32b(f_nan, &fp_cfg, &out)); + printf("Not-a-Number case passed!\n"); + + // Infinity case + TEST_ASSERT_EQUAL_INT(-1, hal_utils_float_to_fixed_point_32b(f_inf, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(BIT(31) - 1, out); + printf("Infinity case passed!\n"); + + fp_cfg.saturation = true; + // Limit overflow case + TEST_ASSERT_EQUAL_INT(-1, hal_utils_float_to_fixed_point_32b(f0, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(BIT(10) - 1, out); // Limit to the maximum value, integ: 0x03 = 3 | frac: 0xff / 0x100 = 0.99609375 + printf("Limit overflow case passed!\n"); + + // Zero case + TEST_ASSERT_EQUAL_INT(0, hal_utils_float_to_fixed_point_32b(f_zero, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(0, out); // Special case, 0 = 0 + printf("Zero case passed!\n"); + + // Precision case + TEST_ASSERT_EQUAL_INT(0, hal_utils_float_to_fixed_point_32b(f_precise, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(BIT(7), out); // frac: 0x80 / 0x100 = 0.5 + printf("Precision case passed!\n"); + + // Integer case + TEST_ASSERT_EQUAL_INT(0, hal_utils_float_to_fixed_point_32b(f_int, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(BIT(9), out); // integ: 2 | frac: 0x00 / 0x100 = 0 + printf("Integer case passed!\n"); + + // Fraction case + TEST_ASSERT_EQUAL_INT(0, hal_utils_float_to_fixed_point_32b(f_frac, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(0x055, out); // 0x55 / 0x100 = 0.33203125 + printf("Fraction case passed!\n"); + + // Decimal case + TEST_ASSERT_EQUAL_INT(0, hal_utils_float_to_fixed_point_32b(f_dec, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(0x173, out); // integ: 0x01 = 1, frac: 0x73 / 0x100 = 0.44921875 + printf("Decimal case passed!\n"); + + // Negative case + TEST_ASSERT_EQUAL_INT(0, hal_utils_float_to_fixed_point_32b(f_neg, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(BIT(10) | BIT(8) | (BIT(6)), out); // sign: 1 | integ: 1 | frac: 0x40 / 0x100 = 0.25 + printf("Negative case passed!\n"); + + fp_cfg.int_bit = 8; + // Integer bits case + TEST_ASSERT_EQUAL_INT(0, hal_utils_float_to_fixed_point_32b(f0, &fp_cfg, &out)); + TEST_ASSERT_EQUAL_UINT32(0x64E6, out); // integ: 0x64 = 100, frac: 0xE6 / 0x100 = 0.8984375 + printf("Integer bits case passed!\n"); +}