diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs
index f7f8dbbe3101dcf82d0f3d1c24962a99a0cb0d58..12715b9b5c56e0405d2e4fcfff7469f3fd225c65 100644
--- a/firmware/rust1/src/bin/heizung.rs
+++ b/firmware/rust1/src/bin/heizung.rs
@@ -19,7 +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::modbus_server::{ModbusServer, ModbusRegisters, ModbusErrorCode};
use heizung::rs485::RS485;
#[embassy_executor::task]
@@ -72,10 +72,33 @@ async fn beeper_task(pwm_channel: embassy_rp::peripherals::PWM_CH3, beeper_pin:
}
#[embassy_executor::task]
-async fn uart_task(this: RS485<ModbusServer>) {
+async fn uart_task(this: RS485<ModbusServer<ModBusRegs>>) {
this.run_task().await;
}
+struct ModBusRegs {
+}
+
+impl ModbusRegisters for ModBusRegs {
+ fn read_modbus_holding_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
+ if addr == 1 {
+ return Ok(42)
+ }
+ Err(ModbusErrorCode::IllegalDataAddress)
+ }
+
+ fn read_modbus_input_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
+ if addr == 2 {
+ return Ok(42)
+ }
+ Err(ModbusErrorCode::IllegalDataAddress)
+ }
+
+ fn write_modbus_register(self: &mut Self, addr: u16, value: u16) -> Result<(), ModbusErrorCode> {
+ Err(ModbusErrorCode::IllegalDataAddress)
+ }
+}
+
#[embassy_executor::main]
async fn main(spawner: Spawner) {
let p = embassy_rp::init(Default::default());
@@ -127,7 +150,7 @@ async fn main(spawner: Spawner) {
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(),
+ ModbusServer::new(ModBusRegs { }),
);
unwrap!(spawner.spawn(uart_task(rs485)));
diff --git a/firmware/rust1/src/modbus_server.rs b/firmware/rust1/src/modbus_server.rs
index 1b08a5c1f91a9ee56fe503c3b1bd8f47f5184d4a..ce71a295e49155e1118ac0b2bbc62497f6c10d76 100644
--- a/firmware/rust1/src/modbus_server.rs
+++ b/firmware/rust1/src/modbus_server.rs
@@ -20,110 +20,10 @@ pub enum ModbusErrorCode {
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();
+pub trait ModbusRegisters {
+ fn read_modbus_holding_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode>;
+ fn read_modbus_input_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode>;
+ fn write_modbus_register(self: &mut Self, addr: u16, value: u16) -> Result<(), ModbusErrorCode>;
}
enum ModbusFrameLength {
@@ -150,27 +50,120 @@ fn get_modbus_frame_length(rxbuf: &[u8]) -> ModbusFrameLength {
const CRC: Crc<u16> = Crc::<u16>::new(&CRC_16_MODBUS);
const TX_BUF_LENGTH: usize = 32;
-pub struct ModbusServer {
+pub struct ModbusServer<REGS: ModbusRegisters> {
rxbuf: Vec<u8, 32>,
rxcrc: Digest<'static, u16>,
rx_expected_bytes: ModbusFrameLength,
rx_received_bytes: u16,
txbuf: Vec<u8, TX_BUF_LENGTH>,
+ regs: REGS,
}
-impl ModbusServer {
- pub fn new() -> ModbusServer {
+impl<REGS: ModbusRegisters> ModbusServer<REGS> {
+ pub fn new(regs: REGS) -> ModbusServer<REGS> {
ModbusServer {
rxbuf: Vec::new(),
rxcrc: CRC.digest(),
rx_expected_bytes: ModbusFrameLength::NeedMoreData(3),
rx_received_bytes: 0,
txbuf: Vec::new(),
+ regs,
}
}
+
+
+ fn modbus_reply_error(self: &mut Self, code: ModbusErrorCode) {
+ self.txbuf.clear();
+ self.txbuf.push(self.rxbuf[0]).unwrap();
+ self.txbuf.push(self.rxbuf[1] | 0x80).unwrap();
+ self.txbuf.push(code as u8).unwrap();
+ }
+
+ fn handle_modbus_frame2(self: &mut Self) -> Result<(), ModbusErrorCode> {
+ use ModbusErrorCode::*;
+
+ let rxbuf = &self.rxbuf;
+ let txbuf = &mut self.txbuf;
+ 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 = self.regs.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 = self.regs.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(self: &mut Self) {
+ match self.handle_modbus_frame2() {
+ Ok(()) => {
+ if self.txbuf.capacity() - self.txbuf.len() < 2 {
+ // We don't have enough space for the CRC so reply with error instead.
+ self.modbus_reply_error(ModbusErrorCode::ServerDeviceFailure);
+ }
+ },
+ Err(code) => {
+ self.modbus_reply_error(code);
+ }
+ }
+
+ const CRC: Crc<u16> = Crc::<u16>::new(&CRC_16_MODBUS);
+ let x = CRC.checksum(self.txbuf.as_slice());
+ self.txbuf.push((x & 0xff) as u8).unwrap();
+ self.txbuf.push((x >> 8) as u8).unwrap();
+ }
}
-impl RS485Handler for ModbusServer {
+impl<REGS: ModbusRegisters> RS485Handler for ModbusServer<REGS> {
//type CommandFuture = !;
const TX_BUF_LENGTH: usize = TX_BUF_LENGTH;
@@ -209,7 +202,7 @@ impl RS485Handler for ModbusServer {
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);
+ self.handle_modbus_frame();
if !self.txbuf.is_empty() {
info!("Modbus reply: {:?}", self.txbuf);