1 /*
2 * Copyright (c) 2023 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8 #include "hpm_common.h"
9 #include "hpm_rdc_drv.h"
10
rdc_output_config(RDC_Type * ptr,rdc_output_cfg_t * cfg)11 void rdc_output_config(RDC_Type *ptr, rdc_output_cfg_t *cfg)
12 {
13 uint32_t rate;
14
15 rate = cfg->excitation_period_cycle >> (cfg->excitation_precision + 2);
16 ptr->EXC_TIMMING = RDC_EXC_TIMMING_SMP_RATE_SET(rate) |
17 RDC_EXC_TIMMING_SMP_NUM_SET(cfg->excitation_precision) |
18 RDC_EXC_TIMMING_PWM_PRD_SET(cfg->pwm_period)|
19 RDC_EXC_TIMMING_SWAP_SET(cfg->output_swap);
20 if (cfg->mode == rdc_output_dac) {
21 ptr->EXC_SCALING = RDC_EXC_SCALING_AMP_MAN_SET(cfg->amp_man) |
22 RDC_EXC_SCALING_AMP_EXP_SET(cfg->amp_exp);
23 ptr->EXC_OFFSET = RDC_EXC_OFFSET_AMP_OFFSET_SET(cfg->amp_offset + 0x800000);
24 ptr->OUT_CTL = RDC_OUT_CTL_CH_I_SEL_SET(cfg->dac_chn_i_sel) |
25 RDC_OUT_CTL_CH_Q_SEL_SET(cfg->dac_chn_q_sel);
26 } else if (cfg->mode == rdc_output_pwm) {
27 ptr->PWM_SCALING = RDC_EXC_SCALING_AMP_MAN_SET(cfg->amp_man) |
28 RDC_EXC_SCALING_AMP_EXP_SET(cfg->amp_exp) |
29 RDC_PWM_SCALING_DITHER_SET(cfg->pwm_dither_enable) |
30 RDC_PWM_SCALING_P_POL_SET(cfg->pwm_exc_p_low_active) |
31 RDC_PWM_SCALING_N_POL_SET(cfg->pwm_exc_n_low_active);
32 ptr->PWM_OFFSET = RDC_PWM_OFFSET_AMP_OFFSET_SET(cfg->amp_offset + (rate >> 1));
33 ptr->PWM_DZ = RDC_PWM_DZ_DZ_N_SET(cfg->pwm_deadzone_n) |
34 RDC_PWM_DZ_DZ_P_SET(cfg->pwm_deadzone_p);
35 }
36 if (cfg->trig_by_hw) {
37 ptr->EXC_SYNC_DLY = RDC_EXC_SYNC_DLY_DELAY_SET(cfg->hw_trig_delay);
38 } else {
39 ptr->EXC_SYNC_DLY = RDC_EXC_SYNC_DLY_DISABLE_MASK;
40 }
41 }
42
43
rdc_input_config(RDC_Type * ptr,rdc_input_cfg_t * cfg)44 void rdc_input_config(RDC_Type *ptr, rdc_input_cfg_t *cfg)
45 {
46 ptr->RDC_CTL = (ptr->RDC_CTL & (~(RDC_RDC_CTL_RECTIFY_SEL_MASK | RDC_RDC_CTL_ACC_LEN_MASK
47 #if defined(HPM_IP_FEATURE_RDC_IIR) && (HPM_IP_FEATURE_RDC_IIR)
48 | RDC_RDC_CTL_ACC_FAST_MASK
49 #endif
50 | RDC_RDC_CTL_TS_SEL_MASK)))
51 | RDC_RDC_CTL_RECTIFY_SEL_SET(cfg->rectify_signal_sel)
52 | RDC_RDC_CTL_ACC_LEN_SET(cfg->acc_cycle_len)
53 #if defined(HPM_IP_FEATURE_RDC_IIR) && (HPM_IP_FEATURE_RDC_IIR)
54 | RDC_RDC_CTL_ACC_FAST_SET(cfg->acc_fast)
55 #endif
56 | RDC_RDC_CTL_TS_SEL_SET(cfg->acc_stamp);
57 ptr->IN_CTL = RDC_IN_CTL_PORT_I_SEL_SET(cfg->acc_input_port_i) |
58 RDC_IN_CTL_CH_I_SEL_SET(cfg->acc_input_chn_i) |
59 RDC_IN_CTL_PORT_Q_SEL_SET(cfg->acc_input_port_q) |
60 RDC_IN_CTL_CH_Q_SEL_SET(cfg->acc_input_chn_q);
61 }
62
rdc_get_acc_avl(RDC_Type * ptr,rdc_input_acc_chn_t chn)63 uint32_t rdc_get_acc_avl(RDC_Type *ptr, rdc_input_acc_chn_t chn)
64 {
65 if (chn == rdc_acc_chn_i) {
66 return RDC_ACC_I_ACC_GET(ptr->ACC_I);
67 } else {
68 return RDC_ACC_Q_ACC_GET(ptr->ACC_Q);
69 }
70 }
71
rdc_output_trig_offset_config(RDC_Type * ptr,rdc_output_trig_chn_t chn,int32_t offset)72 void rdc_output_trig_offset_config(RDC_Type *ptr, rdc_output_trig_chn_t chn, int32_t offset)
73 {
74 if (chn == trigger_out_0) {
75 ptr->TRIG_OUT0_CFG = (ptr->TRIG_OUT0_CFG & (~RDC_TRIG_OUT0_CFG_LEAD_TIM_MASK)) |
76 RDC_TRIG_OUT0_CFG_LEAD_TIM_SET(offset + RDC_TRIG_OUT0_CFG_LEAD_TIM_MASK + 1);
77 } else if (chn == trigger_out_1) {
78 ptr->TRIG_OUT1_CFG = (ptr->TRIG_OUT1_CFG & (~RDC_TRIG_OUT1_CFG_LEAD_TIM_MASK)) |
79 RDC_TRIG_OUT1_CFG_LEAD_TIM_SET(offset + RDC_TRIG_OUT1_CFG_LEAD_TIM_MASK + 1);
80 }
81 }
82
rdc_output_trig_enable(RDC_Type * ptr,rdc_output_trig_chn_t chn)83 void rdc_output_trig_enable(RDC_Type *ptr, rdc_output_trig_chn_t chn)
84 {
85 if (chn == trigger_out_0) {
86 ptr->TRIG_OUT0_CFG |= RDC_TRIG_OUT0_CFG_ENABLE_MASK;
87 } else if (chn == trigger_out_1) {
88 ptr->TRIG_OUT1_CFG |= RDC_TRIG_OUT1_CFG_ENABLE_MASK;
89 }
90 }
91
rdc_output_trig_disable(RDC_Type * ptr,rdc_output_trig_chn_t chn)92 void rdc_output_trig_disable(RDC_Type *ptr, rdc_output_trig_chn_t chn)
93 {
94 if (chn == trigger_out_0) {
95 ptr->TRIG_OUT0_CFG &= ~RDC_TRIG_OUT0_CFG_ENABLE_MASK;
96 } else if (chn == trigger_out_1) {
97 ptr->TRIG_OUT1_CFG &= ~RDC_TRIG_OUT1_CFG_ENABLE_MASK;
98 }
99 }
100
rdc_get_i_maxval(RDC_Type * ptr)101 int32_t rdc_get_i_maxval(RDC_Type *ptr)
102 {
103 uint32_t val;
104
105 val = ptr->MAX_I;
106 if (RDC_MAX_I_VALID_GET(val)) {
107 return RDC_MAX_I_MAX_GET(val);
108 } else {
109 return -1;
110 }
111
112 }
113
rdc_get_i_minval(RDC_Type * ptr)114 int32_t rdc_get_i_minval(RDC_Type *ptr)
115 {
116 uint32_t val;
117
118 val = ptr->MIN_I;
119 if (RDC_MIN_I_VALID_GET(val)) {
120 return RDC_MIN_I_MIN_GET(val);
121 } else {
122 return -1;
123 }
124 }
125
rdc_get_q_maxval(RDC_Type * ptr)126 int32_t rdc_get_q_maxval(RDC_Type *ptr)
127 {
128 uint32_t val;
129
130 val = ptr->MAX_Q;
131 if (RDC_MAX_Q_VALID_GET(val)) {
132 return RDC_MAX_Q_MAX_GET(val);
133 } else {
134 return -1;
135 }
136 }
137
rdc_get_q_minval(RDC_Type * ptr)138 int32_t rdc_get_q_minval(RDC_Type *ptr)
139 {
140 uint32_t val;
141
142 val = ptr->MIN_Q;
143 if (RDC_MIN_Q_VALID_GET(val)) {
144 return RDC_MIN_Q_MIN_GET(val);
145 } else {
146 return -1;
147 }
148 }
149
rdc_set_edge_detection_offset(RDC_Type * ptr,rdc_input_acc_chn_t chn,int32_t offset)150 void rdc_set_edge_detection_offset(RDC_Type *ptr, rdc_input_acc_chn_t chn, int32_t offset)
151 {
152 if (chn == rdc_acc_chn_i) {
153 ptr->THRS_I = RDC_THRS_I_THRS_SET(offset);
154 } else {
155 ptr->THRS_Q = RDC_THRS_Q_THRS_SET(offset);
156 }
157 }
158
rdc_set_acc_config(RDC_Type * ptr,rdc_acc_cfg_t * cfg)159 void rdc_set_acc_config(RDC_Type *ptr, rdc_acc_cfg_t *cfg)
160 {
161 ptr->EDG_DET_CTL = RDC_EDG_DET_CTL_FILTER_SET(cfg->continue_edge_num) |
162 RDC_EDG_DET_CTL_HOLD_SET(cfg->edge_distance);
163 if (cfg->right_shift_without_sign < 9) {
164 ptr->ACC_SCALING = RDC_ACC_SCALING_ACC_SHIFT_SET(8 - cfg->right_shift_without_sign);
165 } else {
166 ptr->ACC_SCALING = RDC_ACC_SCALING_ACC_SHIFT_SET(cfg->right_shift_without_sign);
167 }
168 if (cfg->error_data_remove) {
169 ptr->ACC_SCALING |= RDC_ACC_SCALING_TOXIC_LK_MASK;
170 } else {
171 ptr->ACC_SCALING &= ~RDC_ACC_SCALING_TOXIC_LK_MASK;
172 }
173 ptr->EXC_PERIOD = RDC_EXC_PERIOD_EXC_PERIOD_SET(cfg->exc_carrier_period);
174 ptr->SYNC_DELAY_I = RDC_SYNC_DELAY_I_DELAY_SET(cfg->sync_delay_i);
175 ptr->SYNC_DELAY_Q = RDC_SYNC_DELAY_Q_DELAY_SET(cfg->sync_delay_q);
176 ptr->AMP_MAX = RDC_AMP_MAX_MAX_SET(cfg->amp_max);
177 ptr->AMP_MIN = RDC_AMP_MIN_MIN_SET(cfg->amp_min);
178 #if defined(HPM_IP_FEATURE_RDC_IIR) && (HPM_IP_FEATURE_RDC_IIR)
179 ptr->THRS_I = (ptr->THRS_I & ~RDC_THRS_I_THRS4ACC_MASK) | RDC_THRS_I_THRS4ACC_SET(cfg->enable_i_thrs_data_for_acc);
180 ptr->THRS_Q = (ptr->THRS_Q & ~RDC_THRS_Q_THRS4ACC_MASK) | RDC_THRS_Q_THRS4ACC_SET(cfg->enable_q_thrs_data_for_acc);
181 #endif
182 }
183
rdc_set_acc_sync_delay(RDC_Type * ptr,rdc_input_acc_chn_t chn,uint32_t delay)184 void rdc_set_acc_sync_delay(RDC_Type *ptr, rdc_input_acc_chn_t chn, uint32_t delay)
185 {
186 if (chn == rdc_acc_chn_i) {
187 ptr->SYNC_DELAY_I = RDC_SYNC_DELAY_I_DELAY_SET(delay);
188 } else {
189 ptr->SYNC_DELAY_Q = RDC_SYNC_DELAY_Q_DELAY_SET(delay);
190 }
191 }
192
193 #if defined(HPM_IP_FEATURE_RDC_IIR) && (HPM_IP_FEATURE_RDC_IIR)
194
195 #define RDC_IIR_LOG2 (0.301029995663f)
rdc_iir_log(float x)196 static float rdc_iir_log(float x)
197 {
198 if (x <= 0) {
199 return 0.0 / 0.0;
200 }
201
202 float result = 0;
203 float term = (x - 1) / (x + 1);
204 float power = term;
205 for (int n = 1; n <= 100; n += 2) {
206 result += power / n;
207 power *= term * term;
208 }
209
210 return 2 * result;
211 }
212
rdc_config_iir_parameter(RDC_Type * ptr,rdc_iir_cfg_t * iir_cfg)213 hpm_stat_t rdc_config_iir_parameter(RDC_Type *ptr, rdc_iir_cfg_t *iir_cfg)
214 {
215 int32_t b_val, a1_val, a2_val;
216
217 b_val = (rdc_iir_log(1.0f / iir_cfg->b) / RDC_IIR_LOG2) - 5;
218 if ((b_val < 0) || ((uint32_t)b_val > RDC_IIR_B_IIR_B_MASK)) {
219 return status_invalid_argument;
220 }
221 a1_val = iir_cfg->a1 * 256;
222 if ((a1_val < 0) || ((uint32_t)a1_val > RDC_IIR_A_IIR_A1_MASK)) {
223 return status_invalid_argument;
224 }
225 a2_val = iir_cfg->a2 * 256;
226 if ((a2_val < 0) || ((uint32_t)a2_val > (RDC_IIR_A_IIR_A2_MASK >> RDC_IIR_A_IIR_A2_SHIFT))) {
227 return status_invalid_argument;
228 }
229 ptr->IIR_B = RDC_IIR_B_IIR_B_SET(b_val) | RDC_IIR_B_LOWPASS_SET(iir_cfg->enable_lowpass);
230 ptr->IIR_A = RDC_IIR_A_IIR_A1_SET(a1_val) | RDC_IIR_A_IIR_A2_SET(a2_val);
231
232 return status_success;
233 }
234 #endif
235