diff --git a/src/Makefile b/src/Makefile
index 062116a97..4a052b49b 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -496,7 +496,7 @@ ifeq ($(PCB), X9D)
   EEPROMSRC = eeprom_avr.cpp # TODO later eeprom_conversions.cpp
   PULSESSRC = pulses_arm.cpp
   CPPSRC += sky9x/audio.cpp
-  CPPSRC += x9d/pwr_driver.cpp x9d/eeprom_driver.cpp x9d/pulses_driver.cpp x9d/keys_driver.cpp x9d/adc_driver.cpp x9d/trainer_driver.cpp x9d/audio_driver.cpp x9d/delays.cpp x9d/uart.cpp
+  CPPSRC += x9d/pwr_driver.cpp x9d/eeprom_driver.cpp x9d/pulses_driver.cpp x9d/keys_driver.cpp x9d/adc_driver.cpp x9d/trainer_driver.cpp x9d/audio_driver.cpp x9d/delays.cpp
   CPPSRC += bmp.cpp monitors_views.cpp
   SRC += x9d/STM32F2xx_StdPeriph_Lib_V1.1.0/Libraries/CMSIS/Device/ST/STM32F2xx/Source/Templates/system_stm32f2xx.c
   SRC += x9d/STM32F2xx_StdPeriph_Lib_V1.1.0/Libraries/STM32F2xx_StdPeriph_Driver/src/stm32f2xx_gpio.c
@@ -517,6 +517,12 @@ ifeq ($(PCB), X9D)
     CPPDEFS += -DRTCLOCK
     CPPSRC += rtc.cpp x9d/rtc_driver.cpp
   endif
+
+  ifeq ($(DEBUG), YES)
+    CPPSRC += debug.cpp
+  	CPPSRC += x9d/debug_driver.cpp
+  	SRC += sky9x/syscalls.c
+  endif
 endif
 
 ifeq ($(PCB), SKY9X)
@@ -554,7 +560,8 @@ ifeq ($(PCB), SKY9X)
   endif
   
   ifeq ($(DEBUG), YES)
-  	CPPSRC += sky9x/debug.cpp
+    CPPSRC += debug.cpp
+  	CPPSRC += sky9x/debug_driver.cpp
   	SRC += sky9x/syscalls.c
   endif
 endif
diff --git a/src/bmp.cpp b/src/bmp.cpp
index e35405e5a..2f3484024 100644
--- a/src/bmp.cpp
+++ b/src/bmp.cpp
@@ -34,13 +34,16 @@
 
 #include "open9x.h"
 
-const pm_char * bmpLoad(uint8_t *dest, const char *filename)
+const pm_char * bmpLoad(uint8_t *dest, const char *filename, const xcoord_t width, const uint8_t height)
 {
   FIL bmpFile;
   UINT read;
-  uint8_t bmpBuf[64]; /* maximum with 64px for 1-bit bitmaps */
+  uint8_t palette[16];
+  uint8_t bmpBuf[64]; /* maximum with 64px */
   uint8_t *buf = &bmpBuf[0];
 
+  assert(width <= 64);
+
   FRESULT result = f_open(&bmpFile, filename, FA_OPEN_EXISTING | FA_READ);
   if (result != FR_OK) {
     return SDCARD_ERROR(result);
@@ -90,7 +93,7 @@ const pm_char * bmpLoad(uint8_t *dest, const char *filename)
     return STR_INCOMPATIBLE;
   }
 
-  uint32_t width, height;
+  uint32_t w, h;
 
   switch (ihsize){
     case  40: // windib
@@ -98,13 +101,13 @@ const pm_char * bmpLoad(uint8_t *dest, const char *filename)
     case  64: // OS/2 v2
     case 108: // windib v4
     case 124: // windib v5
-      width  = *((uint32_t *)&buf[4]);
-      height = *((uint32_t *)&buf[8]);
+      w  = *((uint32_t *)&buf[4]);
+      h = *((uint32_t *)&buf[8]);
       buf += 12;
       break;
     case  12: // OS/2 v1
-      width  = *((uint16_t *)&buf[4]);
-      height = *((uint16_t *)&buf[6]);
+      w  = *((uint16_t *)&buf[4]);
+      h = *((uint16_t *)&buf[6]);
       buf += 8;
       break;
     default:
@@ -117,59 +120,86 @@ const pm_char * bmpLoad(uint8_t *dest, const char *filename)
     return STR_INCOMPATIBLE;
   }
 
-  if (width > MODEL_BITMAP_WIDTH || height > MODEL_BITMAP_HEIGHT) {
+  if (w > width || h > height) {
     f_close(&bmpFile);
     return STR_INCOMPATIBLE;
   }
 
   uint16_t depth = *((uint16_t *)&buf[2]);
 
-  if (f_lseek(&bmpFile, hsize) != FR_OK) {
-    f_close(&bmpFile);
-    return SDCARD_ERROR(result);
+  buf = &bmpBuf[0];
+
+  if (depth == 4) {
+    if (f_lseek(&bmpFile, hsize-64) != FR_OK || f_read(&bmpFile, buf, 64, &read) != FR_OK || read != 64) {
+      f_close(&bmpFile);
+      return SDCARD_ERROR(result);
+    }
+    for (uint8_t i=0; i<16; i++) {
+      palette[i] = buf[4*i] >> 4;
+    }
+  }
+  else {
+    if (f_lseek(&bmpFile, hsize) != FR_OK) {
+      f_close(&bmpFile);
+      return SDCARD_ERROR(result);
+    }
   }
 
-  *dest++ = width;
-  *dest++ = height;
+  *dest++ = w;
+  *dest++ = h;
+
+  memset(dest, 0, w*(h/8)*4);
 
   uint32_t n;
-  buf = &bmpBuf[0];
 
   switch (depth) {
     case 1:
-      n = width/8;
-      for (uint32_t i=0; i<height; i+=8) {
+      n = w/8;
+      for (uint32_t i=0; i<h; i+=8) {
         result = f_read(&bmpFile, buf, n*8, &read);
         if (result != FR_OK || read != n*8) {
           f_close(&bmpFile);
           return SDCARD_ERROR(result);
         }
-        uint8_t * dst = dest + ((height-i-8)/8 * width);
+        uint8_t * dst = dest + 4*((h-i-8)/8 * w);
         for (uint32_t j=0; j<n; j++) {
-          *dst++ = ~(((buf[j+0*n] >> 7) << 7) + ((buf[j+1*n] >> 7) << 6) + ((buf[j+2*n] >> 7) << 5) + ((buf[j+3*n] >> 7) << 4) + ((buf[j+4*n] >> 7) << 3) + ((buf[j+5*n] >> 7) << 2) + ((buf[j+6*n] >> 7) << 1) + ((buf[j+7*n] >> 7) << 0));
-          *dst++ = ~((((buf[j+0*n] >> 6) & 1) << 7) + (((buf[j+1*n] >> 6) & 1) << 6) + (((buf[j+2*n] >> 6) & 1) << 5) + (((buf[j+3*n] >> 6) & 1) << 4) + (((buf[j+4*n] >> 6) & 1) << 3) + (((buf[j+5*n] >> 6) & 1) << 2) + (((buf[j+6*n] >> 6) & 1) << 1) + (((buf[j+7*n] >> 6) & 1) << 0));
-          *dst++ = ~((((buf[j+0*n] >> 5) & 1) << 7) + (((buf[j+1*n] >> 5) & 1) << 6) + (((buf[j+2*n] >> 5) & 1) << 5) + (((buf[j+3*n] >> 5) & 1) << 4) + (((buf[j+4*n] >> 5) & 1) << 3) + (((buf[j+5*n] >> 5) & 1) << 2) + (((buf[j+6*n] >> 5) & 1) << 1) + (((buf[j+7*n] >> 5) & 1) << 0));
-          *dst++ = ~((((buf[j+0*n] >> 4) & 1) << 7) + (((buf[j+1*n] >> 4) & 1) << 6) + (((buf[j+2*n] >> 4) & 1) << 5) + (((buf[j+3*n] >> 4) & 1) << 4) + (((buf[j+4*n] >> 4) & 1) << 3) + (((buf[j+5*n] >> 4) & 1) << 2) + (((buf[j+6*n] >> 4) & 1) << 1) + (((buf[j+7*n] >> 4) & 1) << 0));
-          *dst++ = ~((((buf[j+0*n] >> 3) & 1) << 7) + (((buf[j+1*n] >> 3) & 1) << 6) + (((buf[j+2*n] >> 3) & 1) << 5) + (((buf[j+3*n] >> 3) & 1) << 4) + (((buf[j+4*n] >> 3) & 1) << 3) + (((buf[j+5*n] >> 3) & 1) << 2) + (((buf[j+6*n] >> 3) & 1) << 1) + (((buf[j+7*n] >> 3) & 1) << 0));
-          *dst++ = ~((((buf[j+0*n] >> 2) & 1) << 7) + (((buf[j+1*n] >> 2) & 1) << 6) + (((buf[j+2*n] >> 2) & 1) << 5) + (((buf[j+3*n] >> 2) & 1) << 4) + (((buf[j+4*n] >> 2) & 1) << 3) + (((buf[j+5*n] >> 2) & 1) << 2) + (((buf[j+6*n] >> 2) & 1) << 1) + (((buf[j+7*n] >> 2) & 1) << 0));
-          *dst++ = ~((((buf[j+0*n] >> 1) & 1) << 7) + (((buf[j+1*n] >> 1) & 1) << 6) + (((buf[j+2*n] >> 1) & 1) << 5) + (((buf[j+3*n] >> 1) & 1) << 4) + (((buf[j+4*n] >> 1) & 1) << 3) + (((buf[j+5*n] >> 1) & 1) << 2) + (((buf[j+6*n] >> 1) & 1) << 1) + (((buf[j+7*n] >> 1) & 1) << 0));
-          *dst++ = ~((((buf[j+0*n] >> 0) & 1) << 7) + (((buf[j+1*n] >> 0) & 1) << 6) + (((buf[j+2*n] >> 0) & 1) << 5) + (((buf[j+3*n] >> 0) & 1) << 4) + (((buf[j+4*n] >> 0) & 1) << 3) + (((buf[j+5*n] >> 0) & 1) << 2) + (((buf[j+6*n] >> 0) & 1) << 1) + (((buf[j+7*n] >> 0) & 1) << 0));
+#define PUSH_4BYTES(x) *dst = *(dst+1) = *(dst+2) = *(dst+3) = (x); dst += 4
+          PUSH_4BYTES(~(((buf[j+0*n] >> 7) << 7) + ((buf[j+1*n] >> 7) << 6) + ((buf[j+2*n] >> 7) << 5) + ((buf[j+3*n] >> 7) << 4) + ((buf[j+4*n] >> 7) << 3) + ((buf[j+5*n] >> 7) << 2) + ((buf[j+6*n] >> 7) << 1) + ((buf[j+7*n] >> 7) << 0)));
+          PUSH_4BYTES(~((((buf[j+0*n] >> 6) & 1) << 7) + (((buf[j+1*n] >> 6) & 1) << 6) + (((buf[j+2*n] >> 6) & 1) << 5) + (((buf[j+3*n] >> 6) & 1) << 4) + (((buf[j+4*n] >> 6) & 1) << 3) + (((buf[j+5*n] >> 6) & 1) << 2) + (((buf[j+6*n] >> 6) & 1) << 1) + (((buf[j+7*n] >> 6) & 1) << 0)));
+          PUSH_4BYTES(~((((buf[j+0*n] >> 5) & 1) << 7) + (((buf[j+1*n] >> 5) & 1) << 6) + (((buf[j+2*n] >> 5) & 1) << 5) + (((buf[j+3*n] >> 5) & 1) << 4) + (((buf[j+4*n] >> 5) & 1) << 3) + (((buf[j+5*n] >> 5) & 1) << 2) + (((buf[j+6*n] >> 5) & 1) << 1) + (((buf[j+7*n] >> 5) & 1) << 0)));
+          PUSH_4BYTES(~((((buf[j+0*n] >> 4) & 1) << 7) + (((buf[j+1*n] >> 4) & 1) << 6) + (((buf[j+2*n] >> 4) & 1) << 5) + (((buf[j+3*n] >> 4) & 1) << 4) + (((buf[j+4*n] >> 4) & 1) << 3) + (((buf[j+5*n] >> 4) & 1) << 2) + (((buf[j+6*n] >> 4) & 1) << 1) + (((buf[j+7*n] >> 4) & 1) << 0)));
+          PUSH_4BYTES(~((((buf[j+0*n] >> 3) & 1) << 7) + (((buf[j+1*n] >> 3) & 1) << 6) + (((buf[j+2*n] >> 3) & 1) << 5) + (((buf[j+3*n] >> 3) & 1) << 4) + (((buf[j+4*n] >> 3) & 1) << 3) + (((buf[j+5*n] >> 3) & 1) << 2) + (((buf[j+6*n] >> 3) & 1) << 1) + (((buf[j+7*n] >> 3) & 1) << 0)));
+          PUSH_4BYTES(~((((buf[j+0*n] >> 2) & 1) << 7) + (((buf[j+1*n] >> 2) & 1) << 6) + (((buf[j+2*n] >> 2) & 1) << 5) + (((buf[j+3*n] >> 2) & 1) << 4) + (((buf[j+4*n] >> 2) & 1) << 3) + (((buf[j+5*n] >> 2) & 1) << 2) + (((buf[j+6*n] >> 2) & 1) << 1) + (((buf[j+7*n] >> 2) & 1) << 0)));
+          PUSH_4BYTES(~((((buf[j+0*n] >> 1) & 1) << 7) + (((buf[j+1*n] >> 1) & 1) << 6) + (((buf[j+2*n] >> 1) & 1) << 5) + (((buf[j+3*n] >> 1) & 1) << 4) + (((buf[j+4*n] >> 1) & 1) << 3) + (((buf[j+5*n] >> 1) & 1) << 2) + (((buf[j+6*n] >> 1) & 1) << 1) + (((buf[j+7*n] >> 1) & 1) << 0)));
+          PUSH_4BYTES(~((((buf[j+0*n] >> 0) & 1) << 7) + (((buf[j+1*n] >> 0) & 1) << 6) + (((buf[j+2*n] >> 0) & 1) << 5) + (((buf[j+3*n] >> 0) & 1) << 4) + (((buf[j+4*n] >> 0) & 1) << 3) + (((buf[j+5*n] >> 0) & 1) << 2) + (((buf[j+6*n] >> 0) & 1) << 1) + (((buf[j+7*n] >> 0) & 1) << 0)));
         }
       }
       break;
 
-    /* case 4:
-      for (uint32_t i=0; i<height; i++) {
-        int j;
-        for (j = 0; j < n; j++){
-          ptr[j*2+0] = (buf[j] >> 4) & 0xF;
-          ptr[j*2+1] = buf[j] & 0xF;
+    case 4:
+      n = w/8;
+      for (int32_t i=h-1; i>=0; i--) {
+        result = f_read(&bmpFile, buf, w/2, &read);
+        if (result != FR_OK || read != w/2) {
+          f_close(&bmpFile);
+          return SDCARD_ERROR(result);
+        }
+        uint8_t mask = 1 << (i%8);
+        for (uint32_t j=0; j<w/2; j++) {
+          uint8_t * dst = dest + (i/8)*w*4 + j*8;
+          uint8_t val = palette[(buf[j] >> 4) & 0x0F];
+          *(dst+0) = ((*(dst+0)) & (~mask)) + ((val & 0x1) ? 0 : mask);
+          *(dst+1) = ((*(dst+1)) & (~mask)) + ((val & 0x2) ? 0 : mask);
+          *(dst+2) = ((*(dst+2)) & (~mask)) + ((val & 0x4) ? 0 : mask);
+          *(dst+3) = ((*(dst+3)) & (~mask)) + ((val & 0x8) ? 0 : mask);
+          val = palette[buf[j] & 0x0F];
+          *(dst+4) = ((*(dst+4)) & (~mask)) + ((val & 0x1) ? 0 : mask);
+          *(dst+5) = ((*(dst+5)) & (~mask)) + ((val & 0x2) ? 0 : mask);
+          *(dst+6) = ((*(dst+6)) & (~mask)) + ((val & 0x4) ? 0 : mask);
+          *(dst+7) = ((*(dst+7)) & (~mask)) + ((val & 0x8) ? 0 : mask);
         }
-        buf += n;
-        ptr += linesize;
       }
       break;
-    */
 
     default:
       f_close(&bmpFile);
@@ -179,3 +209,25 @@ const pm_char * bmpLoad(uint8_t *dest, const char *filename)
   f_close(&bmpFile);
   return 0;
 }
+
+void lcd_bmp(xcoord_t x, uint8_t y, const pm_uchar * img)
+{
+  const pm_uchar *q = img;
+#if LCD >= 260
+  xcoord_t w   = pgm_read_byte(q++);
+  if (w == 255) w += pgm_read_byte(q++);
+#else
+  uint8_t w    = pgm_read_byte(q++);
+#endif
+  uint8_t hb   = (pgm_read_byte(q++)+7)/8;
+  for (uint8_t yb = 0; yb < hb; yb++) {
+    uint8_t *p = &displayBuf[ (y / 8 + yb) * DISPLAY_W + x ];
+    for (xcoord_t i=0; i<w; i++){
+      *(p+0*DISPLAY_PLAN_SIZE) = pgm_read_byte(q++);
+      *(p+1*DISPLAY_PLAN_SIZE) = pgm_read_byte(q++);
+      *(p+2*DISPLAY_PLAN_SIZE) = pgm_read_byte(q++);
+      *(p+3*DISPLAY_PLAN_SIZE) = pgm_read_byte(q++);
+      p++;
+    }
+  }
+}
diff --git a/src/debug.cpp b/src/debug.cpp
new file mode 100644
index 000000000..c5b5786ba
--- /dev/null
+++ b/src/debug.cpp
@@ -0,0 +1,85 @@
+/*
+ * Authors (alphabetical order)
+ * - Andre Bernet <bernet.andre@gmail.com>
+ * - Bertrand Songis <bsongis@gmail.com>
+ * - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
+ * - Cameron Weeks <th9xer@gmail.com>
+ * - Erez Raviv
+ * - Jean-Pierre Parisy
+ * - Karl Szmutny <shadow@privy.de>
+ * - Michael Blandford
+ * - Michal Hlavinka
+ * - Pat Mackenzie
+ * - Philip Moss
+ * - Rob Thomson
+ * - Romolo Manfredini <romolo.manfredini@gmail.com>
+ * - Thomas Husterer
+ *
+ * open9x is based on code named
+ * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
+ * er9x by Erez Raviv: http://code.google.com/p/er9x/,
+ * and the original (and ongoing) project by
+ * Thomas Husterer, th9x: http://code.google.com/p/th9x/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include "../open9x.h"
+#include <stdarg.h>
+#include "fifo.h"
+
+#if !defined(SIMU)
+
+Fifo32 debugFifo;
+
+// Outputs a string to the UART
+void debugPuts(const char *format, ...)
+{
+  va_list arglist;
+  char tmp[256];
+
+  va_start(arglist, format);
+  vsnprintf(tmp, 256, format, arglist);
+  va_end(arglist);
+
+  const char *t = tmp;
+  while (*t) {
+    debugPutc(*t++);
+  }
+}
+
+void dump(unsigned char *data, unsigned int size)
+{
+ debugPuts("DUMP %d bytes ...\n\r", size);
+ unsigned int i = 0, j=0;
+ while (i*32+j < size) {
+   debugPuts("%.2X ", data[i*32+j]);
+   j++;
+   if (j==32) {
+     i++; j=0;
+     debugPuts("\n\r");
+   }
+ }
+ debugPuts("\n\r");
+}
+
+void debugTask(void* pdata)
+{
+  uint8_t rxchar ;
+
+  for (;;) {
+    while (!debugFifo.pop(rxchar))
+      CoTickDelay(5); // 10ms
+
+  }
+}
+
+#endif
diff --git a/src/sky9x/debug.h b/src/debug.h
similarity index 92%
rename from src/sky9x/debug.h
rename to src/debug.h
index fc7c65c9e..0b8fe7969 100644
--- a/src/sky9x/debug.h
+++ b/src/debug.h
@@ -60,9 +60,6 @@ void dump(unsigned char *data, unsigned int size);
 #define TRACE_ERROR(...) debugPuts("-E- " __VA_ARGS__)
 #define DUMP(data, size) dump(data, size)
 
-void DEBUG_UART_Configure( uint32_t baudrate, uint32_t masterClock);
-void DEBUG_UART_Stop();
-
 void debugTask(void* pdata);
 
 #else
diff --git a/src/eeprom_arm.cpp b/src/eeprom_arm.cpp
index 26c072f6d..007c74bea 100644
--- a/src/eeprom_arm.cpp
+++ b/src/eeprom_arm.cpp
@@ -37,9 +37,9 @@
 #include "inttypes.h"
 #include "string.h"
 
-volatile uint32_t Spi_complete; // TODO in the driver ?
-uint8_t  s_eeDirtyMsk;
-uint16_t s_eeDirtyTime10ms;
+volatile  uint32_t Spi_complete; // TODO in the driver ?
+uint8_t   s_eeDirtyMsk;
+tmr10ms_t s_eeDirtyTime10ms;
 
 // Logic for storing to EEPROM/loading from EEPROM
 // If main needs to wait for the eeprom, call mainsequence without actioning menus
diff --git a/src/eeprom_arm.h b/src/eeprom_arm.h
index 4439b94e8..5da3ba225 100644
--- a/src/eeprom_arm.h
+++ b/src/eeprom_arm.h
@@ -20,14 +20,16 @@
 #include <inttypes.h>
 #include <stdint.h>
 
-#ifdef REV0
-#define WRITE_DELAY_10MS 100
+#if defined(SIMU)
+#define WRITE_DELAY_10MS 200
+#elif defined(PCBSKY9X) && defined(REV0)
+#define WRITE_DELAY_10MS 200
 #else
 #define WRITE_DELAY_10MS 500
 #endif
 
-extern uint8_t  s_eeDirtyMsk;
-extern uint16_t s_eeDirtyTime10ms;
+extern uint8_t   s_eeDirtyMsk;
+extern tmr10ms_t s_eeDirtyTime10ms;
 
 // States in Eeprom32_process_state
 #define E32_IDLE                                                        1
diff --git a/src/eeprom_avr.cpp b/src/eeprom_avr.cpp
index 06166c11e..4fcb011b5 100644
--- a/src/eeprom_avr.cpp
+++ b/src/eeprom_avr.cpp
@@ -37,9 +37,9 @@
 #include "inttypes.h"
 #include "string.h"
 
-uint8_t  s_write_err = 0;    // error reasons
-uint8_t  s_eeDirtyMsk;
-uint16_t s_eeDirtyTime10ms;
+uint8_t   s_write_err = 0;    // error reasons
+uint8_t   s_eeDirtyMsk;
+tmr10ms_t s_eeDirtyTime10ms;
 
 RlcFile  theFile;  //used for any file operation
 EeFs     eeFs;
diff --git a/src/eeprom_avr.h b/src/eeprom_avr.h
index 3422f319d..19bd7508a 100644
--- a/src/eeprom_avr.h
+++ b/src/eeprom_avr.h
@@ -37,14 +37,18 @@
 
 #include <inttypes.h>
 
-#if defined(PCBGRUVIN9X) && !defined(REV0)
+#if defined(SIMU)
+#define WRITE_DELAY_10MS 200
+#elif defined(PCBX9D)
+#define WRITE_DELAY_10MS 1000
+#elif defined(PCBGRUVIN9X) && !defined(REV0)
 #define WRITE_DELAY_10MS 500
 #else
 #define WRITE_DELAY_10MS 200
 #endif
 
-extern uint8_t  s_eeDirtyMsk;
-extern uint16_t s_eeDirtyTime10ms;
+extern uint8_t   s_eeDirtyMsk;
+extern tmr10ms_t s_eeDirtyTime10ms;
 
 #if defined(CPUARM)
 #define blkid_t    uint16_t
diff --git a/src/sky9x/fifo.h b/src/fifo.h
similarity index 100%
rename from src/sky9x/fifo.h
rename to src/fifo.h
diff --git a/src/lcd.cpp b/src/lcd.cpp
index 34f841138..3323218bf 100644
--- a/src/lcd.cpp
+++ b/src/lcd.cpp
@@ -62,10 +62,14 @@ void lcd_img(xcoord_t x, uint8_t y, const pm_uchar * img, uint8_t idx, LcdFlags
       ASSERT_IN_DISPLAY(p);
 #if defined(PCBX9D)
       uint8_t val = inv ? ~b : b;
-      if (!(att & GREY1))
+      if (!(att & GREY(1)))
         *p = val;
-      if (!(att & GREY2))
+      if (!(att & GREY(2)))
         *(p+DISPLAY_PLAN_SIZE) = val;
+      if (!(att & GREY(4)))
+        *(p+2*DISPLAY_PLAN_SIZE) = val;
+      if (!(att & GREY(8)))
+        *(p+3*DISPLAY_PLAN_SIZE) = val;
       p++;
 #else
       *p++ = inv ? ~b : b;
@@ -79,10 +83,14 @@ uint8_t lcdLastPos;
 #if defined(PCBX9D)
 #define LCD_BYTE_FILTER(p, keep, add) \
   do { \
-    if (!(flags & GREY1)) \
+    if (!(flags & GREY(1))) \
       *(p) = ((*(p)) & (keep)) | (add); \
-    if (!(flags & GREY2)) \
+    if (!(flags & GREY(2))) \
       *(p+DISPLAY_PLAN_SIZE) = ((*(p+DISPLAY_PLAN_SIZE)) & (keep)) | (add); \
+    if (!(flags & GREY(4))) \
+      *(p+2*DISPLAY_PLAN_SIZE) = ((*(p+2*DISPLAY_PLAN_SIZE)) & (keep)) | (add); \
+    if (!(flags & GREY(8))) \
+      *(p+3*DISPLAY_PLAN_SIZE) = ((*(p+3*DISPLAY_PLAN_SIZE)) & (keep)) | (add); \
   } while (0)
 #else
 #define LCD_BYTE_FILTER(p, keep, add) *(p) = (*(p) & (keep)) | (add)
@@ -463,7 +471,7 @@ void lcd_mask(uint8_t *p, uint8_t mask, LcdFlags att)
 {
   ASSERT_IN_DISPLAY(p);
 
-  if (!(att & GREY1)) {
+  if (!(att & GREY(1))) {
     if (att & FORCE)
       *p |= mask;
     else if (att & ERASE)
@@ -472,9 +480,28 @@ void lcd_mask(uint8_t *p, uint8_t mask, LcdFlags att)
       *p ^= mask;
   }
 
-  if (!(att & GREY2)) {
-    p += DISPLAY_PLAN_SIZE;
+  p += DISPLAY_PLAN_SIZE;
+  if (!(att & GREY(2))) {
+    if (att & FORCE)
+      *p |= mask;
+    else if (att & ERASE)
+      *p &= ~mask;
+    else
+      *p ^= mask;
+  }
 
+  p += DISPLAY_PLAN_SIZE;
+  if (!(att & GREY(4))) {
+    if (att & FORCE)
+      *p |= mask;
+    else if (att & ERASE)
+      *p &= ~mask;
+    else
+      *p ^= mask;
+  }
+
+  p += DISPLAY_PLAN_SIZE;
+  if (!(att & GREY(8))) {
     if (att & FORCE)
       *p |= mask;
     else if (att & ERASE)
@@ -625,6 +652,8 @@ void lcd_invert_line(int8_t y)
   for (xcoord_t x=0; x<DISPLAY_W; x++) {
     ASSERT_IN_DISPLAY(p);
 #if defined(PCBX9D)
+    *(p+3*DISPLAY_PLAN_SIZE) ^= 0xff;
+    *(p+2*DISPLAY_PLAN_SIZE) ^= 0xff;
     *(p+DISPLAY_PLAN_SIZE) ^= 0xff;
 #endif
     *p++ ^= 0xff;
diff --git a/src/lcd.h b/src/lcd.h
index 7a3413c9b..a459f6fbc 100644
--- a/src/lcd.h
+++ b/src/lcd.h
@@ -104,8 +104,8 @@
 #define MIDSIZE       0x0100
 #define SMLSIZE       0x0200
 #define TINSIZE       0x0400
-#define GREY1         0x1000
-#define GREY2         0x2000
+#define GREY(x)       ((x)*0x1000)
+#define GREY_MASK(x)  ((x) & 0xF000)
 #else
 #define MIDSIZE       DBLSIZE
 #define SMLSIZE       0x00
@@ -121,7 +121,7 @@
 #define DISPLAY_PLAN_SIZE (DISPLAY_W*((DISPLAY_H+7)/8))
 
 #if defined(PCBX9D)
-#define DISPLAY_BUF_SIZE   (2*DISPLAY_PLAN_SIZE)
+#define DISPLAY_BUF_SIZE   (4*DISPLAY_PLAN_SIZE)
 #else
 #define DISPLAY_BUF_SIZE   DISPLAY_PLAN_SIZE
 #endif
@@ -216,6 +216,7 @@ inline void lcd_square(xcoord_t x, uint8_t y, xcoord_t w, LcdFlags att=0) { lcd_
     lcd_vline(x+5, y+8, 2);
 
 extern void lcd_img(xcoord_t x, uint8_t y, const pm_uchar * img, uint8_t idx, LcdFlags att=0);
+extern void lcd_bmp(xcoord_t x, uint8_t y, const pm_uchar * img);
 extern void lcdSetRefVolt(unsigned char val);
 extern void lcdInit();
 extern void lcd_clear();
@@ -224,7 +225,7 @@ extern void lcdSetContrast();
 extern void lcdRefresh();
 
 #if defined(PCBX9D)
-const pm_char * bmpLoad(uint8_t *dest, const char *filename);
+const pm_char * bmpLoad(uint8_t *dest, const char *filename, const xcoord_t width, const uint8_t height);
 #endif
 
 #define BLINK_ON_PHASE (g_blinkTmr10ms & (1<<6))
diff --git a/src/main_views.cpp b/src/main_views.cpp
index 823e41c17..22ee54c68 100644
--- a/src/main_views.cpp
+++ b/src/main_views.cpp
@@ -433,8 +433,8 @@ void menuMainView(uint8_t event)
   displaySliders();
 
 #if defined(PCBX9D)
-  // Model Bitmap
-  lcd_img(BITMAP_X, BITMAP_Y, modelBitmap, 0, 0);
+  // Model bitmap
+  lcd_bmp(BITMAP_X, BITMAP_Y, modelBitmap);
 #endif
 
   // Switches
diff --git a/src/menus.cpp b/src/menus.cpp
index 4bd914970..bf061c075 100644
--- a/src/menus.cpp
+++ b/src/menus.cpp
@@ -223,7 +223,7 @@ void check_rotary_encoder(uint8_t & event)
 
   // TODO remove later...
   uint8_t key = event & EVT_KEY_MASK;
-  if (key==KEY_LEFT || key==KEY_RIGHT)
+  if (key==KEY_MOVE_UP || key==KEY_MOVE_DOWN)
     event = 0;
 }
 #elif defined(ROTARY_ENCODERS)
diff --git a/src/open9x.cpp b/src/open9x.cpp
index 4dcb387b8..f84a1c5ad 100644
--- a/src/open9x.cpp
+++ b/src/open9x.cpp
@@ -55,7 +55,7 @@ OS_TID btTaskId;
 OS_STK btStack[BT_STACK_SIZE];
 #endif
 
-#if defined(DEBUG)
+#if defined(PCBSKY9X) && defined(DEBUG)
 OS_TID debugTaskId;
 OS_STK debugStack[DEBUG_STACK_SIZE];
 #endif
@@ -102,7 +102,7 @@ void loadModelBitmap()
   strncpy(lfn+sizeof(BITMAPS_PATH), g_model.bitmap, sizeof(g_model.bitmap));
   lfn[sizeof(BITMAPS_PATH)+sizeof(g_model.bitmap)] = '\0';
   strcat(lfn+sizeof(BITMAPS_PATH), BITMAPS_EXT);
-  bmpLoad(modelBitmap, lfn);
+  bmpLoad(modelBitmap, lfn, MODEL_BITMAP_WIDTH, MODEL_BITMAP_HEIGHT);
 }
 #endif
 
@@ -2549,11 +2549,7 @@ void perOut(uint8_t mode, uint8_t tick10ms)
   mixWarning = lv_mixWarning;
 }
 
-#if defined(SIMU)
-#define TIME_TO_WRITE s_eeDirtyMsk
-#else
-#define TIME_TO_WRITE (s_eeDirtyMsk && (tmr10ms_t)(get_tmr10ms() - s_eeDirtyTime10ms) >= (tmr10ms_t)WRITE_DELAY_10MS)
-#endif
+#define TIME_TO_WRITE() (s_eeDirtyMsk && (tmr10ms_t)(get_tmr10ms() - s_eeDirtyTime10ms) >= (tmr10ms_t)WRITE_DELAY_10MS)
 
 #ifdef BOLD_FONT
 ACTIVE_MIXES_TYPE activeMixes;
@@ -2927,16 +2923,16 @@ void perMain()
 #if defined(PCBSKY9X)
   if (Eeprom32_process_state != E32_IDLE)
     ee32_process();
-  else if (TIME_TO_WRITE)
+  else if (TIME_TO_WRITE())
     eeCheck();
 #elif defined(CPUARM)
-  if (TIME_TO_WRITE)
+  if (TIME_TO_WRITE())
     eeCheck();
 #else
   if (!eeprom_buffer_size) {
     if (theFile.isWriting())
       theFile.nextWriteStep();
-    else if (TIME_TO_WRITE)
+    else if (TIME_TO_WRITE())
       eeCheck();
   }
 #endif
@@ -3665,7 +3661,7 @@ int main(void)
 
   CoInitOS();
 
-#if defined(DEBUG)
+#if defined(PCBSKY9X) && defined(DEBUG)
   debugTaskId = CoCreateTaskEx(debugTask, NULL, 10, &debugStack[DEBUG_STACK_SIZE-1], DEBUG_STACK_SIZE, 1, false);
 #endif
 
diff --git a/src/open9x.h b/src/open9x.h
index 77f622200..1e155af17 100644
--- a/src/open9x.h
+++ b/src/open9x.h
@@ -172,6 +172,10 @@
 #include "stock/board_stock.h"
 #endif
 
+#if defined(CPUARM) && defined(DEBUG)
+#include "debug.h"
+#endif
+
 #if defined(SIMU)
 #include "simpgmspace.h"
 #elif defined(CPUARM)
@@ -222,6 +226,24 @@ extern void boardInit();
 extern char modelNames[MAX_MODELS][sizeof(g_model.name)];
 #endif
 
+#if defined(CPUARM)
+// This doesn't need protection on this processor
+#define tmr10ms_t uint32_t
+extern volatile tmr10ms_t g_tmr10ms;
+#define get_tmr10ms() g_tmr10ms
+#else
+#define tmr10ms_t uint16_t
+extern volatile tmr10ms_t g_tmr10ms;
+extern inline uint16_t get_tmr10ms()
+{
+  uint16_t time  ;
+  cli();
+  time = g_tmr10ms ;
+  sei();
+  return time ;
+}
+#endif
+
 // TODO try to merge the 2 include files
 #if defined(PCBSKY9X)
 #include "eeprom_arm.h"
@@ -238,7 +260,7 @@ extern char modelNames[MAX_MODELS][sizeof(g_model.name)];
 #if defined(PCBX9D)
 #define MODEL_BITMAP_WIDTH  64
 #define MODEL_BITMAP_HEIGHT 32
-#define MODEL_BITMAP_SIZE   (2+MODEL_BITMAP_WIDTH*MODEL_BITMAP_HEIGHT/8)
+#define MODEL_BITMAP_SIZE   (2+4*(MODEL_BITMAP_WIDTH*MODEL_BITMAP_HEIGHT/8))
 extern uint8_t modelBitmap[MODEL_BITMAP_SIZE];
 void loadModelBitmap();
 #define LOAD_MODEL_BITMAP() loadModelBitmap()
@@ -1075,24 +1097,6 @@ extern int16_t calc1000toRESX(int16_t x);
 extern int16_t calcRESXto1000(int16_t x);
 #endif
 
-#if defined(CPUARM)
-// This doesn't need protection on this processor
-#define tmr10ms_t uint32_t
-extern volatile tmr10ms_t g_tmr10ms;
-#define get_tmr10ms() g_tmr10ms
-#else
-#define tmr10ms_t uint16_t
-extern volatile tmr10ms_t g_tmr10ms;
-extern inline uint16_t get_tmr10ms()
-{
-  uint16_t time  ;
-  cli();
-  time = g_tmr10ms ;
-  sei();
-  return time ;
-}
-#endif
-
 #define TMR_VAROFS  5
 
 extern const char stamp1[];
diff --git a/src/simu.cpp b/src/simu.cpp
index 0bd736507..82913e1c9 100644
--- a/src/simu.cpp
+++ b/src/simu.cpp
@@ -381,7 +381,13 @@ long Open9xSim::onTimeout(FXObject*,FXSelector,void*)
 #else
     static FXuint keys2[]={KEY_F8, KEY_F7, KEY_F4, KEY_F3, KEY_F6, KEY_F5, KEY_F1, KEY_F2  };
 #endif
-#if defined(PCBSKY9X)
+#if defined(PCBX9D)
+    GPIOE->IDR |= PIN_TRIM_LH_L | PIN_TRIM_LH_R | PIN_TRIM_LV_DN | PIN_TRIM_LV_UP;
+    GPIOC->IDR |= PIN_TRIM_RV_DN | PIN_TRIM_RV_UP | PIN_TRIM_RH_L | PIN_TRIM_RH_R;
+    GPIOE->IDR |= 0xFFFFFFFF;
+    GPIOB->IDR |= 0xFFFFFFFF;
+    GPIOC->IDR |= 0xFFFFFFFF;
+#elif defined(PCBSKY9X)
     PIOA->PIO_PDSR |= (0x00800000 | 0x01000000 | 0x00000002 | 0x00000001);
     PIOB->PIO_PDSR |= (0x00000050);
     PIOC->PIO_PDSR |= (0x10000000 | 0x00000400 | 0x00000200);
@@ -393,7 +399,8 @@ long Open9xSim::onTimeout(FXObject*,FXSelector,void*)
 
     for(unsigned i=0; i<DIM(keys2);i++){
       if(getApp()->getKeyState(keys2[i])) {
-#if defined(PCBSKY9X)
+#if defined(PCBX9D)
+#elif defined(PCBSKY9X)
         switch(i) {
           case 6:
             PIOA->PIO_PDSR &= ~0x00800000;
@@ -498,32 +505,25 @@ void Open9xSim::refreshDiplay()
   if (lcd_refresh) {
     lcd_refresh = false;
     FXColor offColor = IS_BACKLIGHT_ON() ? BL_COLOR : FXRGB(200,200,200);
+#if !defined(PCBX9D)
     FXColor onColor = FXRGB(0,0,0);
-#if defined(PCBX9D)
-    FXColor grey1Color = FXRGB(0xC0,0xC0,0xC0);
-    FXColor grey2Color = FXRGB(0xA0,0xA0,0xA0);
 #endif
-
     for (int x=0;x<W;x++) {
       for (int y=0; y<H; y++) {
 #if defined(PCBX9D)
-        if ((lcd_buf[x+(y/8)*W] & (1<<(y%8))) && (lcd_buf[DISPLAY_PLAN_SIZE+x+(y/8)*W] & (1<<(y%8)))) {
-          bmp->setPixel(2*x, 2*y, onColor);
-          bmp->setPixel(2*x+1, 2*y, onColor);
-          bmp->setPixel(2*x, 2*y+1, onColor);
-          bmp->setPixel(2*x+1, 2*y+1, onColor);
-        }
-        else if (lcd_buf[DISPLAY_PLAN_SIZE+x+(y/8)*W] & (1<<(y%8))) {
-          bmp->setPixel(2*x, 2*y, grey1Color);
-          bmp->setPixel(2*x+1, 2*y, grey1Color);
-          bmp->setPixel(2*x, 2*y+1, grey1Color);
-          bmp->setPixel(2*x+1, 2*y+1, grey1Color);
-        }
-        else if (lcd_buf[x+(y/8)*W] & (1<<(y%8))) {
-          bmp->setPixel(2*x, 2*y, grey2Color);
-          bmp->setPixel(2*x+1, 2*y, grey2Color);
-          bmp->setPixel(2*x, 2*y+1, grey2Color);
-          bmp->setPixel(2*x+1, 2*y+1, grey2Color);
+        #define PALETTE_IDX(p, x, mask) ((((p)[(x)] & (mask)) ? 0x1 : 0) + (((p)[DISPLAY_PLAN_SIZE+(x)] & (mask)) ? 0x2 : 0) + (((p)[2*DISPLAY_PLAN_SIZE+(x)] & (mask)) ? 0x4 : 0) + (((p)[3*DISPLAY_PLAN_SIZE+(x)] & (mask)) ? 0x8 : 0))
+        uint8_t mask = (1 << (y%8));
+        uint32_t z = PALETTE_IDX(lcd_buf, (y/8)*W+x, mask);
+        if (z) {
+          FXColor color;
+          if (IS_BACKLIGHT_ON())
+            color = FXRGB(47-(z*47)/15, 123-(z*123)/15, 227-(z*227)/15);
+          else
+            color = FXRGB(200-(z*200)/15, 200-(z*200)/15, 200-(z*200)/15);
+          bmp->setPixel(2*x, 2*y, color);
+          bmp->setPixel(2*x+1, 2*y, color);
+          bmp->setPixel(2*x, 2*y+1, color);
+          bmp->setPixel(2*x+1, 2*y+1, color);
         }
 #else
         if (lcd_buf[x+(y/8)*W] & (1<<(y%8))) {
diff --git a/src/sky9x/audio.cpp b/src/sky9x/audio.cpp
index 2e08c9927..98f4191c8 100644
--- a/src/sky9x/audio.cpp
+++ b/src/sky9x/audio.cpp
@@ -144,7 +144,7 @@ bool isAudioFileAvailable(uint32_t i, char * filename)
 {
   uint8_t category = (i >> 24);
 
-#if defined(SIMU)
+#if 0
   printf("isAudioFileAvailable(%08x)\n", i); fflush(stdout);
 #endif
 
diff --git a/src/sky9x/bluetooth.cpp b/src/sky9x/bluetooth.cpp
index 01edfaa58..005f625d8 100644
--- a/src/sky9x/bluetooth.cpp
+++ b/src/sky9x/bluetooth.cpp
@@ -33,7 +33,7 @@
  */
 
 #include "../open9x.h"
-#include "fifo.h"
+#include "../fifo.h"
 
 void btSetBaudrate(uint32_t index)
 {
diff --git a/src/sky9x/board_sky9x.cpp b/src/sky9x/board_sky9x.cpp
index 09df75905..06d249f3e 100644
--- a/src/sky9x/board_sky9x.cpp
+++ b/src/sky9x/board_sky9x.cpp
@@ -36,6 +36,8 @@
 
 void pwrInit();
 void eepromInit();
+void DEBUG_UART_Configure( uint32_t baudrate, uint32_t masterClock);
+void DEBUG_UART_Stop();
 
 uint32_t Master_frequency ;
 volatile uint32_t Tenms ;
diff --git a/src/sky9x/board_sky9x.h b/src/sky9x/board_sky9x.h
index 0c480b1d8..ec0951ad8 100644
--- a/src/sky9x/board_sky9x.h
+++ b/src/sky9x/board_sky9x.h
@@ -39,7 +39,6 @@
 #include "board.h"
 #include "audio_driver.h"
 #include "haptic_driver.h"
-#include "debug.h"
 
 #if defined(BLUETOOTH)
 #include "bluetooth.h"
diff --git a/src/sky9x/debug.cpp b/src/sky9x/debug_driver.cpp
similarity index 77%
rename from src/sky9x/debug.cpp
rename to src/sky9x/debug_driver.cpp
index 782b27f95..bb93bdcb7 100644
--- a/src/sky9x/debug.cpp
+++ b/src/sky9x/debug_driver.cpp
@@ -37,6 +37,8 @@
 
 #if !defined(SIMU)
 
+extern Fifo32 debugFifo;
+
 /*
  * Outputs a character on the UART line.
  *
@@ -54,37 +56,6 @@ void debugPutc(const char c)
   pUart->UART_THR = c;
 }
 
-// Outputs a string to the UART
-void debugPuts(const char *format, ...)
-{
-  va_list arglist;
-  char tmp[256];
-
-  va_start(arglist, format);
-  vsnprintf(tmp, 256, format, arglist);
-  va_end(arglist);
-
-  const char *t = tmp;
-  while (*t) {
-    debugPutc(*t++);
-  }
-}
-
-void dump(unsigned char *data, unsigned int size)
-{
- debugPuts("DUMP %d bytes ...\n\r", size);
- unsigned int i = 0, j=0;
- while (i*32+j < size) {
-   debugPuts("%.2X ", data[i*32+j]);
-   j++;
-   if (j==32) {
-     i++; j=0;
-     debugPuts("\n\r");
-   }
- }
- debugPuts("\n\r");
-}
-
 /**
  * Configures a UART peripheral with the specified parameters.
  *
@@ -128,22 +99,9 @@ void DEBUG_UART_Stop()
   NVIC_DisableIRQ(UART0_IRQn) ;
 }
 
-Fifo32 debugFifo;
-
 extern "C" void UART0_IRQHandler()
 {
   debugFifo.push(CONSOLE_USART->UART_RHR);
 }
 
-void debugTask(void* pdata)
-{
-  uint8_t rxchar ;
-
-  for (;;) {
-    while (!debugFifo.pop(rxchar))
-      CoTickDelay(5); // 10ms
-
-  }
-}
-
 #endif
diff --git a/src/sky9x/syscalls.c b/src/sky9x/syscalls.c
index 9fdca526d..4ab52c11f 100644
--- a/src/sky9x/syscalls.c
+++ b/src/sky9x/syscalls.c
@@ -39,7 +39,7 @@
  *----------------------------------------------------------------------------*/
 
 
-#include "board.h"
+// #include "board.h"
 
 #include <stdio.h>
 #include <stdarg.h>
diff --git a/src/x9d/board_x9d.cpp b/src/x9d/board_x9d.cpp
index 137649268..2093e4810 100644
--- a/src/x9d/board_x9d.cpp
+++ b/src/x9d/board_x9d.cpp
@@ -172,6 +172,9 @@ void boardInit()
   adcInit();
   delaysInit();
   audioInit();
+#if defined(DEBUG)
+  debugInit();
+#endif
 
   // TODO init_ppm() ;
 
@@ -184,7 +187,6 @@ void boardInit()
   __enable_irq() ;
 
   eepromInit();
-  uartInit();
 }
 #endif
 
diff --git a/src/x9d/board_x9d.h b/src/x9d/board_x9d.h
index c9506c9d5..5ff04137e 100644
--- a/src/x9d/board_x9d.h
+++ b/src/x9d/board_x9d.h
@@ -124,6 +124,7 @@ void eepromInit(); // TODO check it's not in another include
 void delaysInit();
 void adcInit();
 void audioInit();
+void debugInit();
 
 void adcRead();
 void init_trainer_ppm();
diff --git a/src/x9d/uart.cpp b/src/x9d/debug_driver.cpp
similarity index 95%
rename from src/x9d/uart.cpp
rename to src/x9d/debug_driver.cpp
index 299b0aacd..da2ca367e 100644
--- a/src/x9d/uart.cpp
+++ b/src/x9d/debug_driver.cpp
@@ -34,16 +34,7 @@
 
 #include "../open9x.h"
 
-void uartSendChar(uint8_t c)
-{
-  USART_SendData(UART_DEBUG, c);
-
-  /* Wait for end of previous transfer */
-  while (USART_GetFlagStatus(UART_DEBUG, USART_FLAG_TC) == RESET);
-}
-
-
-void uartInit(void)
+void debugInit(void)
 {
   USART_InitTypeDef USART_InitStructure;
   GPIO_InitTypeDef GPIO_InitStructure;
@@ -72,3 +63,10 @@ void uartInit(void)
   USART_Cmd(UART_DEBUG, ENABLE);
 }
 
+void debugPutc(const char c)
+{
+  USART_SendData(UART_DEBUG, c);
+
+  /* Wait for end of previous transfer */
+  while (USART_GetFlagStatus(UART_DEBUG, USART_FLAG_TC) == RESET);
+}
diff --git a/src/x9d/hal.h b/src/x9d/hal.h
index deed0f2dc..636f2dd2a 100644
--- a/src/x9d/hal.h
+++ b/src/x9d/hal.h
@@ -26,7 +26,7 @@
 #define PIN_TRIM_LV_UP        GPIO_Pin_5  //PE.05
 
 // Switchs
-#define	PIN_SW_F		          GPIO_Pin_14	//PE.14
+#define	PIN_SW_F		        GPIO_Pin_14	//PE.14
 #define	PIN_SW_E_L		        GPIO_Pin_3	//PB.03
 #define	PIN_SW_E_H		        GPIO_Pin_4	//PB.04
 #define	PIN_SW_B_H		        GPIO_Pin_5	//PB.05
diff --git a/src/x9d/keys_driver.cpp b/src/x9d/keys_driver.cpp
index b01684505..fb506c388 100644
--- a/src/x9d/keys_driver.cpp
+++ b/src/x9d/keys_driver.cpp
@@ -85,6 +85,8 @@ uint32_t readTrims()
   if (~c & PIN_TRIM_RH_R)
     result |= 0x80;         // RH_R
 
+  // printf("readTrims(): %x %x => %x\n", c, e, result); fflush(stdout);
+
   return result;
 }
 
@@ -105,14 +107,12 @@ void readKeysAndTrims()
     ++enuk;
   }
 
-#if !defined(SIMU)
   in = readTrims();
 
   for (i = 1; i < 256; i <<= 1) {
     keys[enuk].input(in & i, (EnumKeys)enuk);
     ++enuk;
   }
-#endif
 }
 
 uint32_t keyState(EnumKeys enuk)
diff --git a/src/x9d/lcd_driver.cpp b/src/x9d/lcd_driver.cpp
index 6fad9f241..d585fb038 100644
--- a/src/x9d/lcd_driver.cpp
+++ b/src/x9d/lcd_driver.cpp
@@ -23,6 +23,9 @@ void Set_Address(u8 x, u8 y)
   WriteCommand(((y>>4)&0x0F)|0x70);    //Set Row Address MSB RA [7:4]
 }
 
+#define PALETTE_IDX(p, x, mask) (((p[x] & mask) ? 0x1 : 0) + ((p[DISPLAY_PLAN_SIZE+x] & mask) ? 0x2 : 0) + ((p[2*DISPLAY_PLAN_SIZE+x] & mask) ? 0x4 : 0) + ((p[3*DISPLAY_PLAN_SIZE+x] & mask) ? 0x8 : 0))
+const uint8_t lcdPalette[4] = { 0, 0x03, 0x06, 0x0F };
+
 void lcdRefresh()
 {  
   for (uint8_t y=0; y<DISPLAY_H; y++) {
@@ -31,8 +34,11 @@ void lcdRefresh()
     Set_Address(0, y);
     AspiCmd(0xAF);
     for (uint8_t x=0; x<DISPLAY_W; x+=2) {
-#if 1
+#if 0
       uint8_t data = (p[x] & mask ? 0x80 : 0) + (p[x+1] & mask ? 0x08 : 0) + (p[DISPLAY_PLAN_SIZE+x] & mask ? 0x40 : 0) + (p[DISPLAY_PLAN_SIZE+x+1] & mask ? 0x04 : 0);
+#elif 1
+      uint8_t data = (PALETTE_IDX(p, x, mask) << 4) + (PALETTE_IDX(p, x+1, mask));
+      //uint8_t data = (lcdPalette(PALETTE_IDX(p, x, mask)) << 4) + lcdPalette(PALETTE_IDX(p, x+1, mask));
 #else
       // this code shows the 16 grey tones...
       uint8_t data = (x * 16) / 212;