From de4aaa80b6d976a3f4edb27eed9b68ea3e6aaf1f Mon Sep 17 00:00:00 2001
From: Phil Howard <phil@gadgetoid.com>
Date: Tue, 27 Jun 2023 12:17:31 +0100
Subject: [PATCH] DV Display: Refactor pio usage with mutex program loader.

---
 drivers/aps6404/aps6404.cpp     | 48 ++++++++++++------
 drivers/dv_display/swd_load.cpp | 88 +++++++++++++++++----------------
 2 files changed, 77 insertions(+), 59 deletions(-)

diff --git a/drivers/aps6404/aps6404.cpp b/drivers/aps6404/aps6404.cpp
index 83de588b..9656a360 100644
--- a/drivers/aps6404/aps6404.cpp
+++ b/drivers/aps6404/aps6404.cpp
@@ -14,6 +14,28 @@ extern "C" {
 }
 #endif
 
+static const pio_program* pio_prog[2] = {nullptr, nullptr};
+static uint16_t pio_offset[2] = {0xffff, 0xffff};
+
+const void pio_remove_exclusive_program(PIO pio) {
+    uint8_t pio_index = pio == pio0 ? 0 : 1;
+    const pio_program* current_program = pio_prog[pio_index];
+    uint16_t current_offset = pio_offset[pio_index];
+    if(current_program) {
+        pio_remove_program(pio, current_program, current_offset);
+        pio_prog[pio_index] = nullptr;
+        pio_offset[pio_index] = 0xffff;
+    }
+}
+
+const uint16_t pio_change_exclusive_program(PIO pio, const pio_program* prog) {
+    pio_remove_exclusive_program(pio);
+    uint8_t pio_index = pio == pio0 ? 0 : 1;
+    pio_prog[pio_index] = prog;
+    pio_offset[pio_index] = pio_add_program(pio, prog);
+    return pio_offset[pio_index];
+};
+
 namespace pimoroni {
     APS6404::APS6404(uint pin_csn, uint pin_d0, PIO pio)
                 : pin_csn(pin_csn)
@@ -27,22 +49,19 @@ namespace pimoroni {
         }
 
         pio_prog = &sram_reset_program;
-        pio_clear_instruction_memory(pio);
-        pio_offset = pio_add_program(pio, &sram_reset_program);
-        pio_sm = 0; //pio_claim_unused_sm(pio, true);
+        pio_offset = pio_change_exclusive_program(pio, &sram_reset_program);
+        pio_sm = 1;
 
         // Claim DMA channels
-        dma_channel = 0;// dma_claim_unused_channel(true);
-        read_cmd_dma_channel = 1;// dma_claim_unused_channel(true);
+        dma_channel = 0;
+        read_cmd_dma_channel = 1;
     }
 
     void APS6404::init() {
         pio_sm_set_enabled(pio, pio_sm, false);
-        pio_clear_instruction_memory(pio);
-        //pio_remove_program(pio, pio_prog, pio_offset);
 
         pio_prog = &sram_reset_program;
-        pio_offset = pio_add_program(pio, &sram_reset_program);
+        pio_offset = pio_change_exclusive_program(pio, &sram_reset_program);
         aps6404_reset_program_init(pio, pio_sm, pio_offset, pin_csn, pin_d0);
 
         sleep_us(200);
@@ -59,10 +78,9 @@ namespace pimoroni {
 
     void APS6404::set_qpi() {
         pio_sm_set_enabled(pio, pio_sm, false);
-        pio_remove_program(pio, pio_prog, pio_offset);
 
         pio_prog = &sram_reset_program;
-        pio_offset = pio_add_program(pio, &sram_reset_program);
+        pio_offset = pio_change_exclusive_program(pio, &sram_reset_program);
         aps6404_reset_program_init(pio, pio_sm, pio_offset, pin_csn, pin_d0);
         pio_sm_put_blocking(pio, pio_sm, 0x00000007u);
         pio_sm_put_blocking(pio, pio_sm, 0x35000000u);
@@ -74,10 +92,9 @@ namespace pimoroni {
 
     void APS6404::set_spi() {
         pio_sm_set_enabled(pio, pio_sm, false);
-        pio_remove_program(pio, pio_prog, pio_offset);
 
         pio_prog = &sram_reset_qpi_program;
-        pio_offset = pio_add_program(pio, &sram_reset_qpi_program);
+        pio_offset = pio_change_exclusive_program(pio, &sram_reset_qpi_program);
         aps6404_program_init(pio, pio_sm, pio_offset, pin_csn, pin_d0, false, false, true);
         pio_sm_put_blocking(pio, pio_sm, 0x00000001u);
         pio_sm_put_blocking(pio, pio_sm, 0xF5000000u);
@@ -85,21 +102,20 @@ namespace pimoroni {
 
     void APS6404::adjust_clock() {
         pio_sm_set_enabled(pio, pio_sm, false);
-        pio_remove_program(pio, pio_prog, pio_offset);
 
         if (clock_get_hz(clk_sys) > 296000000) {
             pio_prog = &sram_fast_program;
-            pio_offset = pio_add_program(pio, &sram_fast_program);
+            pio_offset = pio_change_exclusive_program(pio, &sram_fast_program);
             aps6404_program_init(pio, pio_sm, pio_offset, pin_csn, pin_d0, false, true, false);
         }
         else if (clock_get_hz(clk_sys) < 130000000) {
             pio_prog = &sram_slow_program;
-            pio_offset = pio_add_program(pio, &sram_slow_program);
+            pio_offset = pio_change_exclusive_program(pio, &sram_slow_program);
             aps6404_program_init(pio, pio_sm, pio_offset, pin_csn, pin_d0, true, false, false);
         }
         else {
             pio_prog = &sram_program;
-            pio_offset = pio_add_program(pio, &sram_program);
+            pio_offset = pio_change_exclusive_program(pio, &sram_program);
             aps6404_program_init(pio, pio_sm, pio_offset, pin_csn, pin_d0, false, false, false);
         }
 
diff --git a/drivers/dv_display/swd_load.cpp b/drivers/dv_display/swd_load.cpp
index 0d6bb5fa..87040a9e 100644
--- a/drivers/dv_display/swd_load.cpp
+++ b/drivers/dv_display/swd_load.cpp
@@ -14,42 +14,46 @@ static uint pio_offset;
 static uint pio_sm;
 static const pio_program_t* pio_prog;
 
+PIO swd_pio = pio1;
+
+extern const uint16_t pio_change_exclusive_program(PIO pio, const pio_program* prog);
+extern const void pio_remove_exclusive_program(PIO pio);
+
 static void wait_for_idle() {
     uint pull_offset = (pio_prog == &swd_raw_write_program) ? 2 : 
                        (pio_prog == &swd_raw_read_program) ? 0 : 5;
-    while (!pio_sm_is_tx_fifo_empty(pio0, pio_sm) || pio0_hw->sm[pio_sm].addr != pio_offset + pull_offset);
+    while (!pio_sm_is_tx_fifo_empty(swd_pio, pio_sm) || swd_pio->sm[pio_sm].addr != pio_offset + pull_offset);
 }
 
 static void switch_program(bool read, bool raw = false) {
     wait_for_idle();
-    pio_sm_set_enabled(pio0, pio_sm, false);
-    pio_remove_program(pio0, pio_prog, pio_offset);
+    pio_sm_set_enabled(swd_pio, pio_sm, false);
     pio_prog = raw ? (read ? &swd_raw_read_program : &swd_raw_write_program) :
                      (read ? &swd_read_program : &swd_write_program);
-    pio_offset = pio_add_program(pio0, pio_prog);
+    pio_offset = pio_change_exclusive_program(swd_pio, pio_prog);
     if (raw) {
-        swd_raw_program_init(pio0, pio_sm, pio_offset, 2, 3, read);
+        swd_raw_program_init(swd_pio, pio_sm, pio_offset, 2, 3, read);
     } else {
-        swd_program_init(pio0, pio_sm, pio_offset, 2, 3, read);
+        swd_program_init(swd_pio, pio_sm, pio_offset, 2, 3, read);
         wait_for_idle();
-        pio0_hw->irq = 1;
+        swd_pio->irq = 1;
     }
 }
 
 static void unload_pio() {
-    pio_sm_set_enabled(pio0, pio_sm, false);
-    pio_remove_program(pio0, pio_prog, pio_offset);
-    pio_sm_unclaim(pio0, pio_sm);
+    pio_sm_set_enabled(swd_pio, pio_sm, false);
+    pio_remove_exclusive_program(swd_pio);
+    pio_sm_unclaim(swd_pio, pio_sm);
 }
 
 static bool write_cmd(uint cmd, uint data) {
     if (pio_prog != &swd_write_program) {
         switch_program(false);
     }
-    pio_sm_put_blocking(pio0, pio_sm, cmd);
-    pio_sm_put_blocking(pio0, pio_sm, data);
+    pio_sm_put_blocking(swd_pio, pio_sm, cmd);
+    pio_sm_put_blocking(swd_pio, pio_sm, data);
     wait_for_idle();
-    if (pio0_hw->irq & 0x1) {
+    if (swd_pio->irq & 0x1) {
         return false;
     }
     return true;
@@ -71,13 +75,13 @@ static bool read_cmd(uint cmd, uint& data) {
     if (pio_prog != &swd_read_program) {
         switch_program(true);
     }
-    pio_sm_put_blocking(pio0, pio_sm, cmd);
+    pio_sm_put_blocking(swd_pio, pio_sm, cmd);
     wait_for_idle();
-    if (pio0_hw->irq & 0x1) {
+    if (swd_pio->irq & 0x1) {
         mp_printf(&mp_plat_print, "Read ID failed\n");
         return false;
     }
-    data = pio_sm_get_blocking(pio0, pio_sm);
+    data = pio_sm_get_blocking(swd_pio, pio_sm);
     return true;
 }
 
@@ -90,28 +94,27 @@ static bool read_reg(uint addr, uint &data) {
 
 static void idle() {
     switch_program(false, true);
-    pio_sm_put_blocking(pio0, pio_sm, 7);
-    pio_sm_put_blocking(pio0, pio_sm, 0);
+    pio_sm_put_blocking(swd_pio, pio_sm, 7);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0);
 }
 
 static bool connect(bool first = true, uint core = 0) {
     if (first) {
-        pio_clear_instruction_memory(pio0);
         pio_prog = &swd_raw_write_program;
-        pio_offset = pio_add_program(pio0, &swd_raw_write_program);
-        pio_sm = 0; //pio_claim_unused_sm(pio0, true);
+        pio_offset = pio_change_exclusive_program(swd_pio, &swd_raw_write_program);
+        pio_sm = 0;
 
-        swd_initial_init(pio0, pio_sm, 2, 3);
+        swd_initial_init(swd_pio, pio_sm, 2, 3);
 
-        swd_raw_program_init(pio0, pio_sm, pio_offset, 2, 3, false);
+        swd_raw_program_init(swd_pio, pio_sm, pio_offset, 2, 3, false);
     } else {
         switch_program(false, true);
     }
 
     // Begin transaction: 8 clocks, data low
     mp_printf(&mp_plat_print, "Begin transaction\n");
-    pio_sm_put_blocking(pio0, pio_sm, 7);
-    pio_sm_put_blocking(pio0, pio_sm, 0);
+    pio_sm_put_blocking(swd_pio, pio_sm, 7);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0);
 
     // Go to SWD mode:
     // 8 clocks, data high
@@ -119,34 +122,33 @@ static bool connect(bool first = true, uint core = 0) {
     // 4 clocks, data low
     // 0x1A
     mp_printf(&mp_plat_print, "SWD Mode\n");
-    pio_sm_put_blocking(pio0, pio_sm, 8-1);
-    pio_sm_put_blocking(pio0, pio_sm, 0xFF);
+    pio_sm_put_blocking(swd_pio, pio_sm, 8-1);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0xFF);
 
     mp_printf(&mp_plat_print, "Tag\n");
-    pio_sm_put_blocking(pio0, pio_sm, 32*4+4+8-1);
-    pio_sm_put_blocking(pio0, pio_sm, 0x6209F392);
-    pio_sm_put_blocking(pio0, pio_sm, 0x86852D95);
-    pio_sm_put_blocking(pio0, pio_sm, 0xE3DDAFE9);
-    pio_sm_put_blocking(pio0, pio_sm, 0x19BC0EA2);
-    pio_sm_put_blocking(pio0, pio_sm, 0x1A0);
+    pio_sm_put_blocking(swd_pio, pio_sm, 32*4+4+8-1);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0x6209F392);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0x86852D95);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0xE3DDAFE9);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0x19BC0EA2);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0x1A0);
 
     // Line Reset: 50 high, 8 low
     mp_printf(&mp_plat_print, "Line Reset\n");
-    pio_sm_put_blocking(pio0, pio_sm, 58-1);
-    pio_sm_put_blocking(pio0, pio_sm, 0xFFFFFFFF);
-    pio_sm_put_blocking(pio0, pio_sm, 0x003FFFF);
+    pio_sm_put_blocking(swd_pio, pio_sm, 58-1);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0xFFFFFFFF);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0x003FFFF);
 
     mp_printf(&mp_plat_print, "Target Select\n"); // Must ignore error response
     wait_for_idle();
-    pio_sm_set_enabled(pio0, pio_sm, false);
-    pio_remove_program(pio0, pio_prog, pio_offset);
+    pio_sm_set_enabled(swd_pio, pio_sm, false);
     pio_prog = &swd_write_ignore_error_program;
-    pio_offset = pio_add_program(pio0, pio_prog);
-    swd_program_init(pio0, pio_sm, pio_offset, 2, 3, false);
+    pio_offset = pio_change_exclusive_program(swd_pio, pio_prog);
+    swd_program_init(swd_pio, pio_sm, pio_offset, 2, 3, false);
     wait_for_idle();
-    pio0_hw->irq = 1;
-    pio_sm_put_blocking(pio0, pio_sm, 0x19);
-    pio_sm_put_blocking(pio0, pio_sm, 0x01002927 | (core << 28));
+    swd_pio->irq = 1;
+    pio_sm_put_blocking(swd_pio, pio_sm, 0x19);
+    pio_sm_put_blocking(swd_pio, pio_sm, 0x01002927 | (core << 28));
 
     mp_printf(&mp_plat_print, "Read ID\n");
     uint id;