From 0de5c7b59f6f9e8e4e1086f58cfdf30a00de4951 Mon Sep 17 00:00:00 2001
From: Benjamin Koch <bbbsnowball@gmail.com>
Date: Sun, 28 May 2023 00:24:20 +0200
Subject: [PATCH] add Modbus file for reading from flash
---
firmware/rust1/src/bin/heizung.rs | 16 +++++++++++++++-
firmware/rust1/src/modbus_server.rs | 9 +++++++++
2 files changed, 24 insertions(+), 1 deletion(-)
diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs
index d03b957..6e8762b 100644
--- a/firmware/rust1/src/bin/heizung.rs
+++ b/firmware/rust1/src/bin/heizung.rs
@@ -9,7 +9,8 @@ use embassy_executor::Spawner;
//use embassy_futures::join::join;
use embassy_futures::select::*;
use embassy_rp::adc::{self, Adc};
-use embassy_rp::peripherals::*;
+use embassy_rp::flash::Flash;
+use embassy_rp::peripherals::{*, self};
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::mutex::Mutex;
use embassy_time::{Duration, Timer, Instant};
@@ -310,6 +311,8 @@ enum ReadType {
InputRegister,
}
+const FLASH_SIZE: usize = 2 * 1024 * 1024;
+
struct ModBusRegs<'a> {
led_g: Output<'a, PIN_5>,
button_boot2: Input<'a, PIN_11>,
@@ -318,6 +321,7 @@ struct ModBusRegs<'a> {
reed2: Input<'a, PIN_21>,
reed1: Input<'a, PIN_22>,
adc_data: &'a AdcData,
+ flash: Flash<'a, peripherals::FLASH, FLASH_SIZE>,
consistent_read_type: ReadType,
consistent_read_addr: u16,
@@ -444,6 +448,15 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
pusher.push_bytes(&value[start .. end])?;
Ok(())
},
+ (6, 1) => {
+ pusher.push_fn(record_length as usize * 2, |buf| {
+ self.flash.read(record_number as u32 * 2, buf)
+ .map_err(|err| {
+ info!("Error reading flash: {:?}", err);
+ ModbusErrorCode::ServerDeviceFailure
+ })
+ })
+ },
_ => {
Err(ModbusErrorCode::IllegalDataAddress)
}
@@ -511,6 +524,7 @@ async fn main2(spawner: Spawner) {
button_boot2,
reed1, reed2, reed3, reed4,
adc_data: &ADC_DATA,
+ flash: embassy_rp::flash::Flash::<_, FLASH_SIZE>::new(p.FLASH),
consistent_read_type: ReadType::InputRegister,
consistent_read_addr: u16::default(),
consistent_read_length: u16::default(),
diff --git a/firmware/rust1/src/modbus_server.rs b/firmware/rust1/src/modbus_server.rs
index 94d7f72..cb5e37c 100644
--- a/firmware/rust1/src/modbus_server.rs
+++ b/firmware/rust1/src/modbus_server.rs
@@ -62,6 +62,15 @@ impl<'a, const N: usize> U16Pusher<'a, N> {
pub fn push(self: &mut Self, x: u16) -> Result<(), ModbusErrorCode> {
self.push_bytes(&x.to_be_bytes())
}
+
+ pub fn push_fn<F: FnOnce(&mut [u8]) -> Result<(), ModbusErrorCode>>(self: &mut Self, len_bytes: usize, f: F) -> Result<(), ModbusErrorCode> {
+ let len_before = self.buf.len();
+ for _ in 0..len_bytes {
+ self.buf.push(0).or(Err(ModbusErrorCode::ServerDeviceFailure))?;
+ }
+ let len_after = self.buf.len();
+ f(&mut self.buf[len_before .. len_after])
+ }
}
--
GitLab