tx stale nefunguje

This commit is contained in:
Priec
2025-11-12 23:37:32 +01:00
parent 66521896b3
commit fa343624e7
52 changed files with 133 additions and 150 deletions

View File

@@ -0,0 +1,166 @@
// src/bin/main.rs
#![no_std]
#![no_main]
use defmt::*;
use embassy_executor::Spawner;
use embassy_time::{Instant, Timer, Duration};
use embassy_stm32::dma::Request;
use embassy_stm32::gpio::{Input, Output, Level, Pull, Speed};
use dma_gpio::software_uart::{
dma_timer::{init_tim6_for_uart, init_tim7_for_uart},
gpio_dma_uart_rx::rx_dma_task,
debug::dump_tim6_regs,
};
use dma_gpio::config::{BAUD, RX_OVERSAMPLE, TX_OVERSAMPLE};
use dma_gpio::config::{TX_RING_BYTES, RX_RING_BYTES};
use dma_gpio::software_uart::gpio_dma_uart_tx::tx_dma_task;
use static_cell::StaticCell;
use embassy_futures::yield_now;
use dma_gpio::hw_uart_pc::usart1;
use dma_gpio::hw_uart_pc::driver::uart_task;
use embassy_stm32::usart::{BufferedUart, Config, BufferedInterruptHandler};
use embassy_stm32::peripherals;
use embassy_stm32::bind_interrupts;
use dma_gpio::config::{PIPE_HW_TX, PIPE_HW_RX, PIPE_SW_TX, PIPE_SW_RX};
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pipe::Pipe};
use dma_gpio::hw_uart_internal::usart2;
use dma_gpio::hw_uart_internal::driver::uart_task as uart_task_internal;
use dma_gpio::config::{PIPE_INT_TX, PIPE_INT_RX};
use {defmt_rtt as _, panic_probe as _};
bind_interrupts!(struct Irqs {
USART1 => BufferedInterruptHandler<peripherals::USART1>;
});
bind_interrupts!(struct Irqs2 {
USART2 => BufferedInterruptHandler<peripherals::USART2>;
});
// Software uart
pub const TIM6_UP_REQ: Request = 4;
static SW_TX_RING: StaticCell<[u32; TX_RING_BYTES]> = StaticCell::new();
static SW_RX_RING: StaticCell<[u8; RX_RING_BYTES]> = StaticCell::new();
#[embassy_executor::main]
async fn main(spawner: Spawner) {
info!("boot");
let p = embassy_stm32::init(Default::default());
info!("init m8");
// HARDWARE UART to the PC
let mut cfg = Config::default();
cfg.baudrate = BAUD;
static TX_BUF: StaticCell<[u8; 256]> = StaticCell::new();
static RX_BUF: StaticCell<[u8; 256]> = StaticCell::new();
let uart = BufferedUart::new(
p.USART1,
p.PA10, // RX pin
p.PA9, // TX pin
TX_BUF.init([0; 256]),
RX_BUF.init([0; 256]),
Irqs,
cfg,
).unwrap();
let yield_period = usart1::setup_and_spawn(BAUD);
spawner.spawn(uart_task(uart, &PIPE_HW_TX, &PIPE_HW_RX).unwrap());
// END OF HARDWARE UART to the PC
// INTERNAL HARDWARE UART (USART2)
let mut cfg2 = Config::default();
cfg2.baudrate = BAUD;
static TX_BUF2: StaticCell<[u8; 256]> = StaticCell::new();
static RX_BUF2: StaticCell<[u8; 256]> = StaticCell::new();
let uart2 = BufferedUart::new(
p.USART2,
p.PD6, // RX
p.PA2, // TX
TX_BUF2.init([0; 256]),
RX_BUF2.init([0; 256]),
Irqs2,
cfg2,
).unwrap();
let _ = usart2::setup_and_spawn(BAUD);
spawner.spawn(uart_task_internal(uart2, &PIPE_INT_TX, &PIPE_INT_RX).unwrap());
info!("USART2 ready");
// END OF INTERNAL HARDWARE UART (USART2)
// USART1 <-> USART2 bridge
spawner.spawn(bridge_usart1_rx_to_usart2_tx(&PIPE_HW_RX, &PIPE_INT_TX).unwrap());
spawner.spawn(bridge_usart2_rx_to_usart1_tx(&PIPE_INT_RX, &PIPE_HW_TX).unwrap());
info!("USART1 <-> USART2 bridge active");
// END OF USART1 <-> USART2 bridge
// SOFTWARE UART
let tx = Output::new(p.PB0, Level::High, Speed::VeryHigh);
init_tim6_for_uart(p.TIM6, BAUD, TX_OVERSAMPLE);
dump_tim6_regs();
// Safe one-time init from StaticCell
let sw_tx_ring: &mut [u32; TX_RING_BYTES] = SW_TX_RING.init([0; TX_RING_BYTES]);
let bsrr_ptr = embassy_stm32::pac::GPIOB.bsrr().as_ptr() as *mut u32; // POZOR B REGISTER
// let odr_ptr = embassy_stm32::pac::GPIOA.odr().as_ptr() as *mut u32; // NEEDS DECODE CHANGE
spawner.spawn(tx_dma_task(p.GPDMA1_CH0, bsrr_ptr, sw_tx_ring, &PIPE_SW_TX).unwrap());
// EDN OF SOFTWARE UART
let mut last_yield = Instant::now();
let mut buf = [0u8; 32];
let mut counter: u32 = 0;
loop {
info!("tick start");
// Timer::after(Duration::from_millis(100)).await;
// info!("tick end");
counter += 1;
let msg = if counter % 2 == 0 {
b"AAAAA\n"
} else {
b"Hello\n"
};
PIPE_SW_TX.write(msg).await;
info!("Sent: {:a}", msg);
Timer::after(Duration::from_secs(3)).await;
// Also read any incoming data from SW RX pipe
let n2 = PIPE_INT_RX.read(&mut buf).await;
if n2 > 0 {
info!("HW INT UART RX pipe: {:a}", &buf[..n2]);
}
yield_now().await;
}
}
#[embassy_executor::task]
pub async fn bridge_usart1_rx_to_usart2_tx(
usart1_rx: &'static Pipe<CriticalSectionRawMutex, 1024>,
usart2_tx: &'static Pipe<CriticalSectionRawMutex, 1024>,
) {
let mut buf = [0u8; 64];
loop {
let n = usart1_rx.read(&mut buf).await;
if n > 0 {
let _ = usart2_tx.write(&buf[..n]).await;
info!("bridge: USART1 -> USART2 sent {} bytes", n);
}
}
}
#[embassy_executor::task]
pub async fn bridge_usart2_rx_to_usart1_tx(
usart2_rx: &'static Pipe<CriticalSectionRawMutex, 1024>,
usart1_tx: &'static Pipe<CriticalSectionRawMutex, 1024>,
) {
let mut buf = [0u8; 64];
loop {
let n = usart2_rx.read(&mut buf).await;
if n > 0 {
let _ = usart1_tx.write(&buf[..n]).await;
info!("bridge: USART2 -> USART1 sent {} bytes", n);
}
}
}

View File

@@ -0,0 +1,35 @@
// src/config.rs
use crate::software_uart::uart_emulation::{Parity, StopBits, UartConfig};
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::pipe::Pipe;
pub const BAUD: u32 = 115_200;
// pub const TX_PIN_BIT: u8 = 2; // PA2
// pub const RX_PIN_BIT: u8 = 3; // PA3
pub const TX_PIN_BIT: u8 = 0; // PB2
pub const RX_PIN_BIT: u8 = 3; // PC3
pub const TX_OVERSAMPLE: u16 = 1;
pub const RX_OVERSAMPLE: u16 = 13;
pub const RX_RING_BYTES: usize = 4096;
pub const TX_RING_BYTES: usize = 4096;
pub const PIPE_HW_TX_SIZE: usize = 1024;
pub const PIPE_HW_RX_SIZE: usize = 1024;
pub const PIPE_SW_TX_SIZE: usize = 1024;
pub const PIPE_SW_RX_SIZE: usize = 1024;
pub const PIPE_INT_TX_SIZE: usize = 1024;
pub const PIPE_INT_RX_SIZE: usize = 1024;
pub static PIPE_HW_TX: Pipe<CriticalSectionRawMutex, PIPE_HW_TX_SIZE> = Pipe::new();
pub static PIPE_HW_RX: Pipe<CriticalSectionRawMutex, PIPE_HW_RX_SIZE> = Pipe::new();
pub static PIPE_SW_TX: Pipe<CriticalSectionRawMutex, PIPE_SW_TX_SIZE> = Pipe::new();
pub static PIPE_SW_RX: Pipe<CriticalSectionRawMutex, PIPE_SW_RX_SIZE> = Pipe::new();
pub static PIPE_INT_TX: Pipe<CriticalSectionRawMutex, PIPE_INT_TX_SIZE> = Pipe::new();
pub static PIPE_INT_RX: Pipe<CriticalSectionRawMutex, PIPE_INT_RX_SIZE> = Pipe::new();
pub const UART_CFG: UartConfig = UartConfig {
data_bits: 8,
parity: Parity::None,
stop_bits: StopBits::One,
};

View File

@@ -0,0 +1,39 @@
// src/hw_uart_internal/driver.rs
use defmt::unwrap;
use embassy_futures::select::{select, Either};
use embassy_stm32::usart::BufferedUart;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::pipe::Pipe;
use embedded_io_async::{Read, Write};
use crate::hw_uart_pc::safety::{RX_PIPE_CAP, TX_PIPE_CAP};
#[embassy_executor::task]
pub async fn uart_task(
mut uart: BufferedUart<'static>,
tx_pipe: &'static Pipe<CriticalSectionRawMutex, TX_PIPE_CAP>,
rx_pipe: &'static Pipe<CriticalSectionRawMutex, RX_PIPE_CAP>,
) {
let mut rx_byte = [0u8; 1];
let mut tx_buf = [0u8; 64];
loop {
let rx_fut = uart.read(&mut rx_byte);
let tx_fut = async {
let n = tx_pipe.read(&mut tx_buf).await;
n
};
match select(rx_fut, tx_fut).await {
// Incoming data from UART hardware
Either::First(res) => {
if let Ok(_) = res {
let _ = rx_pipe.write(&rx_byte).await;
}
}
// Outgoing data waiting in TX pipe
Either::Second(n) => {
unwrap!(uart.write(&tx_buf[..n]).await);
}
}
}
}

View File

@@ -0,0 +1,4 @@
// src/hw_uart_internal/mod.rs
pub mod driver;
pub mod usart2;

View File

@@ -0,0 +1,11 @@
// src/hw_uart_internal/usart2.rs
use defmt::info;
use embassy_time::Duration;
use crate::hw_uart_pc::safety::preflight_and_suggest_yield_period;
pub fn setup_and_spawn(baudrate: u32) -> Duration {
let yield_period = preflight_and_suggest_yield_period(baudrate);
info!("HW USART2 safe");
yield_period
}

View File

@@ -0,0 +1,39 @@
// src/hw_uart_pc/driver.rs
use defmt::unwrap;
use embassy_futures::select::{select, Either};
use embassy_stm32::usart::BufferedUart;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::pipe::Pipe;
use embedded_io_async::{Read, Write};
use crate::hw_uart_pc::safety::{RX_PIPE_CAP, TX_PIPE_CAP};
#[embassy_executor::task]
pub async fn uart_task(
mut uart: BufferedUart<'static>,
tx_pipe: &'static Pipe<CriticalSectionRawMutex, TX_PIPE_CAP>,
rx_pipe: &'static Pipe<CriticalSectionRawMutex, RX_PIPE_CAP>,
) {
let mut rx_byte = [0u8; 1];
let mut tx_buf = [0u8; 64];
loop {
let rx_fut = uart.read(&mut rx_byte);
let tx_fut = async {
let n = tx_pipe.read(&mut tx_buf).await;
n
};
match select(rx_fut, tx_fut).await {
// Incoming data from UART hardware
Either::First(res) => {
if let Ok(_) = res {
let _ = rx_pipe.write(&rx_byte).await;
}
}
// Outgoing data waiting in TX pipe
Either::Second(n) => {
unwrap!(uart.write(&tx_buf[..n]).await);
}
}
}
}

View File

@@ -0,0 +1,4 @@
// src/hw_uart_pc/mod.rs
pub mod driver;
pub mod usart1;
pub mod safety;

View File

@@ -0,0 +1,57 @@
// src/safety.rs
use defmt::info;
use embassy_time::Duration;
// ISR RX ring capacity = RX_BUF len
const ISR_RX_BUF_CAP: usize = 256;
// Yield 1/2 the time it takes to fill ISR RX ring.
const YIELD_MARGIN_NUM: u32 = 1;
const YIELD_MARGIN_DEN: u32 = 2;
// Ensure RX_PIPE_CAP can hold this.
const WORST_MAIN_LATENCY_MS: u32 = 20;
pub const TX_PIPE_CAP: usize = 1024;
pub const RX_PIPE_CAP: usize = 1024;
/// Perform safety checks and compute yield timing to avoid buffer overflow.
///
/// # Panics
/// Panics if pipe capacities are too small for the configured baud.
pub fn preflight_and_suggest_yield_period(baud: u32) -> Duration {
// Approx bytes per second for 8N1 (10 bits per byte on the wire)
let bytes_per_sec = (baud / 10).max(1);
// Time until ISR RX ring fills, in microseconds.
let t_fill_us = (ISR_RX_BUF_CAP as u64) * 1_000_000u64 / (bytes_per_sec as u64);
// Choose a yield period as a fraction of t_fill.
let yield_us = (t_fill_us as u64)
.saturating_mul(YIELD_MARGIN_NUM as u64)
/ (YIELD_MARGIN_DEN as u64);
// Verify RX pipe can absorb a worst-case app latency so uart_task
// can always forward without dropping when it runs.
let required_rx_pipe = (bytes_per_sec as u64) * (WORST_MAIN_LATENCY_MS as u64) / 1000;
if (RX_PIPE_CAP as u64) < required_rx_pipe {
core::panic!(
"RX pipe too small: have {}B, need >= {}B for {}ms at {} bps",
RX_PIPE_CAP, required_rx_pipe, WORST_MAIN_LATENCY_MS, baud
);
}
info!(
"Preflight: baud={}, rx_isr={}B, rx_pipe={}B, bytes/s={}, t_fill_us={}, yield_us={}",
baud,
ISR_RX_BUF_CAP,
RX_PIPE_CAP,
bytes_per_sec,
t_fill_us,
yield_us
);
// Never choose zero.
Duration::from_micros(yield_us.max(1) as u64)
}

View File

@@ -0,0 +1,12 @@
// src/uart/usart1.rs
use defmt::info;
use embassy_time::Duration;
use crate::hw_uart_pc::safety::preflight_and_suggest_yield_period;
pub fn setup_and_spawn(baudrate: u32,) -> Duration {
let yield_period: Duration = preflight_and_suggest_yield_period(baudrate);
info!("HW USART1 safe");
yield_period
}

View File

@@ -0,0 +1,6 @@
#![no_std]
pub mod software_uart;
pub mod config;
pub mod hw_uart_pc;
pub mod hw_uart_internal;

View File

@@ -0,0 +1,43 @@
// src/software_uart/debug.rs
use defmt::info;
pub fn dump_tim6_regs() {
use embassy_stm32::pac::timer::TimBasic;
let tim = unsafe { TimBasic::from_ptr(0x4000_1000usize as _) };
let sr = tim.sr().read();
let dier = tim.dier().read();
let cr1 = tim.cr1().read();
let arr = tim.arr().read().arr();
let psc = tim.psc().read();
info!(
"TIM6: CR1.CEN={} DIER.UDE={} SR.UIF={} PSC={} ARR={}",
cr1.cen(),
dier.ude(),
sr.uif(),
psc,
arr
);
}
pub fn dump_dma_ch0_regs() {
use embassy_stm32::pac::gpdma::Gpdma;
let dma = unsafe { Gpdma::from_ptr(0x4002_0000usize as _) };
let ch = dma.ch(0);
let cr = ch.cr().read();
let tr1 = ch.tr1().read();
let tr2 = ch.tr2().read();
let br1 = ch.br1().read();
info!(
"GPDMA1_CH0: EN={} PRIO={} SDW={} DDW={} SINC={} DINC={} REQSEL={} SWREQ={} DREQ={} BNDT={}",
cr.en(),
cr.prio(),
tr1.sdw(),
tr1.ddw(),
tr1.sinc(),
tr1.dinc(),
tr2.reqsel(),
tr2.swreq(),
tr2.dreq(),
br1.bndt()
);
}

View File

@@ -0,0 +1,58 @@
// src/dma_timer.rs
use embassy_stm32::{
peripherals::{TIM6, TIM7},
rcc,
timer::low_level::Timer,
Peri,
};
use core::mem;
use embassy_stm32::timer::BasicInstance;
use embassy_stm32::pac::timer::vals::Urs;
/// Initializes TIM6 to tick at `baud * oversample` frequency.
/// Each TIM6 update event triggers one DMA beat.
pub fn init_tim6_for_uart<'d>(tim6: Peri<'d, TIM6>, baud: u32, oversample: u16) {
rcc::enable_and_reset::<TIM6>();
let ll = Timer::new(tim6);
configure_basic_timer(&ll, baud, oversample);
mem::forget(ll);
}
/// Initializes TIM7 to tick at `baud * oversample` frequency.
/// Each TIM7 update event triggers one DMA beat.
pub fn init_tim7_for_uart<'d>(tim7: Peri<'d, TIM7>, baud: u32, oversample: u16) {
rcc::enable_and_reset::<TIM7>();
let ll = Timer::new(tim7);
configure_basic_timer(&ll, baud, oversample);
mem::forget(ll);
}
// Shared internal helper — identical CR1/ARR setup
fn configure_basic_timer<T: BasicInstance>(ll: &Timer<'_, T>, baud: u32, oversample: u16) {
let f_timer = rcc::frequency::<T>().0;
let target = baud.saturating_mul(oversample.max(1) as u32).max(1);
// Compute ARR (prescaler = 0)
let mut arr = (f_timer / target).saturating_sub(1) as u16;
if arr == 0 { arr = 1; }
ll.regs_basic().cr1().write(|w| {
w.set_cen(false);
w.set_opm(false);
w.set_udis(false);
w.set_urs(Urs::ANY_EVENT);
});
ll.regs_basic().psc().write_value(0u16);
ll.regs_basic().arr().write(|w| w.set_arr(arr));
ll.regs_basic().dier().modify(|w| w.set_ude(true));
ll.regs_basic().egr().write(|w| w.set_ug(true));
ll.regs_basic().cr1().write(|w| {
w.set_opm(false);
w.set_cen(true);
w.set_udis(false);
w.set_urs(Urs::ANY_EVENT);
});
}

View File

@@ -0,0 +1,80 @@
// src/software_uart/runtime.rs
use embassy_executor::task;
use embassy_stm32::{
dma::Request,
peripherals::GPDMA1_CH1,
Peri,
};
use crate::config::RX_PIN_BIT;
use embassy_stm32::dma::{
ReadableRingBuffer,
TransferOptions,
};
use crate::config::{RX_OVERSAMPLE, UART_CFG};
use crate::software_uart::decode_uart_samples;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pipe::Pipe};
use embassy_futures::yield_now;
use defmt::info;
// datasheet tabulka 137
pub const TIM7_UP_REQ: Request = 5;
/// RX DMA task: reads GPIO samples paced by TIM7 and fills PIPE_RX
#[task]
pub async fn rx_dma_task(
ch: Peri<'static, GPDMA1_CH1>,
register: *mut u8,
ring: &'static mut [u8],
pipe_rx: &'static Pipe<CriticalSectionRawMutex, 1024>,
) {
let mut opts = TransferOptions::default();
opts.half_transfer_ir = true;
opts.complete_transfer_ir = true;
// SAFETY: ring is exclusive to this task
let mut rx = unsafe { ReadableRingBuffer::new(ch, TIM7_UP_REQ, register, ring, opts) };
rx.start();
let mut raw_chunk = [0u8; 256];
let mut levels = [0u8; 256];
let mut last_bytes: [u8; 16] = [0; 16]; // remember previous 16 samples
let mut last_value: u8 = 0; // remember previous single IDR sample (optional)
loop {
let _ = rx.read_exact(&mut raw_chunk).await;
// If nothing has changed (all bytes same as previous), skip logging
if raw_chunk[..16] == last_bytes[..] {
continue;
}
// Save for next comparison
last_bytes.copy_from_slice(&raw_chunk[..16]);
let current_avg = raw_chunk[0];
// Optionally only if single sample changed
if current_avg != last_value {
last_value = current_avg;
info!(
"DMA change detected: IDR[0..16]={=[u8]:02X} bit{}={}",
&raw_chunk[..16],
RX_PIN_BIT,
(current_avg >> RX_PIN_BIT) & 1
);
}
// Extract logic levels as before
for (i, b) in raw_chunk.iter().enumerate() {
levels[i] = ((*b >> RX_PIN_BIT) & 1) as u8;
}
let decoded = decode_uart_samples(&levels, RX_OVERSAMPLE, &UART_CFG);
if !decoded.is_empty() {
info!("SW RX decoded {:a}", decoded.as_slice());
pipe_rx.write(&decoded).await;
}
yield_now().await;
}
}

View File

@@ -0,0 +1,84 @@
// src/software_uart/gpio_dma_uart_tx.rs
use embassy_executor::task;
use embassy_stm32::{
dma::{Request, TransferOptions, WritableRingBuffer},
peripherals::GPDMA1_CH0,
Peri,
};
use embassy_futures::yield_now;
use defmt::info;
use embassy_sync::pipe::Pipe;
use crate::config::{TX_PIN_BIT, UART_CFG};
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use crate::software_uart::uart_emulation::encode_uart_byte_cfg;
pub const TIM6_UP_REQ: Request = 4;
pub async fn encode_uart_frames<'a>(
pin_bit: u8,
bytes: &[u8],
out_buf: &'a mut [u32],
) -> usize {
let mut offset = 0;
for &b in bytes {
let mut frame = [0u32; 12];
let used = encode_uart_byte_cfg(pin_bit, b, &UART_CFG, &mut frame);
if offset + used <= out_buf.len() {
out_buf[offset..offset + used].copy_from_slice(&frame[..used]);
offset += used;
} else {
break;
}
// cooperative async yield
yield_now().await;
}
offset
}
/// TX DMA task: encodes UART frames and sends them via DMA at TIM6 rate
#[task]
pub async fn tx_dma_task(
ch: Peri<'static, GPDMA1_CH0>,
register: *mut u32, // Either odr or bsrr
tx_ring_mem: &'static mut [u32],
pipe_tx: &'static Pipe<CriticalSectionRawMutex, 1024>,
) {
let mut tx_opts = TransferOptions::default();
tx_opts.half_transfer_ir = true;
tx_opts.complete_transfer_ir = true;
// SAFETY: tx_ring is exclusive to this task
let mut tx_ring = unsafe {
WritableRingBuffer::new(
ch,
TIM6_UP_REQ,
register,
tx_ring_mem,
tx_opts,
)
};
tx_ring.start();
info!("TX DMA ring started");
let mut frame_buf = [0u32; 4096];
let mut rx_buf = [0u8; 256];
loop {
let n = pipe_tx.read(&mut rx_buf).await;
if n == 0 {
yield_now().await;
continue;
}
let used = encode_uart_frames(TX_PIN_BIT, &rx_buf[..n], &mut frame_buf).await;
if used > 0 {
let _ = tx_ring.write_exact(&frame_buf[..used]).await;
}
info!("tx_dma_task wrote {} words", used);
yield_now().await;
}
}

View File

@@ -0,0 +1,13 @@
// src/software_uart/mod.rs
pub mod gpio_dma_uart_tx;
pub mod gpio_dma_uart_rx;
pub mod dma_timer;
pub mod uart_emulation;
pub mod debug;
pub use gpio_dma_uart_tx::*;
pub use gpio_dma_uart_rx::*;
pub use dma_timer::*;
pub use uart_emulation::*;
pub use debug::*;

View File

@@ -0,0 +1,151 @@
// src/software_uart/uart_emulation.rs
use heapless::Vec;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Parity {
None,
Even,
Odd,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum StopBits {
One,
Two,
}
#[derive(Clone, Copy, Debug)]
pub struct UartConfig {
pub data_bits: u8,
pub parity: Parity,
pub stop_bits: StopBits,
}
impl Default for UartConfig {
fn default() -> Self {
Self {
data_bits: 8,
parity: Parity::None,
stop_bits: StopBits::One,
}
}
}
/// Encodes one byte into a sequence of GPIO BSRR words
pub fn encode_uart_byte_cfg(
pin_bit: u8,
data: u8,
cfg: &UartConfig,
out: &mut [u32; 12],
) -> usize {
// GPIOx_BSRR register str. 636 kap. 13.4.7
let set_high = |bit: u8| -> u32 { 1u32 << bit };
// let set_low = |bit: u8| -> u32 { 0 }; // ODR
let set_low = |bit: u8| -> u32 { 1u32 << (bit as u32 + 16) }; // BSRR
let mut idx = 0usize;
// START bit (LOW)
out[idx] = set_low(pin_bit);
idx += 1;
// Data bits, LSB-first
let nbits = cfg.data_bits.clamp(5, 8);
for i in 0..nbits {
let one = ((data >> i) & 1) != 0;
out[idx] = if one { set_high(pin_bit) } else { set_low(pin_bit) };
idx += 1;
}
// Parity
match cfg.parity {
Parity::None => {}
Parity::Even | Parity::Odd => {
let mask: u8 = if nbits == 8 { 0xFF } else { (1u16 << nbits) as u8 - 1 };
let ones = (data & mask).count_ones() & 1;
let par_bit_is_one = match cfg.parity {
Parity::Even => ones == 1,
Parity::Odd => ones == 0,
_ => false,
};
out[idx] = if par_bit_is_one {
set_high(pin_bit)
} else {
set_low(pin_bit)
};
idx += 1;
}
}
// STOP bits (HIGH)
let stop_ticks = match cfg.stop_bits {
StopBits::One => 1usize,
StopBits::Two => 2usize,
};
for _ in 0..stop_ticks {
out[idx] = set_high(pin_bit);
idx += 1;
}
idx
}
/// Decode an oversampled stream of logic levels into UART bytes.
pub fn decode_uart_samples(
samples: &[u8],
oversample: u16,
cfg: &UartConfig,
) -> heapless::Vec<u8, 256> {
let mut out = Vec::<u8, 256>::new();
let mut idx = 0usize;
let nbits = cfg.data_bits as usize;
while idx + (oversample as usize * (nbits + 3)) < samples.len() {
// Wait for start bit (falling edge: high -> low)
if samples[idx] != 0 && samples[idx + 1] == 0 {
// Align to middle of start bit
idx += (oversample / 2) as usize;
// Sanity check start bit really low
if samples.get(idx).copied().unwrap_or(1) != 0 {
idx += 1;
continue;
}
// Sample data bits
let mut data: u8 = 0;
for bit in 0..nbits {
idx += oversample as usize;
let bit_val = samples
.get(idx)
.map(|&b| if b != 0 { 1u8 } else { 0u8 })
.unwrap_or(1);
data |= bit_val << bit;
}
// Parity: skip / verify
match cfg.parity {
Parity::None => {}
Parity::Even | Parity::Odd => {
idx += oversample as usize;
// You can optionally add parity check here if needed
}
}
// Move past stop bits
let stop_skip = match cfg.stop_bits {
StopBits::One => oversample as usize,
StopBits::Two => (oversample * 2) as usize,
};
idx += stop_skip;
// Push decoded byte
let _ = out.push(data);
} else {
idx += 1;
}
}
out
}