diff --git a/drivers/avr/i2c_master.c b/drivers/avr/i2c_master.c
index f4a4bb7b0..4e76e2e7c 100755
--- a/drivers/avr/i2c_master.c
+++ b/drivers/avr/i2c_master.c
@@ -6,6 +6,7 @@
 #include <util/twi.h>
 
 #include "i2c_master.h"
+#include "timer.h"
 
 #define F_SCL 400000UL // SCL frequency
 #define Prescaler 1
@@ -13,137 +14,205 @@
 
 void i2c_init(void)
 {
+  TWSR = 0;     /* no prescaler */
 	TWBR = (uint8_t)TWBR_val;
 }
 
-uint8_t i2c_start(uint8_t address)
+i2c_status_t i2c_start(uint8_t address, uint16_t timeout)
 {
 	// reset TWI control register
 	TWCR = 0;
-	// transmit START condition 
+	// transmit START condition
 	TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
-	// wait for end of transmission
-	while( !(TWCR & (1<<TWINT)) );
-	
+
+  uint16_t timeout_timer = timer_read();
+  while( !(TWCR & (1<<TWINT)) ) {
+    if ((timeout != I2C_TIMEOUT_INFINITE) && ((timer_read() - timeout_timer) >= timeout)) {
+      return I2C_STATUS_TIMEOUT;
+    }
+  }
+
 	// check if the start condition was successfully transmitted
-	if((TWSR & 0xF8) != TW_START){ return 1; }
-	
+	if(((TW_STATUS & 0xF8) != TW_START) && ((TW_STATUS & 0xF8) != TW_REP_START)){ return I2C_STATUS_ERROR; }
+
 	// load slave address into data register
 	TWDR = address;
 	// start transmission of address
 	TWCR = (1<<TWINT) | (1<<TWEN);
-	// wait for end of transmission
-	while( !(TWCR & (1<<TWINT)) );
-	
+
+  timeout_timer = timer_read();
+  while( !(TWCR & (1<<TWINT)) ) {
+    if ((timeout != I2C_TIMEOUT_INFINITE) && ((timer_read() - timeout_timer) >= timeout)) {
+      return I2C_STATUS_TIMEOUT;
+    }
+  }
+
 	// check if the device has acknowledged the READ / WRITE mode
 	uint8_t twst = TW_STATUS & 0xF8;
-	if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return 1;
-	
-	return 0;
+	if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return I2C_STATUS_ERROR;
+
+	return I2C_STATUS_SUCCESS;
 }
 
-uint8_t i2c_write(uint8_t data)
+i2c_status_t i2c_write(uint8_t data, uint16_t timeout)
 {
 	// load data into data register
 	TWDR = data;
 	// start transmission of data
 	TWCR = (1<<TWINT) | (1<<TWEN);
-	// wait for end of transmission
-	while( !(TWCR & (1<<TWINT)) );
-	
-	if( (TWSR & 0xF8) != TW_MT_DATA_ACK ){ return 1; }
-	
-	return 0;
+
+  uint16_t timeout_timer = timer_read();
+  while( !(TWCR & (1<<TWINT)) ) {
+    if ((timeout != I2C_TIMEOUT_INFINITE) && ((timer_read() - timeout_timer) >= timeout)) {
+      return I2C_STATUS_TIMEOUT;
+    }
+  }
+
+	if( (TW_STATUS & 0xF8) != TW_MT_DATA_ACK ){ return I2C_STATUS_ERROR; }
+
+	return I2C_STATUS_SUCCESS;
 }
 
-uint8_t i2c_read_ack(void)
+int16_t i2c_read_ack(uint16_t timeout)
 {
-	
+
 	// start TWI module and acknowledge data after reception
-	TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); 
-	// wait for end of transmission
-	while( !(TWCR & (1<<TWINT)) );
+	TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
+
+  uint16_t timeout_timer = timer_read();
+  while( !(TWCR & (1<<TWINT)) ) {
+    if ((timeout != I2C_TIMEOUT_INFINITE) && ((timer_read() - timeout_timer) >= timeout)) {
+      return I2C_STATUS_TIMEOUT;
+    }
+  }
+
 	// return received data from TWDR
 	return TWDR;
 }
 
-uint8_t i2c_read_nack(void)
+int16_t i2c_read_nack(uint16_t timeout)
 {
-	
+
 	// start receiving without acknowledging reception
 	TWCR = (1<<TWINT) | (1<<TWEN);
-	// wait for end of transmission
-	while( !(TWCR & (1<<TWINT)) );
+
+  uint16_t timeout_timer = timer_read();
+  while( !(TWCR & (1<<TWINT)) ) {
+    if ((timeout != I2C_TIMEOUT_INFINITE) && ((timer_read() - timeout_timer) >= timeout)) {
+      return I2C_STATUS_TIMEOUT;
+    }
+  }
+
 	// return received data from TWDR
 	return TWDR;
 }
 
-uint8_t i2c_transmit(uint8_t address, uint8_t* data, uint16_t length)
+i2c_status_t i2c_transmit(uint8_t address, uint8_t* data, uint16_t length, uint16_t timeout)
 {
-	if (i2c_start(address | I2C_WRITE)) return 1;
-	
-	for (uint16_t i = 0; i < length; i++)
-	{
-		if (i2c_write(data[i])) return 1;
-	}
-	
-	i2c_stop();
-	
-	return 0;
-}
+  i2c_status_t status = i2c_start(address | I2C_WRITE, timeout);
+	if (status) return status;
 
-uint8_t i2c_receive(uint8_t address, uint8_t* data, uint16_t length)
-{
-	if (i2c_start(address | I2C_READ)) return 1;
-	
-	for (uint16_t i = 0; i < (length-1); i++)
-	{
-		data[i] = i2c_read_ack();
-	}
-	data[(length-1)] = i2c_read_nack();
-	
-	i2c_stop();
-	
-	return 0;
-}
-
-uint8_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length)
-{
-	if (i2c_start(devaddr | 0x00)) return 1;
-
-	i2c_write(regaddr);
-
-	for (uint16_t i = 0; i < length; i++)
-	{
-		if (i2c_write(data[i])) return 1;
+	for (uint16_t i = 0; i < length; i++) {
+		status = i2c_write(data[i], timeout);
+    if (status) return status;
 	}
 
-	i2c_stop();
+	status = i2c_stop(timeout);
+  if (status) return status;
 
-	return 0;
+	return I2C_STATUS_SUCCESS;
 }
 
-uint8_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length)
+i2c_status_t i2c_receive(uint8_t address, uint8_t* data, uint16_t length, uint16_t timeout)
 {
-	if (i2c_start(devaddr)) return 1;
+  i2c_status_t status = i2c_start(address | I2C_READ, timeout);
+	if (status) return status;
 
-	i2c_write(regaddr);
-
-	if (i2c_start(devaddr | 0x01)) return 1;
-
-	for (uint16_t i = 0; i < (length-1); i++)
-	{
-		data[i] = i2c_read_ack();
+	for (uint16_t i = 0; i < (length-1); i++) {
+    status = i2c_read_ack(timeout);
+    if (status >= 0) {
+      data[i] = status;
+    } else {
+      return status;
+    }
 	}
-	data[(length-1)] = i2c_read_nack();
 
-	i2c_stop();
+  status = i2c_read_nack(timeout);
+  if (status >= 0 ) {
+    data[(length-1)] = status;
+  } else {
+    return status;
+  }
 
-	return 0;
+  status = i2c_stop(timeout);
+  if (status) return status;
+
+	return I2C_STATUS_SUCCESS;
 }
 
-void i2c_stop(void)
+i2c_status_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout)
+{
+  i2c_status_t status = i2c_start(devaddr | 0x00, timeout);
+	if (status) return status;
+
+	status = i2c_write(regaddr, timeout);
+  if (status) return status;
+
+	for (uint16_t i = 0; i < length; i++) {
+    status = i2c_write(data[i], timeout);
+		if (status) return status;
+	}
+
+	status = i2c_stop(timeout);
+  if (status) return status;
+
+	return I2C_STATUS_SUCCESS;
+}
+
+i2c_status_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout)
+{
+  i2c_status_t status = i2c_start(devaddr, timeout);
+	if (status) return status;
+
+  status = i2c_write(regaddr, timeout);
+  if (status) return status;
+
+  status = i2c_start(devaddr | 0x01, timeout);
+	if (status) return status;
+
+	for (uint16_t i = 0; i < (length-1); i++) {
+		status = i2c_read_ack(timeout);
+    if (status >= 0) {
+      data[i] = status;
+    } else {
+      return status;
+    }
+	}
+
+  status = i2c_read_nack(timeout);
+  if (status >= 0 ) {
+    data[(length-1)] = status;
+  } else {
+    return status;
+  }
+
+  status = i2c_stop(timeout);
+  if (status) return status;
+
+	return I2C_STATUS_SUCCESS;
+}
+
+i2c_status_t i2c_stop(uint16_t timeout)
 {
 	// transmit STOP condition
 	TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
+
+  uint16_t timeout_timer = timer_read();
+  while(TWCR & (1<<TWSTO)) {
+    if ((timeout != I2C_TIMEOUT_INFINITE) && ((timer_read() - timeout_timer) >= timeout)) {
+      return I2C_STATUS_TIMEOUT;
+    }
+  }
+
+  return I2C_STATUS_SUCCESS;
 }
diff --git a/drivers/avr/i2c_master.h b/drivers/avr/i2c_master.h
index 2479d28d5..cf93680be 100755
--- a/drivers/avr/i2c_master.h
+++ b/drivers/avr/i2c_master.h
@@ -8,15 +8,24 @@
 #define I2C_READ 0x01
 #define I2C_WRITE 0x00
 
+typedef int16_t i2c_status_t;
+
+#define I2C_STATUS_SUCCESS (0)
+#define I2C_STATUS_ERROR   (-1)
+#define I2C_STATUS_TIMEOUT (-2)
+
+#define I2C_TIMEOUT_IMMEDIATE (0)
+#define I2C_TIMEOUT_INFINITE (0xFFFF)
+
 void i2c_init(void);
-uint8_t i2c_start(uint8_t address);
-uint8_t i2c_write(uint8_t data);
-uint8_t i2c_read_ack(void);
-uint8_t i2c_read_nack(void);
-uint8_t i2c_transmit(uint8_t address, uint8_t* data, uint16_t length);
-uint8_t i2c_receive(uint8_t address, uint8_t* data, uint16_t length);
-uint8_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length);
-uint8_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length);
-void i2c_stop(void);
+i2c_status_t i2c_start(uint8_t address, uint16_t timeout);
+i2c_status_t i2c_write(uint8_t data, uint16_t timeout);
+int16_t i2c_read_ack(uint16_t timeout);
+int16_t i2c_read_nack(uint16_t timeout);
+i2c_status_t i2c_transmit(uint8_t address, uint8_t* data, uint16_t length, uint16_t timeout);
+i2c_status_t i2c_receive(uint8_t address, uint8_t* data, uint16_t length, uint16_t timeout);
+i2c_status_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout);
+i2c_status_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout);
+i2c_status_t i2c_stop(uint16_t timeout);
 
 #endif // I2C_MASTER_H
diff --git a/drivers/avr/is31fl3731.c b/drivers/avr/is31fl3731.c
index c7a99e3a3..70813464b 100644
--- a/drivers/avr/is31fl3731.c
+++ b/drivers/avr/is31fl3731.c
@@ -49,6 +49,14 @@
 #define ISSI_COMMANDREGISTER 0xFD
 #define ISSI_BANK_FUNCTIONREG 0x0B    // helpfully called 'page nine'
 
+#ifndef ISSI_TIMEOUT
+  #define ISSI_TIMEOUT 100
+#endif
+
+#ifndef ISSI_PERSISTENCE
+  #define ISSI_PERSISTENCE 0
+#endif
+
 // Transfer buffer for TWITransmitData()
 uint8_t g_twi_transfer_buffer[20];
 
@@ -83,8 +91,14 @@ void IS31FL3731_write_register( uint8_t addr, uint8_t reg, uint8_t data )
 	g_twi_transfer_buffer[0] = reg;
 	g_twi_transfer_buffer[1] = data;
 
-	//Transmit data until succesful
-	while(i2c_transmit(addr << 1, g_twi_transfer_buffer,2) != 0); 
+  #if ISSI_PERSISTENCE > 0
+    for (uint8_t i = 0; i < ISSI_PERSISTENCE; i++) {
+      if (i2c_transmit(addr << 1, g_twi_transfer_buffer, 2, ISSI_TIMEOUT) == 0)
+        break;
+    }
+  #else
+    i2c_transmit(addr << 1, g_twi_transfer_buffer, 2, ISSI_TIMEOUT);
+  #endif
 }
 
 void IS31FL3731_write_pwm_buffer( uint8_t addr, uint8_t *pwm_buffer )
@@ -95,20 +109,24 @@ void IS31FL3731_write_pwm_buffer( uint8_t addr, uint8_t *pwm_buffer )
 	// g_twi_transfer_buffer[] is 20 bytes
 
 	// iterate over the pwm_buffer contents at 16 byte intervals
-	for ( int i = 0; i < 144; i += 16 )
-	{
+	for ( int i = 0; i < 144; i += 16 ) {
 		// set the first register, e.g. 0x24, 0x34, 0x44, etc.
 		g_twi_transfer_buffer[0] = 0x24 + i;
 		// copy the data from i to i+15
 		// device will auto-increment register for data after the first byte
 		// thus this sets registers 0x24-0x33, 0x34-0x43, etc. in one transfer
-		for ( int j = 0; j < 16; j++ )
-		{
+		for ( int j = 0; j < 16; j++ ) {
 			g_twi_transfer_buffer[1 + j] = pwm_buffer[i + j];
 		}
 
-		//Transmit buffer until succesful
-		while(i2c_transmit(addr << 1, g_twi_transfer_buffer,17) != 0);
+    #if ISSI_PERSISTENCE > 0
+      for (uint8_t i = 0; i < ISSI_PERSISTENCE; i++) {
+        if (i2c_transmit(addr << 1, g_twi_transfer_buffer, 17, ISSI_TIMEOUT) == 0)
+          break;
+      }
+    #else
+      i2c_transmit(addr << 1, g_twi_transfer_buffer, 17, ISSI_TIMEOUT);
+    #endif
 	}
 }
 
@@ -165,6 +183,7 @@ void IS31FL3731_init( uint8_t addr )
 	// most usage after initialization is just writing PWM buffers in bank 0
 	// as there's not much point in double-buffering
 	IS31FL3731_write_register( addr, ISSI_COMMANDREGISTER, 0 );
+
 }
 
 void IS31FL3731_set_color( int index, uint8_t red, uint8_t green, uint8_t blue )
@@ -217,7 +236,6 @@ void IS31FL3731_set_led_control_register( uint8_t index, bool red, bool green, b
 
 	g_led_control_registers_update_required = true;
 
-
 }
 
 void IS31FL3731_update_pwm_buffers( uint8_t addr1, uint8_t addr2 )
diff --git a/keyboards/ergodox_ez/config.h b/keyboards/ergodox_ez/config.h
index ae70c4f2e..1285cbe1c 100644
--- a/keyboards/ergodox_ez/config.h
+++ b/keyboards/ergodox_ez/config.h
@@ -81,10 +81,9 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /* fix space cadet rollover issue */
 #define DISABLE_SPACE_CADET_ROLLOVER
 
-// #define RGB_MIDI
-#define RGBW_BB_TWI
+// #define RGBW_BB_TWI
 
-#define RGBW 1
+// #define RGBW 1
 
 /* "debounce" is measured in keyboard scans. Some users reported
  * needing values as high as 15, which was at the time around 50ms.
@@ -102,6 +101,15 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #define USB_MAX_POWER_CONSUMPTION 500
 
+// RGB backlight
+#define DRIVER_ADDR_1 0b1110100
+#define DRIVER_ADDR_2 0b1110111
+#define DRIVER_COUNT 2
+#define DRIVER_1_LED_TOTAL 24
+#define DRIVER_2_LED_TOTAL 24
+#define DRIVER_LED_TOTAL DRIVER_1_LED_TOTAL + DRIVER_2_LED_TOTAL
+#define RGB_MATRIX_SKIP_FRAMES 10
+
 // #define RGBLIGHT_COLOR_LAYER_0 0x00, 0x00, 0xFF
 /* #define RGBLIGHT_COLOR_LAYER_1 0x00, 0x00, 0xFF */
 /* #define RGBLIGHT_COLOR_LAYER_2 0xFF, 0x00, 0x00 */
diff --git a/keyboards/ergodox_ez/ergodox_ez.c b/keyboards/ergodox_ez/ergodox_ez.c
index 437411856..61f910711 100644
--- a/keyboards/ergodox_ez/ergodox_ez.c
+++ b/keyboards/ergodox_ez/ergodox_ez.c
@@ -1,6 +1,4 @@
 #include QMK_KEYBOARD_H
-#include "i2cmaster.h"
-
 
 extern inline void ergodox_board_led_on(void);
 extern inline void ergodox_right_led_1_on(void);
@@ -24,9 +22,8 @@ extern inline void ergodox_right_led_set(uint8_t led, uint8_t n);
 
 extern inline void ergodox_led_all_set(uint8_t n);
 
-
 bool i2c_initialized = 0;
-uint8_t mcp23018_status = 0x20;
+i2c_status_t mcp23018_status = 0x20;
 
 void matrix_init_kb(void) {
    // keyboard LEDs (see "PWM on ports OC1(A|B|C)" in "teensy-2-0.md")
@@ -114,33 +111,36 @@ uint8_t init_mcp23018(void) {
     // uint8_t sreg_prev;
     // sreg_prev=SREG;
     // cli();
+
     if (i2c_initialized == 0) {
         i2c_init();  // on pins D(1,0)
         i2c_initialized = true;
         _delay_ms(1000);
     }
+    // i2c_init(); // on pins D(1,0)
+    // _delay_ms(1000);
 
     // set pin direction
     // - unused  : input  : 1
     // - input   : input  : 1
     // - driving : output : 0
-    mcp23018_status = i2c_start(I2C_ADDR_WRITE);    if (mcp23018_status) goto out;
-    mcp23018_status = i2c_write(IODIRA);            if (mcp23018_status) goto out;
-    mcp23018_status = i2c_write(0b00000000);        if (mcp23018_status) goto out;
-    mcp23018_status = i2c_write(0b00111111);        if (mcp23018_status) goto out;
-    i2c_stop();
+    mcp23018_status = i2c_start(I2C_ADDR_WRITE, ERGODOX_EZ_I2C_TIMEOUT);    if (mcp23018_status) goto out;
+    mcp23018_status = i2c_write(IODIRA, ERGODOX_EZ_I2C_TIMEOUT);            if (mcp23018_status) goto out;
+    mcp23018_status = i2c_write(0b00000000, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;
+    mcp23018_status = i2c_write(0b00111111, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;
+    i2c_stop(ERGODOX_EZ_I2C_TIMEOUT);
 
     // set pull-up
     // - unused  : on  : 1
     // - input   : on  : 1
     // - driving : off : 0
-    mcp23018_status = i2c_start(I2C_ADDR_WRITE);    if (mcp23018_status) goto out;
-    mcp23018_status = i2c_write(GPPUA);             if (mcp23018_status) goto out;
-    mcp23018_status = i2c_write(0b00000000);        if (mcp23018_status) goto out;
-    mcp23018_status = i2c_write(0b00111111);        if (mcp23018_status) goto out;
+    mcp23018_status = i2c_start(I2C_ADDR_WRITE, ERGODOX_EZ_I2C_TIMEOUT);    if (mcp23018_status) goto out;
+    mcp23018_status = i2c_write(GPPUA, ERGODOX_EZ_I2C_TIMEOUT);             if (mcp23018_status) goto out;
+    mcp23018_status = i2c_write(0b00000000, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;
+    mcp23018_status = i2c_write(0b00111111, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;
 
 out:
-    i2c_stop();
+    i2c_stop(ERGODOX_EZ_I2C_TIMEOUT);
 
 #ifdef LEFT_LEDS
     if (!mcp23018_status) mcp23018_status = ergodox_left_leds_update();
@@ -164,22 +164,22 @@ uint8_t ergodox_left_leds_update(void) {
     // - unused  : hi-Z : 1
     // - input   : hi-Z : 1
     // - driving : hi-Z : 1
-    mcp23018_status = i2c_start(I2C_ADDR_WRITE);
+    mcp23018_status = i2c_start(I2C_ADDR_WRITE, ERGODOX_EZ_I2C_TIMEOUT);
     if (mcp23018_status) goto out;
-    mcp23018_status = i2c_write(OLATA);
+    mcp23018_status = i2c_write(OLATA, ERGODOX_EZ_I2C_TIMEOUT);
     if (mcp23018_status) goto out;
     mcp23018_status = i2c_write(0b11111111
-                                & ~(ergodox_left_led_3<<LEFT_LED_3_SHIFT)
-                                );
+                                & ~(ergodox_left_led_3<<LEFT_LED_3_SHIFT),
+                                ERGODOX_EZ_I2C_TIMEOUT);
     if (mcp23018_status) goto out;
     mcp23018_status = i2c_write(0b11111111
                                 & ~(ergodox_left_led_2<<LEFT_LED_2_SHIFT)
-                                & ~(ergodox_left_led_1<<LEFT_LED_1_SHIFT)
-                                );
+                                & ~(ergodox_left_led_1<<LEFT_LED_1_SHIFT),
+                                ERGODOX_EZ_I2C_TIMEOUT);
     if (mcp23018_status) goto out;
 
  out:
-    i2c_stop();
+    i2c_stop(ERGODOX_EZ_I2C_TIMEOUT);
     return mcp23018_status;
 }
 #endif
@@ -207,3 +207,130 @@ const keypos_t hand_swap_config[MATRIX_ROWS][MATRIX_COLS] = {
     {{0,0}, {1,0}, {2,0}, {3,0}, {4,0}, {5,0}},
 };
 #endif
+
+#ifdef RGB_MATRIX_ENABLE
+const is31_led g_is31_leds[DRIVER_LED_TOTAL] = {
+/*   driver
+ *   |  R location
+ *   |  |      G location
+ *   |  |      |      B location
+ *   |  |      |      | */
+    {0, C3_1,  C2_1,  C4_1}, // LED1 on right
+    {0, C6_1,  C5_1,  C7_1}, // LED2
+    {0, C4_2,  C3_2,  C5_2}, // LED3
+    {0, C7_2,  C6_2,  C8_2}, // LED4
+    {0, C2_3,  C1_3,  C3_3}, // LED5
+    {0, C5_3,  C4_3,  C6_3}, // LED6
+    {0, C8_3,  C7_3,  C9_3}, // LED7
+    {0, C2_4,  C1_4,  C3_4}, // LED8
+    {0, C6_4,  C5_4,  C7_4}, // LED9
+    {0, C2_5,  C1_5,  C3_5}, // LED10
+    {0, C7_5,  C6_5,  C8_5}, // LED11
+    {0, C2_6,  C1_6,  C3_6}, // LED12
+    {0, C5_6,  C4_6,  C6_6}, // LED13
+    {0, C8_6,  C7_6,  C9_6}, // LED14
+    {0, C2_7,  C1_7,  C3_7}, // LED15
+    {0, C5_7,  C4_7,  C6_7}, // LED16
+    {0, C2_8,  C1_8,  C3_8}, // LED17
+    {0, C5_8,  C4_8,  C6_8}, // LED18
+
+    {0, C3_9,  C2_9,  C4_9}, // LED19
+    {0, C6_9,  C5_9,  C7_9}, // LED20
+    {0, C4_10, C3_10, C5_10}, // LED21
+    {0, C7_10, C6_10, C8_10}, // LED22
+    {0, C2_11, C1_11, C3_11}, // LED23
+    {0, C5_11, C4_11, C6_11}, // LED24
+
+    {1, C3_1,  C2_1,  C4_1}, // LED1 on left
+    {1, C6_1,  C5_1,  C7_1}, // LED2
+    {1, C4_2,  C3_2,  C5_2}, // LED3
+    {1, C7_2,  C6_2,  C8_2}, // LED4
+    {1, C2_3,  C1_3,  C3_3}, // LED5
+    {1, C5_3,  C4_3,  C6_3}, // LED6
+    {1, C8_3,  C7_3,  C9_3}, // LED7
+    {1, C2_4,  C1_4,  C3_4}, // LED8
+    {1, C6_4,  C5_4,  C7_4}, // LED9
+    {1, C2_5,  C1_5,  C3_5}, // LED10
+    {1, C7_5,  C6_5,  C8_5}, // LED11
+    {1, C2_6,  C1_6,  C3_6}, // LED12
+    {1, C5_6,  C4_6,  C6_6}, // LED13
+    {1, C8_6,  C7_6,  C9_6}, // LED14
+    {1, C2_7,  C1_7,  C3_7}, // LED15
+    {1, C5_7,  C4_7,  C6_7}, // LED16
+    {1, C2_8,  C1_8,  C3_8}, // LED17
+    {1, C5_8,  C4_8,  C6_8}, // LED18
+
+    {1, C3_9,  C2_9,  C4_9}, // LED19
+    {1, C6_9,  C5_9,  C7_9}, // LED20
+    {1, C4_10, C3_10, C5_10}, // LED21
+    {1, C7_10, C6_10, C8_10}, // LED22
+    {1, C2_11, C1_11, C3_11}, // LED23
+    {1, C5_11, C4_11, C6_11} // LED24
+};
+
+
+const rgb_led g_rgb_leds[DRIVER_LED_TOTAL] = {
+
+    /*{row | col << 4}
+      |             {x=0..224, y=0..64}
+      |              |                    modifier
+      |              |                    | */
+    {{0|(0<<4)},   {24.9*5, 16*0}, 0}, // LED 1 on right
+    {{0|(1<<4)},   {24.9*6, 16*0}, 0}, // LED 2
+    {{0|(2<<4)},   {24.9*7, 16*0}, 0}, // LED 3
+    {{0|(3<<4)},   {24.9*8, 16*0}, 0}, // LED 4
+    {{0|(4<<4)},   {24.9*9, 16*0}, 0}, // LED 5
+
+    {{1|(5<<4)},   {24.9*5, 16*1}, 0}, // LED 6
+    {{1|(6<<4)},   {24.9*6, 16*1}, 0}, // LED 7
+    {{1|(7<<4)},   {24.9*7, 16*1}, 0}, // LED 8
+    {{1|(8<<4)},   {24.9*8, 16*1}, 0}, // LED 9
+    {{1|(9<<4)},   {24.9*9, 16*1}, 0}, // LED 10
+
+    {{2|(5<<4)},   {24.9*5, 16*2}, 0}, // LED 11
+    {{2|(6<<4)},   {24.9*6, 16*2}, 0}, // LED 12
+    {{2|(7<<4)},   {24.9*7, 16*2}, 0}, // LED 13
+    {{2|(8<<4)},   {24.9*8, 16*2}, 0}, // LED 14
+    {{2|(9<<4)},   {24.9*9, 16*2}, 0}, // LED 15
+
+    {{3|(5<<4)},   {24.9*5, 16*2}, 0}, // LED 16
+    {{3|(6<<4)},   {24.9*6, 16*2}, 0}, // LED 17
+    {{3|(7<<4)},   {24.9*7, 16*2}, 0}, // LED 18
+    {{3|(8<<4)},   {24.9*8, 16*2}, 0}, // LED 19
+    {{3|(9<<4)},   {24.9*9, 16*2}, 0}, // LED 20
+
+    {{4|(6<<4)},   {24.9*6, 16*2}, 0}, // LED 21
+    {{4|(7<<4)},   {24.9*7, 16*2}, 0}, // LED 22
+    {{4|(8<<4)},   {24.9*8, 16*2}, 0}, // LED 23
+    {{4|(9<<4)},   {24.9*9, 16*2}, 0}, // LED 24
+
+    {{0|(0<<4)},   {24.9*4, 16*0}, 0}, // LED 1 on left
+    {{0|(1<<4)},   {24.9*3, 16*0}, 0}, // LED 2
+    {{0|(2<<4)},   {24.9*2, 16*0}, 0}, // LED 3
+    {{0|(3<<4)},   {24.9*1, 16*0}, 0}, // LED 4
+    {{0|(4<<4)},   {24.9*0, 16*0}, 0}, // LED 5
+
+    {{1|(5<<4)},   {24.9*4, 16*1}, 0}, // LED 6
+    {{1|(6<<4)},   {24.9*3, 16*1}, 0}, // LED 7
+    {{1|(7<<4)},   {24.9*2, 16*1}, 0}, // LED 8
+    {{1|(8<<4)},   {24.9*1, 16*1}, 0}, // LED 9
+    {{1|(9<<4)},   {24.9*0, 16*1}, 0}, // LED 10
+
+    {{2|(5<<4)},   {24.9*4, 16*2}, 0}, // LED 11
+    {{2|(6<<4)},   {24.9*3, 16*2}, 0}, // LED 12
+    {{2|(7<<4)},   {24.9*2, 16*2}, 0}, // LED 13
+    {{2|(8<<4)},   {24.9*1, 16*2}, 0}, // LED 14
+    {{2|(9<<4)},   {24.9*0, 16*2}, 0}, // LED 15
+
+    {{3|(5<<4)},   {24.9*4, 16*2}, 0}, // LED 16
+    {{3|(6<<4)},   {24.9*3, 16*2}, 0}, // LED 17
+    {{3|(7<<4)},   {24.9*2, 16*2}, 0}, // LED 18
+    {{3|(8<<4)},   {24.9*1, 16*2}, 0}, // LED 19
+    {{3|(9<<4)},   {24.9*0, 16*2}, 0}, // LED 20
+
+    {{4|(6<<4)},   {24.9*3, 16*2}, 0}, // LED 21
+    {{4|(7<<4)},   {24.9*2, 16*2}, 0}, // LED 22
+    {{4|(8<<4)},   {24.9*1, 16*2}, 0}, // LED 23
+    {{4|(9<<4)},   {24.9*0, 16*2}, 0}, // LED 24
+};
+#endif
diff --git a/keyboards/ergodox_ez/ergodox_ez.h b/keyboards/ergodox_ez/ergodox_ez.h
index 985dcfae5..383702b95 100644
--- a/keyboards/ergodox_ez/ergodox_ez.h
+++ b/keyboards/ergodox_ez/ergodox_ez.h
@@ -4,7 +4,7 @@
 #include "quantum.h"
 #include <stdint.h>
 #include <stdbool.h>
-#include "i2cmaster.h"
+#include "i2c_master.h"
 #include <util/delay.h>
 
 #define CPU_PRESCALE(n) (CLKPR = 0x80, CLKPR = (n))
@@ -23,7 +23,8 @@
 #define OLATA           0x14            // output latch register
 #define OLATB           0x15
 
-extern uint8_t mcp23018_status;
+extern i2c_status_t mcp23018_status;
+#define ERGODOX_EZ_I2C_TIMEOUT 100
 
 void init_ergodox(void);
 void ergodox_blink_all_leds(void);
diff --git a/keyboards/ergodox_ez/matrix.c b/keyboards/ergodox_ez/matrix.c
index e10171133..2e95c83b6 100644
--- a/keyboards/ergodox_ez/matrix.c
+++ b/keyboards/ergodox_ez/matrix.c
@@ -34,7 +34,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "util.h"
 #include "matrix.h"
 #include QMK_KEYBOARD_H
-#include "i2cmaster.h"
 #ifdef DEBUG_MATRIX_SCAN_RATE
 #include  "timer.h"
 #endif
@@ -70,6 +69,7 @@ static void unselect_rows(void);
 static void select_row(uint8_t row);
 
 static uint8_t mcp23018_reset_loop;
+// static uint16_t mcp23018_reset_loop;
 
 #ifdef DEBUG_MATRIX_SCAN_RATE
 uint32_t matrix_timer;
@@ -177,6 +177,7 @@ uint8_t matrix_scan(void)
 {
     if (mcp23018_status) { // if there was an error
         if (++mcp23018_reset_loop == 0) {
+        // if (++mcp23018_reset_loop >= 1300) {
             // since mcp23018_reset_loop is 8 bit - we'll try to reset once in 255 matrix scans
             // this will be approx bit more frequent than once per second
             print("trying to reset mcp23018\n");
@@ -294,13 +295,14 @@ static matrix_row_t read_cols(uint8_t row)
             return 0;
         } else {
             uint8_t data = 0;
-            mcp23018_status = i2c_start(I2C_ADDR_WRITE);    if (mcp23018_status) goto out;
-            mcp23018_status = i2c_write(GPIOB);             if (mcp23018_status) goto out;
-            mcp23018_status = i2c_start(I2C_ADDR_READ);     if (mcp23018_status) goto out;
-            data = i2c_readNak();
-            data = ~data;
+            mcp23018_status = i2c_start(I2C_ADDR_WRITE, ERGODOX_EZ_I2C_TIMEOUT);    if (mcp23018_status) goto out;
+            mcp23018_status = i2c_write(GPIOB, ERGODOX_EZ_I2C_TIMEOUT);             if (mcp23018_status) goto out;
+            mcp23018_status = i2c_start(I2C_ADDR_READ, ERGODOX_EZ_I2C_TIMEOUT);     if (mcp23018_status) goto out;
+            mcp23018_status = i2c_read_nack(ERGODOX_EZ_I2C_TIMEOUT);                if (mcp23018_status < 0) goto out;
+            data = ~((uint8_t)mcp23018_status);
+            mcp23018_status = I2C_STATUS_SUCCESS;
         out:
-            i2c_stop();
+            i2c_stop(ERGODOX_EZ_I2C_TIMEOUT);
             return data;
         }
     } else {
@@ -349,11 +351,11 @@ static void select_row(uint8_t row)
         } else {
             // set active row low  : 0
             // set other rows hi-Z : 1
-            mcp23018_status = i2c_start(I2C_ADDR_WRITE);        if (mcp23018_status) goto out;
-            mcp23018_status = i2c_write(GPIOA);                 if (mcp23018_status) goto out;
-            mcp23018_status = i2c_write(0xFF & ~(1<<row));      if (mcp23018_status) goto out;
+            mcp23018_status = i2c_start(I2C_ADDR_WRITE, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;
+            mcp23018_status = i2c_write(GPIOA, ERGODOX_EZ_I2C_TIMEOUT);                 if (mcp23018_status) goto out;
+            mcp23018_status = i2c_write(0xFF & ~(1<<row), ERGODOX_EZ_I2C_TIMEOUT);      if (mcp23018_status) goto out;
         out:
-            i2c_stop();
+            i2c_stop(ERGODOX_EZ_I2C_TIMEOUT);
         }
     } else {
         // select on teensy
diff --git a/keyboards/ergodox_ez/rules.mk b/keyboards/ergodox_ez/rules.mk
index 5ee9d5cb8..0e0b3cdef 100644
--- a/keyboards/ergodox_ez/rules.mk
+++ b/keyboards/ergodox_ez/rules.mk
@@ -15,8 +15,8 @@
 #----------------------------------------------------------------------------
 
 # # project specific files
-SRC = twimaster.c \
-	  matrix.c
+SRC = matrix.c \
+  i2c_master.c
 
 # MCU name
 MCU = atmega32u4
@@ -82,6 +82,7 @@ UNICODE_ENABLE   = yes # Unicode
 SWAP_HANDS_ENABLE= yes # Allow swapping hands of keyboard
 SLEEP_LED_ENABLE = no
 API_SYSEX_ENABLE = no
-RGBLIGHT_ENABLE = yes
+RGBLIGHT_ENABLE = no
+RGB_MATRIX_ENABLE = yes
 
 LAYOUTS = ergodox
diff --git a/quantum/quantum.c b/quantum/quantum.c
index cfa3df741..5abd222d1 100644
--- a/quantum/quantum.c
+++ b/quantum/quantum.c
@@ -854,7 +854,7 @@ void matrix_init_quantum() {
     audio_init();
   #endif
   #ifdef RGB_MATRIX_ENABLE
-    rgb_matrix_init_drivers();
+    rgb_matrix_init();
   #endif
   matrix_init_kb();
 }
diff --git a/quantum/rgb_matrix.c b/quantum/rgb_matrix.c
index b7424d637..b4bbc3dc0 100644
--- a/quantum/rgb_matrix.c
+++ b/quantum/rgb_matrix.c
@@ -105,7 +105,6 @@ void map_row_column_to_led( uint8_t row, uint8_t column, uint8_t *led_i, uint8_t
     }
 }
 
-
 void rgb_matrix_update_pwm_buffers(void) {
     IS31FL3731_update_pwm_buffers( DRIVER_ADDR_1, DRIVER_ADDR_2 );
     IS31FL3731_update_led_control_registers( DRIVER_ADDR_1, DRIVER_ADDR_2 );
@@ -119,7 +118,6 @@ void rgb_matrix_set_color_all( uint8_t red, uint8_t green, uint8_t blue ) {
     IS31FL3731_set_color_all( red, green, blue );
 }
 
-
 bool process_rgb_matrix(uint16_t keycode, keyrecord_t *record) {
     if ( record->event.pressed ) {
         uint8_t led[8], led_count;
@@ -222,7 +220,7 @@ void rgb_matrix_single_LED_test(void) {
 }
 
 // All LEDs off
-void rgb_matrix_all_off(void) { 
+void rgb_matrix_all_off(void) {
     rgb_matrix_set_color_all( 0, 0, 0 );
 }
 
@@ -248,7 +246,7 @@ void rgb_matrix_solid_reactive(void) {
 
 // alphas = color1, mods = color2
 void rgb_matrix_alphas_mods(void) {
- 
+
     RGB rgb1 = hsv_to_rgb( (HSV){ .h = rgb_matrix_config.hue, .s = rgb_matrix_config.sat, .v = rgb_matrix_config.val } );
     RGB rgb2 = hsv_to_rgb( (HSV){ .h = (rgb_matrix_config.hue + 180) % 360, .s = rgb_matrix_config.sat, .v = rgb_matrix_config.val } );
 
@@ -726,40 +724,44 @@ void rgb_matrix_indicators_user(void) {}
 //  }
 // }
 
-void rgb_matrix_init_drivers(void) {
-    // Initialize TWI
-    i2c_init();
-    IS31FL3731_init( DRIVER_ADDR_1 );
-    IS31FL3731_init( DRIVER_ADDR_2 );
+void rgb_matrix_init(void) {
+  rgb_matrix_setup_drivers();
 
-    for ( int index = 0; index < DRIVER_LED_TOTAL; index++ ) {
-        bool enabled = true;
-        // This only caches it for later
-        IS31FL3731_set_led_control_register( index, enabled, enabled, enabled );
-    }
-    // This actually updates the LED drivers
-    IS31FL3731_update_led_control_registers( DRIVER_ADDR_1, DRIVER_ADDR_2 );
+  // TODO: put the 1 second startup delay here?
 
-    // TODO: put the 1 second startup delay here?
-
-    // clear the key hits
-    for ( int led=0; led<DRIVER_LED_TOTAL; led++ ) {
-        g_key_hit[led] = 255;
-    }
+  // clear the key hits
+  for ( int led=0; led<DRIVER_LED_TOTAL; led++ ) {
+      g_key_hit[led] = 255;
+  }
 
 
-    if (!eeconfig_is_enabled()) {
-        dprintf("rgb_matrix_init_drivers eeconfig is not enabled.\n");
-        eeconfig_init();
-        eeconfig_update_rgb_matrix_default();
-    }
-    rgb_matrix_config.raw = eeconfig_read_rgb_matrix();
-    if (!rgb_matrix_config.mode) {
-        dprintf("rgb_matrix_init_drivers rgb_matrix_config.mode = 0. Write default values to EEPROM.\n");
-        eeconfig_update_rgb_matrix_default();
-        rgb_matrix_config.raw = eeconfig_read_rgb_matrix();
-    }
-    eeconfig_debug_rgb_matrix(); // display current eeprom values
+  if (!eeconfig_is_enabled()) {
+      dprintf("rgb_matrix_init_drivers eeconfig is not enabled.\n");
+      eeconfig_init();
+      eeconfig_update_rgb_matrix_default();
+  }
+  rgb_matrix_config.raw = eeconfig_read_rgb_matrix();
+  if (!rgb_matrix_config.mode) {
+      dprintf("rgb_matrix_init_drivers rgb_matrix_config.mode = 0. Write default values to EEPROM.\n");
+      eeconfig_update_rgb_matrix_default();
+      rgb_matrix_config.raw = eeconfig_read_rgb_matrix();
+  }
+  eeconfig_debug_rgb_matrix(); // display current eeprom values
+}
+
+void rgb_matrix_setup_drivers(void) {
+  // Initialize TWI
+  i2c_init();
+  IS31FL3731_init( DRIVER_ADDR_1 );
+  IS31FL3731_init( DRIVER_ADDR_2 );
+
+  for ( int index = 0; index < DRIVER_LED_TOTAL; index++ ) {
+    bool enabled = true;
+    // This only caches it for later
+    IS31FL3731_set_led_control_register( index, enabled, enabled, enabled );
+  }
+  // This actually updates the LED drivers
+  IS31FL3731_update_led_control_registers( DRIVER_ADDR_1, DRIVER_ADDR_2 );
 }
 
 // Deals with the messy details of incrementing an integer
diff --git a/quantum/rgb_matrix.h b/quantum/rgb_matrix.h
index aaa85d5f5..576931400 100644
--- a/quantum/rgb_matrix.h
+++ b/quantum/rgb_matrix.h
@@ -95,7 +95,8 @@ void rgb_matrix_indicators_user(void);
 
 void rgb_matrix_single_LED_test(void);
 
-void rgb_matrix_init_drivers(void);
+void rgb_matrix_init(void);
+void rgb_matrix_setup_drivers(void);
 
 void rgb_matrix_set_suspend_state(bool state);
 void rgb_matrix_set_indicator_state(uint8_t state);