wip pico rewrite

pico/Cargo.lock

@@ -1335,6 +1335,7 @@ dependencies = [
  "embassy-futures",
  "embassy-net",
  "embassy-rp",
+ "embassy-sync 0.8.0",
  "embassy-time",
  "embassy-usb-logger",
  "embedded-hal-compat",

pico/Cargo.toml

@@ -14,6 +14,7 @@ as5600 = "0.8.0"
 embassy-futures = "0.1.2"
 embedded-io-async = "0.7.0"
 embedded-io = "0.7.1"
+embassy-sync = "0.8.0"
 arrayvec = { version = "0.7.6", default-features = false }
 embassy-net = { version = "0.9.1",features = ["defmt", "icmp", "tcp", "udp", "raw", "dhcpv4", "medium-ethernet", "dns", "proto-ipv4", "proto-ipv6", "multicast"]}
 embassy-executor = { version = "0.10.0", features = [

pico/src/main.rs

@@ -3,31 +3,42 @@
 
 use core::net::SocketAddrV4;
 use core::str::FromStr;
-use embassy_net::tcp::TcpSocket;
+use embassy_futures::join::join;
-use embassy_net::udp::{PacketMetadata, UdpMetadata, UdpSocket};
+use embassy_net::tcp::{TcpReader, TcpSocket, TcpWriter};
+use embassy_rp::multicore::{Stack, spawn_core1};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::channel::Channel;
+use embassy_sync::mutex::Mutex;
 
 use crate::buffer::{WriteBuffer, wait_for_config};
+use crate::screen::{SCREEN_BUFFER, lcd_display_task};
 use ag_lcd::{Blink, Cursor, Display, LcdDisplay};
 use as5600::As5600;
 use cyw43::{JoinOptions, aligned_bytes};
 use cyw43_pio::{DEFAULT_CLOCK_DIVIDER, PioSpi};
 use defmt::*;
-use embassy_executor::Spawner;
+use embassy_executor::{Executor, Spawner};
-use embassy_net::{Stack, StackResources};
+use embassy_net::StackResources;
 use embassy_rp::clocks::RoscRng;
 use embassy_rp::gpio::{Input, Level, Output};
 use embassy_rp::i2c::{self, Config, I2c};
-use embassy_rp::peripherals::{DMA_CH0, I2C0, PIO0};
+use embassy_rp::peripherals::{DMA_CH0, I2C0, PIN_4, PIN_5, PIN_23, PIN_24, PIN_25, PIN_29, PIO0};
 use embassy_rp::pio::{InterruptHandler, Pio};
-use embassy_rp::{bind_interrupts, dma};
+use embassy_rp::{Peri, bind_interrupts, dma};
 use embassy_rp::{peripherals::USB, usb};
 use embassy_time::{Delay, Duration, Timer};
 use static_cell::StaticCell;
-use ufmt::{uWrite, uwrite};
+use ufmt::uwrite;
 
 use {defmt_rtt as _, panic_probe as _};
 
 mod buffer;
+mod screen;
+
+const WIFI_NETWORK: &str = "flamme";
+const WIFI_PASSWORD: &str = "12345678";
+const TARGET_IP: &str = "84.238.32.253";
+const TARGET_PORT: u16 = 7070;
 
 bind_interrupts!(struct Irqs {
     PIO0_IRQ_0 => InterruptHandler<PIO0>;
@@ -53,13 +64,47 @@ async fn net_task(mut runner: embassy_net::Runner<'static, cyw43::NetDriver<'sta
     runner.run().await
 }
 
-const WIFI_NETWORK: &str = "flamme";
+/// Button input notification channel
-const WIFI_PASSWORD: &str = "12345678";
+static INPUT: Channel<CriticalSectionRawMutex, u8, 64> = Channel::new();
 
-#[embassy_executor::main]
+#[embassy_executor::task]
-async fn main(spawner: Spawner) {
+/// Polls a single pin for falling edge (assumes buttons are pulled up and shorted when pressed)
-    let p = embassy_rp::init(Default::default());
+/// Sends [INPUT] with given id
+async fn button_poll_task(mut button: Input<'static>, id: u8) {
+    loop {
+        button.wait_for_falling_edge().await;
+        INPUT.send(id).await;
+    }
+}
 
+static ANGLE: Mutex<CriticalSectionRawMutex, u16> = Mutex::new(0);
+
+#[embassy_executor::task]
+async fn rotation_read_task(
+    i2c: Peri<'static, I2C0>,
+    scl: Peri<'static, PIN_5>,
+    sda: Peri<'static, PIN_4>,
+) {
+    let i2c = I2c::new_async(i2c, scl, sda, Irqs, Config::default());
+    let mut as5600 = As5600::new(i2c);
+
+    loop {
+        let angle = as5600.angle().unwrap_or(0);
+        *ANGLE.lock().await = angle;
+        Timer::after(Duration::from_millis(50)).await;
+    }
+}
+
+#[embassy_executor::task]
+async fn network_setup_task(
+    pin23: Peri<'static, PIN_23>,
+    pin25: Peri<'static, PIN_25>,
+    pio: Peri<'static, PIO0>,
+    pin24: Peri<'static, PIN_24>,
+    pin29: Peri<'static, PIN_29>,
+    dma: Peri<'static, DMA_CH0>,
+    spawner: Spawner,
+) {
     let fw = aligned_bytes!("../firmware/43439A0.bin");
     let clm = aligned_bytes!("../firmware/43439A0_clm.bin");
     let nvram = aligned_bytes!("../firmware/nvram_rp2040.bin");
@@ -72,48 +117,24 @@ async fn main(spawner: Spawner) {
     //let fw = unsafe { core::slice::from_raw_parts(0x10100000 as *const u8, 230321) };
     //let clm = unsafe { core::slice::from_raw_parts(0x10140000 as *const u8, 4752) };
 
-    let pwr = Output::new(p.PIN_23, Level::Low);
+    let pwr = Output::new(pin23, Level::Low);
-    let cs = Output::new(p.PIN_25, Level::High);
+    let cs = Output::new(pin25, Level::High);
-    let mut pio = Pio::new(p.PIO0, Irqs);
+    let mut pio = Pio::new(pio, Irqs);
     let spi = PioSpi::new(
         &mut pio.common,
         pio.sm0,
         DEFAULT_CLOCK_DIVIDER,
         pio.irq0,
         cs,
-        p.PIN_24,
+        pin24,
-        p.PIN_29,
+        pin29,
-        dma::Channel::new(p.DMA_CH0, Irqs),
+        dma::Channel::new(dma, Irqs),
     );
 
     static STATE: StaticCell<cyw43::State> = StaticCell::new();
     let state = STATE.init(cyw43::State::new());
     let (net_device, mut control, runner) = cyw43::new(state, pwr, spi, fw, nvram).await;
-
     spawner.spawn(unwrap!(cyw43_task(runner)));
-    spawner.spawn(unwrap!(logger_task(p.USB)));
-
-    let edelay = &mut Delay;
-
-    let mut lcd: LcdDisplay<_, _> = LcdDisplay::new(
-        Output::new(p.PIN_15, Level::Low),
-        Output::new(p.PIN_14, Level::Low),
-        edelay,
-    )
-    .with_half_bus(
-        Output::new(p.PIN_13, Level::Low),
-        Output::new(p.PIN_12, Level::Low),
-        Output::new(p.PIN_11, Level::Low),
-        Output::new(p.PIN_10, Level::Low),
-    )
-    .with_display(Display::On)
-    .with_blink(Blink::On)
-    .with_cursor(Cursor::On)
-    .with_lines(ag_lcd::Lines::TwoLines)
-    // .with_autoscroll(ag_lcd::AutoScroll::On)
-    .build();
-    lcd.set_cursor(Cursor::Off);
-    lcd.set_blink(Blink::Off);
 
     control.init(clm).await;
     control
@@ -128,108 +149,173 @@ async fn main(spawner: Spawner) {
         rng.next_u64(),
     );
     spawner.spawn(unwrap!(net_task(runner)));
-    uwrite!(lcd, "con.");
+    uwrite!(SCREEN_BUFFER.lock().await.line1(), "con");
 
     while let Err(err) = control
         .join(WIFI_NETWORK, JoinOptions::new(WIFI_PASSWORD.as_bytes()))
         .await
     {
-        lcd.clear();
         let num = match err {
             cyw43::JoinError::AuthenticationFailure => 200,
             cyw43::JoinError::JoinFailure(e) => e,
             cyw43::JoinError::NetworkNotFound => 201,
         };
-        uwrite!(lcd, "join {}", num);
+        uwrite!(SCREEN_BUFFER.lock().await.line1(), "join {}", num);
     }
 
-    uwrite!(lcd, "link.");
+    // uwrite!(lcd, "link.");
+    uwrite!(SCREEN_BUFFER.lock().await.line1(), "link.");
     stack.wait_link_up().await;
 
-    lcd.clear();
+    // lcd.clear();
-    // lcd.home();
+    // // lcd.home();
-    uwrite!(lcd, "dhcp.");
+    // uwrite!(lcd, "dhcp.");
+    let mut buf = SCREEN_BUFFER.lock().await;
+    buf.clear();
+    uwrite!(buf.line1(), "dhcp.");
+    drop(buf);
     stack.wait_config_up().await;
     let cfg = wait_for_config(stack).await;
     let local_addr = cfg.address.address();
     info!("IP address: {:?}", local_addr);
-    // uwrite!(lcd, "IP address: {:?}", local_addr.octets());
-
-    let i2c = I2c::new_async(p.I2C0, p.PIN_5, p.PIN_4, Irqs, Config::default());
-    let mut as5600 = As5600::new(i2c);
-
-    let mut led = Output::new(p.PIN_1, Level::Low);
-
-    let in1 = Input::new(p.PIN_18, embassy_rp::gpio::Pull::Up);
-    let in2 = Input::new(p.PIN_19, embassy_rp::gpio::Pull::Up);
-    let in3 = Input::new(p.PIN_20, embassy_rp::gpio::Pull::Up);
-    let in4 = Input::new(p.PIN_21, embassy_rp::gpio::Pull::Up);
 
+    let host_addr = embassy_net::Ipv4Address::from_str(TARGET_IP).unwrap();
+    // let target = UdpMetadata::from(SocketAddrV4::new(host_addr, 7070));
     let mut rx_buffer = [0; 4096];
     // let mut rx_meta = [PacketMetadata::EMPTY; 16];
     let mut tx_buffer = [0; 4096];
-    // let mut tx_meta = [PacketMetadata::EMPTY; 16];
-
-    // let mut socket = UdpSocket::new(
-    //     stack,
-    //     &mut rx_meta,
-    //     &mut rx_buffer,
-    //     &mut tx_meta,
-    //     &mut tx_buffer,
-    // );
-    // socket.bind(7070).unwrap();
-
-    let host_addr = embassy_net::Ipv4Address::from_str("84.238.32.253").unwrap();
-    // let target = UdpMetadata::from(SocketAddrV4::new(host_addr, 7070));
     let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
     socket.set_timeout(Some(Duration::from_secs(10)));
-    if let Err(e) = socket.connect(SocketAddrV4::new(host_addr, 7070)).await {
+    if let Err(e) = socket
-        uwrite!(lcd, "tcperr");
+        .connect(SocketAddrV4::new(host_addr, TARGET_PORT))
+        .await
+    {
+        // uwrite!(lcd, "tcperr");
+        uwrite!(SCREEN_BUFFER.lock().await.line1(), "tcperr");
         error!("tcp connect error: {}", e);
     } else {
-        uwrite!(lcd, "tcpok");
+        // uwrite!(lcd, "tcpok");
+        uwrite!(SCREEN_BUFFER.lock().await.line1(), "tcpok");
     }
 
-    let delay = Duration::from_millis(100);
+    let (mut read, mut write) = socket.split();
-    let mut buffer = WriteBuffer::<1024>::new();
-    loop {
-        let in1 = in1.is_low();
-        let in2 = in2.is_low();
-        let in3 = in3.is_low();
-        let in4 = in4.is_low();
-        let angle = as5600.angle().unwrap_or(0);
-        {
-            use embedded_io::Write;
-            let _ = core::writeln!(
-                &mut buffer,
-                "in1={} in2={} in3={} in4={} angle={}",
-                in1,
-                in2,
-                in3,
-                in4,
-                angle
-            );
-        }
-        {
-            let _ = socket.write(&*buffer).await;
-            buffer.clear();
-        }
-        if socket.may_recv() {
-            let mut rx_buffer = [0; 4096];
-            let n = socket.read(&mut rx_buffer).await.unwrap();
-            if n > 0 {
-                lcd.clear();
-                lcd.home();
-                let s = core::str::from_utf8(&rx_buffer[..n]).unwrap_or("");
-                let npos = s.find('\n').unwrap_or(n);
-                let display_text = &s[..npos];
-                lcd.write_str(display_text).ok();
 
-                Timer::after(Duration::from_micros(100)).await;
+    join(tcp_read_loop(read), tcp_write_loop(write)).await;
-                lcd.set_position(0, 1);
-                lcd.write_str(&s[npos..]);
-            }
-        }
-        Timer::after(delay).await;
-    }
 }
+
+async fn tcp_read_loop(read: TcpReader<'_>) {}
+
+async fn tcp_write_loop(write: TcpWriter<'_>) {}
+
+static mut CORE1_STACK: Stack<4096> = Stack::new();
+static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
+static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
+
+#[cortex_m_rt::entry]
+fn main() -> ! {
+    let p = embassy_rp::init(Default::default());
+
+    let pin23 = p.PIN_23;
+    let pin24 = p.PIN_24;
+    let pin25 = p.PIN_25;
+    let pin29 = p.PIN_29;
+    let pio = p.PIO0;
+    let dma = p.DMA_CH0;
+
+    let lcd: LcdDisplay<_, _> = LcdDisplay::new(
+        Output::new(p.PIN_15, Level::Low),
+        Output::new(p.PIN_14, Level::Low),
+        Delay,
+    )
+    .with_half_bus(
+        Output::new(p.PIN_13, Level::Low),
+        Output::new(p.PIN_12, Level::Low),
+        Output::new(p.PIN_11, Level::Low),
+        Output::new(p.PIN_10, Level::Low),
+    )
+    .with_autoscroll(ag_lcd::AutoScroll::Off)
+    .with_display(Display::On)
+    .with_blink(Blink::On)
+    .with_cursor(Cursor::On)
+    .with_lines(ag_lcd::Lines::TwoLines);
+
+    spawn_core1(
+        p.CORE1,
+        unsafe { &mut *core::ptr::addr_of_mut!(CORE1_STACK) },
+        move || {
+            let executor1 = EXECUTOR1.init(Executor::new());
+            executor1.run(|spawner| {
+                let mut lcd = lcd.build();
+                lcd.set_blink(Blink::Off);
+                lcd.set_cursor(Cursor::Off);
+                spawner.spawn(unwrap!(lcd_display_task(lcd)));
+            });
+        },
+    );
+
+    let executor0 = EXECUTOR0.init(Executor::new());
+    executor0.run(|spawner| {
+        spawner.spawn(unwrap!(logger_task(p.USB)));
+        spawner.spawn(unwrap!(network_setup_task(
+            pin23, pin25, pio, pin24, pin29, dma, spawner
+        )));
+        spawner.spawn(unwrap!(rotation_read_task(p.I2C0, p.PIN_5, p.PIN_4,)));
+
+        spawner.spawn(unwrap!(button_poll_task(
+            Input::new(p.PIN_18, embassy_rp::gpio::Pull::Up),
+            1,
+        )));
+        spawner.spawn(unwrap!(button_poll_task(
+            Input::new(p.PIN_19, embassy_rp::gpio::Pull::Up),
+            2,
+        )));
+        spawner.spawn(unwrap!(button_poll_task(
+            Input::new(p.PIN_20, embassy_rp::gpio::Pull::Up),
+            3,
+        )));
+        spawner.spawn(unwrap!(button_poll_task(
+            Input::new(p.PIN_21, embassy_rp::gpio::Pull::Up),
+            4,
+        )));
+    });
+}
+
+// async fn mainz(spawner: Spawner) {
+//     let delay = Duration::from_millis(100);
+//     loop {
+//         // let angle = as5600.angle().unwrap_or(0);
+//         // {
+//         //     use embedded_io::Write;
+//         //     let _ = core::writeln!(
+//         //         &mut buffer,
+//         //         "in1={} in2={} in3={} in4={} angle={}",
+//         //         in1,
+//         //         in2,
+//         //         in3,
+//         //         in4,
+//         //         angle
+//         //     );
+//         // }
+//         // {
+//         //     let _ = write.write(&*buffer).await;
+//         //     buffer.clear();
+//         // }
+//         if read.read_ready().unwrap() {
+//             let mut rx_buffer = [0; 4096];
+//             let n = read.read(&mut rx_buffer).await.unwrap();
+//             if n > 0 {
+//                 lcd.clear();
+//                 lcd.home();
+//                 let s = core::str::from_utf8(&rx_buffer[..n]).unwrap_or("");
+//                 let npos = s.find('\n').unwrap_or(n);
+//                 let display_text = &s[..npos];
+//                 lcd.write_str(display_text).ok();
+
+//                 Timer::after(Duration::from_micros(100)).await;
+//                 lcd.set_position(0, 1);
+//                 lcd.write_str(&s[npos..]);
+//             }
+//         }
+//         Timer::after(delay).await;
+//     }
+// }

pico/src/screen.rs

@@ -0,0 +1,143 @@
+use core::convert::Infallible;
+
+use ag_lcd::LcdDisplay;
+use embassy_futures::block_on;
+use embassy_rp::gpio::Output;
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex, signal::Signal};
+use embassy_time::{Delay, Timer};
+use ufmt::uWrite;
+
+pub struct ScreenBuffer {
+    line1: [u8; 16],
+    line2: [u8; 16],
+    line1_ptr: u8,
+    line2_ptr: u8,
+}
+
+impl ScreenBuffer {
+    pub fn clear(&mut self) {
+        self.line1.fill(0);
+        self.line2.fill(0);
+        self.line1_ptr = 0;
+        self.line2_ptr = 0;
+    }
+
+    pub fn line1(&mut self) -> ScreenBufferWriter<'_> {
+        ScreenBufferWriter { buf: self, line: 0 }
+    }
+
+    pub fn line2(&mut self) -> ScreenBufferWriter<'_> {
+        ScreenBufferWriter { buf: self, line: 1 }
+    }
+}
+
+pub struct ScreenBufferWriter<'a> {
+    buf: &'a mut ScreenBuffer,
+    line: u8,
+}
+
+impl<'a> uWrite for ScreenBufferWriter<'a> {
+    type Error = Infallible;
+
+    fn write_char(&mut self, c: char) -> Result<(), Self::Error> {
+        if self.line == 0 {
+            let ptr = self.buf.line1_ptr as usize;
+            self.buf.line1[ptr] = c as u8;
+            self.buf.line1_ptr = (self.buf.line1_ptr + 1) % 16;
+        } else {
+            let ptr = self.buf.line2_ptr as usize;
+            self.buf.line2[ptr] = c as u8;
+            self.buf.line2_ptr = (self.buf.line2_ptr + 1) % 16;
+        }
+        Ok(())
+    }
+
+    fn write_str(&mut self, s: &str) -> Result<(), Self::Error> {
+        for c in s.chars() {
+            self.write_char(c)?;
+        }
+        Ok(())
+    }
+}
+
+impl Drop for ScreenBufferWriter<'_> {
+    fn drop(&mut self) {
+        LCD_UPDATE.signal(());
+    }
+}
+
+pub static SCREEN_BUFFER: Mutex<CriticalSectionRawMutex, ScreenBuffer> = Mutex::new(ScreenBuffer {
+    line1: [0; 16],
+    line1_ptr: 0,
+    line2: [0; 16],
+    line2_ptr: 0,
+});
+
+struct LcdWriter<const LINE: u8>;
+pub type LcdLine1 = LcdWriter<0>;
+pub type LcdLine2 = LcdWriter<1>;
+
+impl<const LINE: u8> uWrite for LcdWriter<LINE> {
+    type Error = Infallible;
+
+    fn write_char(&mut self, c: char) -> Result<(), Self::Error> {
+        block_on(async {
+            let mut buffer = SCREEN_BUFFER.lock().await;
+            if LINE == 0 {
+                let ptr = buffer.line1_ptr as usize;
+                buffer.line1[ptr] = c as u8;
+                buffer.line1_ptr = (buffer.line1_ptr + 1) % 16;
+            } else {
+                let ptr = buffer.line2_ptr as usize;
+                buffer.line2[ptr] = c as u8;
+                buffer.line2_ptr = (buffer.line2_ptr + 1) % 16;
+            }
+            LCD_UPDATE.signal(());
+        });
+        Ok(())
+    }
+    fn write_str(&mut self, s: &str) -> Result<(), Self::Error> {
+        block_on(async {
+            let mut buffer = SCREEN_BUFFER.lock().await;
+            for c in s.chars() {
+                if LINE == 0 {
+                    let ptr = buffer.line1_ptr as usize;
+                    buffer.line1[ptr] = c as u8;
+                    buffer.line1_ptr = (buffer.line1_ptr + 1) % 16;
+                } else {
+                    let ptr = buffer.line2_ptr as usize;
+                    buffer.line2[ptr] = c as u8;
+                    buffer.line2_ptr = (buffer.line2_ptr + 1) % 16;
+                }
+            }
+            LCD_UPDATE.signal(());
+        });
+        Ok(())
+    }
+}
+
+static LCD_UPDATE: Signal<CriticalSectionRawMutex, ()> = Signal::new();
+
+#[embassy_executor::task]
+pub async fn lcd_display_task(mut lcd: LcdDisplay<Output<'static>, Delay>) {
+    loop {
+        LCD_UPDATE.wait().await;
+        let buffer = SCREEN_BUFFER.lock().await;
+        lcd.clear();
+        for byte in &buffer.line1 {
+            lcd.write(*byte);
+        }
+        lcd.set_position(0, 1);
+        for byte in &buffer.line2 {
+            lcd.write(*byte);
+        }
+        Timer::after_millis(20).await;
+    }
+}
+
+pub async fn overwrite_lcd(line1: &str, line2: &str) {
+    let mut buffer = SCREEN_BUFFER.lock().await;
+    buffer.line1.copy_from_slice(line1.as_bytes());
+    buffer.line2.copy_from_slice(line2.as_bytes());
+    LCD_UPDATE.signal(());
+}