From c5a4be647996347f220dc49bb2ef7c7581eae972 Mon Sep 17 00:00:00 2001
From: Renze Nicolai <renze@rnplus.nl>
Date: Fri, 11 Mar 2022 04:41:34 +0100
Subject: [PATCH] Add workaround for unstable writes to RP2040

---
 components/mch2022-bsp/rp2040.c | 10 +++++++++-
 main/main.c                     |  2 +-
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/components/mch2022-bsp/rp2040.c b/components/mch2022-bsp/rp2040.c
index 1eea96f..ae2b049 100644
--- a/components/mch2022-bsp/rp2040.c
+++ b/components/mch2022-bsp/rp2040.c
@@ -73,7 +73,15 @@ esp_err_t rp2040_get_lcd_mode(RP2040* device, lcd_mode_t* mode) {
 }
 
 esp_err_t rp2040_set_lcd_mode(RP2040* device, lcd_mode_t mode) {
-    return i2c_write_reg_n(device->i2c_bus, device->i2c_address, RP2040_REG_LCD_MODE, (uint8_t*) &mode, 1);
+    esp_err_t res;
+    lcd_mode_t verification;
+    do {
+        res = i2c_write_reg_n(device->i2c_bus, device->i2c_address, RP2040_REG_LCD_MODE, (uint8_t*) &mode, 1);
+        if (res != ESP_OK) return res;
+        res = rp2040_get_lcd_mode(device, &verification);
+        if (res != ESP_OK) return res;
+    } while (verification != mode);
+    return res;
 }
 
 esp_err_t rp2040_get_lcd_backlight(RP2040* device, uint8_t* brightness) {
diff --git a/main/main.c b/main/main.c
index bac20d8..790e71e 100644
--- a/main/main.c
+++ b/main/main.c
@@ -492,7 +492,7 @@ void fpga_test(void) {
         reload_fpga = false;
         ili9341_deinit(ili9341);
         ili9341_select(ili9341, false);
-        vTaskDelay(100 / portTICK_PERIOD_MS);
+        vTaskDelay(200 / portTICK_PERIOD_MS);
         ili9341_select(ili9341, true);
 
         ESP_LOGI(TAG, "Loading app bitstream into FPGA...");