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 7ba7d4815..0778f8e0f 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 b39e3054a..000000000
--- 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 23fd1f09a..000000000
--- 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 000000000..f4a4bb7b0
--- /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 000000000..2479d28d5
--- /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 e5941cf41..c7a99e3a3 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 f3d012bc3..992ce99de 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 );