From 90b88607fcbc3928b5b558ff9931f9336d3b1f7f Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 30 Jan 2025 17:43:05 -0500
Subject: [PATCH] message decoder

---
 southbridge/Cargo.lock  | 14 ++++++++++
 southbridge/Cargo.toml  |  2 ++
 southbridge/src/main.rs | 58 +++++++++++++++++++++++++++++++++++++----
 3 files changed, 69 insertions(+), 5 deletions(-)

diff --git a/southbridge/Cargo.lock b/southbridge/Cargo.lock
index 6414175..f3d03c3 100644
--- a/southbridge/Cargo.lock
+++ b/southbridge/Cargo.lock
@@ -1073,6 +1073,9 @@ name = "portable-atomic"
 version = "1.10.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6"
+dependencies = [
+ "critical-section",
+]
 
 [[package]]
 name = "postcard"
@@ -1368,8 +1371,10 @@ dependencies = [
  "log",
  "mpu6050",
  "nalgebra",
+ "portable-atomic",
  "postcard",
  "serde",
+ "static_cell",
 ]
 
 [[package]]
@@ -1397,6 +1402,15 @@ version = "1.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
 
+[[package]]
+name = "static_cell"
+version = "2.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e"
+dependencies = [
+ "portable-atomic",
+]
+
 [[package]]
 name = "string_cache"
 version = "0.8.7"
diff --git a/southbridge/Cargo.toml b/southbridge/Cargo.toml
index 947f32b..240c3a4 100644
--- a/southbridge/Cargo.toml
+++ b/southbridge/Cargo.toml
@@ -28,3 +28,5 @@ mpu6050 = { git = "https://git.ank.dev/ank/mpu6050" }
 nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"] }
 serde = { version = "1.0.203", default-features = false, features = ["derive"] }
 postcard = "1.0.0"
+static_cell = "2.1"
+portable-atomic = { version = "1.5", features = ["critical-section"] }
diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs
index 4cb248a..9aa906d 100644
--- a/southbridge/src/main.rs
+++ b/southbridge/src/main.rs
@@ -3,16 +3,24 @@
 
 use core::panic::PanicInfo;
 
+use common::Command;
 use embassy_executor::Spawner;
-use embassy_rp::{bind_interrupts, peripherals::USB, pwm::{self, Pwm}, usb::Driver};
+use embassy_rp::{bind_interrupts, peripherals::{UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, Config}, usb::Driver};
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
 use embassy_time::{Duration, Timer};
-use log::{error, info};
+use embedded_io_async::{BufRead, Read};
+use framed::{bytes, FRAME_END_SYMBOL};
+use log::{error, info, warn};
+use static_cell::{ConstStaticCell, StaticCell};
 
 bind_interrupts!(struct Irqs {
-    I2C0_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C0>;
+    I2C1_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C1>;
     USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
+    UART1_IRQ => BufferedInterruptHandler<UART1>;
 });
 
+pub static COMMANDS: Channel<CriticalSectionRawMutex, Command, 5> = Channel::new();
+
 #[embassy_executor::task]
 async fn logger_task(driver: Driver<'static, USB>) {
     embassy_usb_logger::run!(1024, log::LevelFilter::Debug, driver);
@@ -36,13 +44,53 @@ async fn main(spawner: Spawner) {
     drive_conf.top = 62500; // 20ms
     drive_conf.compare_b = 4687; // 1.5ms
     drive_conf.compare_a = 4687; // 1.5ms
-    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, drive_conf.clone());
+    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, drive_conf.clone());
 
     let config = embassy_rp::i2c::Config::default();
-    let bus = embassy_rp::i2c::I2c::new_async(p.I2C0, p.PIN_21, p.PIN_20, Irqs, config);
+    let bus = embassy_rp::i2c::I2c::new_async(p.I2C1, p.PIN_19, p.PIN_18, Irqs, config);
+
+    static TX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0u8;1024]);
+    let tx_buf = TX_BUF.take();
+    static RX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0u8;1024]);
+    let rx_buf = RX_BUF.take();
+
+    let uart = BufferedUart::new(p.UART1, Irqs, p.PIN_20, p.PIN_21, tx_buf, rx_buf, Config::default());
+
+    let (mut tx,rx) = uart.split();
+
+    spawner.spawn(decoder(rx)).unwrap();
+
 
     loop {
         info!("Blink");
         Timer::after(Duration::from_millis(100)).await;
     }
 }
+
+/// Receive data from the pi 0 over UART and deserialize it
+#[embassy_executor::task]
+async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
+    info!("Reading...");
+    let mut codec = framed::bytes::Config::default().to_codec();
+
+    loop {
+        let data = rx.fill_buf().await.unwrap();
+        let Some(end) =  data.iter().position(|e| *e == FRAME_END_SYMBOL) else { continue; };
+        let data = &data[..end];
+
+        let mut buf = [0u8; 1024];
+        let Ok(len) = codec.decode_to_slice(data, &mut buf) else {
+            error!("frame decode fail");
+            continue;
+        };
+
+        let Ok(data) = postcard::from_bytes::<Command>(&buf[..len]) else {
+            error!("message decode fail");
+            continue;
+        };
+
+        COMMANDS.send(data).await;
+
+        rx.consume(end);
+    }
+}