From b816fbcfcccb423870424c4a0f66453ce4cdc660 Mon Sep 17 00:00:00 2001
From: Benjamin Koch <bbbsnowball@gmail.com>
Date: Sun, 21 May 2023 00:46:37 +0200
Subject: [PATCH] add very simple Modbus communication
---
firmware/rust1/Cargo.lock | 16 +++
firmware/rust1/Cargo.toml | 1 +
firmware/rust1/src/rs485.rs | 240 +++++++++++++++++++++++++++++++++---
3 files changed, 240 insertions(+), 17 deletions(-)
diff --git a/firmware/rust1/Cargo.lock b/firmware/rust1/Cargo.lock
index 4d809ce..1826c0d 100644
--- a/firmware/rust1/Cargo.lock
+++ b/firmware/rust1/Cargo.lock
@@ -259,6 +259,15 @@ dependencies = [
"syn 1.0.109",
]
+[[package]]
+name = "crc"
+version = "3.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "86ec7a15cbe22e59248fc7eadb1907dab5ba09372595da4d73dd805ed4417dfe"
+dependencies = [
+ "crc-catalog",
+]
+
[[package]]
name = "crc-any"
version = "2.4.3"
@@ -268,6 +277,12 @@ dependencies = [
"debug-helper",
]
+[[package]]
+name = "crc-catalog"
+version = "2.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9cace84e55f07e7301bae1c519df89cdad8cc3cd868413d3fdbdeca9ff3db484"
+
[[package]]
name = "critical-section"
version = "1.1.1"
@@ -1014,6 +1029,7 @@ dependencies = [
"byte-slice-cast 1.2.2",
"cortex-m",
"cortex-m-rt",
+ "crc",
"defmt",
"defmt-rtt",
"display-interface",
diff --git a/firmware/rust1/Cargo.toml b/firmware/rust1/Cargo.toml
index 7268c1f..d2db0ff 100644
--- a/firmware/rust1/Cargo.toml
+++ b/firmware/rust1/Cargo.toml
@@ -55,6 +55,7 @@ pio-proc = "0.2"
pio = "0.2.1"
heapless = "0.7.16"
+crc = "3.0.1"
[profile.release]
debug = true
diff --git a/firmware/rust1/src/rs485.rs b/firmware/rust1/src/rs485.rs
index 2b1919d..9c78b1e 100644
--- a/firmware/rust1/src/rs485.rs
+++ b/firmware/rust1/src/rs485.rs
@@ -13,6 +13,8 @@ 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 crate::dont_abort::DontAbort;
use crate::dont_abort::DontAbortMode::*;
@@ -37,6 +39,7 @@ fn pin_io<P: gpio::Pin>(pin: &P) -> pac::io::Gpio {
block.gpio(pin.pin() as _)
}
+#[allow(dead_code)]
async fn debug_print_pio_addr(sm: pac::pio::StateMachine) {
let mut prev = 42u8;
loop {
@@ -49,6 +52,146 @@ 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 {
pub fn new(
uart: UART0, rx_pin: peripherals::PIN_17, tx_pin: peripherals::PIN_16, tx_en_pin: peripherals::PIN_15,
@@ -246,25 +389,26 @@ impl RS485 {
sm1.set_pin_dirs(embassy_rp::pio::Direction::Out, &[&tx_en_pin_pio, &tx_pin_pio]);
sm1.set_enable(true);
- let mut tx_data = [0, 'H' as u32, 'e' as u32, 'l' as u32, 'l' as u32, 'o' as u32, '\r' as u32, '\n' as u32];
- tx_data[0] = (tx_data.len() - 2) as u32;
- for i in 1..tx_data.len() {
- let x = tx_data[i] & 0xff;
- let mut parity = 0;
- for j in 0..8 {
- parity ^= x>>j;
- }
- tx_data[i] = x | ((parity&1) << 8);
- }
+ //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;
+ let mut rx_char_prev: u8 = 0;
+ const TX_BUF_LENGTH: usize = 32;
+ let mut txbuf = Vec::<u8, TX_BUF_LENGTH>::new();
+ let mut tx_data = [0; TX_BUF_LENGTH+1];
let mut dma_in_ref = self.dma_channel.into_ref();
let mut dma_tx_ref = self.tx_dma_channel.into_ref();
let mut din = [42u32; 9];
let mut bit_index = 0;
- let mut rx_buf = [0; 1];
- let mut rx_future = DontAbort::new(self.rx.read(&mut rx_buf), PanicIfReused);
- let mut tx_future = DontAbort::new(sm1.tx().dma_push(dma_tx_ref.reborrow(), &tx_data), HangIfReused);
+ 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);
loop {
let x = select4(
irq0.wait(),
@@ -368,16 +512,78 @@ impl RS485 {
drop(rx_future);
match x {
Result::Ok(()) => {
- info!("RX {:?}", rx_buf);
+ info!("RX {:?}", rx_buf_one);
+ let rx_char = rx_buf_one[0];
- drop(tx_future);
- tx_future = DontAbort::new(sm1.tx().dma_push(dma_tx_ref.reborrow(), &tx_data), HangIfReused);
+ 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 received_crc = rx_char_prev as u16 | ((rx_char as u16) << 8);
+ let calculated_crc = rxcrc.finalize();
+ rxcrc = CRC.digest();
+ info!("received_crc: {:04x}, calculated_crc: {:04x}", received_crc, calculated_crc);
+
+ //FIXME In case of CRC mismatch, wait for gap/idle of >=1.5 chars.
+ const CORRECT_CRC: u16 = 0; // because we include the CRC bytes in our calculation
+ 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);
+ }
+
+ tx_future = DontAbort::new(sm1.tx().dma_push(dma_tx_ref.reborrow(),
+ &tx_data[0..(txbuf.len()+1)]), HangIfReused);
+ }
+ }
+
+ rxbuf.clear();
+ rxcrc = CRC.digest();
+ rx_expected_bytes = ModbusFrameLength::NeedMoreData(3);
+ rx_received_bytes = 0;
+ }
+ }
+
+ rx_char_prev = rx_char;
},
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;
},
}
- rx_future = DontAbort::new(self.rx.read(&mut rx_buf), PanicIfReused);
+ rx_future = DontAbort::new(self.rx.read(&mut rx_buf_one), PanicIfReused);
},
_ => {
},
--
GitLab