esp-idf/components/hal/hal_utils.c

142 lines
5.0 KiB
C
Raw Normal View History

/*
* SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include "hal/hal_utils.h"
#include "hal/assert.h"
/**
* @brief helper function, calculate the Greatest Common Divisor
* @note gcd(a, b) = gcd(b, a % b)
* @param a bigger value
* @param b smaller value
* @return result of gcd(a, b)
*/
__attribute__((always_inline))
static inline uint32_t _gcd(uint32_t a, uint32_t b)
{
uint32_t c = a % b;
while (c != 0) {
a = b;
b = c;
c = a % b;
}
return b;
}
__attribute__((always_inline))
static inline uint32_t _sub_abs(uint32_t a, uint32_t b)
{
return a > b ? a - b : b - a;
}
uint32_t hal_utils_calc_clk_div_frac_fast(const hal_utils_clk_info_t *clk_info, hal_utils_clk_div_t *clk_div)
{
HAL_ASSERT(clk_info->max_fract > 2);
uint32_t div_denom = 2;
uint32_t div_numer = 0;
uint32_t div_integ = clk_info->src_freq_hz / clk_info->exp_freq_hz;
uint32_t freq_error = clk_info->src_freq_hz % clk_info->exp_freq_hz;
// fractional divider
if (freq_error) {
// Carry bit if the decimal is greater than 1.0 - 1.0 / ((max_fract - 1) * 2)
if (freq_error < clk_info->exp_freq_hz - clk_info->exp_freq_hz / (clk_info->max_fract - 1) * 2) {
// Calculate the Greatest Common Divisor, time complexity O(log n)
uint32_t gcd = _gcd(clk_info->exp_freq_hz, freq_error);
// divide by the Greatest Common Divisor to get the accurate fraction before normalization
div_denom = clk_info->exp_freq_hz / gcd;
div_numer = freq_error / gcd;
// normalize div_denom and div_numer
uint32_t d = div_denom / clk_info->max_fract + 1;
// divide by the normalization coefficient to get the denominator and numerator within range of clk_info->max_fract
div_denom /= d;
div_numer /= d;
} else {
div_integ++;
}
}
// If the expect frequency is too high or too low to satisfy the integral division range, failed and return 0
if (div_integ < clk_info->min_integ || div_integ >= clk_info->max_integ || div_integ == 0) {
return 0;
}
// Assign result
clk_div->integer = div_integ;
clk_div->denominator = div_denom;
clk_div->numerator = div_numer;
// Return the actual frequency
if (div_numer) {
uint32_t temp = div_integ * div_denom + div_numer;
return (uint32_t)(((uint64_t)clk_info->src_freq_hz * div_denom + temp / 2) / temp);
}
return clk_info->src_freq_hz / div_integ;
}
uint32_t hal_utils_calc_clk_div_frac_accurate(const hal_utils_clk_info_t *clk_info, hal_utils_clk_div_t *clk_div)
{
HAL_ASSERT(clk_info->max_fract > 2);
uint32_t div_denom = 2;
uint32_t div_numer = 0;
uint32_t div_integ = clk_info->src_freq_hz / clk_info->exp_freq_hz;
uint32_t freq_error = clk_info->src_freq_hz % clk_info->exp_freq_hz;
if (freq_error) {
// Carry bit if the decimal is greater than 1.0 - 1.0 / ((max_fract - 1) * 2)
if (freq_error < clk_info->exp_freq_hz - clk_info->exp_freq_hz / (clk_info->max_fract - 1) * 2) {
// Search the closest fraction, time complexity O(n)
for (uint32_t sub = 0, a = 2, b = 0, min = UINT32_MAX; min && a < clk_info->max_fract; a++) {
b = (a * freq_error + clk_info->exp_freq_hz / 2) / clk_info->exp_freq_hz;
sub = _sub_abs(clk_info->exp_freq_hz * b, freq_error * a);
if (sub < min) {
div_denom = a;
div_numer = b;
min = sub;
}
}
} else {
div_integ++;
}
}
// If the expect frequency is too high or too low to satisfy the integral division range, failed and return 0
if (div_integ < clk_info->min_integ || div_integ >= clk_info->max_integ || div_integ == 0) {
return 0;
}
// Assign result
clk_div->integer = div_integ;
clk_div->denominator = div_denom;
clk_div->numerator = div_numer;
// Return the actual frequency
if (div_numer) {
uint32_t temp = div_integ * div_denom + div_numer;
return (uint32_t)(((uint64_t)clk_info->src_freq_hz * div_denom + temp / 2) / temp);
}
return clk_info->src_freq_hz / div_integ;
}
uint32_t hal_utils_calc_clk_div_integer(const hal_utils_clk_info_t *clk_info, uint32_t *int_div)
{
uint32_t div_integ = clk_info->src_freq_hz / clk_info->exp_freq_hz;
uint32_t freq_error = clk_info->src_freq_hz % clk_info->exp_freq_hz;
/* If there is error and always round up,
Or, do the normal rounding and error >= (src/n + src/(n+1)) / 2,
then carry the bit */
if ((freq_error && clk_info->round_opt == HAL_DIV_ROUND_UP) || (clk_info->round_opt == HAL_DIV_ROUND &&
(freq_error >= clk_info->src_freq_hz / (2 * div_integ * (div_integ + 1))))) {
div_integ++;
}
// Assign result
*int_div = div_integ;
// Return the actual frequency
return clk_info->src_freq_hz / div_integ;
}