Skip to content
Snippets Groups Projects
Commit 8726fbd8 authored by Benjamin Koch's avatar Benjamin Koch
Browse files

move implementation of Modbus registers into main file

parent 093e8306
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
firmware/rust1/src/bin/heizung.rs
+ 26
− 3
View file @ 8726fbd8
......@@ -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)));
......
firmware/rust1/src/modbus_server.rs
+ 102
− 109
View file @ 8726fbd8
......@@ -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);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment