diff --git a/src/lcd/lcd.c b/src/lcd/lcd.c
new file mode 100644
index 0000000000000000000000000000000000000000..d5f7ea57d35a4eaf8e37774722b174db097a55a6
--- /dev/null
+++ b/src/lcd/lcd.c
@@ -0,0 +1,340 @@
+/* Author: Steve Gunn
+ * Licence: This work is licensed under the Creative Commons Attribution License.
+ * View this license at http://creativecommons.org/about/licenses/
+ *
+ *
+ * - Jan 2015 Modified for LaFortuna (Rev A, black edition) [KPZ]
+ */
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/delay.h>
+#include "font.h"
+#include "ili934x.h"
+#include "lcd.h"
+
+lcd display;
+
+void init_lcd()
+{
+ /* Enable extended memory interface with 10 bit addressing */
+ XMCRB = _BV(XMM2) | _BV(XMM1);
+ XMCRA = _BV(SRE);
+ DDRC |= _BV(RESET);
+ DDRB |= _BV(BLC);
+ _delay_ms(1);
+ PORTC &= ~_BV(RESET);
+ _delay_ms(20);
+ PORTC |= _BV(RESET);
+ _delay_ms(120);
+ write_cmd(DISPLAY_OFF);
+ write_cmd(SLEEP_OUT);
+ _delay_ms(60);
+ write_cmd_data(INTERNAL_IC_SETTING, 0x01);
+ write_cmd(POWER_CONTROL_1);
+ write_data16(0x2608);
+ write_cmd_data(POWER_CONTROL_2, 0x10);
+ write_cmd(VCOM_CONTROL_1);
+ write_data16(0x353E);
+ write_cmd_data(VCOM_CONTROL_2, 0xB5);
+ write_cmd_data(INTERFACE_CONTROL, 0x01);
+ write_data16(0x0000);
+ write_cmd_data(PIXEL_FORMAT_SET, 85); /* 16bit/pixel */
+ set_orientation(West);
+ clear_screen();
+ display.x = 0;
+ display.y = 0;
+ display.background = BLACK;
+ display.foreground = GREEN;
+ write_cmd(DISPLAY_ON);
+ _delay_ms(50);
+ write_cmd_data(TEARING_EFFECT_LINE_ON, 0x00);
+ EICRB |= _BV(ISC61);
+ PORTB |= _BV(BLC);
+}
+
+void lcd_brightness(uint8_t i)
+{
+ /* Configure Timer 2 Fast PWM Mode 3 */
+ TCCR2A = _BV(COM2A1) | _BV(WGM21) | _BV(WGM20);
+ TCCR2B = _BV(CS20);
+ OCR2A = i;
+}
+
+void set_orientation(orientation o)
+{
+ display.orient = o;
+ write_cmd(MEMORY_ACCESS_CONTROL);
+ if (o==North) {
+ display.width = LCDWIDTH;
+ display.height = LCDHEIGHT;
+ write_data(0x48);
+ }
+ else if (o==West) {
+ display.width = LCDHEIGHT;
+ display.height = LCDWIDTH;
+ write_data(0xE8);
+ }
+ else if (o==South) {
+ display.width = LCDWIDTH;
+ display.height = LCDHEIGHT;
+ write_data(0x88);
+ }
+ else if (o==East) {
+ display.width = LCDHEIGHT;
+ display.height = LCDWIDTH;
+ write_data(0x28);
+ }
+ write_cmd(COLUMN_ADDRESS_SET);
+ write_data16(0);
+ write_data16(display.width-1);
+ write_cmd(PAGE_ADDRESS_SET);
+ write_data16(0);
+ write_data16(display.height-1);
+}
+
+
+
+void set_frame_rate_hz(uint8_t f)
+{
+ uint8_t diva, rtna, period;
+ if (f>118)
+ f = 118;
+ if (f<8)
+ f = 8;
+ if (f>60) {
+ diva = 0x00;
+ } else if (f>30) {
+ diva = 0x01;
+ } else if (f>15) {
+ diva = 0x02;
+ } else {
+ diva = 0x03;
+ }
+ /* !!! FIXME !!! [KPZ-30.01.2015] */
+ /* Check whether this works for diva > 0 */
+ /* See ILI9341 datasheet, page 155 */
+ period = 1920.0/f;
+ rtna = period >> diva;
+ write_cmd(FRAME_CONTROL_IN_NORMAL_MODE);
+ write_data(diva);
+ write_data(rtna);
+}
+
+void fill_rectangle(rectangle r, uint16_t col)
+{
+ setupDraw(r);
+/* uint16_t x, y;
+ for(x=r.left; x<=r.right; x++)
+ for(y=r.top; y<=r.bottom; y++)
+ write_data16(col);
+*/
+ uint16_t wpixels = r.right - r.left + 1;
+ uint16_t hpixels = r.bottom - r.top + 1;
+ uint8_t mod8, div8;
+ uint16_t odm8, odd8;
+ if (hpixels > wpixels) {
+ mod8 = hpixels & 0x07;
+ div8 = hpixels >> 3;
+ odm8 = wpixels*mod8;
+ odd8 = wpixels*div8;
+ } else {
+ mod8 = wpixels & 0x07;
+ div8 = wpixels >> 3;
+ odm8 = hpixels*mod8;
+ odd8 = hpixels*div8;
+ }
+ uint8_t pix1 = odm8 & 0x07;
+ while(pix1--)
+ write_data16(col);
+
+ uint16_t pix8 = odd8 + (odm8 >> 3);
+ while(pix8--) {
+ write_data16(col);
+ write_data16(col);
+ write_data16(col);
+ write_data16(col);
+ write_data16(col);
+ write_data16(col);
+ write_data16(col);
+ write_data16(col);
+ }
+}
+
+void fill_rectangle_indexed(rectangle r, uint16_t* col)
+{
+ uint16_t x, y;
+ setupDraw(r);
+ for(x=r.left; x<=r.right; x++)
+ for(y=r.top; y<=r.bottom; y++)
+ write_data16(*col++);
+}
+
+void fill_rectangle_indexed_MEM(rectangle r, PGM_P imgdata)
+{
+ uint16_t x, y;
+ rectangleInRegion(&r);
+ setupDraw(r);
+ uint16_t pixVal;
+ for(x=r.left; x<=r.right; x++)
+ for(y=r.top; y<=r.bottom; y++){
+ pixVal = pgm_read_word(imgdata);
+ imgdata+=2;
+ write_data16(pixVal);
+ }
+}
+
+void compositeTest(rectangle r, uint16_t* copyReg){
+ uint16_t x, y;
+ write_cmd(COLUMN_ADDRESS_SET);
+ write_data16(r.left);
+ write_data16(r.right);
+ write_cmd(PAGE_ADDRESS_SET);
+ write_data16(r.top);
+ write_data16(r.bottom);
+ write_cmd(MEMORY_READ);
+ uint16_t* ptr = copyReg;read_data();
+ for(x=r.left; x<=r.right; x++)
+ for(y=r.top; y<=r.bottom; y++){
+ unsigned char p0 = read_data();
+ unsigned char p1 = read_data();
+ unsigned char p2 = read_data();
+ unsigned char r = p0 >> 2;
+ unsigned char g = p1 >> 2;
+ unsigned char b = p2 >> 2;
+ *ptr = (((uint16_t)b)>>1) + (((uint16_t)g) << 5) + ((((uint16_t)r)>>1) << 11);
+ ptr+=2;
+ }
+
+ write_cmd(MEMORY_WRITE);
+ ptr = copyReg;
+ for(x=r.left; x<=r.right; x++)
+ for(y=r.top; y<=r.bottom; y++){
+ write_data16(*ptr);
+ ptr+=2;
+ }
+}
+
+void screenDump(rectangle r){
+ write_cmd(COLUMN_ADDRESS_SET);
+ write_data16(r.left);
+ write_data16(r.right);
+ write_cmd(PAGE_ADDRESS_SET);
+ write_data16(r.top);
+ write_data16(r.bottom);
+ write_cmd(MEMORY_READ);
+ uint16_t x, y;
+ read_data(); //Throw away a byte
+ for(x=r.left; x<=r.right; x++)
+ for(y=r.top; y<=r.bottom; y++){
+ unsigned char r = read_data() >> 2;
+ unsigned char g = read_data() >> 2;
+ unsigned char b = read_data() >> 2;
+ uint16_t value = (((uint16_t)b)>>1) + (((uint16_t)g) << 5) + ((((uint16_t)r)>>1) << 11);
+ }
+}
+
+void clear_screen()
+{
+ display.x = 0;
+ display.y = 0;
+ rectangle r = {0, display.width-1, 0, display.height-1};
+ fill_rectangle(r, display.background);
+}
+
+void display_char(char c)
+{
+ if (c == '\n') {
+ display.x = 0;
+ display.y += 16;
+ }
+ if (display.y > 200) {
+ display.y = 0;
+ rectangle r;
+ r.left = 0;
+ r.right = 319;
+ r.top = 0;
+ r.bottom = 239;
+ fill_rectangle(r, 0);
+ }
+
+ uint16_t x, y;
+ PGM_P fdata;
+ uint8_t bits, mask;
+ uint16_t sc=display.x, ec=display.x + 4, sp=display.y, ep=display.y + 7;
+
+ if (c < 32 || c > 126) return;
+ fdata = (c - ' ')*5 + font5x7;
+ write_cmd(PAGE_ADDRESS_SET);
+ write_data16(sp);
+ write_data16(ep);
+ for(x=sc; x<=ec; x++) {
+ write_cmd(COLUMN_ADDRESS_SET);
+ write_data16(x);
+ write_data16(x);
+ write_cmd(MEMORY_WRITE);
+ bits = pgm_read_byte(fdata++);
+ for(y=sp, mask=0x01; y<=ep; y++, mask<<=1)
+ write_data16((bits & mask) ? display.foreground : display.background);
+ }
+ write_cmd(COLUMN_ADDRESS_SET);
+ write_data16(x);
+ write_data16(x);
+ write_cmd(MEMORY_WRITE);
+ for(y=sp; y<=ep; y++)
+ write_data16(display.background);
+
+ display.x += 6;
+ if (display.x >= display.width) { display.x=0; display.y+=8; }
+}
+
+void display_string(char *str)
+{
+ uint8_t i;
+ for(i=0; str[i]; i++)
+ display_char(str[i]);
+}
+
+void display_string_xy(char *str, uint16_t x, uint16_t y)
+{
+ uint8_t i;
+ display.x = x;
+ display.y = y;
+ for(i=0; str[i]; i++)
+ display_char(str[i]);
+}
+
+void rectangleInRegion(rectangle* r){ //LETS FIX STUFFFF!!
+ if(r->top > r->bottom)
+ r->top = r->bottom;
+ if(r->top > 32000)
+ r->top = 0;
+ if(r->bottom > 32000)
+ r->bottom = 0;
+ if(r->top >= display.height)
+ r->top = display.height-1;
+ if(r->bottom >= display.height)
+ r->bottom = display.height-1;
+
+ if(r->left > r->right)
+ r->left = r->right;
+ if(r->left > 32000)
+ r->left = 0;
+ if(r->right > 32000)
+ r->right = 0;
+ if(r->left >= display.width)
+ r->left = display.width-1;
+ if(r->right >= display.width)
+ r->right = display.width-1;
+}
+
+void setupDraw(rectangle r){
+ write_cmd(COLUMN_ADDRESS_SET);
+ write_data16(r.left);
+ write_data16(r.right);
+ write_cmd(PAGE_ADDRESS_SET);
+ write_data16(r.top);
+ write_data16(r.bottom);
+ write_cmd(MEMORY_WRITE);
+}
+