Rgb matrix fixes, I2C library can now retry if it has failed (#2943)

* Added Modular keyboards L,R and NUM

Created code modules for the 3 modules of the modular keyboard.
Original idea by MechboardsUK. Uses i2c implementation similar to lets
split

* Remove modular from master

This is to fix incorrect branching

* General fixes for RGB_matrix

- Complited speed support for all effects
- Fixed raindrop effects to initialized after toggle
- Fixed raindrop effects to use all available LEDs
- Fixed effect step reverse function
- Moved RGB_MATRIX_SOLID_REACTIVE under correct flag

* Documentation update for RGBmatrix

* More doc updates

* I2C library can now retry if it has failed

- Replaced the original TWIlib by LFKeyboard's modified version
- Allows for an extra argument on TWITransmitData, if blocking is set to 1 function will retry to transmit on failure. Good for noisy boards.

* RGB Matrix, use alternative I2C library

TWIlib seems to be hanging for me sometimes probably due to ISR routine. I have used i2c_master as a good alternative.

Note: this commit is for Wilba6582 to verify before merge

* Update rgb_matrix.c

* RGB matrix cleanup

- Remove TWIlib

7 files changed, 185 insertions, 344 deletions

diff --git a/common_features.mk b/common_features.mk
index 7ba7d48154..0778f8e0f5 100644
--- a/common_features.mk
+++ b/common_features.mk
@@ -117,7 +117,7 @@ endif
 ifeq ($(strip $(RGB_MATRIX_ENABLE)), yes)
     OPT_DEFS += -DRGB_MATRIX_ENABLE
     SRC += is31fl3731.c
-    SRC += TWIlib.c
+    SRC += i2c_master.c
     SRC += $(QUANTUM_DIR)/color.c
     SRC += $(QUANTUM_DIR)/rgb_matrix.c
     CIE1931_CURVE = yes
diff --git a/drivers/avr/TWIlib.c b/drivers/avr/TWIlib.c
deleted file mode 100644
index b39e3054a5..0000000000
--- a/drivers/avr/TWIlib.c
+++ /dev/null
@@ -1,232 +0,0 @@
-/*
- * TWIlib.c
- *
- *  Created: 6/01/2014 10:41:33 PM
- *  Author: Chris Herring
- *  http://www.chrisherring.net/all/tutorial-interrupt-driven-twi-interface-for-avr-part1/
- */ 
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include "TWIlib.h"
-#include "util/delay.h"
-
-void TWIInit()
-{
-	TWIInfo.mode = Ready;
-	TWIInfo.errorCode = 0xFF;
-	TWIInfo.repStart = 0;
-	// Set pre-scalers (no pre-scaling)
-	TWSR = 0;
-	// Set bit rate
-	TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;
-	// Enable TWI and interrupt
-	TWCR = (1 << TWIE) | (1 << TWEN);
-}
-
-uint8_t isTWIReady()
-{
-	if ( (TWIInfo.mode == Ready) | (TWIInfo.mode == RepeatedStartSent) )
-	{
-		return 1;
-	}
-	else
-	{
-		return 0;
-	}
-}
-
-uint8_t TWITransmitData(void *const TXdata, uint8_t dataLen, uint8_t repStart)
-{
-	if (dataLen <= TXMAXBUFLEN)
-	{
-		// Wait until ready
-		while (!isTWIReady()) {_delay_us(1);}
-		// Set repeated start mode
-		TWIInfo.repStart = repStart;
-		// Copy data into the transmit buffer
-		uint8_t *data = (uint8_t *)TXdata;
-		for (int i = 0; i < dataLen; i++)
-		{
-			TWITransmitBuffer[i] = data[i];
-		}
-		// Copy transmit info to global variables
-		TXBuffLen = dataLen;
-		TXBuffIndex = 0;
-		
-		// If a repeated start has been sent, then devices are already listening for an address
-		// and another start does not need to be sent. 
-		if (TWIInfo.mode == RepeatedStartSent)
-		{
-			TWIInfo.mode = Initializing;
-			TWDR = TWITransmitBuffer[TXBuffIndex++]; // Load data to transmit buffer
-			TWISendTransmit(); // Send the data
-		}
-		else // Otherwise, just send the normal start signal to begin transmission.
-		{
-			TWIInfo.mode = Initializing;
-			TWISendStart();
-		}
-		
-	}
-	else
-	{
-		return 1; // return an error if data length is longer than buffer
-	}
-	return 0;
-}
-
-uint8_t TWIReadData(uint8_t TWIaddr, uint8_t bytesToRead, uint8_t repStart)
-{
-	// Check if number of bytes to read can fit in the RXbuffer
-	if (bytesToRead < RXMAXBUFLEN)
-	{
-		// Reset buffer index and set RXBuffLen to the number of bytes to read
-		RXBuffIndex = 0;
-		RXBuffLen = bytesToRead;
-		// Create the one value array for the address to be transmitted
-		uint8_t TXdata[1];
-		// Shift the address and AND a 1 into the read write bit (set to write mode)
-		TXdata[0] = (TWIaddr << 1) | 0x01;
-		// Use the TWITransmitData function to initialize the transfer and address the slave
-		TWITransmitData(TXdata, 1, repStart);
-	}
-	else
-	{
-		return 0;
-	}
-	return 1;
-}
-
-ISR (TWI_vect)
-{
-	switch (TWI_STATUS)
-	{
-		// ----\/ ---- MASTER TRANSMITTER OR WRITING ADDRESS ----\/ ----  //
-		case TWI_MT_SLAW_ACK: // SLA+W transmitted and ACK received
-		// Set mode to Master Transmitter
-		TWIInfo.mode = MasterTransmitter;
-		case TWI_START_SENT: // Start condition has been transmitted
-		case TWI_MT_DATA_ACK: // Data byte has been transmitted, ACK received
-			if (TXBuffIndex < TXBuffLen) // If there is more data to send
-			{
-				TWDR = TWITransmitBuffer[TXBuffIndex++]; // Load data to transmit buffer
-				TWIInfo.errorCode = TWI_NO_RELEVANT_INFO;
-				TWISendTransmit(); // Send the data
-			}
-			// This transmission is complete however do not release bus yet
-			else if (TWIInfo.repStart)
-			{
-				TWIInfo.errorCode = 0xFF;
-				TWISendStart();
-			}
-			// All transmissions are complete, exit
-			else
-			{
-				TWIInfo.mode = Ready;
-				TWIInfo.errorCode = 0xFF;
-				TWISendStop();
-			}
-			break;
-		
-		// ----\/ ---- MASTER RECEIVER ----\/ ----  //
-		
-		case TWI_MR_SLAR_ACK: // SLA+R has been transmitted, ACK has been received
-			// Switch to Master Receiver mode
-			TWIInfo.mode = MasterReceiver;
-			// If there is more than one byte to be read, receive data byte and return an ACK
-			if (RXBuffIndex < RXBuffLen-1)
-			{
-				TWIInfo.errorCode = TWI_NO_RELEVANT_INFO;
-				TWISendACK();
-			}
-			// Otherwise when a data byte (the only data byte) is received, return NACK
-			else
-			{
-				TWIInfo.errorCode = TWI_NO_RELEVANT_INFO;
-				TWISendNACK();
-			}
-			break;
-		
-		case TWI_MR_DATA_ACK: // Data has been received, ACK has been transmitted.
-		
-			/// -- HANDLE DATA BYTE --- ///
-			TWIReceiveBuffer[RXBuffIndex++] = TWDR;
-			// If there is more than one byte to be read, receive data byte and return an ACK
-			if (RXBuffIndex < RXBuffLen-1)
-			{
-				TWIInfo.errorCode = TWI_NO_RELEVANT_INFO;
-				TWISendACK();
-			}
-			// Otherwise when a data byte (the only data byte) is received, return NACK
-			else
-			{
-				TWIInfo.errorCode = TWI_NO_RELEVANT_INFO;
-				TWISendNACK();
-			}
-			break;
-		
-		case TWI_MR_DATA_NACK: // Data byte has been received, NACK has been transmitted. End of transmission.
-		
-			/// -- HANDLE DATA BYTE --- ///
-			TWIReceiveBuffer[RXBuffIndex++] = TWDR;	
-			// This transmission is complete however do not release bus yet
-			if (TWIInfo.repStart)
-			{
-				TWIInfo.errorCode = 0xFF;
-				TWISendStart();
-			}
-			// All transmissions are complete, exit
-			else
-			{
-				TWIInfo.mode = Ready;
-				TWIInfo.errorCode = 0xFF;
-				TWISendStop();
-			}
-			break;
-		
-		// ----\/ ---- MT and MR common ----\/ ---- //
-		
-		case TWI_MR_SLAR_NACK: // SLA+R transmitted, NACK received
-		case TWI_MT_SLAW_NACK: // SLA+W transmitted, NACK received
-		case TWI_MT_DATA_NACK: // Data byte has been transmitted, NACK received
-		case TWI_LOST_ARBIT: // Arbitration has been lost
-			// Return error and send stop and set mode to ready
-			if (TWIInfo.repStart)
-			{				
-				TWIInfo.errorCode = TWI_STATUS;
-				TWISendStart();
-			}
-			// All transmissions are complete, exit
-			else
-			{
-				TWIInfo.mode = Ready;
-				TWIInfo.errorCode = TWI_STATUS;
-				TWISendStop();
-			}
-			break;
-		case TWI_REP_START_SENT: // Repeated start has been transmitted
-			// Set the mode but DO NOT clear TWINT as the next data is not yet ready
-			TWIInfo.mode = RepeatedStartSent;
-			break;
-		
-		// ----\/ ---- SLAVE RECEIVER ----\/ ----  //
-		
-		// TODO  IMPLEMENT SLAVE RECEIVER FUNCTIONALITY
-		
-		// ----\/ ---- SLAVE TRANSMITTER ----\/ ----  //
-		
-		// TODO  IMPLEMENT SLAVE TRANSMITTER FUNCTIONALITY
-		
-		// ----\/ ---- MISCELLANEOUS STATES ----\/ ----  //
-		case TWI_NO_RELEVANT_INFO: // It is not really possible to get into this ISR on this condition
-								   // Rather, it is there to be manually set between operations
-			break;
-		case TWI_ILLEGAL_START_STOP: // Illegal START/STOP, abort and return error
-			TWIInfo.errorCode = TWI_ILLEGAL_START_STOP;
-			TWIInfo.mode = Ready;
-			TWISendStop();
-			break;
-	}
-	
-}
diff --git a/drivers/avr/TWIlib.h b/drivers/avr/TWIlib.h
deleted file mode 100644
index 23fd1f09aa..0000000000
--- a/drivers/avr/TWIlib.h
+++ /dev/null
@@ -1,82 +0,0 @@
-/*
- * TWIlib.h
- *
- * Created: 6/01/2014 10:38:42 PM
- *  Author: Chris Herring
- *  http://www.chrisherring.net/all/tutorial-interrupt-driven-twi-interface-for-avr-part1/
- */ 
-
-
-#ifndef TWILIB_H_
-#define TWILIB_H_
-// TWI bit rate (was 100000)
-#define TWI_FREQ 400000
-// Get TWI status
-#define TWI_STATUS	(TWSR & 0xF8) 
-// Transmit buffer length
-#define TXMAXBUFLEN 20
-// Receive buffer length
-#define RXMAXBUFLEN 20
-// Global transmit buffer
-uint8_t TWITransmitBuffer[TXMAXBUFLEN];
-// Global receive buffer
-volatile uint8_t TWIReceiveBuffer[RXMAXBUFLEN];
-// Buffer indexes
-volatile int TXBuffIndex; // Index of the transmit buffer. Is volatile, can change at any time.
-int RXBuffIndex; // Current index in the receive buffer
-// Buffer lengths
-int TXBuffLen; // The total length of the transmit buffer
-int RXBuffLen; // The total number of bytes to read (should be less than RXMAXBUFFLEN)
-
-typedef enum {
-	Ready,
-	Initializing,
-	RepeatedStartSent,
-	MasterTransmitter,
-	MasterReceiver,
-	SlaceTransmitter,
-	SlaveReciever
-	} TWIMode;
-
- typedef struct TWIInfoStruct{
-	TWIMode mode;
-	uint8_t errorCode;
-	uint8_t repStart;	
-	}TWIInfoStruct;
-TWIInfoStruct TWIInfo;
-
-
-// TWI Status Codes
-#define TWI_START_SENT			0x08 // Start sent
-#define TWI_REP_START_SENT		0x10 // Repeated Start sent
-// Master Transmitter Mode
-#define TWI_MT_SLAW_ACK			0x18 // SLA+W sent and ACK received
-#define TWI_MT_SLAW_NACK		0x20 // SLA+W sent and NACK received
-#define TWI_MT_DATA_ACK			0x28 // DATA sent and ACK received
-#define TWI_MT_DATA_NACK		0x30 // DATA sent and NACK received
-// Master Receiver Mode
-#define TWI_MR_SLAR_ACK			0x40 // SLA+R sent, ACK received
-#define TWI_MR_SLAR_NACK		0x48 // SLA+R sent, NACK received
-#define TWI_MR_DATA_ACK			0x50 // Data received, ACK returned
-#define TWI_MR_DATA_NACK		0x58 // Data received, NACK returned
-
-// Miscellaneous States
-#define TWI_LOST_ARBIT			0x38 // Arbitration has been lost
-#define TWI_NO_RELEVANT_INFO	0xF8 // No relevant information available
-#define TWI_ILLEGAL_START_STOP	0x00 // Illegal START or STOP condition has been detected
-#define TWI_SUCCESS				0xFF // Successful transfer, this state is impossible from TWSR as bit2 is 0 and read only
-
-
-#define TWISendStart()		(TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN)|(1<<TWIE)) // Send the START signal, enable interrupts and TWI, clear TWINT flag to resume transfer.
-#define TWISendStop()		(TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN)|(1<<TWIE)) // Send the STOP signal, enable interrupts and TWI, clear TWINT flag.
-#define TWISendTransmit()	(TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWIE)) // Used to resume a transfer, clear TWINT and ensure that TWI and interrupts are enabled.
-#define TWISendACK()		(TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWIE)|(1<<TWEA)) // FOR MR mode. Resume a transfer, ensure that TWI and interrupts are enabled and respond with an ACK if the device is addressed as a slave or after it receives a byte.
-#define TWISendNACK()		(TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWIE)) // FOR MR mode. Resume a transfer, ensure that TWI and interrupts are enabled but DO NOT respond with an ACK if the device is addressed as a slave or after it receives a byte.
-
-// Function declarations
-uint8_t TWITransmitData(void *const TXdata, uint8_t dataLen, uint8_t repStart);
-void TWIInit(void);
-uint8_t TWIReadData(uint8_t TWIaddr, uint8_t bytesToRead, uint8_t repStart);
-uint8_t isTWIReady(void);
-
-#endif // TWICOMMS_H_ 
\ No newline at end of file
diff --git a/drivers/avr/i2c_master.c b/drivers/avr/i2c_master.c
new file mode 100755
index 0000000000..f4a4bb7b0b
--- /dev/null
+++ b/drivers/avr/i2c_master.c
@@ -0,0 +1,149 @@
+/* Library made by: g4lvanix
+ * Github repository: https://github.com/g4lvanix/I2C-master-lib
+ */
+
+#include <avr/io.h>
+#include <util/twi.h>
+
+#include "i2c_master.h"
+
+#define F_SCL 400000UL // SCL frequency
+#define Prescaler 1
+#define TWBR_val ((((F_CPU / F_SCL) / Prescaler) - 16 ) / 2)
+
+void i2c_init(void)
+{
+	TWBR = (uint8_t)TWBR_val;
+}
+
+uint8_t i2c_start(uint8_t address)
+{
+	// reset TWI control register
+	TWCR = 0;
+	// transmit START condition 
+	TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
+	// wait for end of transmission
+	while( !(TWCR & (1<<TWINT)) );
+	
+	// check if the start condition was successfully transmitted
+	if((TWSR & 0xF8) != TW_START){ return 1; }
+	
+	// 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)) );
+	
+	// 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;
+}
+
+uint8_t i2c_write(uint8_t data)
+{
+	// 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;
+}
+
+uint8_t i2c_read_ack(void)
+{
+	
+	// start TWI module and acknowledge data after reception
+	TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); 
+	// wait for end of transmission
+	while( !(TWCR & (1<<TWINT)) );
+	// return received data from TWDR
+	return TWDR;
+}
+
+uint8_t i2c_read_nack(void)
+{
+	
+	// start receiving without acknowledging reception
+	TWCR = (1<<TWINT) | (1<<TWEN);
+	// wait for end of transmission
+	while( !(TWCR & (1<<TWINT)) );
+	// return received data from TWDR
+	return TWDR;
+}
+
+uint8_t i2c_transmit(uint8_t address, uint8_t* data, uint16_t length)
+{
+	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;
+}
+
+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;
+	}
+
+	i2c_stop();
+
+	return 0;
+}
+
+uint8_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length)
+{
+	if (i2c_start(devaddr)) return 1;
+
+	i2c_write(regaddr);
+
+	if (i2c_start(devaddr | 0x01)) 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;
+}
+
+void i2c_stop(void)
+{
+	// transmit STOP condition
+	TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
+}
diff --git a/drivers/avr/i2c_master.h b/drivers/avr/i2c_master.h
new file mode 100755
index 0000000000..2479d28d52
--- /dev/null
+++ b/drivers/avr/i2c_master.h
@@ -0,0 +1,22 @@
+/* Library made by: g4lvanix
+ * Github repository: https://github.com/g4lvanix/I2C-master-lib
+ */
+
+#ifndef I2C_MASTER_H
+#define I2C_MASTER_H
+
+#define I2C_READ 0x01
+#define I2C_WRITE 0x00
+
+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);
+
+#endif // I2C_MASTER_H
diff --git a/drivers/avr/is31fl3731.c b/drivers/avr/is31fl3731.c
index e5941cf41e..c7a99e3a3d 100644
--- a/drivers/avr/is31fl3731.c
+++ b/drivers/avr/is31fl3731.c
@@ -20,7 +20,7 @@
 #include <avr/io.h>
 #include <util/delay.h>
 #include <string.h>
-#include "TWIlib.h"
+#include "i2c_master.h"
 #include "progmem.h"
 
 // This is a 7-bit address, that gets left-shifted and bit 0
@@ -50,7 +50,7 @@
 #define ISSI_BANK_FUNCTIONREG 0x0B    // helpfully called 'page nine'
 
 // Transfer buffer for TWITransmitData()
-uint8_t g_twi_transfer_buffer[TXMAXBUFLEN];
+uint8_t g_twi_transfer_buffer[20];
 
 // These buffers match the IS31FL3731 PWM registers 0x24-0xB3.
 // Storing them like this is optimal for I2C transfers to the registers.
@@ -80,17 +80,11 @@ bool g_led_control_registers_update_required = false;
 
 void IS31FL3731_write_register( uint8_t addr, uint8_t reg, uint8_t data )
 {
-	g_twi_transfer_buffer[0] = (addr << 1) | 0x00;
-	g_twi_transfer_buffer[1] = reg;
-	g_twi_transfer_buffer[2] = data;
-
-	// Set the error code to have no relevant information
-	TWIInfo.errorCode = TWI_NO_RELEVANT_INFO;
-	// Continuously attempt to transmit data until a successful transmission occurs
-	//while ( TWIInfo.errorCode != 0xFF )
-	//{
-		TWITransmitData( g_twi_transfer_buffer, 3, 0 );
-	//}
+	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); 
 }
 
 void IS31FL3731_write_pwm_buffer( uint8_t addr, uint8_t *pwm_buffer )
@@ -100,29 +94,21 @@ void IS31FL3731_write_pwm_buffer( uint8_t addr, uint8_t *pwm_buffer )
 	// transmit PWM registers in 9 transfers of 16 bytes
 	// g_twi_transfer_buffer[] is 20 bytes
 
-	// set the I2C address
-	g_twi_transfer_buffer[0] = (addr << 1) | 0x00;
-
 	// iterate over the pwm_buffer contents at 16 byte intervals
 	for ( int i = 0; i < 144; i += 16 )
 	{
 		// set the first register, e.g. 0x24, 0x34, 0x44, etc.
-		g_twi_transfer_buffer[1] = 0x24 + i;
+		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++ )
 		{
-			g_twi_transfer_buffer[2 + j] = pwm_buffer[i + j];
+			g_twi_transfer_buffer[1 + j] = pwm_buffer[i + j];
 		}
 
-		// Set the error code to have no relevant information
-		TWIInfo.errorCode = TWI_NO_RELEVANT_INFO;
-		// Continuously attempt to transmit data until a successful transmission occurs
-		while ( TWIInfo.errorCode != 0xFF )
-		{
-			TWITransmitData( g_twi_transfer_buffer, 16 + 2, 0 );
-		}
+		//Transmit buffer until succesful
+		while(i2c_transmit(addr << 1, g_twi_transfer_buffer,17) != 0);
 	}
 }
 
diff --git a/quantum/rgb_matrix.c b/quantum/rgb_matrix.c
index f3d012bc3e..992ce99de9 100644
--- a/quantum/rgb_matrix.c
+++ b/quantum/rgb_matrix.c
@@ -18,7 +18,7 @@
 
 #include "rgb_matrix.h"
 #include <avr/io.h>
-#include "TWIlib.h"
+#include "i2c_master.h"
 #include <util/delay.h>
 #include <avr/interrupt.h>
 #include "progmem.h"
@@ -722,10 +722,8 @@ void rgb_matrix_indicators_user(void) {}
 // }
 
 void rgb_matrix_init_drivers(void) {
-    //sei();
-
     // Initialize TWI
-    TWIInit();
+    i2c_init();
     IS31FL3731_init( DRIVER_ADDR_1 );
     IS31FL3731_init( DRIVER_ADDR_2 );