1 files changed, 20 insertions, 0 deletions
diff --git a/src/lcd_st7920.c b/src/lcd_st7920.c
index 8cd76c50..0f6a444d 100644
--- a/src/lcd_st7920.c
+++ b/src/lcd_st7920.c
@@ -4,6 +4,7 @@
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
+#include "autoconf.h" // CONFIG_CLOCK_FREQ
 #include "basecmd.h" // oid_alloc
 #include "board/gpio.h" // gpio_out_write
 #include "board/irq.h" // irq_poll
@@ -21,6 +22,23 @@ struct st7920 {
  * Transmit functions
  ****************************************************************/
 
+static uint32_t
+nsecs_to_ticks(uint32_t ns)
+{
+    return timer_from_us(ns * 1000) / 1000000;
+}
+
+static inline void
+ndelay(uint32_t nsecs)
+{
+    if (CONFIG_CLOCK_FREQ <= 48000000)
+        // Slower MCUs don't require a delay
+        return;
+    uint32_t end = timer_read_time() + nsecs_to_ticks(nsecs);
+    while (timer_is_before(timer_read_time(), end))
+        irq_poll();
+}
+
 #define SYNC_CMD  0xf8
 #define SYNC_DATA 0xfa
 
@@ -35,7 +53,9 @@ st7920_xmit_byte(struct st7920 *s, uint8_t data)
             gpio_out_toggle(sid);
             data = ~data;
         }
+        ndelay(200);
         gpio_out_toggle(sclk);
+        ndelay(200);
         data <<= 1;
         gpio_out_toggle(sclk);
     }