From cc08524a5c41ba8b22af6ee2530ad9e180d04cf6 Mon Sep 17 00:00:00 2001
From: Benjamin Koch <bbbsnowball@gmail.com>
Date: Tue, 30 May 2023 00:38:14 +0200
Subject: [PATCH] first implementation of writing data for bootloader -
completely untested
---
firmware/rust1/Cargo.lock | 132 +++++++++++
firmware/rust1/Cargo.toml | 6 +
firmware/rust1/memory.x | 3 +
firmware/rust1/src/bin/heizung.rs | 122 +++++++---
firmware/rust1/src/lib.rs | 2 +
firmware/rust1/src/modbus_server.rs | 36 ++-
firmware/rust1/src/uf2.rs | 50 ++++
firmware/rust1/src/uf2updater.rs | 351 ++++++++++++++++++++++++++++
8 files changed, 672 insertions(+), 30 deletions(-)
create mode 100644 firmware/rust1/src/uf2.rs
create mode 100644 firmware/rust1/src/uf2updater.rs
diff --git a/firmware/rust1/Cargo.lock b/firmware/rust1/Cargo.lock
index 1826c0d..003f717 100644
--- a/firmware/rust1/Cargo.lock
+++ b/firmware/rust1/Cargo.lock
@@ -162,6 +162,27 @@ version = "1.3.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
+[[package]]
+name = "bitvec"
+version = "1.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1bc2832c24239b0141d5674bb9174f9d68a8b5b3f2753311927c172ca46f7e9c"
+dependencies = [
+ "funty",
+ "radium",
+ "tap",
+ "wyz",
+]
+
+[[package]]
+name = "block-buffer"
+version = "0.10.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71"
+dependencies = [
+ "generic-array 0.14.7",
+]
+
[[package]]
name = "byte-slice-cast"
version = "0.3.5"
@@ -295,6 +316,16 @@ version = "0.2.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "7a81dae078cea95a014a339291cec439d2f232ebe854a9d672b796c6afafa9b7"
+[[package]]
+name = "crypto-common"
+version = "0.1.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1bfb12502f3fc46cca1bb51ac28df9d618d813cdc3d2f25b9fe775a34af26bb3"
+dependencies = [
+ "generic-array 0.14.7",
+ "typenum",
+]
+
[[package]]
name = "crypto-mac"
version = "0.10.1"
@@ -404,6 +435,16 @@ version = "0.1.13"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "56254986775e3233ffa9c4d7d3faaf6d36a2c09d30b20687e9f88bc8bafc16c8"
+[[package]]
+name = "digest"
+version = "0.10.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292"
+dependencies = [
+ "block-buffer",
+ "crypto-common",
+]
+
[[package]]
name = "dirs-next"
version = "2.0.0"
@@ -448,6 +489,32 @@ version = "1.8.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91"
+[[package]]
+name = "embassy-boot"
+version = "0.1.1"
+dependencies = [
+ "defmt",
+ "digest",
+ "embassy-sync",
+ "embedded-storage",
+ "signature",
+]
+
+[[package]]
+name = "embassy-boot-rp"
+version = "0.1.0"
+dependencies = [
+ "cfg-if",
+ "cortex-m",
+ "cortex-m-rt",
+ "defmt",
+ "embassy-boot",
+ "embassy-rp",
+ "embassy-sync",
+ "embassy-time",
+ "embedded-storage",
+]
+
[[package]]
name = "embassy-cortex-m"
version = "0.1.0"
@@ -872,6 +939,12 @@ version = "1.0.7"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1"
+[[package]]
+name = "funty"
+version = "2.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e6d5a32815ae3f33302d95fdcb2ce17862f8c65363dcfd29360480ba1001fc9c"
+
[[package]]
name = "futures"
version = "0.3.28"
@@ -1026,6 +1099,7 @@ dependencies = [
name = "heizung"
version = "0.1.0"
dependencies = [
+ "bitvec",
"byte-slice-cast 1.2.2",
"cortex-m",
"cortex-m-rt",
@@ -1034,6 +1108,8 @@ dependencies = [
"defmt-rtt",
"display-interface",
"display-interface-spi",
+ "embassy-boot",
+ "embassy-boot-rp",
"embassy-cortex-m",
"embassy-embedded-hal",
"embassy-executor",
@@ -1064,7 +1140,9 @@ dependencies = [
"pio-proc",
"smart-leds",
"st7789",
+ "static_assertions",
"static_cell",
+ "zerocopy",
]
[[package]]
@@ -1472,6 +1550,12 @@ dependencies = [
"proc-macro2",
]
+[[package]]
+name = "radium"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dc33ff2d4973d518d823d61aa239014831e521c75da58e3df4840d3f47749d09"
+
[[package]]
name = "rand_core"
version = "0.6.4"
@@ -1620,6 +1704,12 @@ version = "1.0.163"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "2113ab51b87a539ae008b5c6c02dc020ffa39afd2d83cffcb3f4eb2722cebec2"
+[[package]]
+name = "signature"
+version = "1.6.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "74233d3b3b2f6d4b006dc19dee745e73e2a6bfb6f93607cd3b02bd5b00797d7c"
+
[[package]]
name = "siphasher"
version = "0.3.10"
@@ -1702,6 +1792,12 @@ version = "1.2.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
+[[package]]
+name = "static_assertions"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f"
+
[[package]]
name = "static_cell"
version = "1.0.0"
@@ -1758,6 +1854,12 @@ dependencies = [
"unicode-ident",
]
+[[package]]
+name = "tap"
+version = "1.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "55937e1799185b12863d447f42597ed69d9928686b8d88a1df17376a097d8369"
+
[[package]]
name = "term"
version = "0.7.0"
@@ -2067,3 +2169,33 @@ name = "windows_x86_64_msvc"
version = "0.48.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "1a515f5799fe4961cb532f983ce2b23082366b898e52ffbce459c86f67c8378a"
+
+[[package]]
+name = "wyz"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "05f360fc0b24296329c78fda852a1e9ae82de9cf7b27dae4b7f62f118f77b9ed"
+dependencies = [
+ "tap",
+]
+
+[[package]]
+name = "zerocopy"
+version = "0.6.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "332f188cc1bcf1fe1064b8c58d150f497e697f49774aa846f2dc949d9a25f236"
+dependencies = [
+ "byteorder",
+ "zerocopy-derive",
+]
+
+[[package]]
+name = "zerocopy-derive"
+version = "0.3.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6505e6815af7de1746a08f69c69606bb45695a17149517680f3b2149713b19a3"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn 1.0.109",
+]
diff --git a/firmware/rust1/Cargo.toml b/firmware/rust1/Cargo.toml
index d2db0ff..fefd5c0 100644
--- a/firmware/rust1/Cargo.toml
+++ b/firmware/rust1/Cargo.toml
@@ -21,6 +21,8 @@ embassy-net = { version = "0.1.0", path = "./embassy/embassy-net", features = ["
embassy-futures = { version = "0.1.0", path = "./embassy/embassy-futures" }
embassy-usb-logger = { version = "0.1.0", path = "./embassy/embassy-usb-logger" }
embassy-lora = { version = "0.1.0", path = "./embassy/embassy-lora", features = ["time", "defmt"] }
+embassy-boot = { version = "0.1.1", path = "./embassy/embassy-boot/boot", features = [] }
+embassy-boot-rp = { version = "0.1.0", path = "./embassy/embassy-boot/rp", features = ["defmt"] }
lora-phy = { version = "1" }
lorawan-device = { version = "0.10.0", default-features = false, features = ["async", "external-lora-phy"] }
lorawan = { version = "0.7.3", default-features = false, features = ["default-crypto"] }
@@ -56,6 +58,10 @@ pio = "0.2.1"
heapless = "0.7.16"
crc = "3.0.1"
+#elf2uf2-rs = "1.3.7" # not a library
+zerocopy = "0.6.1"
+static_assertions = "1.1.0"
+bitvec = { version = "1", features = ["atomic"], default-features = false }
[profile.release]
debug = true
diff --git a/firmware/rust1/memory.x b/firmware/rust1/memory.x
index c194731..7578153 100644
--- a/firmware/rust1/memory.x
+++ b/firmware/rust1/memory.x
@@ -11,5 +11,8 @@ MEMORY
__bootloader_state_start = ORIGIN(BOOTLOADER_STATE) - ORIGIN(BOOT2);
__bootloader_state_end = ORIGIN(BOOTLOADER_STATE) + LENGTH(BOOTLOADER_STATE) - ORIGIN(BOOT2);
+__bootloader_active_start = ORIGIN(FLASH) - ORIGIN(BOOT2);
+__bootloader_active_end = ORIGIN(FLASH) + LENGTH(FLASH) - ORIGIN(BOOT2);
+
__bootloader_dfu_start = ORIGIN(DFU) - ORIGIN(BOOT2);
__bootloader_dfu_end = ORIGIN(DFU) + LENGTH(DFU) - ORIGIN(BOOT2);
diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs
index e10bdd2..720b822 100644
--- a/firmware/rust1/src/bin/heizung.rs
+++ b/firmware/rust1/src/bin/heizung.rs
@@ -7,15 +7,14 @@ use core::sync::atomic::*;
use defmt::*;
use embassy_executor::Spawner;
//use embassy_futures::join::join;
-use embassy_futures::select::*;
+//use embassy_futures::select::*;
use embassy_rp::adc::{self, Adc};
-use embassy_rp::flash::Flash;
use embassy_rp::peripherals::{*, self};
use embassy_rp::watchdog::Watchdog;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::mutex::Mutex;
use embassy_time::{Duration, Timer, Instant};
-use embassy_rp::gpio::{Input, Level, Output, Pull, Flex};
+use embassy_rp::gpio::{Input, Level, Output, Pull};
use embassy_rp::i2c;
use embassy_rp::interrupt;
//use embedded_hal_async::i2c::I2c;
@@ -28,6 +27,7 @@ use heapless::String;
use heizung::i2c_scan::{self, ScanResultForBus};
use heizung::modbus_server::{ModbusServer, ModbusRegisters, ModbusErrorCode, ModbusAdressMatch, U16Pusher};
use heizung::rs485::RS485;
+use heizung::uf2updater::UF2UpdateHandler;
#[embassy_executor::task]
async fn i2c_task(mut i2c_peri: embassy_rp::peripherals::I2C0,
@@ -306,13 +306,17 @@ async fn adc_task(shared_data: &'static AdcData, adc: ADC, mut en_measure_curren
}
+const FLASH_SIZE: usize = 2 * 1024 * 1024;
+
#[derive(Clone, Copy, PartialEq, Eq)]
enum ReadType {
HoldingRegister,
InputRegister,
}
-const FLASH_SIZE: usize = 2 * 1024 * 1024;
+const DEVICE_STATE_RESET: u16 = 1;
+const DEVICE_STATE_RESPOND_DETECT: u16 = 2;
+const DEVICE_STATE_PROCESS_BROADCAST_PROGRAM: u16 = 4;
struct ModBusRegs<'a> {
led_g: Output<'a, PIN_5>,
@@ -322,7 +326,8 @@ struct ModBusRegs<'a> {
reed2: Input<'a, PIN_21>,
reed1: Input<'a, PIN_22>,
adc_data: &'a AdcData,
- flash: Flash<'a, peripherals::FLASH, FLASH_SIZE>,
+ device_state: u16,
+ uf2updater: UF2UpdateHandler<FLASH_SIZE>,
consistent_read_type: ReadType,
consistent_read_addr: u16,
@@ -331,21 +336,35 @@ struct ModBusRegs<'a> {
}
impl<'a> ModBusRegs<'a> {
- fn read_reg_u64(self: &mut Self, read_type: ReadType, base_addr: u16, addr: u16, value: u64) -> Result<u16, ModbusErrorCode> {
+ fn read_reg(self: &mut Self, read_type: ReadType, base_addr: u16, addr: u16, value: &[u16]) -> Result<u16, ModbusErrorCode> {
defmt::assert!(addr >= base_addr);
- defmt::assert!(addr < base_addr + 64/16);
+ defmt::assert!(addr < base_addr + value.len() as u16);
+ defmt::assert!(value.len() <= self.consistent_read_data.len());
self.consistent_read_type = read_type;
self.consistent_read_addr = base_addr;
- self.consistent_read_length = 64/16;
- self.consistent_read_data = [
+ self.consistent_read_length = value.len() as u16;
+ self.consistent_read_data[0..value.len()].copy_from_slice(value);
+
+ Ok(self.consistent_read_data[(addr - base_addr) as usize])
+ }
+
+ fn read_reg_2x_u32(self: &mut Self, read_type: ReadType, base_addr: u16, addr: u16, value: [u32; 2]) -> Result<u16, ModbusErrorCode> {
+ self.read_reg(read_type, base_addr, addr, &[
+ ((value[0] >> 0) & 0xffff) as u16,
+ ((value[0] >> 16) & 0xffff) as u16,
+ ((value[1] >> 0) & 0xffff) as u16,
+ ((value[1] >> 16) & 0xffff) as u16,
+ ])
+ }
+
+ fn read_reg_u64(self: &mut Self, read_type: ReadType, base_addr: u16, addr: u16, value: u64) -> Result<u16, ModbusErrorCode> {
+ self.read_reg(read_type, base_addr, addr, &[
((value >> 0) & 0xffff) as u16,
((value >> 16) & 0xffff) as u16,
((value >> 32) & 0xffff) as u16,
((value >> 48) & 0xffff) as u16,
- ];
-
- Ok(self.consistent_read_data[(addr - base_addr) as usize])
+ ])
}
}
@@ -373,18 +392,6 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
}
}
- fn read_holding_register(self: &mut Self, _device_addr: u8, addr: u16) -> Result<u16, ModbusErrorCode> {
- if self.consistent_read_type == ReadType::HoldingRegister
- && addr >= self.consistent_read_addr && addr < self.consistent_read_addr + self.consistent_read_length {
- return Ok(self.consistent_read_data[(addr - self.consistent_read_addr) as usize]);
- }
-
- if addr == 1 {
- return Ok(42)
- }
- Err(ModbusErrorCode::IllegalDataAddress)
- }
-
fn read_input_register(self: &mut Self, _device_addr: u8, addr: u16) -> Result<u16, ModbusErrorCode> {
if self.consistent_read_type == ReadType::InputRegister
&& addr >= self.consistent_read_addr && addr < self.consistent_read_addr + self.consistent_read_length {
@@ -419,6 +426,11 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
self.adc_data.currents[1].current_on_time.try_lock().map(
|x| x.as_micros()).unwrap_or(0)),
+ 25 | 26 | 27 | 28 => {
+ let (from, to) = self.uf2updater.get_missing_block_info();
+ self.read_reg_2x_u32(ReadType::InputRegister, 25, addr, [from, to])
+ },
+
_ => Err(ModbusErrorCode::IllegalDataAddress)
}
}
@@ -430,11 +442,43 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
}
}
+ fn read_holding_register(self: &mut Self, _device_addr: u8, addr: u16) -> Result<u16, ModbusErrorCode> {
+ if self.consistent_read_type == ReadType::HoldingRegister
+ && addr >= self.consistent_read_addr && addr < self.consistent_read_addr + self.consistent_read_length {
+ return Ok(self.consistent_read_data[(addr - self.consistent_read_addr) as usize]);
+ }
+
+ match addr {
+ 0 => Ok(self.device_state),
+ 1 => Ok(42),
+ _ => Err(ModbusErrorCode::IllegalDataAddress),
+ }
+ }
+
fn write_register(self: &mut Self, _device_addr: u8, addr: u16, value: u16) -> Result<u16, ModbusErrorCode> {
- Err(ModbusErrorCode::IllegalDataAddress)
+ match addr {
+ 0 => { self.device_state = value; Ok(self.device_state) },
+ 1 => {
+ const OK: u16 = 'O' as u16 | (('K' as u16) << 8);
+ const UP: u16 = 'U' as u16 | (('P' as u16) << 8);
+ match value {
+ OK => {
+ self.uf2updater.mark_booted().map(|_| value)
+ },
+ UP if self.uf2updater.successfully_programmed() => {
+ self.uf2updater.mark_updated().map(|_| value)
+ },
+ UP => {
+ Ok(0)
+ },
+ _ => Err(ModbusErrorCode::IllegalDataValue),
+ }
+ },
+ _ => Err(ModbusErrorCode::IllegalDataAddress),
+ }
}
- fn read_file_records(self: &mut Self, _device_addr: u8,
+ fn read_file_record(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) {
@@ -451,7 +495,7 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
},
(6, 1) => {
pusher.push_fn(record_length as usize * 2, |buf| {
- self.flash.read(record_number as u32 * 2, buf)
+ self.uf2updater.flash.read(record_number as u32 * 2, buf)
.map_err(|err| {
info!("Error reading flash: {:?}", err);
ModbusErrorCode::ServerDeviceFailure
@@ -463,6 +507,27 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
}
}
}
+
+ fn write_file_record(self: &mut Self, device_addr: u8,
+ ref_type: u8, file_number: u16, record_number: u16, data: &[u8])
+ -> Result<(), ModbusErrorCode> {
+ match (ref_type, file_number) {
+ (6, 0) => {
+ Ok(())
+ },
+ (6, 1) => {
+ if device_addr != 0x55 || (self.device_state & DEVICE_STATE_PROCESS_BROADCAST_PROGRAM) != 0 {
+ //NOTE This will use blocking flash operations and we won't be able to respond to Modbus
+ // during that time. There isn't much that we can do about this because the flash won't
+ // be in XIP mode so we cannot continue running our program from flash.
+ self.uf2updater.write(record_number as u32 * 2, data)
+ } else {
+ Ok(())
+ }
+ },
+ _ => Err(ModbusErrorCode::IllegalDataAddress),
+ }
+ }
}
// The pause bits are on by default after reset but I think Watchdog::enable()
@@ -563,7 +628,8 @@ 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),
+ device_state: DEVICE_STATE_RESET | DEVICE_STATE_RESPOND_DETECT,
+ uf2updater: UF2UpdateHandler::new(p.FLASH),
consistent_read_type: ReadType::InputRegister,
consistent_read_addr: u16::default(),
consistent_read_length: u16::default(),
diff --git a/firmware/rust1/src/lib.rs b/firmware/rust1/src/lib.rs
index a22b737..1340495 100644
--- a/firmware/rust1/src/lib.rs
+++ b/firmware/rust1/src/lib.rs
@@ -5,3 +5,5 @@ pub mod i2c_scan;
pub mod rs485;
pub mod modbus_server;
pub mod clear_bootloader_state;
+pub mod uf2;
+pub mod uf2updater;
diff --git a/firmware/rust1/src/modbus_server.rs b/firmware/rust1/src/modbus_server.rs
index cb5e37c..09ae5d6 100644
--- a/firmware/rust1/src/modbus_server.rs
+++ b/firmware/rust1/src/modbus_server.rs
@@ -45,6 +45,15 @@ impl<'a, E: Clone> Cursor<'a, E> {
self.read(&mut buf)?;
Ok(u16::from_be_bytes(buf))
}
+
+ fn read_bytes(self: &mut Self, len: usize) -> Result<&'a [u8], E> {
+ if self.1 + len >= self.0.len() {
+ return Err(self.2.clone())
+ }
+ let data = &self.0[self.1 .. self.1 + len];
+ self.1 += len;
+ Ok(data)
+ }
}
pub struct U16Pusher<'a, const N: usize> {
@@ -101,9 +110,12 @@ 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,
+ fn read_file_record(self: &mut Self, device_addr: u8,
ref_type: u8, file_number: u16, record_number: u16, record_length: u16, pusher: U16Pusher<'_, 256>)
-> Result<(), ModbusErrorCode>;
+ fn write_file_record(self: &mut Self, device_addr: u8,
+ ref_type: u8, file_number: u16, record_number: u16, data: &[u8])
+ -> Result<(), ModbusErrorCode>;
}
#[derive(PartialEq, Eq, Format)]
@@ -365,7 +377,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
}
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)?;
+ self.regs.read_file_record(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 {
@@ -384,6 +396,26 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
txbuf[2] = txbuf.len() as u8 - 3;
Ok(())
},
+ 0x15 => {
+ 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()?;
+ if record_length > 127 || record_length as usize * 2 > capacity - txbuf.len() {
+ return Err(IllegalDataValue)
+ }
+ let data = rx.read_bytes(record_length as usize * 2)?;
+ self.regs.write_file_record(device_addr, ref_type, file_number, record_number, data)?;
+ }
+ // We have to include the data in the reply? Really? Oh well, let's do it then.
+ push_many(txbuf, &rxbuf[0..rxbuf.len()-2])?;
+ Ok(())
+ },
_ => {
Err(IllegalFunction)
},
diff --git a/firmware/rust1/src/uf2.rs b/firmware/rust1/src/uf2.rs
new file mode 100644
index 0000000..4f9d8e7
--- /dev/null
+++ b/firmware/rust1/src/uf2.rs
@@ -0,0 +1,50 @@
+// copied from elfuf2-rs (because we need it as a library and without std):
+// https://github.com/JoNil/elf2uf2-rs/blob/b861f6b3c9540bcb27e88ec496e09763e590dc76/src/uf2.rs
+// 0BSD license
+
+#![allow(dead_code)]
+
+use static_assertions::const_assert;
+use core::mem;
+use zerocopy::{AsBytes, FromBytes};
+
+pub const UF2_MAGIC_START0: u32 = 0x0A324655;
+pub const UF2_MAGIC_START1: u32 = 0x9E5D5157;
+pub const UF2_MAGIC_END: u32 = 0x0AB16F30;
+
+pub const UF2_FLAG_NOT_MAIN_FLASH: u32 = 0x00000001;
+pub const UF2_FLAG_FILE_CONTAINER: u32 = 0x00001000;
+pub const UF2_FLAG_FAMILY_ID_PRESENT: u32 = 0x00002000;
+pub const UF2_FLAG_MD5_PRESENT: u32 = 0x00004000;
+
+pub const RP2040_FAMILY_ID: u32 = 0xe48bff56;
+
+#[repr(packed)]
+#[derive(AsBytes, FromBytes)]
+pub struct Uf2BlockHeader {
+ pub magic_start0: u32,
+ pub magic_start1: u32,
+ pub flags: u32,
+ pub target_addr: u32,
+ pub payload_size: u32,
+ pub block_no: u32,
+ pub num_blocks: u32,
+ pub file_size: u32, // or familyID
+}
+
+pub type Uf2BlockData = [u8; 476];
+
+#[repr(packed)]
+#[derive(AsBytes, FromBytes)]
+pub struct Uf2BlockFooter {
+ pub magic_end: u32,
+}
+
+const_assert!(mem::size_of::<Uf2BlockHeader>() == 32);
+const_assert!(mem::size_of::<Uf2BlockFooter>() == 4);
+const_assert!(
+ mem::size_of::<Uf2BlockHeader>()
+ + mem::size_of::<Uf2BlockData>()
+ + mem::size_of::<Uf2BlockFooter>()
+ == 512
+);
diff --git a/firmware/rust1/src/uf2updater.rs b/firmware/rust1/src/uf2updater.rs
new file mode 100644
index 0000000..17db334
--- /dev/null
+++ b/firmware/rust1/src/uf2updater.rs
@@ -0,0 +1,351 @@
+use core::mem::size_of;
+
+use bitvec::{BitArr, bitarr};
+use bitvec::order::Lsb0;
+use defmt::*;
+use embassy_boot::{Partition, AlignedBuffer, FirmwareUpdater};
+use embassy_rp::{flash::Flash, peripherals};
+use zerocopy::FromBytes;
+
+use crate::{modbus_server::ModbusErrorCode, uf2::*};
+
+#[derive(PartialEq, Eq)]
+enum PositionInSector {
+ Start, StartPartial, Middle, End
+}
+
+struct BootLoaderPartitions {
+ _state: Partition,
+ active: Partition,
+ dfu: Partition,
+}
+// copied from embassy/embassy-boot/rp/src/lib.rs because fields are private for BootLoader
+impl Default for BootLoaderPartitions {
+ /// Create a new bootloader instance using parameters from linker script
+ fn default() -> Self {
+ extern "C" {
+ static __bootloader_state_start: u32;
+ static __bootloader_state_end: u32;
+ static __bootloader_active_start: u32;
+ static __bootloader_active_end: u32;
+ static __bootloader_dfu_start: u32;
+ static __bootloader_dfu_end: u32;
+ }
+
+ let active = unsafe {
+ Partition::new(
+ &__bootloader_active_start as *const u32 as u32,
+ &__bootloader_active_end as *const u32 as u32,
+ )
+ };
+ let dfu = unsafe {
+ Partition::new(
+ &__bootloader_dfu_start as *const u32 as u32,
+ &__bootloader_dfu_end as *const u32 as u32,
+ )
+ };
+ let _state = unsafe {
+ Partition::new(
+ &__bootloader_state_start as *const u32 as u32,
+ &__bootloader_state_end as *const u32 as u32,
+ )
+ };
+
+ BootLoaderPartitions{ active, dfu, _state }
+ }
+}
+
+const FLASH_SIZE_GLOBAL: usize = 2 * 1024 * 1024;
+const MAX_UF2_SECTORS_MIN: usize = FLASH_SIZE_GLOBAL / 2 / 256;
+const MAX_UF2_SECTORS: usize = (MAX_UF2_SECTORS_MIN + 31) / 32 * 32;
+
+pub struct UF2UpdateHandler<const FLASH_SIZE: usize> {
+ buf: AlignedBuffer<4096>,
+ write_pos: u32,
+
+ pub flash: Flash<'static, peripherals::FLASH, FLASH_SIZE>,
+ uf2_seen_bitmask: BitArr!(for MAX_UF2_SECTORS, in u32),
+ uf2_num_blocks: u32,
+ flash_erased_address_and_first_block: Option<(u32, usize)>,
+}
+
+impl<const FLASH_SIZE: usize> UF2UpdateHandler<FLASH_SIZE> {
+ pub fn new(flash: peripherals::FLASH) -> Self {
+ defmt::assert!(FLASH_SIZE == FLASH_SIZE_GLOBAL);
+ UF2UpdateHandler {
+ buf: AlignedBuffer([0; 4096]),
+ write_pos: 0,
+ flash: Flash::new(flash),
+ uf2_seen_bitmask: bitarr!(u32, Lsb0; 0; MAX_UF2_SECTORS),
+ uf2_num_blocks: 0,
+ flash_erased_address_and_first_block: None,
+ }
+ }
+
+ pub fn write(self: &mut Self, pos: u32, data: &[u8]) -> Result<(), ModbusErrorCode> {
+ if data.len() > 256 {
+ // We could handle these cases but we are limited by Modbus frames anyway.
+ info!("Too much data in one call to UF2UpdateHandler::write()");
+ self.write_pos = 0;
+ return Err(ModbusErrorCode::IllegalDataValue);
+ }
+
+ if pos % 512 == self.write_pos {
+ // ok
+ } else if pos == 0 {
+ // We are aborting a previous write but that can be ok.
+ self.write_pos = 0;
+ } else {
+ info!("Unexpected write address in UF2UpdateHandler: {}", pos);
+ self.write_pos = 0;
+
+ // Re-sync if this write crosses the 512-byte boundary, i.e. keep data at start of the next sector.
+ let write_size_to_end_of_sector = 512 - (pos as usize % 512);
+ if write_size_to_end_of_sector < data.len() {
+ let write_size2 = data.len() - write_size_to_end_of_sector;
+ self.buf.0[0 .. write_size2].copy_from_slice(&data[write_size_to_end_of_sector .. data.len()]);
+ self.write_pos = write_size2 as u32;
+ }
+
+ return Err(ModbusErrorCode::IllegalDataValue);
+ }
+
+ let write_size1 = core::cmp::min(512 - (self.write_pos as usize % 512), data.len());
+ if write_size1 > 0 {
+ self.buf.0[self.write_pos as usize .. self.write_pos as usize + write_size1].copy_from_slice(&data[0 .. write_size1]);
+ self.write_pos += write_size1 as u32;
+
+ if self.write_pos == 512 {
+ let result = self.process_sector();
+
+ self.write_pos = 0;
+ if data.len() > write_size1 {
+ let write_size2 = data.len() - write_size1;
+ self.buf.0[0 .. write_size2].copy_from_slice(&data[write_size1 .. data.len()]);
+ self.write_pos = write_size2 as u32;
+ }
+
+ result?;
+ }
+ }
+
+ Ok(())
+ }
+
+ fn process_sector(self: &mut Self) -> Result<(), ModbusErrorCode> {
+ defmt::assert!(self.write_pos == 512);
+
+ let uf2_header = Uf2BlockHeader::read_from_prefix(self.buf.0.as_slice()).unwrap();
+ let uf2_footer = Uf2BlockFooter::read_from_suffix(self.buf.0.as_slice()).unwrap();
+ if uf2_header.magic_start0 != UF2_MAGIC_START0 || uf2_header.magic_start1 != UF2_MAGIC_START1 || uf2_footer.magic_end != UF2_MAGIC_END {
+ warn!("Invalid magic in UF2 block");
+ return Err(ModbusErrorCode::IllegalDataValue)
+ }
+ if uf2_header.num_blocks as usize > self.uf2_seen_bitmask.len() {
+ warn!("We cannot support that many blocks in one UF2 file.");
+ return Err(ModbusErrorCode::IllegalDataValue);
+ }
+ if uf2_header.block_no >= uf2_header.num_blocks {
+ warn!("Invalid block_no in UF2 header");
+ return Err(ModbusErrorCode::IllegalDataValue);
+ }
+ if uf2_header.payload_size as usize > 512 - size_of::<Uf2BlockHeader>() - size_of::<Uf2BlockFooter>() {
+ warn!("Invalid block_no in UF2 header");
+ return Err(ModbusErrorCode::IllegalDataValue);
+ }
+ if (uf2_header.flags & UF2_FLAG_FAMILY_ID_PRESENT) == 0 || uf2_header.file_size != RP2040_FAMILY_ID {
+ // not for us but that shouldn't be treated as an error
+ self.uf2_seen_bitmask.set(uf2_header.block_no as usize, true);
+ return Ok(())
+ }
+ if (uf2_header.flags & (UF2_FLAG_NOT_MAIN_FLASH | UF2_FLAG_FILE_CONTAINER)) != 0 {
+ // not for DFU partition
+ self.uf2_seen_bitmask.set(uf2_header.block_no as usize, true);
+ return Ok(())
+ }
+
+ if uf2_header.block_no != self.uf2_num_blocks {
+ self.uf2_seen_bitmask.fill_with(|_| false);
+ self.uf2_num_blocks = uf2_header.block_no;
+ self.flash_erased_address_and_first_block = None;
+ }
+
+ let data_start = size_of::<Uf2BlockHeader>();
+ let data_end = data_start + uf2_header.payload_size as usize;
+ let addr = uf2_header.target_addr as usize;
+ let til_end_of_page = 4096 - (addr & 4096);
+ if (addr & 4096) == 0 {
+ self.process_sector_part(PositionInSector::Start, uf2_header.block_no as usize, addr as u32, data_start, data_end);
+ } else if (uf2_header.payload_size as usize) < til_end_of_page {
+ self.process_sector_part(PositionInSector::Middle, uf2_header.block_no as usize, addr as u32, data_start, data_end);
+ } else {
+ self.process_sector_part(PositionInSector::End, uf2_header.block_no as usize, addr as u32, data_start, data_start + til_end_of_page);
+ let remaining = uf2_header.payload_size as usize - til_end_of_page;
+ if remaining > 0 {
+ self.process_sector_part(PositionInSector::StartPartial, uf2_header.block_no as usize,
+ (addr + til_end_of_page) as u32, til_end_of_page, data_end);
+ }
+ }
+
+ Ok(())
+ }
+
+ fn process_sector_part(self: &mut Self, pos: PositionInSector, block_no: usize, addr: u32, data_start: usize, data_end: usize) {
+ use PositionInSector::*;
+
+ let data = &self.buf.0[data_start .. data_end];
+
+ let partitions = BootLoaderPartitions::default();
+ if addr < partitions.active.from || addr >= partitions.active.to {
+ // We don't want to write this.
+ if pos != StartPartial {
+ self.uf2_seen_bitmask.set(block_no, true);
+ }
+ return;
+ }
+ //FIXME We have to use FirmwareUpdater, I think.
+ let addr = addr - partitions.active.from + partitions.dfu.to;
+
+ let abort_previous: bool;
+ let process_current: bool;
+ let is_start = match pos {
+ Start | StartPartial => true,
+ Middle | End => false,
+ };
+
+ if let Some((erased_addr, first_block)) = self.flash_erased_address_and_first_block {
+ if is_start {
+ // previous block is not done but we are starting a new one -> abort previous one
+ abort_previous = true;
+ process_current = true;
+ } else if erased_addr == addr {
+ // address matches current partially written block -> continue writing to it
+ abort_previous = false;
+ process_current = true;
+ } else {
+ // address doesn't match -> abort previous and we can't do anything useful with the current one either
+ abort_previous = true;
+ process_current = false;
+ }
+
+ // If there is a partially written page, mark the blocks as not seen so we will later try again.
+ if abort_previous {
+ for i in first_block .. block_no {
+ self.uf2_seen_bitmask.set(i, false);
+ }
+ self.flash_erased_address_and_first_block = None;
+ }
+ } else {
+ if is_start {
+ // previous block was done and we are starting a new one -> continue with that
+ process_current = true;
+ } else {
+ // previous block was done but this is not the start of a new one -> ignore it
+ process_current = false;
+ }
+ }
+
+ if !process_current {
+ return;
+ }
+
+ let already_processed = match pos {
+ StartPartial => block_no+1 < self.uf2_seen_bitmask.len() && self.uf2_seen_bitmask[block_no+1],
+ _ => self.uf2_seen_bitmask[block_no],
+ };
+ if already_processed {
+ return;
+ }
+
+ if is_start {
+ // We have to erase the block.
+ match self.flash.erase(addr, addr+4096) {
+ Ok(()) => (),
+ Err(e) => {
+ error!("erase: {:?}", e);
+ return;
+ }
+ }
+
+ let first_block = match pos {
+ Start => block_no,
+ StartPartial => block_no+1,
+ _ => defmt::unreachable!(),
+ };
+
+ self.flash_erased_address_and_first_block = Some((addr, first_block));
+ }
+
+ let (mut prev_addr, first_block) = self.flash_erased_address_and_first_block.unwrap();
+ defmt::assert!(prev_addr == addr);
+ //FIXME We might have to handle alignment concerns for address and data.
+ let r = self.flash.write(addr, data);
+ if let Err(e) = r {
+ error!("write: {:?}", e);
+
+ // abort current block
+ for i in first_block .. block_no {
+ self.uf2_seen_bitmask.set(i, false);
+ }
+ self.flash_erased_address_and_first_block = None;
+ return;
+ }
+
+ prev_addr += data.len() as u32;
+ if (prev_addr & 4096) == 0 {
+ self.flash_erased_address_and_first_block = None;
+ } else {
+ self.flash_erased_address_and_first_block = Some((prev_addr, first_block));
+ }
+
+ if pos != StartPartial {
+ self.uf2_seen_bitmask.set(block_no, true);
+ }
+ }
+
+ pub fn get_missing_block_info(self: &mut Self) -> (u32, u32) {
+ if self.uf2_num_blocks == 0 {
+ return (0, 0)
+ } else if (self.uf2_num_blocks as usize) > self.uf2_seen_bitmask.len() {
+ return (self.uf2_num_blocks, self.uf2_num_blocks)
+ } else {
+ match self.uf2_seen_bitmask.first_zero() {
+ None => (1, 1),
+ Some(pos) if pos >= self.uf2_num_blocks as usize => (1, 1),
+ Some(first_missing) => {
+ let (_, first_present) = self.uf2_seen_bitmask.split_at(first_missing);
+ let first_present = first_present.first_one()
+ .map(|x| (x + first_missing) as u32)
+ .unwrap_or(self.uf2_num_blocks);
+ (first_missing as u32, first_present)
+ }
+ }
+ }
+ }
+
+ pub fn successfully_programmed(self: &mut Self) -> bool {
+ self.get_missing_block_info() == (1, 1)
+ }
+
+ pub fn mark_updated(self: &mut Self) -> Result<(), ModbusErrorCode> {
+ let mut updater = FirmwareUpdater::default();
+ match updater.mark_updated_blocking(&mut self.flash, &mut self.buf.0[..1]) {
+ Ok(()) => Ok(()),
+ Err(e) => {
+ error!("mark_updated_blocking: {:?}", e);
+ Err(ModbusErrorCode::ServerDeviceFailure)
+ }
+ }
+ }
+
+ pub fn mark_booted(self: &mut Self) -> Result<(), ModbusErrorCode> {
+ let mut updater = FirmwareUpdater::default();
+ match updater.mark_booted_blocking(&mut self.flash, &mut self.buf.0[..1]) {
+ Ok(()) => Ok(()),
+ Err(e) => {
+ error!("mark_booted_blocking: {:?}", e);
+ Err(ModbusErrorCode::ServerDeviceFailure)
+ }
+ }
+ }
+}
--
GitLab