aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authoreZio Pan <[email protected]>2024-03-15 15:12:51 +0800
committereZio Pan <[email protected]>2024-03-23 09:15:25 +0800
commitb595d942442a8b267e1311bcadedc8558183aa61 (patch)
tree99f599067d49df98e4abab75d4ec708496b2501f
parentcf065d439efed2141aaf09454beb445e80dc7539 (diff)
stm32 CORDIC: split into multiple files
Diffstat
-rw-r--r--embassy-stm32/src/cordic.rs460
-rw-r--r--embassy-stm32/src/cordic/enums.rs82
-rw-r--r--embassy-stm32/src/cordic/mod.rs206
-rw-r--r--embassy-stm32/src/cordic/sealed.rs116
-rw-r--r--embassy-stm32/src/cordic/utils.rs59
5 files changed, 463 insertions, 460 deletions
diff --git a/embassy-stm32/src/cordic.rs b/embassy-stm32/src/cordic.rs
deleted file mode 100644
index 952ee187a..000000000
--- a/embassy-stm32/src/cordic.rs
+++ /dev/null
@@ -1,460 +0,0 @@
1//! CORDIC co-processor
2
3use crate::peripherals;
4use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef};
5
6pub use enums::*;
7
8mod enums {
9 /// CORDIC function
10 #[allow(missing_docs)]
11 #[derive(Clone, Copy)]
12 pub enum Function {
13 Cos = 0,
14 Sin,
15 Phase,
16 Modulus,
17 Arctan,
18 Cosh,
19 Sinh,
20 Arctanh,
21 Ln,
22 Sqrt,
23 }
24
25 /// CORDIC precision
26 #[allow(missing_docs)]
27 #[derive(Clone, Copy)]
28 pub enum Precision {
29 Iters4 = 1,
30 Iters8,
31 Iters12,
32 Iters16,
33 Iters20,
34 Iters24,
35 Iters28,
36 Iters32,
37 Iters36,
38 Iters40,
39 Iters44,
40 Iters48,
41 Iters52,
42 Iters56,
43 Iters60,
44 }
45
46 /// CORDIC scale
47 #[allow(non_camel_case_types)]
48 #[allow(missing_docs)]
49 #[derive(Clone, Copy, Default)]
50 pub enum Scale {
51 #[default]
52 A1_R1 = 0,
53 A1o2_R2,
54 A1o4_R4,
55 A1o8_R8,
56 A1o16_R16,
57 A1o32_R32,
58 A1o64_R64,
59 A1o128_R128,
60 }
61
62 /// CORDIC argument/result count
63 #[allow(missing_docs)]
64 #[derive(Clone, Copy, Default)]
65 pub enum Count {
66 #[default]
67 One,
68 Two,
69 }
70
71 /// CORDIC argument/result data width
72 #[allow(missing_docs)]
73 #[derive(Clone, Copy)]
74 pub enum Width {
75 Bits32,
76 Bits16,
77 }
78
79 /// Cordic driver running mode
80 #[derive(Clone, Copy)]
81 pub enum Mode {
82 /// After caculation start, a read to RDATA register will block AHB until the caculation finished
83 ZeroOverhead,
84
85 /// Use CORDIC interrupt to trigger a read result value
86 Interrupt,
87
88 /// Use DMA to write/read value
89 Dma,
90 }
91}
92
93/// Low-level CORDIC access.
94#[cfg(feature = "unstable-pac")]
95pub mod low_level {
96 pub use super::sealed::*;
97}
98
99pub(crate) mod sealed {
100 use super::*;
101 use crate::pac::cordic::vals;
102
103 /// Cordic instance
104 pub trait Instance {
105 /// Get access to CORDIC registers
106 fn regs() -> crate::pac::cordic::Cordic;
107
108 /// Set Function value
109 fn set_func(&self, func: Function) {
110 Self::regs()
111 .csr()
112 .modify(|v| v.set_func(vals::Func::from_bits(func as u8)));
113 }
114
115 /// Set Precision value
116 fn set_precision(&self, precision: Precision) {
117 Self::regs()
118 .csr()
119 .modify(|v| v.set_precision(vals::Precision::from_bits(precision as u8)))
120 }
121
122 /// Set Scale value
123 fn set_scale(&self, scale: Scale) {
124 Self::regs()
125 .csr()
126 .modify(|v| v.set_scale(vals::Scale::from_bits(scale as u8)))
127 }
128
129 /// Enable global interrupt
130 fn enable_irq(&self) {
131 Self::regs().csr().modify(|v| v.set_ien(true))
132 }
133
134 /// Disable global interrupt
135 fn disable_irq(&self) {
136 Self::regs().csr().modify(|v| v.set_ien(false))
137 }
138
139 /// Enable Read DMA
140 fn enable_read_dma(&self) {
141 Self::regs().csr().modify(|v| {
142 v.set_dmaren(true);
143 })
144 }
145
146 /// Disable Read DMA
147 fn disable_read_dma(&self) {
148 Self::regs().csr().modify(|v| {
149 v.set_dmaren(false);
150 })
151 }
152
153 /// Enable Write DMA
154 fn enable_write_dma(&self) {
155 Self::regs().csr().modify(|v| {
156 v.set_dmawen(true);
157 })
158 }
159
160 /// Disable Write DMA
161 fn disable_write_dma(&self) {
162 Self::regs().csr().modify(|v| {
163 v.set_dmawen(false);
164 })
165 }
166
167 /// Set NARGS value
168 fn set_argument_count(&self, n: Count) {
169 Self::regs().csr().modify(|v| {
170 v.set_nargs(match n {
171 Count::One => vals::Num::NUM1,
172 Count::Two => vals::Num::NUM2,
173 })
174 })
175 }
176
177 /// Set NRES value
178 fn set_result_count(&self, n: Count) {
179 Self::regs().csr().modify(|v| {
180 v.set_nres(match n {
181 Count::One => vals::Num::NUM1,
182 Count::Two => vals::Num::NUM2,
183 });
184 })
185 }
186
187 /// Set ARGSIZE and RESSIZE value
188 fn set_data_width(&self, arg: Width, res: Width) {
189 Self::regs().csr().modify(|v| {
190 v.set_argsize(match arg {
191 Width::Bits32 => vals::Size::BITS32,
192 Width::Bits16 => vals::Size::BITS16,
193 });
194 v.set_ressize(match res {
195 Width::Bits32 => vals::Size::BITS32,
196 Width::Bits16 => vals::Size::BITS16,
197 })
198 })
199 }
200
201 /// Read RRDY flag
202 fn ready_to_read(&self) -> bool {
203 Self::regs().csr().read().rrdy()
204 }
205
206 /// Write value to WDATA
207 fn write_argument(&self, arg: u32) {
208 Self::regs().wdata().write_value(arg)
209 }
210
211 /// Read value from RDATA
212 fn read_result(&self) -> u32 {
213 Self::regs().rdata().read()
214 }
215 }
216}
217
218/// CORDIC driver
219pub struct Cordic<'d, T: Instance> {
220 cordic: PeripheralRef<'d, T>,
221 config: Config,
222 //state: State,
223}
224
225/// CORDIC instance trait
226pub trait Instance: sealed::Instance + Peripheral<P = Self> + crate::rcc::RccPeripheral {}
227
228/// CORDIC configuration
229pub struct Config {
230 function: Function,
231 precision: Precision,
232 scale: Scale,
233 mode: Mode,
234 first_result: bool,
235}
236
237// CORDIC running state
238//struct State {
239// input_buf: [u32; 8],
240// buf_len: usize,
241//}
242
243impl Config {
244 /// Create a config for Cordic driver
245 pub fn new(function: Function, precision: Precision, scale: Option<Scale>, mode: Mode, first_result: bool) -> Self {
246 Self {
247 function,
248 precision,
249 scale: scale.unwrap_or_default(),
250 mode,
251 first_result,
252 }
253 }
254
255 fn check_scale(&self) -> bool {
256 let scale_raw = self.scale as u8;
257
258 match self.function {
259 Function::Cos | Function::Sin | Function::Phase | Function::Modulus => 0 == scale_raw,
260 Function::Arctan => (0..=7).contains(&scale_raw),
261 Function::Cosh | Function::Sinh | Function::Arctanh => 1 == scale_raw,
262 Function::Ln => (1..=4).contains(&scale_raw),
263 Function::Sqrt => (0..=2).contains(&scale_raw),
264 }
265 }
266}
267
268impl<'d, T: Instance> Cordic<'d, T> {
269 /// Create a Cordic driver instance
270 ///
271 /// Note:
272 /// If you need a periperhal -> CORDIC -> peripehral mode,
273 /// you may want to set Cordic into [Mode::ZeroOverhead] mode, and add extra arguemnts with [Self::extra_config]
274 pub fn new(cordic: impl Peripheral<P = T> + 'd, config: Config) -> Self {
275 T::enable_and_reset();
276
277 into_ref!(cordic);
278
279 if !config.check_scale() {
280 panic!("Scale value is not compatible with Function")
281 }
282
283 let mut instance = Self {
284 cordic,
285 config,
286 // state: State {
287 // input_buf: [0u32; 8],
288 // buf_len: 0,
289 // },
290 };
291
292 instance.reconfigure();
293
294 instance
295 }
296
297 /// Set a new config for Cordic driver
298 pub fn set_config(&mut self, config: Config) {
299 self.config = config;
300 self.reconfigure();
301 }
302
303 /// Set extra config for data count and data width.
304 pub fn extra_config(&mut self, arg_cnt: Count, arg_width: Width, res_width: Width) {
305 let peri = &self.cordic;
306 peri.set_argument_count(arg_cnt);
307 peri.set_data_width(arg_width, res_width);
308 }
309
310 fn reconfigure(&mut self) {
311 let peri = &self.cordic;
312 let config = &self.config;
313
314 if peri.ready_to_read() {
315 warn!("At least 1 result hasn't been read, reconfigure will cause DATA LOST");
316 };
317
318 peri.disable_irq();
319 peri.disable_write_dma();
320 peri.disable_read_dma();
321
322 // clean RRDY flag
323 while peri.ready_to_read() {
324 peri.read_result();
325 }
326
327 peri.set_func(config.function);
328 peri.set_precision(config.precision);
329 peri.set_scale(config.scale);
330 if config.first_result {
331 peri.set_result_count(Count::One)
332 } else {
333 peri.set_result_count(Count::Two)
334 }
335
336 match config.mode {
337 Mode::ZeroOverhead => (),
338 Mode::Interrupt => {
339 peri.enable_irq();
340 }
341 Mode::Dma => {
342 peri.enable_write_dma();
343 peri.enable_read_dma();
344 }
345 }
346
347 //self.state.input_buf.fill(0u32);
348 }
349
350 /// Run a CORDIC calculation
351 pub fn calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize {
352 match self.config.mode {
353 Mode::ZeroOverhead => {
354 if arg2s.is_none() {
355 self.cordic.set_argument_count(Count::One);
356
357 self.cordic.set_result_count(if self.config.first_result {
358 if output.len() < arg1s.len() {
359 panic!("Output buf length is not long enough")
360 }
361 Count::One
362 } else {
363 if output.len() < 2 * arg1s.len() {
364 panic!("Output buf length is not long enough")
365 }
366 Count::Two
367 });
368
369 let mut cnt = 0;
370
371 for &arg in arg1s.iter() {
372 self.cordic.write_argument(f64_to_q1_31(arg));
373 output[cnt] = q1_31_to_f64(self.cordic.read_result());
374 cnt += 1;
375 }
376
377 cnt
378 } else {
379 todo!()
380 }
381 }
382 Mode::Interrupt => todo!(),
383 Mode::Dma => todo!(),
384 }
385 }
386}
387
388impl<'d, T: Instance> Drop for Cordic<'d, T> {
389 fn drop(&mut self) {
390 T::disable();
391 }
392}
393
394foreach_interrupt!(
395 ($inst:ident, cordic, CORDIC, GLOBAL, $irq:ident) => {
396 impl Instance for peripherals::$inst {
397 }
398
399 impl sealed::Instance for peripherals::$inst {
400 fn regs() -> crate::pac::cordic::Cordic {
401 crate::pac::$inst
402 }
403 }
404 };
405);
406
407macro_rules! floating_fixed_convert {
408 ($f_to_q:ident, $q_to_f:ident, $unsigned_bin_typ:ty, $signed_bin_typ:ty, $float_ty:ty, $offset:literal, $min_positive:literal) => {
409 /// convert float point to fixed point format
410 pub fn $f_to_q(value: $float_ty) -> $unsigned_bin_typ {
411 const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) };
412
413 assert!(
414 (-1.0 as $float_ty) <= value,
415 "input value {} should be equal or greater than -1",
416 value
417 );
418
419 let value = if value == 1.0 as $float_ty{
420 (1.0 as $float_ty) - MIN_POSITIVE
421 } else {
422 assert!(
423 value <= (1.0 as $float_ty) - MIN_POSITIVE,
424 "input value {} should be equal or less than 1-2^(-{})",
425 value, $offset
426 );
427 value
428 };
429
430 (value * ((1 as $unsigned_bin_typ << $offset) as $float_ty)) as $unsigned_bin_typ
431 }
432
433 #[inline(always)]
434 /// convert fixed point to float point format
435 pub fn $q_to_f(value: $unsigned_bin_typ) -> $float_ty {
436 // It's needed to convert from unsigned to signed first, for correct result.
437 -(value as $signed_bin_typ as $float_ty) / ((1 as $unsigned_bin_typ << $offset) as $float_ty)
438 }
439 };
440}
441
442floating_fixed_convert!(
443 f64_to_q1_31,
444 q1_31_to_f64,
445 u32,
446 i32,
447 f64,
448 31,
449 0x3E00_0000_0000_0000u64 // binary form of 1f64^(-31)
450);
451
452floating_fixed_convert!(
453 f32_to_q1_15,
454 q1_15_to_f32,
455 u16,
456 i16,
457 f32,
458 15,
459 0x3800_0000u32 // binary form of 1f32^(-15)
460);
diff --git a/embassy-stm32/src/cordic/enums.rs b/embassy-stm32/src/cordic/enums.rs
new file mode 100644
index 000000000..4697a1df1
--- /dev/null
+++ b/embassy-stm32/src/cordic/enums.rs
@@ -0,0 +1,82 @@
1/// CORDIC function
2#[allow(missing_docs)]
3#[derive(Clone, Copy)]
4pub enum Function {
5 Cos = 0,
6 Sin,
7 Phase,
8 Modulus,
9 Arctan,
10 Cosh,
11 Sinh,
12 Arctanh,
13 Ln,
14 Sqrt,
15}
16
17/// CORDIC precision
18#[allow(missing_docs)]
19#[derive(Clone, Copy)]
20pub enum Precision {
21 Iters4 = 1,
22 Iters8,
23 Iters12,
24 Iters16,
25 Iters20,
26 Iters24,
27 Iters28,
28 Iters32,
29 Iters36,
30 Iters40,
31 Iters44,
32 Iters48,
33 Iters52,
34 Iters56,
35 Iters60,
36}
37
38/// CORDIC scale
39#[allow(non_camel_case_types)]
40#[allow(missing_docs)]
41#[derive(Clone, Copy, Default)]
42pub enum Scale {
43 #[default]
44 A1_R1 = 0,
45 A1o2_R2,
46 A1o4_R4,
47 A1o8_R8,
48 A1o16_R16,
49 A1o32_R32,
50 A1o64_R64,
51 A1o128_R128,
52}
53
54/// CORDIC argument/result count
55#[allow(missing_docs)]
56#[derive(Clone, Copy, Default)]
57pub enum Count {
58 #[default]
59 One,
60 Two,
61}
62
63/// CORDIC argument/result data width
64#[allow(missing_docs)]
65#[derive(Clone, Copy)]
66pub enum Width {
67 Bits32,
68 Bits16,
69}
70
71/// Cordic driver running mode
72#[derive(Clone, Copy)]
73pub enum Mode {
74 /// After caculation start, a read to RDATA register will block AHB until the caculation finished
75 ZeroOverhead,
76
77 /// Use CORDIC interrupt to trigger a read result value
78 Interrupt,
79
80 /// Use DMA to write/read value
81 Dma,
82}
diff --git a/embassy-stm32/src/cordic/mod.rs b/embassy-stm32/src/cordic/mod.rs
new file mode 100644
index 000000000..c0a69b757
--- /dev/null
+++ b/embassy-stm32/src/cordic/mod.rs
@@ -0,0 +1,206 @@
1//! CORDIC co-processor
2
3use crate::peripherals;
4use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef};
5
6mod enums;
7pub use enums::*;
8
9pub mod utils;
10
11pub(crate) mod sealed;
12
13/// Low-level CORDIC access.
14#[cfg(feature = "unstable-pac")]
15pub mod low_level {
16 pub use super::sealed::*;
17}
18
19/// CORDIC driver
20pub struct Cordic<'d, T: Instance> {
21 cordic: PeripheralRef<'d, T>,
22 config: Config,
23 //state: State,
24}
25
26/// CORDIC instance trait
27pub trait Instance: sealed::Instance + Peripheral<P = Self> + crate::rcc::RccPeripheral {}
28
29/// CORDIC configuration
30pub struct Config {
31 function: Function,
32 precision: Precision,
33 scale: Scale,
34 mode: Mode,
35 first_result: bool,
36}
37
38// CORDIC running state
39//struct State {
40// input_buf: [u32; 8],
41// buf_len: usize,
42//}
43
44impl Config {
45 /// Create a config for Cordic driver
46 pub fn new(function: Function, precision: Precision, scale: Option<Scale>, mode: Mode, first_result: bool) -> Self {
47 Self {
48 function,
49 precision,
50 scale: scale.unwrap_or_default(),
51 mode,
52 first_result,
53 }
54 }
55
56 fn check_scale(&self) -> bool {
57 let scale_raw = self.scale as u8;
58
59 match self.function {
60 Function::Cos | Function::Sin | Function::Phase | Function::Modulus => 0 == scale_raw,
61 Function::Arctan => (0..=7).contains(&scale_raw),
62 Function::Cosh | Function::Sinh | Function::Arctanh => 1 == scale_raw,
63 Function::Ln => (1..=4).contains(&scale_raw),
64 Function::Sqrt => (0..=2).contains(&scale_raw),
65 }
66 }
67}
68
69impl<'d, T: Instance> Cordic<'d, T> {
70 /// Create a Cordic driver instance
71 ///
72 /// Note:
73 /// If you need a periperhal -> CORDIC -> peripehral mode,
74 /// you may want to set Cordic into [Mode::ZeroOverhead] mode, and add extra arguemnts with [Self::extra_config]
75 pub fn new(cordic: impl Peripheral<P = T> + 'd, config: Config) -> Self {
76 T::enable_and_reset();
77
78 into_ref!(cordic);
79
80 if !config.check_scale() {
81 panic!("Scale value is not compatible with Function")
82 }
83
84 let mut instance = Self {
85 cordic,
86 config,
87 // state: State {
88 // input_buf: [0u32; 8],
89 // buf_len: 0,
90 // },
91 };
92
93 instance.reconfigure();
94
95 instance
96 }
97
98 /// Set a new config for Cordic driver
99 pub fn set_config(&mut self, config: Config) {
100 self.config = config;
101 self.reconfigure();
102 }
103
104 /// Set extra config for data count and data width.
105 pub fn extra_config(&mut self, arg_cnt: Count, arg_width: Width, res_width: Width) {
106 let peri = &self.cordic;
107 peri.set_argument_count(arg_cnt);
108 peri.set_data_width(arg_width, res_width);
109 }
110
111 fn reconfigure(&mut self) {
112 let peri = &self.cordic;
113 let config = &self.config;
114
115 if peri.ready_to_read() {
116 warn!("At least 1 result hasn't been read, reconfigure will cause DATA LOST");
117 };
118
119 peri.disable_irq();
120 peri.disable_write_dma();
121 peri.disable_read_dma();
122
123 // clean RRDY flag
124 while peri.ready_to_read() {
125 peri.read_result();
126 }
127
128 peri.set_func(config.function);
129 peri.set_precision(config.precision);
130 peri.set_scale(config.scale);
131 if config.first_result {
132 peri.set_result_count(Count::One)
133 } else {
134 peri.set_result_count(Count::Two)
135 }
136
137 match config.mode {
138 Mode::ZeroOverhead => (),
139 Mode::Interrupt => {
140 peri.enable_irq();
141 }
142 Mode::Dma => {
143 peri.enable_write_dma();
144 peri.enable_read_dma();
145 }
146 }
147
148 //self.state.input_buf.fill(0u32);
149 }
150
151 /// Run a CORDIC calculation
152 pub fn calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize {
153 match self.config.mode {
154 Mode::ZeroOverhead => {
155 if arg2s.is_none() {
156 self.cordic.set_argument_count(Count::One);
157
158 self.cordic.set_result_count(if self.config.first_result {
159 if output.len() < arg1s.len() {
160 panic!("Output buf length is not long enough")
161 }
162 Count::One
163 } else {
164 if output.len() < 2 * arg1s.len() {
165 panic!("Output buf length is not long enough")
166 }
167 Count::Two
168 });
169
170 let mut cnt = 0;
171
172 for &arg in arg1s.iter() {
173 self.cordic.write_argument(utils::f64_to_q1_31(arg));
174 output[cnt] = utils::q1_31_to_f64(self.cordic.read_result());
175 cnt += 1;
176 }
177
178 cnt
179 } else {
180 todo!()
181 }
182 }
183 Mode::Interrupt => todo!(),
184 Mode::Dma => todo!(),
185 }
186 }
187}
188
189impl<'d, T: Instance> Drop for Cordic<'d, T> {
190 fn drop(&mut self) {
191 T::disable();
192 }
193}
194
195foreach_interrupt!(
196 ($inst:ident, cordic, CORDIC, GLOBAL, $irq:ident) => {
197 impl Instance for peripherals::$inst {
198 }
199
200 impl sealed::Instance for peripherals::$inst {
201 fn regs() -> crate::pac::cordic::Cordic {
202 crate::pac::$inst
203 }
204 }
205 };
206);
diff --git a/embassy-stm32/src/cordic/sealed.rs b/embassy-stm32/src/cordic/sealed.rs
new file mode 100644
index 000000000..0f00e380c
--- /dev/null
+++ b/embassy-stm32/src/cordic/sealed.rs
@@ -0,0 +1,116 @@
1use super::*;
2use crate::pac::cordic::vals;
3
4/// Cordic instance
5pub trait Instance {
6 /// Get access to CORDIC registers
7 fn regs() -> crate::pac::cordic::Cordic;
8
9 /// Set Function value
10 fn set_func(&self, func: Function) {
11 Self::regs()
12 .csr()
13 .modify(|v| v.set_func(vals::Func::from_bits(func as u8)));
14 }
15
16 /// Set Precision value
17 fn set_precision(&self, precision: Precision) {
18 Self::regs()
19 .csr()
20 .modify(|v| v.set_precision(vals::Precision::from_bits(precision as u8)))
21 }
22
23 /// Set Scale value
24 fn set_scale(&self, scale: Scale) {
25 Self::regs()
26 .csr()
27 .modify(|v| v.set_scale(vals::Scale::from_bits(scale as u8)))
28 }
29
30 /// Enable global interrupt
31 fn enable_irq(&self) {
32 Self::regs().csr().modify(|v| v.set_ien(true))
33 }
34
35 /// Disable global interrupt
36 fn disable_irq(&self) {
37 Self::regs().csr().modify(|v| v.set_ien(false))
38 }
39
40 /// Enable Read DMA
41 fn enable_read_dma(&self) {
42 Self::regs().csr().modify(|v| {
43 v.set_dmaren(true);
44 })
45 }
46
47 /// Disable Read DMA
48 fn disable_read_dma(&self) {
49 Self::regs().csr().modify(|v| {
50 v.set_dmaren(false);
51 })
52 }
53
54 /// Enable Write DMA
55 fn enable_write_dma(&self) {
56 Self::regs().csr().modify(|v| {
57 v.set_dmawen(true);
58 })
59 }
60
61 /// Disable Write DMA
62 fn disable_write_dma(&self) {
63 Self::regs().csr().modify(|v| {
64 v.set_dmawen(false);
65 })
66 }
67
68 /// Set NARGS value
69 fn set_argument_count(&self, n: Count) {
70 Self::regs().csr().modify(|v| {
71 v.set_nargs(match n {
72 Count::One => vals::Num::NUM1,
73 Count::Two => vals::Num::NUM2,
74 })
75 })
76 }
77
78 /// Set NRES value
79 fn set_result_count(&self, n: Count) {
80 Self::regs().csr().modify(|v| {
81 v.set_nres(match n {
82 Count::One => vals::Num::NUM1,
83 Count::Two => vals::Num::NUM2,
84 });
85 })
86 }
87
88 /// Set ARGSIZE and RESSIZE value
89 fn set_data_width(&self, arg: Width, res: Width) {
90 Self::regs().csr().modify(|v| {
91 v.set_argsize(match arg {
92 Width::Bits32 => vals::Size::BITS32,
93 Width::Bits16 => vals::Size::BITS16,
94 });
95 v.set_ressize(match res {
96 Width::Bits32 => vals::Size::BITS32,
97 Width::Bits16 => vals::Size::BITS16,
98 })
99 })
100 }
101
102 /// Read RRDY flag
103 fn ready_to_read(&self) -> bool {
104 Self::regs().csr().read().rrdy()
105 }
106
107 /// Write value to WDATA
108 fn write_argument(&self, arg: u32) {
109 Self::regs().wdata().write_value(arg)
110 }
111
112 /// Read value from RDATA
113 fn read_result(&self) -> u32 {
114 Self::regs().rdata().read()
115 }
116}
diff --git a/embassy-stm32/src/cordic/utils.rs b/embassy-stm32/src/cordic/utils.rs
new file mode 100644
index 000000000..3f055c34b
--- /dev/null
+++ b/embassy-stm32/src/cordic/utils.rs
@@ -0,0 +1,59 @@
1//! Common match utils
2
3macro_rules! floating_fixed_convert {
4 ($f_to_q:ident, $q_to_f:ident, $unsigned_bin_typ:ty, $signed_bin_typ:ty, $float_ty:ty, $offset:literal, $min_positive:literal) => {
5 /// convert float point to fixed point format
6 pub fn $f_to_q(value: $float_ty) -> $unsigned_bin_typ {
7 const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) };
8
9 assert!(
10 (-1.0 as $float_ty) <= value,
11 "input value {} should be equal or greater than -1",
12 value
13 );
14
15
16 let value = if value == 1.0 as $float_ty{
17 // make a exception for user specifing exact 1.0 float point,
18 // convert 1.0 to max representable value of q1.x format
19 (1.0 as $float_ty) - MIN_POSITIVE
20 } else {
21 assert!(
22 value <= (1.0 as $float_ty) - MIN_POSITIVE,
23 "input value {} should be equal or less than 1-2^(-{})",
24 value, $offset
25 );
26 value
27 };
28
29 (value * ((1 as $unsigned_bin_typ << $offset) as $float_ty)) as $unsigned_bin_typ
30 }
31
32 #[inline(always)]
33 /// convert fixed point to float point format
34 pub fn $q_to_f(value: $unsigned_bin_typ) -> $float_ty {
35 // It's needed to convert from unsigned to signed first, for correct result.
36 -(value as $signed_bin_typ as $float_ty) / ((1 as $unsigned_bin_typ << $offset) as $float_ty)
37 }
38 };
39}
40
41floating_fixed_convert!(
42 f64_to_q1_31,
43 q1_31_to_f64,
44 u32,
45 i32,
46 f64,
47 31,
48 0x3E00_0000_0000_0000u64 // binary form of 1f64^(-31)
49);
50
51floating_fixed_convert!(
52 f32_to_q1_15,
53 q1_15_to_f32,
54 u16,
55 i16,
56 f32,
57 15,
58 0x3800_0000u32 // binary form of 1f32^(-15)
59);
generated by cgit (git 2.34.1) at 2026-04-07 06:20:35 +0000