1 /*
2 * Copyright (c) 2024 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8 #include "hpm_clc_drv.h"
9 #include "math.h"
10
clc_config_param(CLC_Type * clc,clc_chn_t chn,clc_param_config_t * param)11 void clc_config_param(CLC_Type *clc, clc_chn_t chn, clc_param_config_t *param)
12 {
13 clc->VDVQ_CHAN[chn].EADC_LOWTH = (uint32_t)param->eadc_lowth;
14 clc->VDVQ_CHAN[chn].EADC_HIGHTH = (uint32_t)param->eadc_highth;
15 clc->VDVQ_CHAN[chn].EADC_MIDLOWTH = (uint32_t)param->eadc_mid_lowth;
16 clc->VDVQ_CHAN[chn].EADC_MIDHIGHTH = (uint32_t)param->eadc_mid_highth;
17 clc->VDVQ_CHAN[chn].P2Z2_CLAMP_LO = (uint32_t)param->_2p2z_clamp_lowth;
18 clc->VDVQ_CHAN[chn].P2Z2_CLAMP_HI = (uint32_t)param->_2p2z_clamp_highth;
19 clc->VDVQ_CHAN[chn].P3Z3_CLAMP_LO = (uint32_t)param->_3p3z_clamp_lowth;
20 clc->VDVQ_CHAN[chn].P3Z3_CLAMP_HI = (uint32_t)param->_3p3z_clamp_highth;
21 clc->VDVQ_CHAN[chn].P3Z3_FORBID_LO = (uint32_t)param->output_forbid_lowth;
22 clc->VDVQ_CHAN[chn].P3Z3_FORBID_MD = (uint32_t)param->output_forbid_mid;
23 clc->VDVQ_CHAN[chn].P3Z3_FORBID_HI = (uint32_t)param->output_forbid_highth;
24 }
25
clc_config_coeff(CLC_Type * clc,clc_chn_t chn,clc_coeff_zone_t zone,clc_coeff_config_t * coeff)26 hpm_stat_t clc_config_coeff(CLC_Type *clc, clc_chn_t chn, clc_coeff_zone_t zone, clc_coeff_config_t *coeff)
27 {
28 float coeff_abs[7];
29 float f_max;
30 float f_coeff;
31 uint32_t u_max;
32 uint32_t coeff_ks;
33 uint32_t shift;
34
35 coeff_abs[0] = fabsf(coeff->b0);
36 coeff_abs[1] = fabsf(coeff->b1);
37 coeff_abs[2] = fabsf(coeff->b2);
38 coeff_abs[3] = fabsf(coeff->b3);
39 coeff_abs[4] = fabsf(coeff->a0);
40 coeff_abs[5] = fabsf(coeff->a1);
41 coeff_abs[6] = fabsf(coeff->a2);
42
43 if ((coeff_abs[0] > 0x7FFFFFFFFu) || (coeff_abs[1] > 0x7FFFFFFFFu) || (coeff_abs[2] > 0x7FFFFFFFFu) || (coeff_abs[3] >= 1.0f)
44 || (coeff_abs[4] > 0x7FFFFFFFFu) || (coeff_abs[5] > 0x7FFFFFFFFu) || (coeff_abs[6] >= 1.0f)) {
45 return status_invalid_argument;
46 }
47
48 f_max = coeff_abs[0];
49 for (uint8_t i = 1; i < 7u; i++) {
50 if (f_max < coeff_abs[i]) {
51 f_max = coeff_abs[i];
52 }
53 }
54 u_max = (uint32_t)f_max;
55
56 coeff_ks = 0;
57 for (uint8_t i = 0; i < 32u; i++) {
58 if (u_max == 0) {
59 coeff_ks = i;
60 break;
61 }
62 u_max >>= 1u;
63 }
64
65 shift = 31u - coeff_ks;
66 f_coeff = (float)(uint32_t)(1u << shift);
67 clc->VDVQ_CHAN[chn].COEFF[zone].COEFF_KS = coeff_ks;
68 clc->VDVQ_CHAN[chn].COEFF[zone].COEFF_B0 = (uint32_t)(int32_t)(coeff->b0 * f_coeff);
69 clc->VDVQ_CHAN[chn].COEFF[zone].COEFF_B1 = (uint32_t)(int32_t)(coeff->b1 * f_coeff);
70 clc->VDVQ_CHAN[chn].COEFF[zone].COEFF_B2 = (uint32_t)(int32_t)(coeff->b2 * f_coeff);
71 clc->VDVQ_CHAN[chn].COEFF[zone].COEFF_B3 = (uint32_t)(int32_t)(coeff->b3 * (float)(0x80000000u));
72 clc->VDVQ_CHAN[chn].COEFF[zone].COEFF_A0 = (uint32_t)(int32_t)(coeff->a0 * f_coeff);
73 clc->VDVQ_CHAN[chn].COEFF[zone].COEFF_A1 = (uint32_t)(int32_t)(coeff->a1 * f_coeff);
74 clc->VDVQ_CHAN[chn].COEFF[zone].COEFF_A2 = (uint32_t)(int32_t)(coeff->a2 * (float)(0x80000000u));
75
76 return status_success;
77 }
78
clc_sw_inject_dq_adc_value(CLC_Type * clc,uint32_t d_value,uint32_t q_value)79 void clc_sw_inject_dq_adc_value(CLC_Type *clc, uint32_t d_value, uint32_t q_value)
80 {
81 clc_sw_inject_adc_value(clc, clc_vd_chn, d_value);
82 clc_sw_inject_adc_value(clc, clc_vq_chn, q_value);
83 clc_set_sw_inject_dq_adc_value_ready(clc);
84 }
85