diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs
index 688b033ddc553b01bc6616d5889a681e627692c2..d03b9575936da9c45941c4e243d5389b9171bec3 100644
--- a/firmware/rust1/src/bin/heizung.rs
+++ b/firmware/rust1/src/bin/heizung.rs
@@ -24,7 +24,7 @@ use {defmt_rtt as _, panic_probe as _};
use heapless::String;
use heizung::i2c_scan::{self, ScanResultForBus};
-use heizung::modbus_server::{ModbusServer, ModbusRegisters, ModbusErrorCode, ModbusAdressMatch};
+use heizung::modbus_server::{ModbusServer, ModbusRegisters, ModbusErrorCode, ModbusAdressMatch, U16Pusher};
use heizung::rs485::RS485;
#[embassy_executor::task]
@@ -428,6 +428,27 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
fn write_register(self: &mut Self, _device_addr: u8, addr: u16, value: u16) -> Result<u16, ModbusErrorCode> {
Err(ModbusErrorCode::IllegalDataAddress)
}
+
+ fn read_file_records(self: &mut Self, _device_addr: u8,
+ ref_type: u8, file_number: u16, record_number: u16, record_length: u16, mut pusher: U16Pusher<'_, 256>)
+ -> Result<(), ModbusErrorCode> {
+ match (ref_type, file_number) {
+ (6, 0) => {
+ let value = "Hello, World!!".as_bytes();
+ if record_number as usize >= value.len()/2 {
+ return Ok(()) // or should that be an error?
+ }
+ let start = 2 * record_number as usize;
+ let end = start + 2 * record_length as usize;
+ let end = core::cmp::min(end, value.len()); // or should too large length be an error?
+ pusher.push_bytes(&value[start .. end])?;
+ Ok(())
+ },
+ _ => {
+ Err(ModbusErrorCode::IllegalDataAddress)
+ }
+ }
+ }
}
//#[embassy_executor::main]
diff --git a/firmware/rust1/src/modbus_server.rs b/firmware/rust1/src/modbus_server.rs
index 9d8500b15b6eee5fd6f88f8a19ad22a5a6bc1af2..94d7f72a6441e56a230f7abf166d110e7e2530bb 100644
--- a/firmware/rust1/src/modbus_server.rs
+++ b/firmware/rust1/src/modbus_server.rs
@@ -19,6 +19,10 @@ impl<'a, E: Clone> Cursor<'a, E> {
self.0
}
+ fn has_more_data(self: &Self) -> bool {
+ self.1 < self.0.len()
+ }
+
fn read_u8(self: &mut Self) -> Result<u8, E> {
if self.1 < self.0.len() {
let x = self.0[self.1];
@@ -43,6 +47,23 @@ impl<'a, E: Clone> Cursor<'a, E> {
}
}
+pub struct U16Pusher<'a, const N: usize> {
+ buf: &'a mut Vec<u8, N>
+}
+
+impl<'a, const N: usize> U16Pusher<'a, N> {
+ pub fn push_bytes(self: &mut Self, xs: &[u8]) -> Result<(), ModbusErrorCode> {
+ for x in xs {
+ self.buf.push(*x).or(Err(ModbusErrorCode::ServerDeviceFailure))?;
+ }
+ Ok(())
+ }
+
+ pub fn push(self: &mut Self, x: u16) -> Result<(), ModbusErrorCode> {
+ self.push_bytes(&x.to_be_bytes())
+ }
+}
+
#[repr(u8)]
#[derive(PartialEq, Eq, Format, Clone)]
@@ -71,6 +92,9 @@ pub trait ModbusRegisters {
fn read_input_register(self: &mut Self, device_addr: u8, addr: u16) -> Result<u16, ModbusErrorCode>;
fn write_coil(self: &mut Self, device_addr: u8, addr: u16, value: bool) -> Result<(), ModbusErrorCode>;
fn write_register(self: &mut Self, device_addr: u8, addr: u16, value: u16) -> Result<u16, ModbusErrorCode>;
+ fn read_file_records(self: &mut Self, device_addr: u8,
+ ref_type: u8, file_number: u16, record_number: u16, record_length: u16, pusher: U16Pusher<'_, 256>)
+ -> Result<(), ModbusErrorCode>;
}
#[derive(PartialEq, Eq, Format)]
@@ -91,6 +115,7 @@ fn get_modbus_frame_length(rxbuf: &[u8]) -> ModbusFrameLength {
match rxbuf[1] {
0x01..=0x06 => Length(8),
0x0f | 0x10 => if rxbuf.len() == 7 { Length(9 + rxbuf[6] as u16) } else { NeedMoreData(7) },
+ 0x14 => if rxbuf.len() == 3 { Length(5 + rxbuf[2] as u16) } else { NeedMoreData(3) },
_ => Unknown,
}
}
@@ -131,7 +156,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
use ModbusErrorCode::*;
let rxbuf = &self.rxbuf;
- let mut rx = Cursor::new(&rxbuf, ModbusErrorCode::IllegalDataValue);
+ let mut rx = Cursor::new(&rxbuf[0..rxbuf.len()-2], ModbusErrorCode::IllegalDataValue);
let txbuf = &mut self.txbuf;
info!("Modbus frame: {:?}", rxbuf.as_slice());
@@ -154,7 +179,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
push_many(txbuf, &x.to_be_bytes())
}
- let _device_addr = rx.read_u8()?;
+ let device_addr = rx.read_u8()?;
// see https://modbus.org/docs/Modbus_Application_Protocol_V1_1b3.pdf
let function = rx.read_u8()?;
@@ -173,7 +198,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
let mut x = 0;
for j in 0..7 {
if i*8+j < quantity {
- x |= (if self.regs.read_discrete_input(rxbuf[0], start + i*8 + j)? {1} else {0}) << j;
+ x |= (if self.regs.read_discrete_input(device_addr, start + i*8 + j)? {1} else {0}) << j;
}
}
push(txbuf, x)?;
@@ -195,7 +220,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
push_many(txbuf, &rxbuf[0..2])?;
push(txbuf, byte_count as u8)?;
for i in 0..quantity {
- let value = self.regs.read_holding_register(rxbuf[0], start + i)?;
+ let value = self.regs.read_holding_register(device_addr, start + i)?;
push_u16be(txbuf, value)?;
}
Ok(())
@@ -214,7 +239,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
push_many(txbuf, &rxbuf[0..2])?;
push(txbuf, (quantity*2) as u8)?;
for i in 0..quantity {
- let value = self.regs.read_input_register(rxbuf[0], start + i)?;
+ let value = self.regs.read_input_register(device_addr, start + i)?;
push_u16be(txbuf, value)?;
}
Ok(())
@@ -234,7 +259,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
} else {
return Err(IllegalDataValue);
};
- self.regs.write_coil(rxbuf[0], start, value)?;
+ self.regs.write_coil(device_addr, start, value)?;
if should_reply {
push_many(txbuf, &rxbuf[0..6])?;
}
@@ -248,7 +273,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
}
let start = rx.read_u16be()?;
let value = rx.read_u16be()?;
- let actual_new_value = self.regs.write_register(rxbuf[0], start, value)?;
+ let actual_new_value = self.regs.write_register(device_addr, start, value)?;
if should_reply {
push_many(txbuf, &rxbuf[0..4])?;
push_u16be(txbuf, actual_new_value)?;
@@ -276,7 +301,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
let x = rxbuf[7 + i as usize];
for j in 0..7 {
if i*8+j < quantity {
- self.regs.write_coil(rxbuf[0], start + i*8 + j, (x >> j) != 0)?;
+ self.regs.write_coil(device_addr, start + i*8 + j, (x >> j) != 0)?;
}
}
}
@@ -304,13 +329,52 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
}
for i in 0..quantity {
let value = rx.read_u16be()?;
- self.regs.write_register(rxbuf[0], start + i, value)?;
+ self.regs.write_register(device_addr, start + i, value)?;
}
if should_reply {
push_many(txbuf, &rxbuf[0..6])?;
}
Ok(())
},
+ 0x14 => {
+ push_many(txbuf, &rxbuf[0..2])?;
+ push(txbuf, 0)?;
+ let byte_count = rx.read_u8()?;
+ if byte_count < 7 || byte_count > 0xf5 {
+ return Err(IllegalDataValue)
+ }
+ while rx.has_more_data() {
+ let ref_type = rx.read_u8()?;
+ let file_number = rx.read_u16be()?;
+ let record_number = rx.read_u16be()?;
+ let record_length = rx.read_u16be()?;
+ let pos_of_length = txbuf.len();
+ push(txbuf, 1 + 2 * record_length as u8)?;
+ push(txbuf, ref_type)?;
+ if record_length > 127 || record_length as usize * 2 > capacity - txbuf.len() {
+ return Err(IllegalDataValue)
+ }
+ let len_before = txbuf.len();
+ let pusher = U16Pusher { buf: txbuf };
+ self.regs.read_file_records(device_addr, ref_type, file_number, record_number, record_length, pusher)?;
+ let len_after = txbuf.len();
+ let len_added = len_after - len_before;
+ if len_added % 2 != 0 {
+ error!("Number of bytes added by read_file_records is not a multiple of 2 (u16)");
+ return Err(ServerDeviceFailure)
+ } else if len_added/2 > record_length as usize {
+ error!("Too many record added by read_file_records: expected {} but got {}", record_length, len_added/2);
+ return Err(ServerDeviceFailure)
+ } else if len_added >= 255 {
+ error!("Too many records added");
+ return Err(ServerDeviceFailure)
+ }
+ txbuf[pos_of_length] = 1 + len_added as u8;
+ }
+ // update byte_count
+ txbuf[2] = txbuf.len() as u8 - 3;
+ Ok(())
+ },
_ => {
Err(IllegalFunction)
},