From 093e83068461fce42bafdbd2f263ec2928b06416 Mon Sep 17 00:00:00 2001
From: Benjamin Koch <bbbsnowball@gmail.com>
Date: Wed, 24 May 2023 02:44:52 +0200
Subject: [PATCH] move most of the Modbus implementation out of the RS485
struct
---
firmware/rust1/src/bin/heizung.rs | 9 +-
firmware/rust1/src/dont_abort.rs | 31 +--
firmware/rust1/src/lib.rs | 1 +
firmware/rust1/src/modbus_server.rs | 261 ++++++++++++++++++++++++
firmware/rust1/src/rs485.rs | 306 ++++++----------------------
5 files changed, 355 insertions(+), 253 deletions(-)
create mode 100644 firmware/rust1/src/modbus_server.rs
diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs
index 5b9d0d9..f7f8dbb 100644
--- a/firmware/rust1/src/bin/heizung.rs
+++ b/firmware/rust1/src/bin/heizung.rs
@@ -19,6 +19,7 @@ use {defmt_rtt as _, panic_probe as _};
use heapless::String;
use heizung::i2c_scan::{self, ScanResultForBus};
+use heizung::modbus_server::ModbusServer;
use heizung::rs485::RS485;
#[embassy_executor::task]
@@ -71,7 +72,7 @@ async fn beeper_task(pwm_channel: embassy_rp::peripherals::PWM_CH3, beeper_pin:
}
#[embassy_executor::task]
-async fn uart_task(this: RS485) {
+async fn uart_task(this: RS485<ModbusServer>) {
this.run_task().await;
}
@@ -123,7 +124,11 @@ async fn main(spawner: Spawner) {
let mut uart_config = uart::Config::default();
uart_config.baudrate = 19200;
uart_config.parity = Parity::ParityEven;
- let rs485 = RS485::new(p.UART0, rx, tx, tx_en, interrupt::take!(UART0_IRQ), p.DMA_CH1, uart_config, p.PIO0, p.DMA_CH0, p.DMA_CH2);
+ let rs485 = RS485::new(
+ p.UART0, rx, tx, tx_en, interrupt::take!(UART0_IRQ), p.DMA_CH1, uart_config,
+ p.PIO0, p.DMA_CH0, p.DMA_CH2,
+ ModbusServer::new(),
+ );
unwrap!(spawner.spawn(uart_task(rs485)));
if false {
diff --git a/firmware/rust1/src/dont_abort.rs b/firmware/rust1/src/dont_abort.rs
index 8ea3b3f..5c093c6 100644
--- a/firmware/rust1/src/dont_abort.rs
+++ b/firmware/rust1/src/dont_abort.rs
@@ -8,7 +8,7 @@ pub enum DontAbortMode {
}
pub struct DontAbort<A> {
- a: A,
+ a: Option<A>,
done: bool,
mode: DontAbortMode,
}
@@ -19,7 +19,11 @@ pub struct DontAbortFuture<'a, A> {
impl<A> DontAbort<A> {
pub fn new(a: A, mode: DontAbortMode) -> DontAbort<A> {
- DontAbort { a, done: false, mode }
+ DontAbort { a: Option::Some(a), done: false, mode }
+ }
+
+ pub fn hang() -> DontAbort<A> {
+ DontAbort { a: Option::None, done: false, mode: DontAbortMode::HangIfReused }
}
#[allow(dead_code)]
@@ -43,17 +47,20 @@ where
fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
let this = unsafe { self.get_unchecked_mut() };
- if this.inner.done {
- match this.inner.mode {
- DontAbortMode::PanicIfReused => panic!("Ready future is called again!"),
- DontAbortMode::HangIfReused => return Poll::Pending, // will never finish because we haven't scheduled ourselves again
+ match (this.inner.done, this.inner.a.as_mut()) {
+ (true, _) | (_, Option::None) =>
+ match this.inner.mode {
+ DontAbortMode::PanicIfReused => panic!("Ready future is called again!"),
+ DontAbortMode::HangIfReused => return Poll::Pending, // will never finish because we haven't scheduled ourselves again
+ },
+ (false, Option::Some(a)) => {
+ let a = unsafe { Pin::new_unchecked(a) };
+ if let Poll::Ready(x) = a.poll(cx) {
+ this.inner.done = true;
+ return Poll::Ready(x);
+ }
+ Poll::Pending
}
}
- let a = unsafe { Pin::new_unchecked(&mut this.inner.a) };
- if let Poll::Ready(x) = a.poll(cx) {
- this.inner.done = true;
- return Poll::Ready(x);
- }
- Poll::Pending
}
}
diff --git a/firmware/rust1/src/lib.rs b/firmware/rust1/src/lib.rs
index c969570..0af2093 100644
--- a/firmware/rust1/src/lib.rs
+++ b/firmware/rust1/src/lib.rs
@@ -3,3 +3,4 @@ pub mod rp;
pub mod dont_abort;
pub mod i2c_scan;
pub mod rs485;
+pub mod modbus_server;
diff --git a/firmware/rust1/src/modbus_server.rs b/firmware/rust1/src/modbus_server.rs
new file mode 100644
index 0000000..1b08a5c
--- /dev/null
+++ b/firmware/rust1/src/modbus_server.rs
@@ -0,0 +1,261 @@
+use defmt::*;
+use {defmt_rtt as _, panic_probe as _};
+use futures::Future;
+use heapless::Vec;
+use crc::{Crc, CRC_16_MODBUS, Digest};
+use embassy_rp::uart;
+
+use crate::rs485::{RS485Handler};
+
+#[repr(u8)]
+pub enum ModbusErrorCode {
+ IllegalFunction = 1,
+ IllegalDataAddress = 2,
+ IllegalDataValue = 3,
+ ServerDeviceFailure = 4,
+ Acknowledge = 5,
+ ServerDeviceBusy = 6,
+ MemoryParityError = 7,
+ GatewayPathUnavailable = 0xa,
+ GatewayTargetDeviceFailedToRespond = 0xb,
+}
+
+fn read_modbus_holding_register(addr: u16) -> Result<u16, ModbusErrorCode> {
+ if addr == 1 {
+ return Ok(42)
+ }
+ Err(ModbusErrorCode::IllegalDataAddress)
+}
+
+fn read_modbus_input_register(addr: u16) -> Result<u16, ModbusErrorCode> {
+ if addr == 2 {
+ return Ok(42)
+ }
+ Err(ModbusErrorCode::IllegalDataAddress)
+}
+
+fn write_modbus_register(addr: u16, value: u16) -> Result<(), ModbusErrorCode> {
+ Err(ModbusErrorCode::IllegalDataAddress)
+}
+
+fn modbus_reply_error(rxbuf: &Vec<u8, 32>, txbuf: &mut Vec<u8, 32>, code: ModbusErrorCode) {
+ txbuf.clear();
+ txbuf.push(rxbuf[0]).unwrap();
+ txbuf.push(rxbuf[1] | 0x80).unwrap();
+ txbuf.push(code as u8).unwrap();
+}
+
+fn handle_modbus_frame2(rxbuf: &Vec<u8, 32>, txbuf: &mut Vec<u8, 32>) -> Result<(), ModbusErrorCode> {
+ use ModbusErrorCode::*;
+
+ info!("Modbus frame: {:?}", rxbuf.as_slice());
+
+ match rxbuf[1] {
+ 0x03 => {
+ // read holding registers
+ if rxbuf.len() != 8 {
+ // we shouldn't get here
+ return Err(ServerDeviceFailure);
+ }
+ let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
+ let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
+ if quantity as usize > (txbuf.capacity() - 5) / 2 || quantity >= 128 {
+ return Err(IllegalDataValue); // is that right?
+ }
+ txbuf.push(rxbuf[0]).or(Err(ServerDeviceFailure))?;
+ txbuf.push(rxbuf[1]).or(Err(ServerDeviceFailure))?;
+ txbuf.push((quantity*2) as u8).or(Err(ServerDeviceFailure))?;
+ for i in 0..quantity {
+ let value = read_modbus_holding_register(start + i)?;
+ txbuf.push((value >> 8) as u8).or(Err(ServerDeviceFailure))?;
+ txbuf.push((value & 0xff) as u8).or(Err(ServerDeviceFailure))?;
+ }
+ Ok(())
+ },
+ 0x04 => {
+ // read input registers
+ if rxbuf.len() != 8 {
+ // we shouldn't get here
+ return Err(ServerDeviceFailure);
+ }
+ let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
+ let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
+ if quantity as usize > (txbuf.capacity() - 5) / 2 || quantity >= 128 {
+ return Err(IllegalDataValue); // is that right?
+ }
+ txbuf.push(rxbuf[0]).or(Err(ServerDeviceFailure))?;
+ txbuf.push(rxbuf[1]).or(Err(ServerDeviceFailure))?;
+ txbuf.push((quantity*2) as u8).or(Err(ServerDeviceFailure))?;
+ for i in 0..quantity {
+ let value = read_modbus_input_register(start + i)?;
+ txbuf.push((value >> 8) as u8).or(Err(ServerDeviceFailure))?;
+ txbuf.push((value & 0xff) as u8).or(Err(ServerDeviceFailure))?;
+ }
+ Ok(())
+ },
+ 0x06 => {
+ // write register
+ Err(IllegalDataAddress)
+ },
+ 0x10 => {
+ // write multiple registers
+ Err(IllegalDataAddress)
+ },
+ _ => {
+ Err(IllegalFunction)
+ },
+ }
+}
+
+fn handle_modbus_frame(rxbuf: &Vec<u8, 32>, txbuf: &mut Vec<u8, 32>) {
+ match handle_modbus_frame2(rxbuf, txbuf) {
+ Ok(()) => {
+ if txbuf.capacity() - txbuf.len() < 2 {
+ // We don't have enough space for the CRC so reply with error instead.
+ modbus_reply_error(rxbuf, txbuf, ModbusErrorCode::ServerDeviceFailure);
+ }
+ },
+ Err(code) => {
+ modbus_reply_error(rxbuf, txbuf, code);
+ }
+ }
+
+ const CRC: Crc<u16> = Crc::<u16>::new(&CRC_16_MODBUS);
+ let x = CRC.checksum(txbuf.as_slice());
+ txbuf.push((x & 0xff) as u8).unwrap();
+ txbuf.push((x >> 8) as u8).unwrap();
+}
+
+enum ModbusFrameLength {
+ NeedMoreData(u16),
+ Length(u16),
+ Unknown,
+}
+
+//FIXME This won't work if this is a response frame!
+fn get_modbus_frame_length(rxbuf: &[u8]) -> ModbusFrameLength {
+ use ModbusFrameLength::*;
+
+ if rxbuf.len() < 3 {
+ return NeedMoreData(3);
+ }
+
+ match rxbuf[1] {
+ 0x01..=0x06 => Length(8),
+ 0x0f | 0x10 => if rxbuf.len() == 7 { Length(9 + rxbuf[6] as u16) } else { NeedMoreData(7) },
+ _ => Unknown,
+ }
+}
+
+const CRC: Crc<u16> = Crc::<u16>::new(&CRC_16_MODBUS);
+const TX_BUF_LENGTH: usize = 32;
+
+pub struct ModbusServer {
+ rxbuf: Vec<u8, 32>,
+ rxcrc: Digest<'static, u16>,
+ rx_expected_bytes: ModbusFrameLength,
+ rx_received_bytes: u16,
+ txbuf: Vec<u8, TX_BUF_LENGTH>,
+}
+
+impl ModbusServer {
+ pub fn new() -> ModbusServer {
+ ModbusServer {
+ rxbuf: Vec::new(),
+ rxcrc: CRC.digest(),
+ rx_expected_bytes: ModbusFrameLength::NeedMoreData(3),
+ rx_received_bytes: 0,
+ txbuf: Vec::new(),
+ }
+ }
+}
+
+impl RS485Handler for ModbusServer {
+ //type CommandFuture = !;
+ const TX_BUF_LENGTH: usize = TX_BUF_LENGTH;
+
+ fn on_rx<F>(self: &mut Self, rx: Result<u8, uart::Error>, reply: Option<F>)
+ where F: FnOnce(&[u8]) {
+ match rx {
+ Ok(rx_char) => {
+ info!("RX {:?}", rx_char);
+
+ self.rxcrc.update(&[rx_char]);
+ if !self.rxbuf.is_full() {
+ self.rxbuf.push(rx_char).unwrap_or_default();
+ }
+ self.rx_received_bytes += 1;
+
+ if let ModbusFrameLength::NeedMoreData(x) = self.rx_expected_bytes {
+ if x == self.rx_received_bytes {
+ self.rx_expected_bytes = get_modbus_frame_length(self.rxbuf.as_slice());
+ match self.rx_expected_bytes {
+ ModbusFrameLength::Unknown => {
+ //FIXME Wait for pause.
+ },
+ _ => {}
+ }
+ }
+ }
+ if let ModbusFrameLength::Length(x) = self.rx_expected_bytes {
+ if x == self.rx_received_bytes {
+ let calculated_crc = self.rxcrc.clone().finalize();
+ const CORRECT_CRC: u16 = 0; // because we include the CRC bytes in our calculation
+ if calculated_crc != CORRECT_CRC {
+ info!("CRC: {:04x} (should be zero)", calculated_crc);
+ }
+
+ //FIXME In case of CRC mismatch, wait for gap/idle of >=1.5 chars.
+ const OUR_ADDRESS: u8 = 1;
+ if self.rxbuf[0] == OUR_ADDRESS && calculated_crc == CORRECT_CRC {
+ self.txbuf.clear();
+ handle_modbus_frame(&self.rxbuf, &mut self.txbuf);
+
+ if !self.txbuf.is_empty() {
+ info!("Modbus reply: {:?}", self.txbuf);
+ match reply {
+ Option::Some(reply) => reply(self.txbuf.as_slice()),
+ Option::None => warn!("Cannot send reply because a reply is already in progress!"),
+ }
+ }
+ }
+
+ self.rxbuf.clear();
+ self.rxcrc = CRC.digest();
+ self.rx_expected_bytes = ModbusFrameLength::NeedMoreData(3);
+ self.rx_received_bytes = 0;
+ }
+ }
+ },
+ Err(e) => {
+ info!("RX error {:?}", e);
+
+ //FIXME wait for gap/idle of >=1.5 chars.
+ self.rxbuf.clear();
+ self.rxcrc = CRC.digest();
+ self.rx_expected_bytes = ModbusFrameLength::NeedMoreData(3);
+ self.rx_received_bytes = 0;
+ }
+ }
+ }
+
+ fn on_idle(self: &mut Self) {
+ if !self.rxbuf.is_empty() {
+ warn!("Partial frame in rx buffer, cut short by inter-byte gap: {:?}", self.rxbuf);
+ }
+
+ self.rxbuf.clear();
+ self.rxcrc = CRC.digest();
+ self.rx_expected_bytes = ModbusFrameLength::NeedMoreData(3);
+ self.rx_received_bytes = 0;
+ }
+
+ fn on_tx_done(self: &mut Self) {
+ //TODO
+ }
+
+ fn on_autobaud_success(self: &mut Self, baudrate: f32) {
+ //TODO
+ info!("Guessed baud rate: {}", baudrate);
+ }
+}
diff --git a/firmware/rust1/src/rs485.rs b/firmware/rust1/src/rs485.rs
index 38915c8..d3e3ad2 100644
--- a/firmware/rust1/src/rs485.rs
+++ b/firmware/rust1/src/rs485.rs
@@ -13,20 +13,31 @@ use embassy_rp::Peripheral;
use {defmt_rtt as _, panic_probe as _};
use fixed::traits::ToFixed;
use fixed_macro::types::U56F8;
-use heapless::Vec;
-use crc::{Crc, CRC_16_MODBUS};
+use futures::Future;
use crate::dont_abort::DontAbort;
use crate::dont_abort::DontAbortMode::*;
-pub struct RS485 {
+pub trait RS485Handler {
+ //type CommandFuture: Future<Output = ()>;
+ const TX_BUF_LENGTH: usize;
+
+ fn on_rx<F>(self: &mut Self, rx: Result<u8, uart::Error>, reply: Option<F>)
+ where F: FnOnce(&[u8]);
+ fn on_idle(self: &mut Self);
+ fn on_tx_done(self: &mut Self);
+ fn on_autobaud_success(self: &mut Self, baudrate: f32);
+}
+
+pub struct RS485<H: RS485Handler> {
pio: peripherals::PIO0,
rx_pin: peripherals::PIN_17,
tx_pin: peripherals::PIN_16,
tx_en_pin: peripherals::PIN_15,
dma_channel: peripherals::DMA_CH0,
tx_dma_channel: peripherals::DMA_CH2,
- rx: UartRx<'static, UART0, uart::Async>
+ rx: UartRx<'static, UART0, uart::Async>,
+ handler: H
}
fn pin_io<P: gpio::Pin>(pin: &P) -> pac::io::Gpio {
@@ -52,152 +63,13 @@ async fn debug_print_pio_addr(sm: pac::pio::StateMachine) {
}
}
-#[repr(u8)]
-pub enum ModbusErrorCode {
- IllegalFunction = 1,
- IllegalDataAddress = 2,
- IllegalDataValue = 3,
- ServerDeviceFailure = 4,
- Acknowledge = 5,
- ServerDeviceBusy = 6,
- MemoryParityError = 7,
- GatewayPathUnavailable = 0xa,
- GatewayTargetDeviceFailedToRespond = 0xb,
-}
-
-fn read_modbus_holding_register(addr: u16) -> Result<u16, ModbusErrorCode> {
- if addr == 1 {
- return Ok(42)
- }
- Err(ModbusErrorCode::IllegalDataAddress)
-}
-
-fn read_modbus_input_register(addr: u16) -> Result<u16, ModbusErrorCode> {
- if addr == 2 {
- return Ok(42)
- }
- Err(ModbusErrorCode::IllegalDataAddress)
-}
-
-fn write_modbus_register(addr: u16, value: u16) -> Result<(), ModbusErrorCode> {
- Err(ModbusErrorCode::IllegalDataAddress)
-}
-
-fn modbus_reply_error(rxbuf: &Vec<u8, 32>, txbuf: &mut Vec<u8, 32>, code: ModbusErrorCode) {
- txbuf.clear();
- txbuf.push(rxbuf[0]).unwrap();
- txbuf.push(rxbuf[1] | 0x80).unwrap();
- txbuf.push(code as u8).unwrap();
-}
-
-fn handle_modbus_frame2(rxbuf: &Vec<u8, 32>, txbuf: &mut Vec<u8, 32>) -> Result<(), ModbusErrorCode> {
- use ModbusErrorCode::*;
-
- info!("Modbus frame: {:?}", rxbuf.as_slice());
-
- match rxbuf[1] {
- 0x03 => {
- // read holding registers
- if rxbuf.len() != 8 {
- // we shouldn't get here
- return Err(ServerDeviceFailure);
- }
- let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
- let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
- if quantity as usize > (txbuf.capacity() - 5) / 2 || quantity >= 128 {
- return Err(IllegalDataValue); // is that right?
- }
- txbuf.push(rxbuf[0]).or(Err(ServerDeviceFailure))?;
- txbuf.push(rxbuf[1]).or(Err(ServerDeviceFailure))?;
- txbuf.push((quantity*2) as u8).or(Err(ServerDeviceFailure))?;
- for i in 0..quantity {
- let value = read_modbus_holding_register(start + i)?;
- txbuf.push((value >> 8) as u8).or(Err(ServerDeviceFailure))?;
- txbuf.push((value & 0xff) as u8).or(Err(ServerDeviceFailure))?;
- }
- Ok(())
- },
- 0x04 => {
- // read input registers
- if rxbuf.len() != 8 {
- // we shouldn't get here
- return Err(ServerDeviceFailure);
- }
- let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
- let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
- if quantity as usize > (txbuf.capacity() - 5) / 2 || quantity >= 128 {
- return Err(IllegalDataValue); // is that right?
- }
- txbuf.push(rxbuf[0]).or(Err(ServerDeviceFailure))?;
- txbuf.push(rxbuf[1]).or(Err(ServerDeviceFailure))?;
- txbuf.push((quantity*2) as u8).or(Err(ServerDeviceFailure))?;
- for i in 0..quantity {
- let value = read_modbus_input_register(start + i)?;
- txbuf.push((value >> 8) as u8).or(Err(ServerDeviceFailure))?;
- txbuf.push((value & 0xff) as u8).or(Err(ServerDeviceFailure))?;
- }
- Ok(())
- },
- 0x06 => {
- // write register
- Err(IllegalDataAddress)
- },
- 0x10 => {
- // write multiple registers
- Err(IllegalDataAddress)
- },
- _ => {
- Err(IllegalFunction)
- },
- }
-}
-
-fn handle_modbus_frame(rxbuf: &Vec<u8, 32>, txbuf: &mut Vec<u8, 32>) {
- match handle_modbus_frame2(rxbuf, txbuf) {
- Ok(()) => {
- if txbuf.capacity() - txbuf.len() < 2 {
- // We don't have enough space for the CRC so reply with error instead.
- modbus_reply_error(rxbuf, txbuf, ModbusErrorCode::ServerDeviceFailure);
- }
- },
- Err(code) => {
- modbus_reply_error(rxbuf, txbuf, code);
- }
- }
-
- const CRC: Crc<u16> = Crc::<u16>::new(&CRC_16_MODBUS);
- let x = CRC.checksum(txbuf.as_slice());
- txbuf.push((x & 0xff) as u8).unwrap();
- txbuf.push((x >> 8) as u8).unwrap();
-}
-
-enum ModbusFrameLength {
- NeedMoreData(u16),
- Length(u16),
- Unknown,
-}
-
-//FIXME This won't work if this is a response frame!
-fn get_modbus_frame_length(rxbuf: &[u8]) -> ModbusFrameLength {
- use ModbusFrameLength::*;
-
- if rxbuf.len() < 3 {
- return NeedMoreData(3);
- }
-
- match rxbuf[1] {
- 0x01..=0x06 => Length(8),
- 0x0f | 0x10 => if rxbuf.len() == 7 { Length(9 + rxbuf[6] as u16) } else { NeedMoreData(7) },
- _ => Unknown,
- }
-}
-
-impl RS485 {
+impl<H: RS485Handler> RS485<H> {
pub fn new(
uart: UART0, rx_pin: peripherals::PIN_17, tx_pin: peripherals::PIN_16, tx_en_pin: peripherals::PIN_15,
rx_irq: UART0_IRQ, rx_dma_channel: peripherals::DMA_CH1, uart_config: uart::Config,
- pio: peripherals::PIO0, dma_channel: peripherals::DMA_CH0, tx_dma_channel: peripherals::DMA_CH2
- ) -> RS485 {
+ pio: peripherals::PIO0, dma_channel: peripherals::DMA_CH0, tx_dma_channel: peripherals::DMA_CH2,
+ handler: H
+ ) -> RS485<H> {
// SAFETY: The auto-baud will only read from this pin and we will set it back to UART mode after initialising the PIO.
let rx_pin_for_autobaud = unsafe { rx_pin.clone_unchecked() };
@@ -208,7 +80,7 @@ impl RS485 {
rx_dma_channel,
uart_config,
);
- RS485 { pio, rx_pin: rx_pin_for_autobaud, tx_pin, tx_en_pin, dma_channel, tx_dma_channel, rx: uart_rx }
+ RS485 { pio, rx_pin: rx_pin_for_autobaud, tx_pin, tx_en_pin, dma_channel, tx_dma_channel, rx: uart_rx, handler }
}
pub async fn run_task(mut self: Self) {
@@ -237,7 +109,7 @@ impl RS485 {
// Wait for pin to be high for a longer time (UART is idle), copy waiting time to X
// and decrement until done, start again when pin changes.
- "wait_idle:",
+ "public wait_idle:",
"wait 1 pin 0",
"mov x, osr",
"continue_wait_idle:",
@@ -249,6 +121,8 @@ impl RS485 {
// We have a long 1, i.e. UART is idle. Wait for start bit.
"have_long_high:",
"irq set 1", // notify MCU in case they want to know about this (end of Modbus frame)
+ "public wait_for_not_idle:",
+ ".define public wait_for_not_idle wait_for_not_idle",
"wait 0 pin 0",
"nop [4]",
"jmp pin wait_idle", // abort if zero was just a glitch
@@ -366,7 +240,9 @@ impl RS485 {
pin_io(&self.rx_pin).ctrl().write(|w| w.set_funcsel(embassy_rp::pac::io::vals::Gpio17ctrlFuncsel::UART0_RX.0));
}
- info!("Program size for auto-baud: {}, for tx: {}", prg_autobaud.program.code.len(), prg_tx.program.code.len());
+ info!("Program size for auto-baud: {}, for tx: {}, wait_for_not_idle is at: {}",
+ prg_autobaud.program.code.len(), prg_tx.program.code.len(),
+ prg_autobaud.public_defines.wait_for_not_idle);
let relocated = RelocatedProgram::new(&prg_tx.program);
@@ -390,16 +266,9 @@ impl RS485 {
sm1.set_pin_dirs(embassy_rp::pio::Direction::Out, &[&tx_en_pin_pio, &tx_pin_pio]);
sm1.set_enable(true);
- //FIXME Can we split Modbus parts from UART/RS485?
- let mut rxbuf = Vec::<u8, 32>::new();
- const CRC: Crc<u16> = Crc::<u16>::new(&CRC_16_MODBUS);
- let mut rxcrc = CRC.digest();
- let mut rx_expected_bytes = ModbusFrameLength::NeedMoreData(3);
- let mut rx_received_bytes = 0u16;
-
- const TX_BUF_LENGTH: usize = 32;
- let mut txbuf = Vec::<u8, TX_BUF_LENGTH>::new();
- let mut tx_data = [0; TX_BUF_LENGTH+1];
+ const TX_BUF_LENGTH: usize = 32; //H::TX_BUF_LENGTH;
+ let mut tx_data = [0u32; TX_BUF_LENGTH+1];
+ let mut tx_in_progress = false;
let mut dma_in_ref = self.dma_channel.into_ref();
let mut dma_tx_ref = self.tx_dma_channel.into_ref();
@@ -407,18 +276,18 @@ impl RS485 {
let mut bit_index = 0;
let mut rx_buf_one = [0; 1];
let mut rx_future = DontAbort::new(self.rx.read(&mut rx_buf_one), PanicIfReused);
- // We transmit with length zero to have a dummy no-op future with the right type. This seems to work ok.
- let mut tx_future = DontAbort::new(sm1.tx().dma_push(dma_tx_ref.reborrow(), &tx_data[0..0]), HangIfReused);
+ let mut tx_future = DontAbort::hang();
loop {
let x = select4(
irq0.wait(),
sm.rx().dma_pull(dma_in_ref.reborrow(), &mut din),
//debug_print_pio_addr(embassy_rp::pac::PIO0.sm(0)),
tx_future.continue_wait(),
- select(
+ select3(
rx_future.continue_wait(),
//debug_print_pio_addr(embassy_rp::pac::PIO0.sm(1)),
core::future::pending::<()>(),
+ irq1.wait(),
),
).await;
match x {
@@ -489,7 +358,7 @@ impl RS485 {
if got_to_eight && ok {
let millis = sum as f32 / SM_FREQ as f32 * 1000. * 2. / 8.;
let baud = 1000. / millis;
- info!("Guessed baud rate: {}", baud);
+ self.handler.on_autobaud_success(baud);
}
},
Either4::Second(_) => {
@@ -505,86 +374,45 @@ impl RS485 {
}
},
Either4::Third(_) => {
- //drop(tx_future);
- //tx_future = DontAbort::new(sm1.tx().dma_push(dma_tx_ref.reborrow(), &tx_data), HangIfReused);
+ tx_in_progress = false;
+ self.handler.on_tx_done();
},
- Either4::Fourth(Either::First(x)) => {
+ Either4::Fourth(Either3::First(x)) => {
drop(rx_future);
- match x {
- Result::Ok(()) => {
- info!("RX {:?}", rx_buf_one);
- let rx_char = rx_buf_one[0];
-
- rxcrc.update(&[rx_char]);
- if !rxbuf.is_full() {
- rxbuf.push(rx_char).unwrap_or_default();
- }
- rx_received_bytes += 1;
-
- if let ModbusFrameLength::NeedMoreData(x) = rx_expected_bytes {
- if x == rx_received_bytes {
- rx_expected_bytes = get_modbus_frame_length(rxbuf.as_slice());
- match rx_expected_bytes {
- ModbusFrameLength::Unknown => {
- //FIXME Wait for pause.
- },
- _ => {}
- }
- }
- }
- if let ModbusFrameLength::Length(x) = rx_expected_bytes {
- if x == rx_received_bytes {
- let calculated_crc = rxcrc.finalize();
- rxcrc = CRC.digest(); // re-init to placate borrow checker
- const CORRECT_CRC: u16 = 0; // because we include the CRC bytes in our calculation
- if calculated_crc != CORRECT_CRC {
- info!("CRC: {:04x} (should be zero)", calculated_crc);
- }
-
- //FIXME In case of CRC mismatch, wait for gap/idle of >=1.5 chars.
- const OUR_ADDRESS: u8 = 1;
- if rxbuf[0] == OUR_ADDRESS && calculated_crc == CORRECT_CRC {
- txbuf.clear();
- handle_modbus_frame(&rxbuf, &mut txbuf);
-
- if !txbuf.is_empty() {
- drop(tx_future);
-
- tx_data[0] = (txbuf.len() - 1) as u32;
- for i in 0..txbuf.len() {
- let x = txbuf[i] & 0xff;
- let mut parity = 0;
- for j in 0..8 {
- parity ^= x>>j;
- }
- tx_data[i + 1] = x as u32 | (((parity as u32) & 1) << 8);
- }
-
- info!("Modbus reply: {:?}", txbuf);
-
- tx_future = DontAbort::new(sm1.tx().dma_push(dma_tx_ref.reborrow(),
- &tx_data[0..(txbuf.len()+1)]), HangIfReused);
- }
- }
-
- rxbuf.clear();
- //rxcrc = CRC.digest(); // re-init already done above, don't repeat to avoid warning
- rx_expected_bytes = ModbusFrameLength::NeedMoreData(3);
- rx_received_bytes = 0;
+ let rx_char = x.map(|_| -> u8 { rx_buf_one[0] });
+ rx_future = DontAbort::new(self.rx.read(&mut rx_buf_one), PanicIfReused);
+
+ if tx_in_progress {
+ // use a dummy closure to fix the type of the option because I don't know a better way...
+ let dummy = |_txdata: &[u8]| {};
+ self.handler.on_rx(rx_char, if false {Option::Some(dummy)} else {Option::None});
+ } else {
+ drop(tx_future);
+ tx_future = DontAbort::hang();
+ let mut txlen = Option::None;
+
+ self.handler.on_rx(rx_char, Option::Some(|txdata: &[u8]| {
+ tx_data[0] = (txdata.len() - 1) as u32;
+ for i in 0..txdata.len() {
+ let x = txdata[i] & 0xff;
+ let mut parity = 0;
+ for j in 0..8 {
+ parity ^= x>>j;
}
+ tx_data[i + 1] = x as u32 | (((parity as u32) & 1) << 8);
}
- },
- Result::Err(e) => {
- info!("RX error {:?}", e);
-
- //FIXME wait for gap/idle of >=1.5 chars.
- rxbuf.clear();
- rxcrc = CRC.digest();
- rx_expected_bytes = ModbusFrameLength::NeedMoreData(3);
- rx_received_bytes = 0;
- },
+ txlen = Option::Some(txdata.len());
+ }));
+ if let Option::Some(len) = txlen {
+ //FIXME Wait for idle and then wait again from `wait_idle` before sending. We should have a gap of 3.5 chars.
+ tx_future = DontAbort::new(sm1.tx().dma_push(dma_tx_ref.reborrow(),
+ &tx_data[0..(len+1)]), HangIfReused);
+ tx_in_progress = true;
+ }
}
- rx_future = DontAbort::new(self.rx.read(&mut rx_buf_one), PanicIfReused);
+ },
+ Either4::Fourth(Either3::Third(())) => {
+ self.handler.on_idle();
},
_ => {
},
--
GitLab