Merge remote-tracking branch 'upstream/master'

17 files changed, 2647 insertions, 0 deletions

diff --git a/drivers/avr/analog.c b/drivers/avr/analog.c
new file mode 100644
index 000000000..1ec38df75
--- /dev/null
+++ b/drivers/avr/analog.c
@@ -0,0 +1,69 @@
+/* Copyright 2015 Jack Humbert
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+// Simple analog to digitial conversion
+
+#include <avr/io.h>
+#include <avr/pgmspace.h>
+#include <stdint.h>
+#include "analog.h"
+
+
+static uint8_t aref = (1<<REFS0); // default to AREF = Vcc
+
+
+void analogReference(uint8_t mode)
+{
+	aref = mode & 0xC0;
+}
+
+
+// Arduino compatible pin input
+int16_t analogRead(uint8_t pin)
+{
+#if defined(__AVR_ATmega32U4__)
+	static const uint8_t PROGMEM pin_to_mux[] = {
+		0x00, 0x01, 0x04, 0x05, 0x06, 0x07,
+		0x25, 0x24, 0x23, 0x22, 0x21, 0x20};
+	if (pin >= 12) return 0;
+	return adc_read(pgm_read_byte(pin_to_mux + pin));
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+	if (pin >= 8) return 0;
+	return adc_read(pin);
+#else
+	return 0;
+#endif
+}
+
+// Mux input
+int16_t adc_read(uint8_t mux)
+{
+#if defined(__AVR_AT90USB162__)
+	return 0;
+#else
+	uint8_t low;
+
+	ADCSRA = (1<<ADEN) | ADC_PRESCALER;		// enable ADC
+	ADCSRB = (1<<ADHSM) | (mux & 0x20);		// high speed mode
+	ADMUX = aref | (mux & 0x1F);			// configure mux input
+	ADCSRA = (1<<ADEN) | ADC_PRESCALER | (1<<ADSC);	// start the conversion
+	while (ADCSRA & (1<<ADSC)) ;			// wait for result
+	low = ADCL;					// must read LSB first
+	return (ADCH << 8) | low;			// must read MSB only once!
+#endif
+}
+
+
diff --git a/drivers/avr/analog.h b/drivers/avr/analog.h
new file mode 100644
index 000000000..8d93de7dc
--- /dev/null
+++ b/drivers/avr/analog.h
@@ -0,0 +1,52 @@
+/* Copyright 2015 Jack Humbert
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _analog_h_included__
+#define _analog_h_included__
+
+#include <stdint.h>
+
+void analogReference(uint8_t mode);
+int16_t analogRead(uint8_t pin);
+int16_t adc_read(uint8_t mux);
+
+#define ADC_REF_POWER     (1<<REFS0)
+#define ADC_REF_INTERNAL  ((1<<REFS1) | (1<<REFS0))
+#define ADC_REF_EXTERNAL  (0)
+
+// These prescaler values are for high speed mode, ADHSM = 1
+#if F_CPU == 16000000L
+#define ADC_PRESCALER ((1<<ADPS2) | (1<<ADPS1))
+#elif F_CPU == 8000000L
+#define ADC_PRESCALER ((1<<ADPS2) | (1<<ADPS0))
+#elif F_CPU == 4000000L
+#define ADC_PRESCALER ((1<<ADPS2))
+#elif F_CPU == 2000000L
+#define ADC_PRESCALER ((1<<ADPS1) | (1<<ADPS0))
+#elif F_CPU == 1000000L
+#define ADC_PRESCALER ((1<<ADPS1))
+#else
+#define ADC_PRESCALER ((1<<ADPS0))
+#endif
+
+// some avr-libc versions do not properly define ADHSM
+#if defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+#if !defined(ADHSM)
+#define ADHSM (7)
+#endif
+#endif
+
+#endif
diff --git a/drivers/avr/glcdfont.c b/drivers/avr/glcdfont.c
new file mode 100644
index 000000000..6f88bd23a
--- /dev/null
+++ b/drivers/avr/glcdfont.c
@@ -0,0 +1,276 @@
+// This is the 'classic' fixed-space bitmap font for Adafruit_GFX since 1.0.
+// See gfxfont.h for newer custom bitmap font info.
+
+#ifndef FONT5X7_H
+#define FONT5X7_H
+
+#ifdef __AVR__
+ #include <avr/io.h>
+ #include <avr/pgmspace.h>
+#elif defined(ESP8266)
+ #include <pgmspace.h>
+#else
+ #define PROGMEM
+#endif
+
+// Standard ASCII 5x7 font
+
+static const unsigned char font[] PROGMEM = {
+	0x00, 0x00, 0x00, 0x00, 0x00,
+	0x3E, 0x5B, 0x4F, 0x5B, 0x3E,
+	0x3E, 0x6B, 0x4F, 0x6B, 0x3E,
+	0x1C, 0x3E, 0x7C, 0x3E, 0x1C,
+	0x18, 0x3C, 0x7E, 0x3C, 0x18,
+	0x1C, 0x57, 0x7D, 0x57, 0x1C,
+	0x1C, 0x5E, 0x7F, 0x5E, 0x1C,
+	0x00, 0x18, 0x3C, 0x18, 0x00,
+	0xFF, 0xE7, 0xC3, 0xE7, 0xFF,
+	0x00, 0x18, 0x24, 0x18, 0x00,
+	0xFF, 0xE7, 0xDB, 0xE7, 0xFF,
+	0x30, 0x48, 0x3A, 0x06, 0x0E,
+	0x26, 0x29, 0x79, 0x29, 0x26,
+	0x40, 0x7F, 0x05, 0x05, 0x07,
+	0x40, 0x7F, 0x05, 0x25, 0x3F,
+	0x5A, 0x3C, 0xE7, 0x3C, 0x5A,
+	0x7F, 0x3E, 0x1C, 0x1C, 0x08,
+	0x08, 0x1C, 0x1C, 0x3E, 0x7F,
+	0x14, 0x22, 0x7F, 0x22, 0x14,
+	0x5F, 0x5F, 0x00, 0x5F, 0x5F,
+	0x06, 0x09, 0x7F, 0x01, 0x7F,
+	0x00, 0x66, 0x89, 0x95, 0x6A,
+	0x60, 0x60, 0x60, 0x60, 0x60,
+	0x94, 0xA2, 0xFF, 0xA2, 0x94,
+	0x08, 0x04, 0x7E, 0x04, 0x08,
+	0x10, 0x20, 0x7E, 0x20, 0x10,
+	0x08, 0x08, 0x2A, 0x1C, 0x08,
+	0x08, 0x1C, 0x2A, 0x08, 0x08,
+	0x1E, 0x10, 0x10, 0x10, 0x10,
+	0x0C, 0x1E, 0x0C, 0x1E, 0x0C,
+	0x30, 0x38, 0x3E, 0x38, 0x30,
+	0x06, 0x0E, 0x3E, 0x0E, 0x06,
+	0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x5F, 0x00, 0x00,
+	0x00, 0x07, 0x00, 0x07, 0x00,
+	0x14, 0x7F, 0x14, 0x7F, 0x14,
+	0x24, 0x2A, 0x7F, 0x2A, 0x12,
+	0x23, 0x13, 0x08, 0x64, 0x62,
+	0x36, 0x49, 0x56, 0x20, 0x50,
+	0x00, 0x08, 0x07, 0x03, 0x00,
+	0x00, 0x1C, 0x22, 0x41, 0x00,
+	0x00, 0x41, 0x22, 0x1C, 0x00,
+	0x2A, 0x1C, 0x7F, 0x1C, 0x2A,
+	0x08, 0x08, 0x3E, 0x08, 0x08,
+	0x00, 0x80, 0x70, 0x30, 0x00,
+	0x08, 0x08, 0x08, 0x08, 0x08,
+	0x00, 0x00, 0x60, 0x60, 0x00,
+	0x20, 0x10, 0x08, 0x04, 0x02,
+	0x3E, 0x51, 0x49, 0x45, 0x3E,
+	0x00, 0x42, 0x7F, 0x40, 0x00,
+	0x72, 0x49, 0x49, 0x49, 0x46,
+	0x21, 0x41, 0x49, 0x4D, 0x33,
+	0x18, 0x14, 0x12, 0x7F, 0x10,
+	0x27, 0x45, 0x45, 0x45, 0x39,
+	0x3C, 0x4A, 0x49, 0x49, 0x31,
+	0x41, 0x21, 0x11, 0x09, 0x07,
+	0x36, 0x49, 0x49, 0x49, 0x36,
+	0x46, 0x49, 0x49, 0x29, 0x1E,
+	0x00, 0x00, 0x14, 0x00, 0x00,
+	0x00, 0x40, 0x34, 0x00, 0x00,
+	0x00, 0x08, 0x14, 0x22, 0x41,
+	0x14, 0x14, 0x14, 0x14, 0x14,
+	0x00, 0x41, 0x22, 0x14, 0x08,
+	0x02, 0x01, 0x59, 0x09, 0x06,
+	0x3E, 0x41, 0x5D, 0x59, 0x4E,
+	0x7C, 0x12, 0x11, 0x12, 0x7C,
+	0x7F, 0x49, 0x49, 0x49, 0x36,
+	0x3E, 0x41, 0x41, 0x41, 0x22,
+	0x7F, 0x41, 0x41, 0x41, 0x3E,
+	0x7F, 0x49, 0x49, 0x49, 0x41,
+	0x7F, 0x09, 0x09, 0x09, 0x01,
+	0x3E, 0x41, 0x41, 0x51, 0x73,
+	0x7F, 0x08, 0x08, 0x08, 0x7F,
+	0x00, 0x41, 0x7F, 0x41, 0x00,
+	0x20, 0x40, 0x41, 0x3F, 0x01,
+	0x7F, 0x08, 0x14, 0x22, 0x41,
+	0x7F, 0x40, 0x40, 0x40, 0x40,
+	0x7F, 0x02, 0x1C, 0x02, 0x7F,
+	0x7F, 0x04, 0x08, 0x10, 0x7F,
+	0x3E, 0x41, 0x41, 0x41, 0x3E,
+	0x7F, 0x09, 0x09, 0x09, 0x06,
+	0x3E, 0x41, 0x51, 0x21, 0x5E,
+	0x7F, 0x09, 0x19, 0x29, 0x46,
+	0x26, 0x49, 0x49, 0x49, 0x32,
+	0x03, 0x01, 0x7F, 0x01, 0x03,
+	0x3F, 0x40, 0x40, 0x40, 0x3F,
+	0x1F, 0x20, 0x40, 0x20, 0x1F,
+	0x3F, 0x40, 0x38, 0x40, 0x3F,
+	0x63, 0x14, 0x08, 0x14, 0x63,
+	0x03, 0x04, 0x78, 0x04, 0x03,
+	0x61, 0x59, 0x49, 0x4D, 0x43,
+	0x00, 0x7F, 0x41, 0x41, 0x41,
+	0x02, 0x04, 0x08, 0x10, 0x20,
+	0x00, 0x41, 0x41, 0x41, 0x7F,
+	0x04, 0x02, 0x01, 0x02, 0x04,
+	0x40, 0x40, 0x40, 0x40, 0x40,
+	0x00, 0x03, 0x07, 0x08, 0x00,
+	0x20, 0x54, 0x54, 0x78, 0x40,
+	0x7F, 0x28, 0x44, 0x44, 0x38,
+	0x38, 0x44, 0x44, 0x44, 0x28,
+	0x38, 0x44, 0x44, 0x28, 0x7F,
+	0x38, 0x54, 0x54, 0x54, 0x18,
+	0x00, 0x08, 0x7E, 0x09, 0x02,
+	0x18, 0xA4, 0xA4, 0x9C, 0x78,
+	0x7F, 0x08, 0x04, 0x04, 0x78,
+	0x00, 0x44, 0x7D, 0x40, 0x00,
+	0x20, 0x40, 0x40, 0x3D, 0x00,
+	0x7F, 0x10, 0x28, 0x44, 0x00,
+	0x00, 0x41, 0x7F, 0x40, 0x00,
+	0x7C, 0x04, 0x78, 0x04, 0x78,
+	0x7C, 0x08, 0x04, 0x04, 0x78,
+	0x38, 0x44, 0x44, 0x44, 0x38,
+	0xFC, 0x18, 0x24, 0x24, 0x18,
+	0x18, 0x24, 0x24, 0x18, 0xFC,
+	0x7C, 0x08, 0x04, 0x04, 0x08,
+	0x48, 0x54, 0x54, 0x54, 0x24,
+	0x04, 0x04, 0x3F, 0x44, 0x24,
+	0x3C, 0x40, 0x40, 0x20, 0x7C,
+	0x1C, 0x20, 0x40, 0x20, 0x1C,
+	0x3C, 0x40, 0x30, 0x40, 0x3C,
+	0x44, 0x28, 0x10, 0x28, 0x44,
+	0x4C, 0x90, 0x90, 0x90, 0x7C,
+	0x44, 0x64, 0x54, 0x4C, 0x44,
+	0x00, 0x08, 0x36, 0x41, 0x00,
+	0x00, 0x00, 0x77, 0x00, 0x00,
+	0x00, 0x41, 0x36, 0x08, 0x00,
+	0x02, 0x01, 0x02, 0x04, 0x02,
+	0x3C, 0x26, 0x23, 0x26, 0x3C,
+	0x1E, 0xA1, 0xA1, 0x61, 0x12,
+	0x3A, 0x40, 0x40, 0x20, 0x7A,
+	0x38, 0x54, 0x54, 0x55, 0x59,
+	0x21, 0x55, 0x55, 0x79, 0x41,
+	0x22, 0x54, 0x54, 0x78, 0x42, // a-umlaut
+	0x21, 0x55, 0x54, 0x78, 0x40,
+	0x20, 0x54, 0x55, 0x79, 0x40,
+	0x0C, 0x1E, 0x52, 0x72, 0x12,
+	0x39, 0x55, 0x55, 0x55, 0x59,
+	0x39, 0x54, 0x54, 0x54, 0x59,
+	0x39, 0x55, 0x54, 0x54, 0x58,
+	0x00, 0x00, 0x45, 0x7C, 0x41,
+	0x00, 0x02, 0x45, 0x7D, 0x42,
+	0x00, 0x01, 0x45, 0x7C, 0x40,
+	0x7D, 0x12, 0x11, 0x12, 0x7D, // A-umlaut
+	0xF0, 0x28, 0x25, 0x28, 0xF0,
+	0x7C, 0x54, 0x55, 0x45, 0x00,
+	0x20, 0x54, 0x54, 0x7C, 0x54,
+	0x7C, 0x0A, 0x09, 0x7F, 0x49,
+	0x32, 0x49, 0x49, 0x49, 0x32,
+	0x3A, 0x44, 0x44, 0x44, 0x3A, // o-umlaut
+	0x32, 0x4A, 0x48, 0x48, 0x30,
+	0x3A, 0x41, 0x41, 0x21, 0x7A,
+	0x3A, 0x42, 0x40, 0x20, 0x78,
+	0x00, 0x9D, 0xA0, 0xA0, 0x7D,
+	0x3D, 0x42, 0x42, 0x42, 0x3D, // O-umlaut
+	0x3D, 0x40, 0x40, 0x40, 0x3D,
+	0x3C, 0x24, 0xFF, 0x24, 0x24,
+	0x48, 0x7E, 0x49, 0x43, 0x66,
+	0x2B, 0x2F, 0xFC, 0x2F, 0x2B,
+	0xFF, 0x09, 0x29, 0xF6, 0x20,
+	0xC0, 0x88, 0x7E, 0x09, 0x03,
+	0x20, 0x54, 0x54, 0x79, 0x41,
+	0x00, 0x00, 0x44, 0x7D, 0x41,
+	0x30, 0x48, 0x48, 0x4A, 0x32,
+	0x38, 0x40, 0x40, 0x22, 0x7A,
+	0x00, 0x7A, 0x0A, 0x0A, 0x72,
+	0x7D, 0x0D, 0x19, 0x31, 0x7D,
+	0x26, 0x29, 0x29, 0x2F, 0x28,
+	0x26, 0x29, 0x29, 0x29, 0x26,
+	0x30, 0x48, 0x4D, 0x40, 0x20,
+	0x38, 0x08, 0x08, 0x08, 0x08,
+	0x08, 0x08, 0x08, 0x08, 0x38,
+	0x2F, 0x10, 0xC8, 0xAC, 0xBA,
+	0x2F, 0x10, 0x28, 0x34, 0xFA,
+	0x00, 0x00, 0x7B, 0x00, 0x00,
+	0x08, 0x14, 0x2A, 0x14, 0x22,
+	0x22, 0x14, 0x2A, 0x14, 0x08,
+	0x55, 0x00, 0x55, 0x00, 0x55, // #176 (25% block) missing in old code
+	0xAA, 0x55, 0xAA, 0x55, 0xAA, // 50% block
+	0xFF, 0x55, 0xFF, 0x55, 0xFF, // 75% block
+	0x00, 0x00, 0x00, 0xFF, 0x00,
+	0x10, 0x10, 0x10, 0xFF, 0x00,
+	0x14, 0x14, 0x14, 0xFF, 0x00,
+	0x10, 0x10, 0xFF, 0x00, 0xFF,
+	0x10, 0x10, 0xF0, 0x10, 0xF0,
+	0x14, 0x14, 0x14, 0xFC, 0x00,
+	0x14, 0x14, 0xF7, 0x00, 0xFF,
+	0x00, 0x00, 0xFF, 0x00, 0xFF,
+	0x14, 0x14, 0xF4, 0x04, 0xFC,
+	0x14, 0x14, 0x17, 0x10, 0x1F,
+	0x10, 0x10, 0x1F, 0x10, 0x1F,
+	0x14, 0x14, 0x14, 0x1F, 0x00,
+	0x10, 0x10, 0x10, 0xF0, 0x00,
+	0x00, 0x00, 0x00, 0x1F, 0x10,
+	0x10, 0x10, 0x10, 0x1F, 0x10,
+	0x10, 0x10, 0x10, 0xF0, 0x10,
+	0x00, 0x00, 0x00, 0xFF, 0x10,
+	0x10, 0x10, 0x10, 0x10, 0x10,
+	0x10, 0x10, 0x10, 0xFF, 0x10,
+	0x00, 0x00, 0x00, 0xFF, 0x14,
+	0x00, 0x00, 0xFF, 0x00, 0xFF,
+	0x00, 0x00, 0x1F, 0x10, 0x17,
+	0x00, 0x00, 0xFC, 0x04, 0xF4,
+	0x14, 0x14, 0x17, 0x10, 0x17,
+	0x14, 0x14, 0xF4, 0x04, 0xF4,
+	0x00, 0x00, 0xFF, 0x00, 0xF7,
+	0x14, 0x14, 0x14, 0x14, 0x14,
+	0x14, 0x14, 0xF7, 0x00, 0xF7,
+	0x14, 0x14, 0x14, 0x17, 0x14,
+	0x10, 0x10, 0x1F, 0x10, 0x1F,
+	0x14, 0x14, 0x14, 0xF4, 0x14,
+	0x10, 0x10, 0xF0, 0x10, 0xF0,
+	0x00, 0x00, 0x1F, 0x10, 0x1F,
+	0x00, 0x00, 0x00, 0x1F, 0x14,
+	0x00, 0x00, 0x00, 0xFC, 0x14,
+	0x00, 0x00, 0xF0, 0x10, 0xF0,
+	0x10, 0x10, 0xFF, 0x10, 0xFF,
+	0x14, 0x14, 0x14, 0xFF, 0x14,
+	0x10, 0x10, 0x10, 0x1F, 0x00,
+	0x00, 0x00, 0x00, 0xF0, 0x10,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
+	0xFF, 0xFF, 0xFF, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0xFF, 0xFF,
+	0x0F, 0x0F, 0x0F, 0x0F, 0x0F,
+	0x38, 0x44, 0x44, 0x38, 0x44,
+	0xFC, 0x4A, 0x4A, 0x4A, 0x34, // sharp-s or beta
+	0x7E, 0x02, 0x02, 0x06, 0x06,
+	0x02, 0x7E, 0x02, 0x7E, 0x02,
+	0x63, 0x55, 0x49, 0x41, 0x63,
+	0x38, 0x44, 0x44, 0x3C, 0x04,
+	0x40, 0x7E, 0x20, 0x1E, 0x20,
+	0x06, 0x02, 0x7E, 0x02, 0x02,
+	0x99, 0xA5, 0xE7, 0xA5, 0x99,
+	0x1C, 0x2A, 0x49, 0x2A, 0x1C,
+	0x4C, 0x72, 0x01, 0x72, 0x4C,
+	0x30, 0x4A, 0x4D, 0x4D, 0x30,
+	0x30, 0x48, 0x78, 0x48, 0x30,
+	0xBC, 0x62, 0x5A, 0x46, 0x3D,
+	0x3E, 0x49, 0x49, 0x49, 0x00,
+	0x7E, 0x01, 0x01, 0x01, 0x7E,
+	0x2A, 0x2A, 0x2A, 0x2A, 0x2A,
+	0x44, 0x44, 0x5F, 0x44, 0x44,
+	0x40, 0x51, 0x4A, 0x44, 0x40,
+	0x40, 0x44, 0x4A, 0x51, 0x40,
+	0x00, 0x00, 0xFF, 0x01, 0x03,
+	0xE0, 0x80, 0xFF, 0x00, 0x00,
+	0x08, 0x08, 0x6B, 0x6B, 0x08,
+	0x36, 0x12, 0x36, 0x24, 0x36,
+	0x06, 0x0F, 0x09, 0x0F, 0x06,
+	0x00, 0x00, 0x18, 0x18, 0x00,
+	0x00, 0x00, 0x10, 0x10, 0x00,
+	0x30, 0x40, 0xFF, 0x01, 0x01,
+	0x00, 0x1F, 0x01, 0x01, 0x1E,
+	0x00, 0x19, 0x1D, 0x17, 0x12,
+	0x00, 0x3C, 0x3C, 0x3C, 0x3C,
+	0x00, 0x00, 0x00, 0x00, 0x00  // #255 NBSP
+};
+#endif // FONT5X7_H
diff --git a/drivers/avr/pro_micro.h b/drivers/avr/pro_micro.h
new file mode 100644
index 000000000..f9e7ed75d
--- /dev/null
+++ b/drivers/avr/pro_micro.h
@@ -0,0 +1,362 @@
+/*
+  pins_arduino.h - Pin definition functions for Arduino
+  Part of Arduino - http://www.arduino.cc/
+
+  Copyright (c) 2007 David A. Mellis
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library 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
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General
+  Public License along with this library; if not, write to the
+  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
+  Boston, MA  02111-1307  USA
+
+  $Id: wiring.h 249 2007-02-03 16:52:51Z mellis $
+*/
+
+#ifndef Pins_Arduino_h
+#define Pins_Arduino_h
+
+#include <avr/pgmspace.h>
+
+// Workaround for wrong definitions in "iom32u4.h".
+// This should be fixed in the AVR toolchain.
+#undef UHCON
+#undef UHINT
+#undef UHIEN
+#undef UHADDR
+#undef UHFNUM
+#undef UHFNUML
+#undef UHFNUMH
+#undef UHFLEN
+#undef UPINRQX
+#undef UPINTX
+#undef UPNUM
+#undef UPRST
+#undef UPCONX
+#undef UPCFG0X
+#undef UPCFG1X
+#undef UPSTAX
+#undef UPCFG2X
+#undef UPIENX
+#undef UPDATX
+#undef TCCR2A
+#undef WGM20
+#undef WGM21
+#undef COM2B0
+#undef COM2B1
+#undef COM2A0
+#undef COM2A1
+#undef TCCR2B
+#undef CS20
+#undef CS21
+#undef CS22
+#undef WGM22
+#undef FOC2B
+#undef FOC2A
+#undef TCNT2
+#undef TCNT2_0
+#undef TCNT2_1
+#undef TCNT2_2
+#undef TCNT2_3
+#undef TCNT2_4
+#undef TCNT2_5
+#undef TCNT2_6
+#undef TCNT2_7
+#undef OCR2A
+#undef OCR2_0
+#undef OCR2_1
+#undef OCR2_2
+#undef OCR2_3
+#undef OCR2_4
+#undef OCR2_5
+#undef OCR2_6
+#undef OCR2_7
+#undef OCR2B
+#undef OCR2_0
+#undef OCR2_1
+#undef OCR2_2
+#undef OCR2_3
+#undef OCR2_4
+#undef OCR2_5
+#undef OCR2_6
+#undef OCR2_7
+
+#define NUM_DIGITAL_PINS  30
+#define NUM_ANALOG_INPUTS 12
+
+#define TX_RX_LED_INIT  DDRD |= (1<<5), DDRB |= (1<<0)
+#define TXLED0          PORTD |= (1<<5)
+#define TXLED1          PORTD &= ~(1<<5)
+#define RXLED0          PORTB |= (1<<0)
+#define RXLED1          PORTB &= ~(1<<0)
+
+static const uint8_t SDA = 2;
+static const uint8_t SCL = 3;
+#define LED_BUILTIN 13
+
+// Map SPI port to 'new' pins D14..D17
+static const uint8_t SS   = 17;
+static const uint8_t MOSI = 16;
+static const uint8_t MISO = 14;
+static const uint8_t SCK  = 15;
+
+// Mapping of analog pins as digital I/O
+// A6-A11 share with digital pins
+static const uint8_t ADC0 = 18;
+static const uint8_t ADC1 = 19;
+static const uint8_t ADC2 = 20;
+static const uint8_t ADC3 = 21;
+static const uint8_t ADC4 = 22;
+static const uint8_t ADC5 = 23;
+static const uint8_t ADC6 = 24;   // D4
+static const uint8_t ADC7 = 25;   // D6
+static const uint8_t ADC8 = 26;   // D8
+static const uint8_t ADC9 = 27;   // D9
+static const uint8_t ADC10 = 28;  // D10
+static const uint8_t ADC11 = 29;  // D12
+
+#define digitalPinToPCICR(p)    ((((p) >= 8 && (p) <= 11) || ((p) >= 14 && (p) <= 17) || ((p) >= A8 && (p) <= A10)) ? (&PCICR) : ((uint8_t *)0))
+#define digitalPinToPCICRbit(p) 0
+#define digitalPinToPCMSK(p)    ((((p) >= 8 && (p) <= 11) || ((p) >= 14 && (p) <= 17) || ((p) >= A8 && (p) <= A10)) ? (&PCMSK0) : ((uint8_t *)0))
+#define digitalPinToPCMSKbit(p) ( ((p) >= 8 && (p) <= 11) ? (p) - 4 : ((p) == 14 ? 3 : ((p) == 15 ? 1 : ((p) == 16 ? 2 : ((p) == 17 ? 0 : (p - A8 + 4))))))
+
+//  __AVR_ATmega32U4__ has an unusual mapping of pins to channels
+extern const uint8_t PROGMEM analog_pin_to_channel_PGM[];
+#define analogPinToChannel(P)  ( pgm_read_byte( analog_pin_to_channel_PGM + (P) ) )
+
+#define digitalPinToInterrupt(p) ((p) == 0 ? 2 : ((p) == 1 ? 3 : ((p) == 2 ? 1 : ((p) == 3 ? 0 : ((p) == 7 ? 4 : NOT_AN_INTERRUPT)))))
+
+#ifdef ARDUINO_MAIN
+
+// On the Arduino board, digital pins are also used
+// for the analog output (software PWM).  Analog input
+// pins are a separate set.
+
+// ATMEL ATMEGA32U4 / ARDUINO LEONARDO
+//
+// D0               PD2                 RXD1/INT2
+// D1               PD3                 TXD1/INT3
+// D2               PD1     SDA         SDA/INT1
+// D3#              PD0     PWM8/SCL    OC0B/SCL/INT0
+// D4       A6      PD4                 ADC8
+// D5#              PC6     ???         OC3A/#OC4A
+// D6#      A7      PD7     FastPWM     #OC4D/ADC10
+// D7               PE6                 INT6/AIN0
+//
+// D8       A8      PB4                 ADC11/PCINT4
+// D9#      A9      PB5     PWM16       OC1A/#OC4B/ADC12/PCINT5
+// D10#     A10     PB6     PWM16       OC1B/0c4B/ADC13/PCINT6
+// D11#             PB7     PWM8/16     0C0A/OC1C/#RTS/PCINT7
+// D12      A11     PD6                 T1/#OC4D/ADC9
+// D13#             PC7     PWM10       CLK0/OC4A
+//
+// A0       D18     PF7                 ADC7
+// A1       D19     PF6                 ADC6
+// A2       D20     PF5                 ADC5
+// A3       D21     PF4                 ADC4
+// A4       D22     PF1                 ADC1
+// A5       D23     PF0                 ADC0
+//
+// New pins D14..D17 to map SPI port to digital pins
+//
+// MISO     D14     PB3                 MISO,PCINT3
+// SCK      D15     PB1                 SCK,PCINT1
+// MOSI     D16     PB2                 MOSI,PCINT2
+// SS       D17     PB0                 RXLED,SS/PCINT0
+//
+// Connected LEDs on board for TX and RX
+// TXLED    D24     PD5                 XCK1
+// RXLED    D17     PB0
+// HWB              PE2                 HWB
+
+// these arrays map port names (e.g. port B) to the
+// appropriate addresses for various functions (e.g. reading
+// and writing)
+const uint16_t PROGMEM port_to_mode_PGM[] = {
+    NOT_A_PORT,
+    NOT_A_PORT,
+    (uint16_t) &DDRB,
+    (uint16_t) &DDRC,
+    (uint16_t) &DDRD,
+    (uint16_t) &DDRE,
+    (uint16_t) &DDRF,
+};
+
+const uint16_t PROGMEM port_to_output_PGM[] = {
+    NOT_A_PORT,
+    NOT_A_PORT,
+    (uint16_t) &PORTB,
+    (uint16_t) &PORTC,
+    (uint16_t) &PORTD,
+    (uint16_t) &PORTE,
+    (uint16_t) &PORTF,
+};
+
+const uint16_t PROGMEM port_to_input_PGM[] = {
+    NOT_A_PORT,
+    NOT_A_PORT,
+    (uint16_t) &PINB,
+    (uint16_t) &PINC,
+    (uint16_t) &PIND,
+    (uint16_t) &PINE,
+    (uint16_t) &PINF,
+};
+
+const uint8_t PROGMEM digital_pin_to_port_PGM[] = {
+    PD, // D0 - PD2
+    PD, // D1 - PD3
+    PD, // D2 - PD1
+    PD, // D3 - PD0
+    PD, // D4 - PD4
+    PC, // D5 - PC6
+    PD, // D6 - PD7
+    PE, // D7 - PE6
+
+    PB, // D8 - PB4
+    PB, // D9 - PB5
+    PB, // D10 - PB6
+    PB, // D11 - PB7
+    PD, // D12 - PD6
+    PC, // D13 - PC7
+
+    PB, // D14 - MISO - PB3
+    PB, // D15 - SCK - PB1
+    PB, // D16 - MOSI - PB2
+    PB, // D17 - SS - PB0
+
+    PF, // D18 - A0 - PF7
+    PF, // D19 - A1 - PF6
+    PF, // D20 - A2 - PF5
+    PF, // D21 - A3 - PF4
+    PF, // D22 - A4 - PF1
+    PF, // D23 - A5 - PF0
+
+    PD, // D24 - PD5
+    PD, // D25 / D6 - A7 - PD7
+    PB, // D26 / D8 - A8 - PB4
+    PB, // D27 / D9 - A9 - PB5
+    PB, // D28 / D10 - A10 - PB6
+    PD, // D29 / D12 - A11 - PD6
+};
+
+const uint8_t PROGMEM digital_pin_to_bit_mask_PGM[] = {
+    _BV(2), // D0 - PD2
+    _BV(3), // D1 - PD3
+    _BV(1), // D2 - PD1
+    _BV(0), // D3 - PD0
+    _BV(4), // D4 - PD4
+    _BV(6), // D5 - PC6
+    _BV(7), // D6 - PD7
+    _BV(6), // D7 - PE6
+
+    _BV(4), // D8 - PB4
+    _BV(5), // D9 - PB5
+    _BV(6), // D10 - PB6
+    _BV(7), // D11 - PB7
+    _BV(6), // D12 - PD6
+    _BV(7), // D13 - PC7
+
+    _BV(3), // D14 - MISO - PB3
+    _BV(1), // D15 - SCK - PB1
+    _BV(2), // D16 - MOSI - PB2
+    _BV(0), // D17 - SS - PB0
+
+    _BV(7), // D18 - A0 - PF7
+    _BV(6), // D19 - A1 - PF6
+    _BV(5), // D20 - A2 - PF5
+    _BV(4), // D21 - A3 - PF4
+    _BV(1), // D22 - A4 - PF1
+    _BV(0), // D23 - A5 - PF0
+
+    _BV(5), // D24 - PD5
+    _BV(7), // D25 / D6 - A7 - PD7
+    _BV(4), // D26 / D8 - A8 - PB4
+    _BV(5), // D27 / D9 - A9 - PB5
+    _BV(6), // D28 / D10 - A10 - PB6
+    _BV(6), // D29 / D12 - A11 - PD6
+};
+
+const uint8_t PROGMEM digital_pin_to_timer_PGM[] = {
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    TIMER0B,        /* 3 */
+    NOT_ON_TIMER,
+    TIMER3A,        /* 5 */
+    TIMER4D,        /* 6 */
+    NOT_ON_TIMER,
+
+    NOT_ON_TIMER,
+    TIMER1A,        /* 9 */
+    TIMER1B,        /* 10 */
+    TIMER0A,        /* 11 */
+
+    NOT_ON_TIMER,
+    TIMER4A,        /* 13 */
+
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+    NOT_ON_TIMER,
+};
+
+const uint8_t PROGMEM analog_pin_to_channel_PGM[] = {
+    7,  // A0               PF7                 ADC7
+    6,  // A1               PF6                 ADC6
+    5,  // A2               PF5                 ADC5
+    4,  // A3               PF4                 ADC4
+    1,  // A4               PF1                 ADC1
+    0,  // A5               PF0                 ADC0
+    8,  // A6       D4      PD4                 ADC8
+    10, // A7       D6      PD7                 ADC10
+    11, // A8       D8      PB4                 ADC11
+    12, // A9       D9      PB5                 ADC12
+    13, // A10      D10     PB6                 ADC13
+    9   // A11      D12     PD6                 ADC9
+};
+
+#endif /* ARDUINO_MAIN */
+
+// These serial port names are intended to allow libraries and architecture-neutral
+// sketches to automatically default to the correct port name for a particular type
+// of use.  For example, a GPS module would normally connect to SERIAL_PORT_HARDWARE_OPEN,
+// the first hardware serial port whose RX/TX pins are not dedicated to another use.
+//
+// SERIAL_PORT_MONITOR        Port which normally prints to the Arduino Serial Monitor
+//
+// SERIAL_PORT_USBVIRTUAL     Port which is USB virtual serial
+//
+// SERIAL_PORT_LINUXBRIDGE    Port which connects to a Linux system via Bridge library
+//
+// SERIAL_PORT_HARDWARE       Hardware serial port, physical RX & TX pins.
+//
+// SERIAL_PORT_HARDWARE_OPEN  Hardware serial ports which are open for use.  Their RX & TX
+//                            pins are NOT connected to anything by default.
+#define SERIAL_PORT_MONITOR        Serial
+#define SERIAL_PORT_USBVIRTUAL     Serial
+#define SERIAL_PORT_HARDWARE       Serial1
+#define SERIAL_PORT_HARDWARE_OPEN  Serial1
+
+#endif /* Pins_Arduino_h */
diff --git a/drivers/avr/ssd1306.c b/drivers/avr/ssd1306.c
new file mode 100644
index 000000000..f9312d2dc
--- /dev/null
+++ b/drivers/avr/ssd1306.c
@@ -0,0 +1,470 @@
+#ifdef SSD1306OLED
+
+#include "ssd1306.h"
+#include "config.h"
+#include "i2c.h"
+#include <string.h>
+#include "print.h"
+#include "lets_split.h"
+#include "glcdfont.c"
+#ifdef ADAFRUIT_BLE_ENABLE
+#include "adafruit_ble.h"
+#endif
+#ifdef PROTOCOL_LUFA
+#include "lufa.h"
+#endif
+#include "sendchar.h"
+#include "pincontrol.h"
+
+//assign the right code to your layers
+#define _BASE 0
+#define _LOWER 8
+#define _RAISE 16
+#define _FNLAYER 64
+#define _NUMLAY 128
+#define _NLOWER 136
+#define _NFNLAYER 192
+#define _MOUSECURSOR 256
+#define _ADJUST 65560
+
+// Set this to 1 to help diagnose early startup problems
+// when testing power-on with ble.  Turn it off otherwise,
+// as the latency of printing most of the debug info messes
+// with the matrix scan, causing keys to drop.
+#define DEBUG_TO_SCREEN 0
+
+// Controls the SSD1306 128x32 OLED display via i2c
+
+#define i2cAddress 0x3C
+
+#define DisplayHeight 32
+#define DisplayWidth 128
+
+#define FontHeight 8
+#define FontWidth 6
+
+#define MatrixRows (DisplayHeight / FontHeight)
+#define MatrixCols (DisplayWidth / FontWidth)
+
+struct CharacterMatrix {
+  uint8_t display[MatrixRows][MatrixCols];
+  uint8_t *cursor;
+  bool dirty;
+};
+
+static struct CharacterMatrix display;
+//static uint16_t last_battery_update;
+//static uint32_t vbat;
+//#define BatteryUpdateInterval 10000 /* milliseconds */
+#define ScreenOffInterval 300000 /* milliseconds */
+#if DEBUG_TO_SCREEN
+static uint8_t displaying;
+#endif
+static uint16_t last_flush;
+
+enum ssd1306_cmds {
+  DisplayOff = 0xAE,
+  DisplayOn = 0xAF,
+
+  SetContrast = 0x81,
+  DisplayAllOnResume = 0xA4,
+
+  DisplayAllOn = 0xA5,
+  NormalDisplay = 0xA6,
+  InvertDisplay = 0xA7,
+  SetDisplayOffset = 0xD3,
+  SetComPins = 0xda,
+  SetVComDetect = 0xdb,
+  SetDisplayClockDiv = 0xD5,
+  SetPreCharge = 0xd9,
+  SetMultiPlex = 0xa8,
+  SetLowColumn = 0x00,
+  SetHighColumn = 0x10,
+  SetStartLine = 0x40,
+
+  SetMemoryMode = 0x20,
+  ColumnAddr = 0x21,
+  PageAddr = 0x22,
+
+  ComScanInc = 0xc0,
+  ComScanDec = 0xc8,
+  SegRemap = 0xa0,
+  SetChargePump = 0x8d,
+  ExternalVcc = 0x01,
+  SwitchCapVcc = 0x02,
+
+  ActivateScroll = 0x2f,
+  DeActivateScroll = 0x2e,
+  SetVerticalScrollArea = 0xa3,
+  RightHorizontalScroll = 0x26,
+  LeftHorizontalScroll = 0x27,
+  VerticalAndRightHorizontalScroll = 0x29,
+  VerticalAndLeftHorizontalScroll = 0x2a,
+};
+
+
+// Write command sequence.
+// Returns true on success.
+static inline bool _send_cmd1(uint8_t cmd) {
+  bool res = false;
+
+  if (i2c_start_write(i2cAddress)) {
+    xprintf("failed to start write to %d\n", i2cAddress);
+    goto done;
+  }
+
+  if (i2c_master_write(0x0 /* command byte follows */)) {
+    print("failed to write control byte\n");
+
+    goto done;
+  }
+
+  if (i2c_master_write(cmd)) {
+    xprintf("failed to write command %d\n", cmd);
+    goto done;
+  }
+  res = true;
+done:
+  i2c_master_stop();
+  return res;
+}
+
+// Write 2-byte command sequence.
+// Returns true on success
+static inline bool _send_cmd2(uint8_t cmd, uint8_t opr) {
+  if (!_send_cmd1(cmd)) {
+    return false;
+  }
+  return _send_cmd1(opr);
+}
+
+// Write 3-byte command sequence.
+// Returns true on success
+static inline bool _send_cmd3(uint8_t cmd, uint8_t opr1, uint8_t opr2) {
+  if (!_send_cmd1(cmd)) {
+    return false;
+  }
+  if (!_send_cmd1(opr1)) {
+    return false;
+  }
+  return _send_cmd1(opr2);
+}
+
+#define send_cmd1(c) if (!_send_cmd1(c)) {goto done;}
+#define send_cmd2(c,o) if (!_send_cmd2(c,o)) {goto done;}
+#define send_cmd3(c,o1,o2) if (!_send_cmd3(c,o1,o2)) {goto done;}
+
+static void matrix_clear(struct CharacterMatrix *matrix);
+
+static void clear_display(void) {
+  matrix_clear(&display);
+
+  // Clear all of the display bits (there can be random noise
+  // in the RAM on startup)
+  send_cmd3(PageAddr, 0, (DisplayHeight / 8) - 1);
+  send_cmd3(ColumnAddr, 0, DisplayWidth - 1);
+
+  if (i2c_start_write(i2cAddress)) {
+    goto done;
+  }
+  if (i2c_master_write(0x40)) {
+    // Data mode
+    goto done;
+  }
+  for (uint8_t row = 0; row < MatrixRows; ++row) {
+    for (uint8_t col = 0; col < DisplayWidth; ++col) {
+      i2c_master_write(0);
+    }
+  }
+
+  display.dirty = false;
+
+done:
+  i2c_master_stop();
+}
+
+#if DEBUG_TO_SCREEN
+#undef sendchar
+static int8_t capture_sendchar(uint8_t c) {
+  sendchar(c);
+  iota_gfx_write_char(c);
+
+  if (!displaying) {
+    iota_gfx_flush();
+  }
+  return 0;
+}
+#endif
+
+bool iota_gfx_init(void) {
+  bool success = false;
+
+  send_cmd1(DisplayOff);
+  send_cmd2(SetDisplayClockDiv, 0x80);
+  send_cmd2(SetMultiPlex, DisplayHeight - 1);
+
+  send_cmd2(SetDisplayOffset, 0);
+
+
+  send_cmd1(SetStartLine | 0x0);
+  send_cmd2(SetChargePump, 0x14 /* Enable */);
+  send_cmd2(SetMemoryMode, 0 /* horizontal addressing */);
+
+/// Flips the display orientation 0 degrees
+  send_cmd1(SegRemap | 0x1);
+  send_cmd1(ComScanDec);
+/*
+// the following Flip the display orientation 180 degrees
+  send_cmd1(SegRemap);
+  send_cmd1(ComScanInc);
+// end flip */
+  send_cmd2(SetComPins, 0x2);
+  send_cmd2(SetContrast, 0x8f);
+  send_cmd2(SetPreCharge, 0xf1);
+  send_cmd2(SetVComDetect, 0x40);
+  send_cmd1(DisplayAllOnResume);
+  send_cmd1(NormalDisplay);
+  send_cmd1(DeActivateScroll);
+  send_cmd1(DisplayOn);
+
+  send_cmd2(SetContrast, 0); // Dim
+
+  clear_display();
+
+  success = true;
+
+  iota_gfx_flush();
+
+#if DEBUG_TO_SCREEN
+  print_set_sendchar(capture_sendchar);
+#endif
+
+done:
+  return success;
+}
+
+bool iota_gfx_off(void) {
+  bool success = false;
+
+  send_cmd1(DisplayOff);
+  success = true;
+
+done:
+  return success;
+} 
+
+bool iota_gfx_on(void) {
+  bool success = false;
+
+  send_cmd1(DisplayOn);
+  success = true;
+
+done:
+  return success;
+}
+
+static void matrix_write_char_inner(struct CharacterMatrix *matrix, uint8_t c) {
+  *matrix->cursor = c;
+  ++matrix->cursor;
+
+  if (matrix->cursor - &matrix->display[0][0] == sizeof(matrix->display)) {
+    // We went off the end; scroll the display upwards by one line
+    memmove(&matrix->display[0], &matrix->display[1],
+            MatrixCols * (MatrixRows - 1));
+    matrix->cursor = &matrix->display[MatrixRows - 1][0];
+    memset(matrix->cursor, ' ', MatrixCols);
+  }
+}
+
+static void matrix_write_char(struct CharacterMatrix *matrix, uint8_t c) {
+  matrix->dirty = true;
+
+  if (c == '\n') {
+    // Clear to end of line from the cursor and then move to the
+    // start of the next line
+    uint8_t cursor_col = (matrix->cursor - &matrix->display[0][0]) % MatrixCols;
+
+    while (cursor_col++ < MatrixCols) {
+      matrix_write_char_inner(matrix, ' ');
+    }
+    return;
+  }
+
+  matrix_write_char_inner(matrix, c);
+}
+
+void iota_gfx_write_char(uint8_t c) {
+  matrix_write_char(&display, c);
+}
+
+static void matrix_write(struct CharacterMatrix *matrix, const char *data) {
+  const char *end = data + strlen(data);
+  while (data < end) {
+    matrix_write_char(matrix, *data);
+    ++data;
+  }
+}
+
+void iota_gfx_write(const char *data) {
+  matrix_write(&display, data);
+}
+
+static void matrix_write_P(struct CharacterMatrix *matrix, const char *data) {
+  while (true) {
+    uint8_t c = pgm_read_byte(data);
+    if (c == 0) {
+      return;
+    }
+    matrix_write_char(matrix, c);
+    ++data;
+  }
+}
+
+void iota_gfx_write_P(const char *data) {
+  matrix_write_P(&display, data);
+}
+
+static void matrix_clear(struct CharacterMatrix *matrix) {
+  memset(matrix->display, ' ', sizeof(matrix->display));
+  matrix->cursor = &matrix->display[0][0];
+  matrix->dirty = true;
+}
+
+void iota_gfx_clear_screen(void) {
+  matrix_clear(&display);
+}
+
+static void matrix_render(struct CharacterMatrix *matrix) {
+  last_flush = timer_read();
+  iota_gfx_on();
+#if DEBUG_TO_SCREEN
+  ++displaying;
+#endif
+
+  // Move to the home position
+  send_cmd3(PageAddr, 0, MatrixRows - 1);
+  send_cmd3(ColumnAddr, 0, (MatrixCols * FontWidth) - 1);
+
+  if (i2c_start_write(i2cAddress)) {
+    goto done;
+  }
+  if (i2c_master_write(0x40)) {
+    // Data mode
+    goto done;
+  }
+
+  for (uint8_t row = 0; row < MatrixRows; ++row) {
+    for (uint8_t col = 0; col < MatrixCols; ++col) {
+      const uint8_t *glyph = font + (matrix->display[row][col] * (FontWidth - 1));
+
+      for (uint8_t glyphCol = 0; glyphCol < FontWidth - 1; ++glyphCol) {
+        uint8_t colBits = pgm_read_byte(glyph + glyphCol);
+        i2c_master_write(colBits);
+      }
+
+      // 1 column of space between chars (it's not included in the glyph)
+      i2c_master_write(0);
+    }
+  }
+
+  matrix->dirty = false;
+
+done:
+  i2c_master_stop();
+#if DEBUG_TO_SCREEN
+  --displaying;
+#endif
+}
+
+void iota_gfx_flush(void) {
+  matrix_render(&display);
+}
+
+static void matrix_update(struct CharacterMatrix *dest,
+                          const struct CharacterMatrix *source) {
+  if (memcmp(dest->display, source->display, sizeof(dest->display))) {
+    memcpy(dest->display, source->display, sizeof(dest->display));
+    dest->dirty = true;
+  }
+}
+
+static void render_status_info(void) {
+#if DEBUG_TO_SCREEN
+  if (debug_enable) {
+    return;
+  }
+#endif
+
+  struct CharacterMatrix matrix;
+
+  matrix_clear(&matrix);
+  matrix_write_P(&matrix, PSTR("USB: "));
+#ifdef PROTOCOL_LUFA
+  switch (USB_DeviceState) {
+    case DEVICE_STATE_Unattached:
+      matrix_write_P(&matrix, PSTR("Unattached"));
+      break;
+    case DEVICE_STATE_Suspended:
+      matrix_write_P(&matrix, PSTR("Suspended"));
+      break;
+    case DEVICE_STATE_Configured:
+      matrix_write_P(&matrix, PSTR("Connected"));
+      break;
+    case DEVICE_STATE_Powered:
+      matrix_write_P(&matrix, PSTR("Powered"));
+      break;
+    case DEVICE_STATE_Default:
+      matrix_write_P(&matrix, PSTR("Default"));
+      break;
+    case DEVICE_STATE_Addressed:
+      matrix_write_P(&matrix, PSTR("Addressed"));
+      break;
+    default:
+      matrix_write_P(&matrix, PSTR("Invalid"));
+  }
+#endif
+
+// Define layers here, Have not worked out how to have text displayed for each layer. Copy down the number you see and add a case for it below
+
+  char buf[40];
+  snprintf(buf,sizeof(buf), "Undef-%ld", layer_state);
+  matrix_write_P(&matrix, PSTR("\n\nLayer: "));
+    switch (layer_state) {
+        case _BASE:
+           matrix_write_P(&matrix, PSTR("Default"));
+           break;
+        case _RAISE:
+           matrix_write_P(&matrix, PSTR("Raise"));
+           break;
+        case _LOWER:
+           matrix_write_P(&matrix, PSTR("Lower"));
+           break;
+        case _ADJUST:
+           matrix_write_P(&matrix, PSTR("ADJUST"));
+           break;
+        default:
+           matrix_write(&matrix, buf);
+ }
+  
+  // Host Keyboard LED Status
+  char led[40];
+    snprintf(led, sizeof(led), "\n%s  %s  %s",
+            (host_keyboard_leds() & (1<<USB_LED_NUM_LOCK)) ? "NUMLOCK" : "       ",
+            (host_keyboard_leds() & (1<<USB_LED_CAPS_LOCK)) ? "CAPS" : "    ",
+            (host_keyboard_leds() & (1<<USB_LED_SCROLL_LOCK)) ? "SCLK" : "    ");
+  matrix_write(&matrix, led);
+  matrix_update(&display, &matrix);
+}
+
+void iota_gfx_task(void) {
+  render_status_info();
+
+  if (display.dirty) {
+    iota_gfx_flush();
+  }
+
+  if (timer_elapsed(last_flush) > ScreenOffInterval) {
+    iota_gfx_off();
+  }
+}
+#endif
diff --git a/drivers/avr/ssd1306.h b/drivers/avr/ssd1306.h
new file mode 100644
index 000000000..b0c74f987
--- /dev/null
+++ b/drivers/avr/ssd1306.h
@@ -0,0 +1,17 @@
+#ifndef SSD1306_H
+#define SSD1306_H
+
+#include <stdbool.h>
+#include <stdio.h>
+
+bool iota_gfx_init(void);
+void iota_gfx_task(void);
+bool iota_gfx_off(void);
+bool iota_gfx_on(void);
+void iota_gfx_flush(void);
+void iota_gfx_write_char(uint8_t c);
+void iota_gfx_write(const char *data);
+void iota_gfx_write_P(const char *data);
+void iota_gfx_clear_screen(void);
+
+#endif
diff --git a/drivers/avr/ws2812.c b/drivers/avr/ws2812.c
new file mode 100644
index 000000000..59e032bf7
--- /dev/null
+++ b/drivers/avr/ws2812.c
@@ -0,0 +1,342 @@
+/*
+* light weight WS2812 lib V2.0b
+*
+* Controls WS2811/WS2812/WS2812B RGB-LEDs
+* Author: Tim (cpldcpu@gmail.com)
+*
+* Jan 18th, 2014  v2.0b Initial Version
+* Nov 29th, 2015  v2.3  Added SK6812RGBW support
+*
+* This program is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 2 of the License, or
+* (at your option) any later version.
+*
+* 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.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "ws2812.h"
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <util/delay.h>
+#include "debug.h"
+
+#ifdef RGBW_BB_TWI
+
+// Port for the I2C
+#define I2C_DDR DDRD
+#define I2C_PIN PIND
+#define I2C_PORT PORTD
+
+// Pins to be used in the bit banging
+#define I2C_CLK 0
+#define I2C_DAT 1
+
+#define I2C_DATA_HI()\
+I2C_DDR &= ~ (1 << I2C_DAT);\
+I2C_PORT |= (1 << I2C_DAT);
+#define I2C_DATA_LO()\
+I2C_DDR |= (1 << I2C_DAT);\
+I2C_PORT &= ~ (1 << I2C_DAT);
+
+#define I2C_CLOCK_HI()\
+I2C_DDR &= ~ (1 << I2C_CLK);\
+I2C_PORT |= (1 << I2C_CLK);
+#define I2C_CLOCK_LO()\
+I2C_DDR |= (1 << I2C_CLK);\
+I2C_PORT &= ~ (1 << I2C_CLK);
+
+#define I2C_DELAY 1
+
+void I2C_WriteBit(unsigned char c)
+{
+    if (c > 0)
+    {
+        I2C_DATA_HI();
+    }
+    else
+    {
+        I2C_DATA_LO();
+    }
+
+    I2C_CLOCK_HI();
+    _delay_us(I2C_DELAY);
+
+    I2C_CLOCK_LO();
+    _delay_us(I2C_DELAY);
+
+    if (c > 0)
+    {
+        I2C_DATA_LO();
+    }
+
+    _delay_us(I2C_DELAY);
+}
+
+// Inits bitbanging port, must be called before using the functions below
+//
+void I2C_Init(void)
+{
+    I2C_PORT &= ~ ((1 << I2C_DAT) | (1 << I2C_CLK));
+
+    I2C_CLOCK_HI();
+    I2C_DATA_HI();
+
+    _delay_us(I2C_DELAY);
+}
+
+// Send a START Condition
+//
+void I2C_Start(void)
+{
+    // set both to high at the same time
+    I2C_DDR &= ~ ((1 << I2C_DAT) | (1 << I2C_CLK));
+    _delay_us(I2C_DELAY);
+
+    I2C_DATA_LO();
+    _delay_us(I2C_DELAY);
+
+    I2C_CLOCK_LO();
+    _delay_us(I2C_DELAY);
+}
+
+// Send a STOP Condition
+//
+void I2C_Stop(void)
+{
+    I2C_CLOCK_HI();
+    _delay_us(I2C_DELAY);
+
+    I2C_DATA_HI();
+    _delay_us(I2C_DELAY);
+}
+
+// write a byte to the I2C slave device
+//
+unsigned char I2C_Write(unsigned char c)
+{
+    for (char i = 0; i < 8; i++)
+    {
+        I2C_WriteBit(c & 128);
+
+        c <<= 1;
+    }
+
+    
+    I2C_WriteBit(0);
+    _delay_us(I2C_DELAY);
+    _delay_us(I2C_DELAY);
+  
+    // _delay_us(I2C_DELAY);
+    //return I2C_ReadBit();
+    return 0;
+}
+
+
+#endif
+
+// Setleds for standard RGB
+void inline ws2812_setleds(LED_TYPE *ledarray, uint16_t leds)
+{
+   // ws2812_setleds_pin(ledarray,leds, _BV(ws2812_pin));
+   ws2812_setleds_pin(ledarray,leds, _BV(RGB_DI_PIN & 0xF));
+}
+
+void inline ws2812_setleds_pin(LED_TYPE *ledarray, uint16_t leds, uint8_t pinmask)
+{
+  // ws2812_DDRREG |= pinmask; // Enable DDR
+  // new universal format (DDR)
+  _SFR_IO8((RGB_DI_PIN >> 4) + 1) |= pinmask;
+
+  ws2812_sendarray_mask((uint8_t*)ledarray,leds+leds+leds,pinmask);
+  _delay_us(50);
+}
+
+// Setleds for SK6812RGBW
+void inline ws2812_setleds_rgbw(LED_TYPE *ledarray, uint16_t leds)
+{
+
+  #ifdef RGBW_BB_TWI
+    uint8_t sreg_prev, twcr_prev;
+    sreg_prev=SREG;
+    twcr_prev=TWCR;
+    cli();
+    TWCR &= ~(1<<TWEN);
+    I2C_Init();
+    I2C_Start();
+    I2C_Write(0x84);
+    uint16_t datlen = leds<<2;
+    uint8_t curbyte;
+    uint8_t * data = (uint8_t*)ledarray;
+    while (datlen--) {
+      curbyte=*data++;
+      I2C_Write(curbyte);
+    }
+    I2C_Stop();
+    SREG=sreg_prev;
+    TWCR=twcr_prev;
+  #endif
+
+
+  // ws2812_DDRREG |= _BV(ws2812_pin); // Enable DDR
+  // new universal format (DDR)
+  _SFR_IO8((RGB_DI_PIN >> 4) + 1) |= _BV(RGB_DI_PIN & 0xF);
+
+  ws2812_sendarray_mask((uint8_t*)ledarray,leds<<2,_BV(RGB_DI_PIN & 0xF));
+
+
+  #ifndef RGBW_BB_TWI
+    _delay_us(80);
+  #endif
+}
+
+void ws2812_sendarray(uint8_t *data,uint16_t datlen)
+{
+  ws2812_sendarray_mask(data,datlen,_BV(RGB_DI_PIN & 0xF));
+}
+
+/*
+  This routine writes an array of bytes with RGB values to the Dataout pin
+  using the fast 800kHz clockless WS2811/2812 protocol.
+*/
+
+// Timing in ns
+#define w_zeropulse   350
+#define w_onepulse    900
+#define w_totalperiod 1250
+
+// Fixed cycles used by the inner loop
+#define w_fixedlow    2
+#define w_fixedhigh   4
+#define w_fixedtotal  8
+
+// Insert NOPs to match the timing, if possible
+#define w_zerocycles    (((F_CPU/1000)*w_zeropulse          )/1000000)
+#define w_onecycles     (((F_CPU/1000)*w_onepulse    +500000)/1000000)
+#define w_totalcycles   (((F_CPU/1000)*w_totalperiod +500000)/1000000)
+
+// w1 - nops between rising edge and falling edge - low
+#define w1 (w_zerocycles-w_fixedlow)
+// w2   nops between fe low and fe high
+#define w2 (w_onecycles-w_fixedhigh-w1)
+// w3   nops to complete loop
+#define w3 (w_totalcycles-w_fixedtotal-w1-w2)
+
+#if w1>0
+  #define w1_nops w1
+#else
+  #define w1_nops  0
+#endif
+
+// The only critical timing parameter is the minimum pulse length of the "0"
+// Warn or throw error if this timing can not be met with current F_CPU settings.
+#define w_lowtime ((w1_nops+w_fixedlow)*1000000)/(F_CPU/1000)
+#if w_lowtime>550
+   #error "Light_ws2812: Sorry, the clock speed is too low. Did you set F_CPU correctly?"
+#elif w_lowtime>450
+   #warning "Light_ws2812: The timing is critical and may only work on WS2812B, not on WS2812(S)."
+   #warning "Please consider a higher clockspeed, if possible"
+#endif
+
+#if w2>0
+#define w2_nops w2
+#else
+#define w2_nops  0
+#endif
+
+#if w3>0
+#define w3_nops w3
+#else
+#define w3_nops  0
+#endif
+
+#define w_nop1  "nop      \n\t"
+#define w_nop2  "rjmp .+0 \n\t"
+#define w_nop4  w_nop2 w_nop2
+#define w_nop8  w_nop4 w_nop4
+#define w_nop16 w_nop8 w_nop8
+
+void inline ws2812_sendarray_mask(uint8_t *data,uint16_t datlen,uint8_t maskhi)
+{
+  uint8_t curbyte,ctr,masklo;
+  uint8_t sreg_prev;
+
+  // masklo  =~maskhi&ws2812_PORTREG;
+  // maskhi |=        ws2812_PORTREG;
+  masklo  =~maskhi&_SFR_IO8((RGB_DI_PIN >> 4) + 2);
+  maskhi |=        _SFR_IO8((RGB_DI_PIN >> 4) + 2);
+  sreg_prev=SREG;
+  cli();
+
+  while (datlen--) {
+    curbyte=(*data++);
+
+    asm volatile(
+    "       ldi   %0,8  \n\t"
+    "loop%=:            \n\t"
+    "       out   %2,%3 \n\t"    //  '1' [01] '0' [01] - re
+#if (w1_nops&1)
+w_nop1
+#endif
+#if (w1_nops&2)
+w_nop2
+#endif
+#if (w1_nops&4)
+w_nop4
+#endif
+#if (w1_nops&8)
+w_nop8
+#endif
+#if (w1_nops&16)
+w_nop16
+#endif
+    "       sbrs  %1,7  \n\t"    //  '1' [03] '0' [02]
+    "       out   %2,%4 \n\t"    //  '1' [--] '0' [03] - fe-low
+    "       lsl   %1    \n\t"    //  '1' [04] '0' [04]
+#if (w2_nops&1)
+  w_nop1
+#endif
+#if (w2_nops&2)
+  w_nop2
+#endif
+#if (w2_nops&4)
+  w_nop4
+#endif
+#if (w2_nops&8)
+  w_nop8
+#endif
+#if (w2_nops&16)
+  w_nop16
+#endif
+    "       out   %2,%4 \n\t"    //  '1' [+1] '0' [+1] - fe-high
+#if (w3_nops&1)
+w_nop1
+#endif
+#if (w3_nops&2)
+w_nop2
+#endif
+#if (w3_nops&4)
+w_nop4
+#endif
+#if (w3_nops&8)
+w_nop8
+#endif
+#if (w3_nops&16)
+w_nop16
+#endif
+
+    "       dec   %0    \n\t"    //  '1' [+2] '0' [+2]
+    "       brne  loop%=\n\t"    //  '1' [+3] '0' [+4]
+    :	"=&d" (ctr)
+    :	"r" (curbyte), "I" (_SFR_IO_ADDR(_SFR_IO8((RGB_DI_PIN >> 4) + 2))), "r" (maskhi), "r" (masklo)
+    );
+  }
+
+  SREG=sreg_prev;
+}
diff --git a/drivers/avr/ws2812.h b/drivers/avr/ws2812.h
new file mode 100644
index 000000000..60924a0fb
--- /dev/null
+++ b/drivers/avr/ws2812.h
@@ -0,0 +1,91 @@
+/*
+ * light weight WS2812 lib include
+ *
+ * Version 2.3  - Nev 29th 2015
+ * Author: Tim (cpldcpu@gmail.com)
+ *
+ * Please do not change this file! All configuration is handled in "ws2812_config.h"
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef LIGHT_WS2812_H_
+#define LIGHT_WS2812_H_
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+//#include "ws2812_config.h"
+//#include "i2cmaster.h"
+
+#ifdef RGBW
+  #define LED_TYPE struct cRGBW
+#else
+  #define LED_TYPE struct cRGB
+#endif
+
+
+/*
+ *  Structure of the LED array
+ *
+ * cRGB:     RGB  for WS2812S/B/C/D, SK6812, SK6812Mini, SK6812WWA, APA104, APA106
+ * cRGBW:    RGBW for SK6812RGBW
+ */
+
+struct cRGB  { uint8_t g; uint8_t r; uint8_t b; };
+struct cRGBW { uint8_t g; uint8_t r; uint8_t b; uint8_t w;};
+
+
+
+/* User Interface
+ *
+ * Input:
+ *         ledarray:           An array of GRB data describing the LED colors
+ *         number_of_leds:     The number of LEDs to write
+ *         pinmask (optional): Bitmask describing the output bin. e.g. _BV(PB0)
+ *
+ * The functions will perform the following actions:
+ *         - Set the data-out pin as output
+ *         - Send out the LED data
+ *         - Wait 50�s to reset the LEDs
+ */
+
+void ws2812_setleds     (LED_TYPE *ledarray, uint16_t number_of_leds);
+void ws2812_setleds_pin (LED_TYPE *ledarray, uint16_t number_of_leds,uint8_t pinmask);
+void ws2812_setleds_rgbw(LED_TYPE *ledarray, uint16_t number_of_leds);
+
+/*
+ * Old interface / Internal functions
+ *
+ * The functions take a byte-array and send to the data output as WS2812 bitstream.
+ * The length is the number of bytes to send - three per LED.
+ */
+
+void ws2812_sendarray     (uint8_t *array,uint16_t length);
+void ws2812_sendarray_mask(uint8_t *array,uint16_t length, uint8_t pinmask);
+
+
+/*
+ * Internal defines
+ */
+#ifndef CONCAT
+#define CONCAT(a, b)            a ## b
+#endif
+#ifndef CONCAT_EXP
+#define CONCAT_EXP(a, b)   CONCAT(a, b)
+#endif
+
+// #define ws2812_PORTREG  CONCAT_EXP(PORT,ws2812_port)
+// #define ws2812_DDRREG   CONCAT_EXP(DDR,ws2812_port)
+
+#endif /* LIGHT_WS2812_H_ */
diff --git a/drivers/ugfx/gdisp/is31fl3731c/board_is31fl3731c_template.h b/drivers/ugfx/gdisp/is31fl3731c/board_is31fl3731c_template.h
new file mode 100644
index 000000000..f248cc25b
--- /dev/null
+++ b/drivers/ugfx/gdisp/is31fl3731c/board_is31fl3731c_template.h
@@ -0,0 +1,110 @@
+/*
+Copyright 2016 Fred Sundvik <fsundvik@gmail.com>
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 2 of the License, or
+(at your option) any later version.
+
+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.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _GDISP_LLD_BOARD_H
+#define _GDISP_LLD_BOARD_H
+
+static const I2CConfig i2ccfg = {
+  400000 // clock speed (Hz); 400kHz max for IS31
+};
+
+static const uint8_t led_mask[] = {
+	0xFF, 0x00, /* C1-1 -> C1-16 */
+	0xFF, 0x00, /* C2-1 -> C2-16 */
+	0xFF, 0x00, /* C3-1 -> C3-16 */
+	0xFF, 0x00, /* C4-1 -> C4-16 */
+	0x3F, 0x00, /* C5-1 -> C5-16 */
+	0x00, 0x00, /* C6-1 -> C6-16 */
+	0x00, 0x00, /* C7-1 -> C7-16 */
+	0x00, 0x00, /* C8-1 -> C8-16 */
+	0x00, 0x00, /* C9-1 -> C9-16 */
+};
+
+// The address of the LED
+#define LA(c, r) (c + r * 16 )
+// Need to be an address that is not mapped, but inside the range of the controller matrix
+#define NA LA(8, 8)
+
+// The numbers in the comments are the led numbers DXX on the PCB
+// The mapping is taken from the schematic of left hand side
+static const uint8_t led_mapping[GDISP_SCREEN_HEIGHT][GDISP_SCREEN_WIDTH] = {
+//   45        44        43        42        41        40        39
+   { LA(1, 1), LA(1, 0), LA(0, 4), LA(0, 3), LA(0, 2), LA(0, 1), LA(0, 0)},
+//   52        51        50        49        48        47        46
+   { LA(2, 3), LA(2, 2), LA(2, 1), LA(2, 0), LA(1, 4), LA(1, 3), LA(1, 2) },
+//   58        57        56        55        54        53        N/A
+   { LA(3, 4), LA(3, 3), LA(3, 2), LA(3, 1), LA(3, 0), LA(2, 4), NA },
+//   67        66        65        64        63        62        61
+   { LA(5, 3), LA(5, 2), LA(5, 1), LA(5, 0), LA(4, 4), LA(4, 3), LA(4, 2) },
+//   76        75        74        73        72        60        59
+   { LA(7, 3), LA(7, 2), LA(7, 1), LA(7, 0), LA(6, 3), LA(4, 1), LA(4, 0) },
+//   N/A       N/A       N/A       N/A       N/A       N/A       68
+   { NA,       NA,       NA,       NA,       NA,       NA,       LA(5, 4) },
+//   N/A       N/A       N/A       N/A       71        70        69
+   { NA,       NA,       NA,       NA,       LA(6, 2), LA(6, 1), LA(6, 0) },
+};
+
+
+#define IS31_ADDR_DEFAULT 0x74 // AD connected to GND
+#define IS31_TIMEOUT 5000
+
+static GFXINLINE void init_board(GDisplay *g) {
+    (void) g;
+    /* I2C pins */
+    palSetPadMode(GPIOB, 0, PAL_MODE_ALTERNATIVE_2); // PTB0/I2C0/SCL
+    palSetPadMode(GPIOB, 1, PAL_MODE_ALTERNATIVE_2); // PTB1/I2C0/SDA
+    palSetPadMode(GPIOB, 16, PAL_MODE_OUTPUT_PUSHPULL);
+    palClearPad(GPIOB, 16);
+    /* start I2C */
+    i2cStart(&I2CD1, &i2ccfg);
+    // try high drive (from kiibohd)
+    I2CD1.i2c->C2 |= I2Cx_C2_HDRS;
+    // try glitch fixing (from kiibohd)
+    I2CD1.i2c->FLT = 4;
+}
+
+static GFXINLINE void post_init_board(GDisplay *g) {
+	(void) g;
+}
+
+static GFXINLINE const uint8_t* get_led_mask(GDisplay* g) {
+    (void) g;
+    return led_mask;
+}
+
+static GFXINLINE uint8_t get_led_address(GDisplay* g, uint16_t x, uint16_t y)
+{
+    (void) g;
+    return led_mapping[y][x];
+}
+
+static GFXINLINE void set_hardware_shutdown(GDisplay* g, bool shutdown) {
+    (void) g;
+    if(!shutdown) {
+        palSetPad(GPIOB, 16);
+    }
+    else {
+        palClearPad(GPIOB, 16);
+    }
+}
+
+static GFXINLINE void write_data(GDisplay *g, uint8_t* data, uint16_t length) {
+	(void) g;
+	i2cMasterTransmitTimeout(&I2CD1, IS31_ADDR_DEFAULT, data, length, 0, 0, US2ST(IS31_TIMEOUT));
+}
+
+#endif /* _GDISP_LLD_BOARD_H */
diff --git a/drivers/ugfx/gdisp/is31fl3731c/driver.mk b/drivers/ugfx/gdisp/is31fl3731c/driver.mk
new file mode 100644
index 000000000..4364787c9
--- /dev/null
+++ b/drivers/ugfx/gdisp/is31fl3731c/driver.mk
@@ -0,0 +1,3 @@
+GFXINC += drivers/ugfx/gdisp/is31fl3731c
+GFXSRC += drivers/ugfx/gdisp/is31fl3731c/gdisp_is31fl3731c.c
+GDISP_DRIVER_LIST += GDISPVMT_IS31FL3731C_QMK
\ No newline at end of file
diff --git a/drivers/ugfx/gdisp/is31fl3731c/gdisp_is31fl3731c.c b/drivers/ugfx/gdisp/is31fl3731c/gdisp_is31fl3731c.c
new file mode 100644
index 000000000..917adadb8
--- /dev/null
+++ b/drivers/ugfx/gdisp/is31fl3731c/gdisp_is31fl3731c.c
@@ -0,0 +1,308 @@
+/*
+Copyright 2016 Fred Sundvik <fsundvik@gmail.com>
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 2 of the License, or
+(at your option) any later version.
+
+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.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "gfx.h"
+
+#if GFX_USE_GDISP
+
+#define GDISP_DRIVER_VMT          GDISPVMT_IS31FL3731C_QMK
+#define GDISP_SCREEN_HEIGHT       LED_HEIGHT
+#define GDISP_SCREEN_WIDTH        LED_WIDTH
+
+#include "gdisp_lld_config.h"
+#include "src/gdisp/gdisp_driver.h"
+
+#include "board_is31fl3731c.h"
+
+
+// Can't include led_tables from here
+extern const uint8_t CIE1931_CURVE[];
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+#ifndef GDISP_INITIAL_CONTRAST
+    #define GDISP_INITIAL_CONTRAST    0
+#endif
+#ifndef GDISP_INITIAL_BACKLIGHT
+    #define GDISP_INITIAL_BACKLIGHT   0
+#endif
+
+#define GDISP_FLG_NEEDFLUSH           (GDISP_FLG_DRIVER<<0)
+
+#define IS31_ADDR_DEFAULT 0x74
+
+#define IS31_REG_CONFIG   0x00
+// bits in reg
+#define IS31_REG_CONFIG_PICTUREMODE   0x00
+#define IS31_REG_CONFIG_AUTOPLAYMODE  0x08
+#define IS31_REG_CONFIG_AUDIOPLAYMODE 0x18
+// D2:D0 bits are starting frame for autoplay mode
+
+#define IS31_REG_PICTDISP 0x01 // D2:D0 frame select for picture mode
+
+#define IS31_REG_AUTOPLAYCTRL1 0x02
+// D6:D4 number of loops (000=infty)
+// D2:D0 number of frames to be used
+
+#define IS31_REG_AUTOPLAYCTRL2 0x03 // D5:D0 delay time (*11ms)
+
+#define IS31_REG_DISPLAYOPT 0x05
+#define IS31_REG_DISPLAYOPT_INTENSITY_SAME 0x20 // same intensity for all frames
+#define IS31_REG_DISPLAYOPT_BLINK_ENABLE 0x8
+// D2:D0 bits blink period time (*0.27s)
+
+#define IS31_REG_AUDIOSYNC 0x06
+#define IS31_REG_AUDIOSYNC_ENABLE 0x1
+
+#define IS31_REG_FRAMESTATE 0x07
+
+#define IS31_REG_BREATHCTRL1 0x08
+// D6:D4 fade out time (26ms*2^i)
+// D2:D0 fade in time (26ms*2^i)
+
+#define IS31_REG_BREATHCTRL2 0x09
+#define IS31_REG_BREATHCTRL2_ENABLE 0x10
+// D2:D0 extinguish time (3.5ms*2^i)
+
+#define IS31_REG_SHUTDOWN 0x0A
+#define IS31_REG_SHUTDOWN_OFF 0x0
+#define IS31_REG_SHUTDOWN_ON 0x1
+
+#define IS31_REG_AGCCTRL 0x0B
+#define IS31_REG_ADCRATE 0x0C
+
+#define IS31_COMMANDREGISTER 0xFD
+#define IS31_FUNCTIONREG 0x0B    // helpfully called 'page nine'
+#define IS31_FUNCTIONREG_SIZE 0xD
+
+#define IS31_FRAME_SIZE 0xB4
+
+#define IS31_PWM_REG 0x24
+#define IS31_PWM_SIZE 0x90
+
+#define IS31_LED_MASK_SIZE 0x12
+
+#define IS31
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+typedef struct{
+    uint8_t write_buffer_offset;
+    uint8_t write_buffer[IS31_FRAME_SIZE];
+    uint8_t frame_buffer[GDISP_SCREEN_HEIGHT * GDISP_SCREEN_WIDTH];
+    uint8_t page;
+}__attribute__((__packed__)) PrivData;
+
+// Some common routines and macros
+#define PRIV(g)                         ((PrivData*)g->priv)
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+static GFXINLINE void write_page(GDisplay* g, uint8_t page) {
+    uint8_t tx[2] __attribute__((aligned(2)));
+    tx[0] = IS31_COMMANDREGISTER;
+    tx[1] = page;
+    write_data(g, tx, 2);
+}
+
+static GFXINLINE void write_register(GDisplay* g, uint8_t page, uint8_t reg, uint8_t data) {
+    uint8_t tx[2] __attribute__((aligned(2)));
+    tx[0] = reg;
+    tx[1] = data;
+    write_page(g, page);
+    write_data(g, tx, 2);
+}
+
+static GFXINLINE void write_ram(GDisplay *g, uint8_t page, uint16_t offset, uint16_t length) {
+    PRIV(g)->write_buffer_offset = offset;
+    write_page(g, page);
+    write_data(g, (uint8_t*)PRIV(g), length + 1);
+}
+
+LLDSPEC bool_t gdisp_lld_init(GDisplay *g) {
+    // The private area is the display surface.
+    g->priv = gfxAlloc(sizeof(PrivData));
+    __builtin_memset(PRIV(g), 0, sizeof(PrivData));
+    PRIV(g)->page = 0;
+
+    // Initialise the board interface
+    init_board(g);
+    gfxSleepMilliseconds(10);
+
+    // zero function page, all registers (assuming full_page is all zeroes)
+    write_ram(g, IS31_FUNCTIONREG, 0, IS31_FUNCTIONREG_SIZE);
+    set_hardware_shutdown(g, false);
+    gfxSleepMilliseconds(10);
+    // software shutdown
+    write_register(g, IS31_FUNCTIONREG, IS31_REG_SHUTDOWN, IS31_REG_SHUTDOWN_OFF);
+    gfxSleepMilliseconds(10);
+    // zero function page, all registers
+    write_ram(g, IS31_FUNCTIONREG, 0, IS31_FUNCTIONREG_SIZE);
+    gfxSleepMilliseconds(10);
+
+
+    // zero all LED registers on all 8 pages, and enable the mask
+    __builtin_memcpy(PRIV(g)->write_buffer, get_led_mask(g), IS31_LED_MASK_SIZE);
+    for(uint8_t i=0; i<8; i++) {
+        write_ram(g, i, 0, IS31_FRAME_SIZE);
+        gfxSleepMilliseconds(1);
+    }
+
+    // software shutdown disable (i.e. turn stuff on)
+    write_register(g, IS31_FUNCTIONREG, IS31_REG_SHUTDOWN, IS31_REG_SHUTDOWN_OFF);
+    gfxSleepMilliseconds(10);
+
+    // Finish Init
+    post_init_board(g);
+
+    /* Initialise the GDISP structure */
+    g->g.Width = GDISP_SCREEN_WIDTH;
+    g->g.Height = GDISP_SCREEN_HEIGHT;
+    g->g.Orientation = GDISP_ROTATE_0;
+    g->g.Powermode = powerOff;
+    g->g.Backlight = GDISP_INITIAL_BACKLIGHT;
+    g->g.Contrast = GDISP_INITIAL_CONTRAST;
+    return TRUE;
+}
+
+#if GDISP_HARDWARE_FLUSH
+    LLDSPEC void gdisp_lld_flush(GDisplay *g) {
+        // Don't flush if we don't need it.
+        if (!(g->flags & GDISP_FLG_NEEDFLUSH))
+            return;
+
+        PRIV(g)->page++;
+        PRIV(g)->page %= 2;
+        // TODO: some smarter algorithm for this
+        // We should run only one physical page at a time
+        // This way we don't need to send so much data, and
+        // we could use slightly less memory
+        uint8_t* src = PRIV(g)->frame_buffer;
+        for (int y=0;y<GDISP_SCREEN_HEIGHT;y++) {
+            for (int x=0;x<GDISP_SCREEN_WIDTH;x++) {
+                uint8_t val = (uint16_t)*src * g->g.Backlight / 100;
+                PRIV(g)->write_buffer[get_led_address(g, x, y)]=CIE1931_CURVE[val];
+                ++src;
+            }
+        }
+        write_ram(g, PRIV(g)->page, IS31_PWM_REG, IS31_PWM_SIZE);
+        gfxSleepMilliseconds(1);
+        write_register(g, IS31_FUNCTIONREG, IS31_REG_PICTDISP, PRIV(g)->page);
+
+        g->flags &= ~GDISP_FLG_NEEDFLUSH;
+    }
+#endif
+
+#if GDISP_HARDWARE_DRAWPIXEL
+    LLDSPEC void gdisp_lld_draw_pixel(GDisplay *g) {
+        coord_t        x, y;
+
+        switch(g->g.Orientation) {
+        default:
+        case GDISP_ROTATE_0:
+            x = g->p.x;
+            y = g->p.y;
+            break;
+        case GDISP_ROTATE_180:
+            x = GDISP_SCREEN_WIDTH-1 - g->p.x;
+            y = g->p.y;
+            break;
+        }
+        PRIV(g)->frame_buffer[y * GDISP_SCREEN_WIDTH + x] = gdispColor2Native(g->p.color);
+        g->flags |= GDISP_FLG_NEEDFLUSH;
+    }
+#endif
+
+#if GDISP_HARDWARE_PIXELREAD
+    LLDSPEC color_t gdisp_lld_get_pixel_color(GDisplay *g) {
+        coord_t        x, y;
+
+        switch(g->g.Orientation) {
+        default:
+        case GDISP_ROTATE_0:
+            x = g->p.x;
+            y = g->p.y;
+            break;
+        case GDISP_ROTATE_180:
+            x = GDISP_SCREEN_WIDTH-1 - g->p.x;
+            y = g->p.y;
+            break;
+        }
+        return gdispNative2Color(PRIV(g)->frame_buffer[y * GDISP_SCREEN_WIDTH + x]);
+    }
+#endif
+
+#if GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL
+    LLDSPEC void gdisp_lld_control(GDisplay *g) {
+        switch(g->p.x) {
+        case GDISP_CONTROL_POWER:
+            if (g->g.Powermode == (powermode_t)g->p.ptr)
+                return;
+            switch((powermode_t)g->p.ptr) {
+            case powerOff:
+            case powerSleep:
+            case powerDeepSleep:
+                write_register(g, IS31_FUNCTIONREG, IS31_REG_SHUTDOWN, IS31_REG_SHUTDOWN_OFF);
+                break;
+            case powerOn:
+                write_register(g, IS31_FUNCTIONREG, IS31_REG_SHUTDOWN, IS31_REG_SHUTDOWN_ON);
+                break;
+            default:
+                return;
+            }
+            g->g.Powermode = (powermode_t)g->p.ptr;
+            return;
+
+        case GDISP_CONTROL_ORIENTATION:
+            if (g->g.Orientation == (orientation_t)g->p.ptr)
+                return;
+            switch((orientation_t)g->p.ptr) {
+            /* Rotation is handled by the drawing routines */
+            case GDISP_ROTATE_0:
+            case GDISP_ROTATE_180:
+                g->g.Height = GDISP_SCREEN_HEIGHT;
+                g->g.Width = GDISP_SCREEN_WIDTH;
+                break;
+            case GDISP_ROTATE_90:
+            case GDISP_ROTATE_270:
+                g->g.Height = GDISP_SCREEN_WIDTH;
+                g->g.Width = GDISP_SCREEN_HEIGHT;
+                break;
+            default:
+                return;
+            }
+            g->g.Orientation = (orientation_t)g->p.ptr;
+            return;
+
+        case GDISP_CONTROL_BACKLIGHT:
+            if (g->g.Backlight == (unsigned)g->p.ptr)
+                return;
+            unsigned val = (unsigned)g->p.ptr;
+            g->g.Backlight = val > 100 ? 100 : val;
+            g->flags |= GDISP_FLG_NEEDFLUSH;
+            return;
+        }
+    }
+#endif // GDISP_NEED_CONTROL
+
+#endif // GFX_USE_GDISP
diff --git a/drivers/ugfx/gdisp/is31fl3731c/gdisp_lld_config.h b/drivers/ugfx/gdisp/is31fl3731c/gdisp_lld_config.h
new file mode 100644
index 000000000..588d688cf
--- /dev/null
+++ b/drivers/ugfx/gdisp/is31fl3731c/gdisp_lld_config.h
@@ -0,0 +1,36 @@
+/*
+Copyright 2016 Fred Sundvik <fsundvik@gmail.com>
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 2 of the License, or
+(at your option) any later version.
+
+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.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _GDISP_LLD_CONFIG_H
+#define _GDISP_LLD_CONFIG_H
+
+#if GFX_USE_GDISP
+
+/*===========================================================================*/
+/* Driver hardware support.                                                  */
+/*===========================================================================*/
+
+#define GDISP_HARDWARE_FLUSH            TRUE        // This controller requires flushing
+#define GDISP_HARDWARE_DRAWPIXEL        TRUE
+#define GDISP_HARDWARE_PIXELREAD        TRUE
+#define GDISP_HARDWARE_CONTROL          TRUE
+
+#define GDISP_LLD_PIXELFORMAT           GDISP_PIXELFORMAT_GRAY256
+
+#endif    /* GFX_USE_GDISP */
+
+#endif    /* _GDISP_LLD_CONFIG_H */
diff --git a/drivers/ugfx/gdisp/st7565/board_st7565_template.h b/drivers/ugfx/gdisp/st7565/board_st7565_template.h
new file mode 100644
index 000000000..9ab636c95
--- /dev/null
+++ b/drivers/ugfx/gdisp/st7565/board_st7565_template.h
@@ -0,0 +1,113 @@
+/*
+ * This file is subject to the terms of the GFX License. If a copy of
+ * the license was not distributed with this file, you can obtain one at:
+ *
+ *              http://ugfx.org/license.html
+ */
+
+#ifndef _GDISP_LLD_BOARD_H
+#define _GDISP_LLD_BOARD_H
+
+#define ST7565_LCD_BIAS         ST7565_LCD_BIAS_9 // actually 6
+#define ST7565_ADC              ST7565_ADC_NORMAL
+#define ST7565_COM_SCAN         ST7565_COM_SCAN_DEC
+#define ST7565_PAGE_ORDER       0,1,2,3
+/*
+ * Custom page order for several LCD boards, e.g. HEM12864-99
+ * #define ST7565_PAGE_ORDER       4,5,6,7,0,1,2,3
+ */
+
+#define ST7565_GPIOPORT GPIOC
+#define ST7565_PORT PORTC
+#define ST7565_A0_PIN 7
+#define ST7565_RST_PIN 8
+#define ST7565_MOSI_PIN 6
+#define ST7565_SLCK_PIN 5
+#define ST7565_SS_PIN 4
+
+#define palSetPadModeRaw(portname, bits) \
+    ST7565_PORT->PCR[ST7565_##portname##_PIN] = bits
+
+#define palSetPadModeNamed(portname, portmode) \
+    palSetPadMode(ST7565_GPIOPORT, ST7565_##portname##_PIN, portmode)
+
+#define ST7565_SPI_MODE PORTx_PCRn_DSE | PORTx_PCRn_MUX(2)
+// DSPI Clock and Transfer Attributes
+// Frame Size: 8 bits
+// MSB First
+// CLK Low by default
+static const SPIConfig spi1config = {
+   // Operation complete callback or @p NULL.
+  .end_cb = NULL,
+   //The chip select line port - when not using pcs.
+  .ssport = ST7565_GPIOPORT,
+   // brief The chip select line pad number - when not using pcs.
+  .sspad=ST7565_SS_PIN,
+   // SPI initialization data.
+  .tar0 =
+    SPIx_CTARn_FMSZ(7) // Frame size = 8 bytes
+    | SPIx_CTARn_ASC(1) // After SCK Delay Scaler (min 50 ns) = 55.56ns
+    | SPIx_CTARn_DT(0) // Delay After Transfer Scaler (no minimum)= 27.78ns
+    | SPIx_CTARn_CSSCK(0) // PCS to SCK Delay Scaler (min 20 ns) = 27.78ns
+    | SPIx_CTARn_PBR(0) // Baud Rate Prescaler = 2
+    | SPIx_CTARn_BR(0) // Baud rate (min 50ns) = 55.56ns
+};
+
+static GFXINLINE void acquire_bus(GDisplay *g) {
+    (void) g;
+    // Only the LCD is using the SPI bus, so no need to acquire
+    // spiAcquireBus(&SPID1);
+    spiSelect(&SPID1);
+}
+
+static GFXINLINE void release_bus(GDisplay *g) {
+    (void) g;
+    // Only the LCD is using the SPI bus, so no need to release
+    //spiReleaseBus(&SPID1);
+    spiUnselect(&SPID1);
+}
+
+static GFXINLINE void init_board(GDisplay *g) {
+    (void) g;
+    palSetPadModeNamed(A0, PAL_MODE_OUTPUT_PUSHPULL);
+    palSetPad(ST7565_GPIOPORT, ST7565_A0_PIN);
+    palSetPadModeNamed(RST, PAL_MODE_OUTPUT_PUSHPULL);
+    palSetPad(ST7565_GPIOPORT, ST7565_RST_PIN);
+    palSetPadModeRaw(MOSI, ST7565_SPI_MODE);
+    palSetPadModeRaw(SLCK, ST7565_SPI_MODE);
+    palSetPadModeNamed(SS, PAL_MODE_OUTPUT_PUSHPULL);
+
+    spiInit();
+    spiStart(&SPID1, &spi1config);
+    release_bus(g);
+}
+
+static GFXINLINE void post_init_board(GDisplay *g) {
+    (void) g;
+}
+
+static GFXINLINE void setpin_reset(GDisplay *g, bool_t state) {
+    (void) g;
+    if (state) {
+        palClearPad(ST7565_GPIOPORT, ST7565_RST_PIN);
+    }
+    else {
+        palSetPad(ST7565_GPIOPORT, ST7565_RST_PIN);
+    }
+}
+
+static GFXINLINE void enter_data_mode(GDisplay *g) {
+    palSetPad(ST7565_GPIOPORT, ST7565_A0_PIN);
+}
+
+static GFXINLINE void enter_cmd_mode(GDisplay *g) {
+    palClearPad(ST7565_GPIOPORT, ST7565_A0_PIN);
+}
+
+
+static GFXINLINE void write_data(GDisplay *g, uint8_t* data, uint16_t length) {
+    (void) g;
+    spiSend(&SPID1, length, data);
+}
+
+#endif /* _GDISP_LLD_BOARD_H */
diff --git a/drivers/ugfx/gdisp/st7565/driver.mk b/drivers/ugfx/gdisp/st7565/driver.mk
new file mode 100644
index 000000000..31fc8f1c7
--- /dev/null
+++ b/drivers/ugfx/gdisp/st7565/driver.mk
@@ -0,0 +1,3 @@
+GFXINC += drivers/ugfx/gdisp/st7565
+GFXSRC += drivers/ugfx/gdisp/st7565/gdisp_lld_ST7565.c
+GDISP_DRIVER_LIST += GDISPVMT_ST7565_QMK
\ No newline at end of file
diff --git a/drivers/ugfx/gdisp/st7565/gdisp_lld_ST7565.c b/drivers/ugfx/gdisp/st7565/gdisp_lld_ST7565.c
new file mode 100644
index 000000000..c38194b06
--- /dev/null
+++ b/drivers/ugfx/gdisp/st7565/gdisp_lld_ST7565.c
@@ -0,0 +1,329 @@
+/*
+ * This file is subject to the terms of the GFX License. If a copy of
+ * the license was not distributed with this file, you can obtain one at:
+ *
+ *              http://ugfx.org/license.html
+ */
+
+#include "gfx.h"
+
+#if GFX_USE_GDISP
+
+#define GDISP_DRIVER_VMT            GDISPVMT_ST7565_QMK
+#include "gdisp_lld_config.h"
+#include "src/gdisp/gdisp_driver.h"
+
+#include "board_st7565.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+#ifndef GDISP_SCREEN_HEIGHT
+#define GDISP_SCREEN_HEIGHT         LCD_HEIGHT
+#endif
+#ifndef GDISP_SCREEN_WIDTH
+#define GDISP_SCREEN_WIDTH          LCD_WIDTH
+#endif
+#ifndef GDISP_INITIAL_CONTRAST
+#define GDISP_INITIAL_CONTRAST      35
+#endif
+#ifndef GDISP_INITIAL_BACKLIGHT
+#define GDISP_INITIAL_BACKLIGHT     100
+#endif
+
+#define GDISP_FLG_NEEDFLUSH         (GDISP_FLG_DRIVER<<0)
+
+#include "st7565.h"
+
+/*===========================================================================*/
+/* Driver config defaults for backward compatibility.                        */
+/*===========================================================================*/
+#ifndef ST7565_LCD_BIAS
+#define ST7565_LCD_BIAS         ST7565_LCD_BIAS_7
+#endif
+#ifndef ST7565_ADC
+#define ST7565_ADC              ST7565_ADC_NORMAL
+#endif
+#ifndef ST7565_COM_SCAN
+#define ST7565_COM_SCAN         ST7565_COM_SCAN_INC
+#endif
+#ifndef ST7565_PAGE_ORDER
+#define ST7565_PAGE_ORDER       0,1,2,3
+#endif
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+typedef struct{
+    bool_t buffer2;
+    uint8_t data_pos;
+    uint8_t data[16];
+    uint8_t ram[GDISP_SCREEN_HEIGHT * GDISP_SCREEN_WIDTH / 8];
+}PrivData;
+
+// Some common routines and macros
+#define PRIV(g)                         ((PrivData*)g->priv)
+#define RAM(g)                          (PRIV(g)->ram)
+
+static GFXINLINE void write_cmd(GDisplay* g, uint8_t cmd) {
+    PRIV(g)->data[PRIV(g)->data_pos++] = cmd;
+}
+
+static GFXINLINE void flush_cmd(GDisplay* g) {
+    write_data(g, PRIV(g)->data, PRIV(g)->data_pos);
+    PRIV(g)->data_pos = 0;
+}
+
+#define write_cmd2(g, cmd1, cmd2)        { write_cmd(g, cmd1); write_cmd(g, cmd2); }
+#define write_cmd3(g, cmd1, cmd2, cmd3)  { write_cmd(g, cmd1); write_cmd(g, cmd2); write_cmd(g, cmd3); }
+
+// Some common routines and macros
+#define delay(us)           gfxSleepMicroseconds(us)
+#define delay_ms(ms)        gfxSleepMilliseconds(ms)
+
+#define xyaddr(x, y)        ((x) + ((y)>>3)*GDISP_SCREEN_WIDTH)
+#define xybit(y)            (1<<((y)&7))
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/*
+ * As this controller can't update on a pixel boundary we need to maintain the
+ * the entire display surface in memory so that we can do the necessary bit
+ * operations. Fortunately it is a small display in monochrome.
+ * 64 * 128 / 8 = 1024 bytes.
+ */
+
+LLDSPEC bool_t gdisp_lld_init(GDisplay *g) {
+    // The private area is the display surface.
+    g->priv = gfxAlloc(sizeof(PrivData));
+    PRIV(g)->buffer2 = false;
+    PRIV(g)->data_pos = 0;
+
+    // Initialise the board interface
+    init_board(g);
+
+    // Hardware reset
+    setpin_reset(g, TRUE);
+    gfxSleepMilliseconds(20);
+    setpin_reset(g, FALSE);
+    gfxSleepMilliseconds(20);
+    acquire_bus(g);
+    enter_cmd_mode(g);
+
+    write_cmd(g, ST7565_RESET);
+    write_cmd(g, ST7565_LCD_BIAS);
+    write_cmd(g, ST7565_ADC);
+    write_cmd(g, ST7565_COM_SCAN);
+
+    write_cmd(g, ST7565_RESISTOR_RATIO | 0x1);
+    write_cmd2(g, ST7565_CONTRAST, GDISP_INITIAL_CONTRAST);
+
+    // turn on internal power supply (VC=1, VR=1, VF=1)
+    write_cmd(g, ST7565_POWER_CONTROL | 0x07);
+
+    write_cmd(g, ST7565_INVERT_DISPLAY);
+    write_cmd(g, ST7565_ALLON_NORMAL);
+
+    write_cmd(g, ST7565_START_LINE | 0);
+    write_cmd(g, ST7565_RMW);
+    flush_cmd(g);
+
+    // Finish Init
+    post_init_board(g);
+
+    // Release the bus
+    release_bus(g);
+
+    /* Initialise the GDISP structure */
+    g->g.Width = GDISP_SCREEN_WIDTH;
+    g->g.Height = GDISP_SCREEN_HEIGHT;
+    g->g.Orientation = GDISP_ROTATE_0;
+    g->g.Powermode = powerOff;
+    g->g.Backlight = GDISP_INITIAL_BACKLIGHT;
+    g->g.Contrast = GDISP_INITIAL_CONTRAST;
+    return TRUE;
+}
+
+#if GDISP_HARDWARE_FLUSH
+LLDSPEC void gdisp_lld_flush(GDisplay *g) {
+    unsigned    p;
+
+    // Don't flush if we don't need it.
+    if (!(g->flags & GDISP_FLG_NEEDFLUSH))
+        return;
+
+    acquire_bus(g);
+    enter_cmd_mode(g);
+    unsigned dstOffset = (PRIV(g)->buffer2 ? 4 : 0);
+    for (p = 0; p < 4; p++) {
+        write_cmd(g, ST7565_PAGE | (p + dstOffset));
+        write_cmd(g, ST7565_COLUMN_MSB | 0);
+        write_cmd(g, ST7565_COLUMN_LSB | 0);
+        write_cmd(g, ST7565_RMW);
+        flush_cmd(g);
+        enter_data_mode(g);
+        write_data(g, RAM(g) + (p*GDISP_SCREEN_WIDTH), GDISP_SCREEN_WIDTH);
+        enter_cmd_mode(g);
+    }
+    unsigned line = (PRIV(g)->buffer2 ? 32 : 0);
+    write_cmd(g, ST7565_START_LINE | line);
+    flush_cmd(g);
+    PRIV(g)->buffer2 = !PRIV(g)->buffer2;
+    release_bus(g);
+
+    g->flags &= ~GDISP_FLG_NEEDFLUSH;
+}
+#endif
+
+#if GDISP_HARDWARE_DRAWPIXEL
+LLDSPEC void gdisp_lld_draw_pixel(GDisplay *g) {
+    coord_t        x, y;
+
+    switch(g->g.Orientation) {
+    default:
+    case GDISP_ROTATE_0:
+        x = g->p.x;
+        y = g->p.y;
+        break;
+    case GDISP_ROTATE_90:
+        x = g->p.y;
+        y = GDISP_SCREEN_HEIGHT-1 - g->p.x;
+        break;
+    case GDISP_ROTATE_180:
+        x = GDISP_SCREEN_WIDTH-1 - g->p.x;
+        y = GDISP_SCREEN_HEIGHT-1 - g->p.y;
+        break;
+    case GDISP_ROTATE_270:
+        x = GDISP_SCREEN_HEIGHT-1 - g->p.y;
+        y = g->p.x;
+        break;
+    }
+    if (gdispColor2Native(g->p.color) != Black)
+        RAM(g)[xyaddr(x, y)] |= xybit(y);
+    else
+        RAM(g)[xyaddr(x, y)] &= ~xybit(y);
+    g->flags |= GDISP_FLG_NEEDFLUSH;
+}
+#endif
+
+#if GDISP_HARDWARE_PIXELREAD
+LLDSPEC color_t gdisp_lld_get_pixel_color(GDisplay *g) {
+    coord_t        x, y;
+
+    switch(g->g.Orientation) {
+    default:
+    case GDISP_ROTATE_0:
+        x = g->p.x;
+        y = g->p.y;
+        break;
+    case GDISP_ROTATE_90:
+        x = g->p.y;
+        y = GDISP_SCREEN_HEIGHT-1 - g->p.x;
+        break;
+    case GDISP_ROTATE_180:
+        x = GDISP_SCREEN_WIDTH-1 - g->p.x;
+        y = GDISP_SCREEN_HEIGHT-1 - g->p.y;
+        break;
+    case GDISP_ROTATE_270:
+        x = GDISP_SCREEN_HEIGHT-1 - g->p.y;
+        y = g->p.x;
+        break;
+    }
+    return (RAM(g)[xyaddr(x, y)] & xybit(y)) ? White : Black;
+}
+#endif
+
+LLDSPEC void gdisp_lld_blit_area(GDisplay *g) {
+    uint8_t* buffer = (uint8_t*)g->p.ptr;
+    int linelength = g->p.cx;
+    for (int i = 0; i < g->p.cy; i++) {
+        unsigned dstx = g->p.x;
+        unsigned dsty = g->p.y + i;
+        unsigned srcx = g->p.x1;
+        unsigned srcy = g->p.y1 + i;
+        unsigned srcbit = srcy * g->p.x2 + srcx;
+        for(int j=0; j < linelength; j++) {
+            uint8_t src = buffer[srcbit / 8];
+            uint8_t bit = 7-(srcbit % 8);
+            uint8_t bitset = (src >> bit) & 1;
+            uint8_t* dst = &(RAM(g)[xyaddr(dstx, dsty)]);
+            if (bitset) {
+                *dst |= xybit(dsty);
+            }
+            else {
+                *dst &= ~xybit(dsty);
+            }
+            dstx++;
+            srcbit++;
+        }
+    }
+    g->flags |= GDISP_FLG_NEEDFLUSH;
+}
+
+#if GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL
+LLDSPEC void gdisp_lld_control(GDisplay *g) {
+    switch(g->p.x) {
+    case GDISP_CONTROL_POWER:
+        if (g->g.Powermode == (powermode_t)g->p.ptr)
+            return;
+        switch((powermode_t)g->p.ptr) {
+        case powerOff:
+        case powerSleep:
+        case powerDeepSleep:
+            acquire_bus(g);
+            enter_cmd_mode(g);
+            write_cmd(g, ST7565_DISPLAY_OFF);
+            flush_cmd(g);
+            release_bus(g);
+            break;
+        case powerOn:
+            acquire_bus(g);
+            enter_cmd_mode(g);
+            write_cmd(g, ST7565_DISPLAY_ON);
+            flush_cmd(g);
+            release_bus(g);
+            break;
+        default:
+            return;
+        }
+        g->g.Powermode = (powermode_t)g->p.ptr;
+        return;
+
+        case GDISP_CONTROL_ORIENTATION:
+            if (g->g.Orientation == (orientation_t)g->p.ptr)
+                return;
+            switch((orientation_t)g->p.ptr) {
+            /* Rotation is handled by the drawing routines */
+            case GDISP_ROTATE_0:
+            case GDISP_ROTATE_180:
+                g->g.Height = GDISP_SCREEN_HEIGHT;
+                g->g.Width = GDISP_SCREEN_WIDTH;
+                break;
+            case GDISP_ROTATE_90:
+            case GDISP_ROTATE_270:
+                g->g.Height = GDISP_SCREEN_WIDTH;
+                g->g.Width = GDISP_SCREEN_HEIGHT;
+                break;
+            default:
+                return;
+            }
+            g->g.Orientation = (orientation_t)g->p.ptr;
+            return;
+
+            case GDISP_CONTROL_CONTRAST:
+                g->g.Contrast = (unsigned)g->p.ptr & 63;
+                acquire_bus(g);
+                enter_cmd_mode(g);
+                write_cmd2(g, ST7565_CONTRAST, g->g.Contrast);
+                flush_cmd(g);
+                release_bus(g);
+                return;
+    }
+}
+#endif // GDISP_NEED_CONTROL
+
+#endif // GFX_USE_GDISP
diff --git a/drivers/ugfx/gdisp/st7565/gdisp_lld_config.h b/drivers/ugfx/gdisp/st7565/gdisp_lld_config.h
new file mode 100644
index 000000000..4446bd38b
--- /dev/null
+++ b/drivers/ugfx/gdisp/st7565/gdisp_lld_config.h
@@ -0,0 +1,27 @@
+/*
+ * This file is subject to the terms of the GFX License. If a copy of
+ * the license was not distributed with this file, you can obtain one at:
+ *
+ *              http://ugfx.org/license.html
+ */
+
+#ifndef _GDISP_LLD_CONFIG_H
+#define _GDISP_LLD_CONFIG_H
+
+#if GFX_USE_GDISP
+
+/*===========================================================================*/
+/* Driver hardware support.                                                  */
+/*===========================================================================*/
+
+#define GDISP_HARDWARE_FLUSH            TRUE        // This controller requires flushing
+#define GDISP_HARDWARE_DRAWPIXEL        TRUE
+#define GDISP_HARDWARE_PIXELREAD        TRUE
+#define GDISP_HARDWARE_CONTROL          TRUE
+#define GDISP_HARDWARE_BITFILLS         TRUE
+
+#define GDISP_LLD_PIXELFORMAT           GDISP_PIXELFORMAT_MONO
+
+#endif    /* GFX_USE_GDISP */
+
+#endif    /* _GDISP_LLD_CONFIG_H */
diff --git a/drivers/ugfx/gdisp/st7565/st7565.h b/drivers/ugfx/gdisp/st7565/st7565.h
new file mode 100644
index 000000000..24924ff05
--- /dev/null
+++ b/drivers/ugfx/gdisp/st7565/st7565.h
@@ -0,0 +1,39 @@
+/*
+ * This file is subject to the terms of the GFX License. If a copy of
+ * the license was not distributed with this file, you can obtain one at:
+ *
+ *              http://ugfx.org/license.html
+ */
+
+#ifndef _ST7565_H
+#define _ST7565_H
+
+#define ST7565_CONTRAST             0x81
+#define ST7565_ALLON_NORMAL         0xA4
+#define ST7565_ALLON                0xA5
+#define ST7565_POSITIVE_DISPLAY     0xA6
+#define ST7565_INVERT_DISPLAY       0xA7
+#define ST7565_DISPLAY_OFF          0xAE
+#define ST7565_DISPLAY_ON           0xAF
+
+#define ST7565_LCD_BIAS_7           0xA3
+#define ST7565_LCD_BIAS_9           0xA2
+
+#define ST7565_ADC_NORMAL           0xA0
+#define ST7565_ADC_REVERSE          0xA1
+
+#define ST7565_COM_SCAN_INC         0xC0
+#define ST7565_COM_SCAN_DEC         0xC8
+
+#define ST7565_START_LINE           0x40
+#define ST7565_PAGE                 0xB0
+#define ST7565_COLUMN_MSB           0x10
+#define ST7565_COLUMN_LSB           0x00
+#define ST7565_RMW                  0xE0
+
+#define ST7565_RESISTOR_RATIO       0x20
+#define ST7565_POWER_CONTROL        0x28
+
+#define ST7565_RESET                0xE2
+
+#endif /* _ST7565_H */