diff --git a/firmware/basic/config.c b/firmware/basic/config.c
index d4a0b5a..8933203 100644
--- a/firmware/basic/config.c
+++ b/firmware/basic/config.c
@@ -15,7 +15,7 @@ struct CDESC the_config[]= {
     {"lcdbacklight",     50,    0, 100},
     {"lcdmirror",        0,     0, 1  },
     {"lcdinvert",        0,     0, 1  },
-    {"lcdcontrast",      3,     1, 6  },
+    {"lcdcontrast",      3,     0, 31  },
     { NULL,              0,     0, 0  },
 };
 
diff --git a/firmware/lcd/display.c b/firmware/lcd/display.c
index 3ffd916..e8da386 100644
--- a/firmware/lcd/display.c
+++ b/firmware/lcd/display.c
@@ -61,6 +61,55 @@ static void lcdWrite(uint8_t cd, uint8_t data) {
     frame = SSP_SSP0DR;
 }
 
+#define CS 2,1
+#define SCK 2,11
+#define SDA 0,9
+#define RST 2,2
+
+uint8_t lcdRead(uint8_t data)
+{
+    uint8_t i;
+
+    gpioSetDir(SDA, 1);
+    gpioSetValue(SCK, 0);
+    delayms(1);
+    gpioSetValue(CS, 0);
+    delayms(1);
+
+    gpioSetValue(SDA, 0);
+    delayms(1);
+    gpioSetValue(SCK, 1);
+    delayms(1);
+    
+    for(i=0; i<8; i++){
+        gpioSetValue(SCK, 0);
+        delayms(1);
+        if( data & 0x80 )
+            gpioSetValue(SDA, 1);
+        else
+            gpioSetValue(SDA, 0);
+        data <<= 1;
+        gpioSetValue(SCK, 1);
+        delayms(1);
+    }
+    uint8_t ret = 0;
+
+    gpioSetDir(SDA, 0);
+    for(i=0; i<8; i++){
+        gpioSetValue(SCK, 0);
+        delayms(1);
+        ret <<= 1;
+        ret |= gpioGetValue(SDA);
+        gpioSetValue(SCK, 1);
+        delayms(1);
+    }
+
+    gpioSetValue(CS, 0);
+    gpioSetDir(SDA, 1);
+    delayms(1);
+}
+
+
 void lcdInit(void) {
     sspInit(0, sspClockPolarity_Low, sspClockPhase_RisingEdge);
 
@@ -172,8 +221,9 @@ inline void lcdInvert(void) {
 }
 
 void lcdSetContrast(int c) {
-    c+=0x20;
-    if(c>0x2e) c=0x24;
+    c+=0x80;
+    if(c>0x9F)
+        return;
     lcd_select();
     lcdWrite(TYPE_CMD,c);
     lcd_deselect();
diff --git a/firmware/lcd/display.h b/firmware/lcd/display.h
index 79f671e..d1bcd64 100644
--- a/firmware/lcd/display.h
+++ b/firmware/lcd/display.h
@@ -20,6 +20,7 @@
 /* Display buffer */
 extern uint8_t lcdBuffer[RESX*RESY_B];
 
+uint8_t lcdRead(uint8_t data);
 void lcdInit(void);
 void lcdFill(char f);
 void lcdDisplay(void);