diff --git a/left/src/init_peripherals.c b/left/src/init_peripherals.c
index 1e538ca..8dac980 100644
--- a/left/src/init_peripherals.c
+++ b/left/src/init_peripherals.c
@@ -12,24 +12,19 @@
 
 static void i2cSlaveCallback(I2C_Type *base, i2c_slave_transfer_t *xfer, void *userData)
 {
-    switch (xfer->event)
-    {
+    switch (xfer->event) {
         case kI2C_SlaveTransmitEvent:
             SlaveProtocolHandler();
-            xfer->data = SlaveTxBuffer;
-            xfer->dataSize = SlaveTxSize;
+            xfer->data = (uint8_t*)&txMessage;
+            xfer->dataSize = txMessage.length+3;
             break;
         case kI2C_SlaveReceiveEvent:
+            xfer->data = (uint8_t*)&rxMessage;
+//            xfer->dataSize = I2C_BUFFER_MAX_LENGTH;
             SlaveProtocolHandler();
-            xfer->data = SlaveRxBuffer;
-            xfer->dataSize = SLAVE_RX_BUFFER_SIZE;
             break;
         case kI2C_SlaveCompletionEvent:
-            xfer->data = NULL;
-            xfer->dataSize = 0;
-            break;
-        case kI2C_SlaveTransmitAckEvent:
-            break;
+            //SlaveProtocolHandler();
         default:
             break;
     }
diff --git a/left/src/slave_protocol_handler.c b/left/src/slave_protocol_handler.c
index 56f100f..400f813 100644
--- a/left/src/slave_protocol_handler.c
+++ b/left/src/slave_protocol_handler.c
@@ -10,12 +10,15 @@
 #include "bool_array_converter.h"
 #include "bootloader.h"
 
+i2c_message_t rxMessage;
+i2c_message_t txMessage;
+
 void SetError(uint8_t error);
 void SetGenericError(void);
 void SetResponseByte(uint8_t response);
 
 void SetError(uint8_t error) {
-    SlaveTxBuffer[0] = error;
+    txMessage.data[0] = error;
 }
 
 void SetGenericError(void)
@@ -26,26 +29,26 @@ void SetGenericError(void)
 // Set a single byte as the response.
 void SetResponseByte(uint8_t response)
 {
-    SlaveTxBuffer[1] = response;
+    txMessage.data[1] = response;
 }
 
 void SlaveProtocolHandler(void)
 {
-    uint8_t commandId = SlaveRxBuffer[0];
+    uint8_t commandId = rxMessage.data[0];
     switch (commandId) {
         case SlaveCommand_RequestKeyStates:
-            SlaveTxSize = KEY_STATE_BUFFER_SIZE;
-            BoolBytesToBits(keyMatrix.keyStates, SlaveTxBuffer, LEFT_KEYBOARD_HALF_KEY_COUNT);
-            CRC16_AppendToMessage(SlaveTxBuffer, KEY_STATE_SIZE);
+            BoolBytesToBits(keyMatrix.keyStates, txMessage.data, LEFT_KEYBOARD_HALF_KEY_COUNT);
+            txMessage.length = KEY_STATE_SIZE;
+            CRC16_UpdateMessageChecksum(&txMessage);
             break;
         case SlaveCommand_SetTestLed:
-            SlaveTxSize = 0;
-//            bool isLedOn = SlaveRxBuffer[1];
-//            TEST_LED_SET(isLedOn);
+            txMessage.length = 0;
+            bool isLedOn = rxMessage.data[1];
+            TEST_LED_SET(isLedOn);
             break;
         case SlaveCommand_SetLedPwmBrightness:
-            SlaveTxSize = 0;
-            uint8_t brightnessPercent = SlaveRxBuffer[1];
+            txMessage.length = 0;
+            uint8_t brightnessPercent = rxMessage.data[1];
             LedPwm_SetBrightness(brightnessPercent);
             break;
         case SlaveCommand_JumpToBootloader:
diff --git a/left/src/slave_protocol_handler.h b/left/src/slave_protocol_handler.h
index 1ddac7b..82dfdf0 100644
--- a/left/src/slave_protocol_handler.h
+++ b/left/src/slave_protocol_handler.h
@@ -5,24 +5,20 @@
 
     #include "fsl_port.h"
     #include "crc16.h"
+    #include "slave_protocol.h"
 
 // Macros:
 
-    #define SLAVE_RX_BUFFER_SIZE 100
-    #define SLAVE_TX_BUFFER_SIZE 100
-
     #define PROTOCOL_RESPONSE_SUCCESS       0
     #define PROTOCOL_RESPONSE_GENERIC_ERROR 1
 
     #define LEFT_KEYBOARD_HALF_KEY_COUNT (KEYBOARD_MATRIX_COLS_NUM*KEYBOARD_MATRIX_ROWS_NUM)
     #define KEY_STATE_SIZE (LEFT_KEYBOARD_HALF_KEY_COUNT/8 + 1)
-    #define KEY_STATE_BUFFER_SIZE (KEY_STATE_SIZE + CRC16_HASH_LENGTH)
 
 // Variables:
 
-    uint8_t SlaveRxBuffer[SLAVE_RX_BUFFER_SIZE];
-    uint8_t SlaveTxBuffer[SLAVE_TX_BUFFER_SIZE];
-    uint8_t SlaveTxSize;
+    extern i2c_message_t rxMessage;
+    extern i2c_message_t txMessage;
 
 // Functions:
 
diff --git a/lib/KSDK_2.0_MK22FN512xxx12 b/lib/KSDK_2.0_MK22FN512xxx12
index cd173f9..7142722 160000
--- a/lib/KSDK_2.0_MK22FN512xxx12
+++ b/lib/KSDK_2.0_MK22FN512xxx12
@@ -1 +1 @@
-Subproject commit cd173f98d1710558c518a101957d6d0c5b1b11ed
+Subproject commit 7142722eed9452c07fd88c7e3c9a41b1ca002059
diff --git a/right/src/i2c.c b/right/src/i2c.c
index 5e7067b..2021238 100644
--- a/right/src/i2c.c
+++ b/right/src/i2c.c
@@ -1,4 +1,5 @@
 #include "i2c.h"
+#include "crc16.h"
 
 i2c_master_handle_t I2cMasterHandle;
 i2c_master_transfer_t masterTransfer;
@@ -9,8 +10,19 @@ status_t I2cAsyncWrite(uint8_t i2cAddress, uint8_t *data, size_t dataSize)
     masterTransfer.direction = kI2C_Write;
     masterTransfer.data = data;
     masterTransfer.dataSize = dataSize;
-    status_t status = I2C_MasterTransferNonBlocking(I2C_MAIN_BUS_BASEADDR, &I2cMasterHandle, &masterTransfer);
-    return status;
+    I2cMasterHandle.userData = NULL;
+    return I2C_MasterTransferNonBlocking(I2C_MAIN_BUS_BASEADDR, &I2cMasterHandle, &masterTransfer);
+}
+
+status_t I2cAsyncWriteMessage(uint8_t i2cAddress, i2c_message_t *message)
+{
+    masterTransfer.slaveAddress = i2cAddress;
+    masterTransfer.direction = kI2C_Write;
+    masterTransfer.data = (uint8_t*)message;
+    masterTransfer.dataSize = message->length+3;
+    I2cMasterHandle.userData = NULL;
+    CRC16_UpdateMessageChecksum(message);
+    return I2C_MasterTransferNonBlocking(I2C_MAIN_BUS_BASEADDR, &I2cMasterHandle, &masterTransfer);
 }
 
 status_t I2cAsyncRead(uint8_t i2cAddress, uint8_t *data, size_t dataSize)
@@ -19,6 +31,16 @@ status_t I2cAsyncRead(uint8_t i2cAddress, uint8_t *data, size_t dataSize)
     masterTransfer.direction = kI2C_Read;
     masterTransfer.data = data;
     masterTransfer.dataSize = dataSize;
-    status_t status = I2C_MasterTransferNonBlocking(I2C_MAIN_BUS_BASEADDR, &I2cMasterHandle, &masterTransfer);
-    return status;
+    I2cMasterHandle.userData = NULL;
+    return I2C_MasterTransferNonBlocking(I2C_MAIN_BUS_BASEADDR, &I2cMasterHandle, &masterTransfer);
+}
+
+status_t I2cAsyncReadMessage(uint8_t i2cAddress, i2c_message_t *message)
+{
+    masterTransfer.slaveAddress = i2cAddress;
+    masterTransfer.direction = kI2C_Read;
+    masterTransfer.data = (uint8_t*)message;
+    masterTransfer.dataSize = I2C_MESSAGE_MAX_LENGTH;
+    I2cMasterHandle.userData = (void*)1;
+    return I2C_MasterTransferNonBlocking(I2C_MAIN_BUS_BASEADDR, &I2cMasterHandle, &masterTransfer);
 }
diff --git a/right/src/i2c.h b/right/src/i2c.h
index 752e40e..8a95302 100644
--- a/right/src/i2c.h
+++ b/right/src/i2c.h
@@ -4,6 +4,7 @@
 // Includes:
 
     #include "fsl_i2c.h"
+    #include "slave_protocol.h"
 
 // Macros:
 
@@ -45,5 +46,7 @@
 
     status_t I2cAsyncWrite(uint8_t i2cAddress, uint8_t *data, size_t dataSize);
     status_t I2cAsyncRead(uint8_t i2cAddress, uint8_t *data, size_t dataSize);
+    status_t I2cAsyncWriteMessage(uint8_t i2cAddress, i2c_message_t *message);
+    status_t I2cAsyncReadMessage(uint8_t i2cAddress, i2c_message_t *message);
 
 #endif
diff --git a/right/src/slave_drivers/uhk_module_driver.c b/right/src/slave_drivers/uhk_module_driver.c
index 00e45c1..d0cebfd 100644
--- a/right/src/slave_drivers/uhk_module_driver.c
+++ b/right/src/slave_drivers/uhk_module_driver.c
@@ -10,8 +10,9 @@
 
 uhk_module_state_t UhkModuleStates[UHK_MODULE_MAX_COUNT];
 uhk_module_phase_t uhkModulePhase = UhkModulePhase_RequestKeyStates;
-uint8_t txBuffer[2];
-uint8_t rxBuffer[KEY_STATE_BUFFER_SIZE];
+
+i2c_message_t rxMessage;
+i2c_message_t txMessage;
 
 void UhkModuleSlaveDriver_Init(uint8_t uhkModuleId)
 {
@@ -27,31 +28,34 @@ status_t UhkModuleSlaveDriver_Update(uint8_t uhkModuleId)
 
     switch (uhkModulePhase) {
         case UhkModulePhase_RequestKeyStates:
-            txBuffer[0] = SlaveCommand_RequestKeyStates;
-            status = I2cAsyncWrite(I2C_ADDRESS_LEFT_KEYBOARD_HALF_FIRMWARE, txBuffer, 1);
+            txMessage.data[0] = SlaveCommand_RequestKeyStates;
+            txMessage.length = 1;
+            status = I2cAsyncWriteMessage(I2C_ADDRESS_LEFT_KEYBOARD_HALF_FIRMWARE, &txMessage);
             uhkModulePhase = UhkModulePhase_ReceiveKeystates;
             break;
         case UhkModulePhase_ReceiveKeystates:
-            status = I2cAsyncRead(I2C_ADDRESS_LEFT_KEYBOARD_HALF_FIRMWARE, rxBuffer, KEY_STATE_BUFFER_SIZE);
+            status = I2cAsyncReadMessage(I2C_ADDRESS_LEFT_KEYBOARD_HALF_FIRMWARE, &rxMessage);
             uhkModulePhase = UhkModulePhase_ProcessKeystates;
             break;
         case UhkModulePhase_ProcessKeystates:
-            if (CRC16_IsMessageValid(rxBuffer, KEY_STATE_SIZE)) {
-                BoolBitsToBytes(rxBuffer, CurrentKeyStates[SLOT_ID_LEFT_KEYBOARD_HALF], LEFT_KEYBOARD_HALF_KEY_COUNT);
+            if (CRC16_IsMessageValid(&rxMessage)) {
+                BoolBitsToBytes(rxMessage.data, CurrentKeyStates[SLOT_ID_LEFT_KEYBOARD_HALF], LEFT_KEYBOARD_HALF_KEY_COUNT);
             }
             status = kStatus_Uhk_NoOp;
-            uhkModulePhase = UhkModulePhase_SetLedPwmBrightness;
-            break;
-        case UhkModulePhase_SetLedPwmBrightness:
-            txBuffer[0] = SlaveCommand_SetLedPwmBrightness;
-            txBuffer[1] = uhkModuleInternalState->ledPwmBrightness;
-            status = I2cAsyncWrite(I2C_ADDRESS_LEFT_KEYBOARD_HALF_FIRMWARE, txBuffer, 2);
             uhkModulePhase = UhkModulePhase_SetTestLed;
             break;
         case UhkModulePhase_SetTestLed:
-            txBuffer[0] = SlaveCommand_SetTestLed;
-            txBuffer[1] = uhkModuleInternalState->isTestLedOn;
-            status = I2cAsyncWrite(I2C_ADDRESS_LEFT_KEYBOARD_HALF_FIRMWARE, txBuffer, 2);
+            txMessage.data[0] = SlaveCommand_SetTestLed;
+            txMessage.data[1] = uhkModuleInternalState->isTestLedOn;
+            txMessage.length = 2;
+            status = I2cAsyncWriteMessage(I2C_ADDRESS_LEFT_KEYBOARD_HALF_FIRMWARE, &txMessage);
+            uhkModulePhase = UhkModulePhase_SetLedPwmBrightness;
+            break;
+        case UhkModulePhase_SetLedPwmBrightness:
+            txMessage.data[0] = SlaveCommand_SetLedPwmBrightness;
+            txMessage.data[1] = uhkModuleInternalState->ledPwmBrightness;
+            txMessage.length = 2;
+            status = I2cAsyncWriteMessage(I2C_ADDRESS_LEFT_KEYBOARD_HALF_FIRMWARE, &txMessage);
             uhkModulePhase = UhkModulePhase_RequestKeyStates;
             break;
     }
diff --git a/right/src/slave_drivers/uhk_module_driver.h b/right/src/slave_drivers/uhk_module_driver.h
index 3a081d7..809ee7c 100644
--- a/right/src/slave_drivers/uhk_module_driver.h
+++ b/right/src/slave_drivers/uhk_module_driver.h
@@ -10,7 +10,6 @@
 
     #define UHK_MODULE_MAX_COUNT 3
     #define KEY_STATE_SIZE (LEFT_KEYBOARD_HALF_KEY_COUNT/8 + 1)
-    #define KEY_STATE_BUFFER_SIZE (KEY_STATE_SIZE + CRC16_HASH_LENGTH)
 
 // Typedefs:
 
diff --git a/shared/crc16.c b/shared/crc16.c
index 30027e6..d7a088d 100644
--- a/shared/crc16.c
+++ b/shared/crc16.c
@@ -36,21 +36,20 @@ void crc16_finalize(crc16_data_t *crc16Config, uint16_t *hash)
 
 crc16_data_t crc16data;
 
-void CRC16_AppendToMessage(uint8_t *message, uint32_t lengthInBytes)
+void CRC16_UpdateMessageChecksum(i2c_message_t *message)
 {
     uint16_t hash;
     crc16_init(&crc16data);
-    crc16_update(&crc16data, message, lengthInBytes);
+    crc16_update(&crc16data, message->data, message->length);
     crc16_finalize(&crc16data, &hash);
-    message[lengthInBytes] = hash & 0xff;
-    message[lengthInBytes+1] = hash >> 8;
+    message->crc = hash;
 }
 
-bool CRC16_IsMessageValid(uint8_t *message, uint32_t lengthInBytes)
+bool CRC16_IsMessageValid(i2c_message_t *message)
 {
     uint16_t hash;
     crc16_init(&crc16data);
-    crc16_update(&crc16data, message, lengthInBytes);
+    crc16_update(&crc16data, message->data, message->length);
     crc16_finalize(&crc16data, &hash);
-    return (message[lengthInBytes] == (hash & 0xff)) && (message[lengthInBytes+1] == (hash >> 8));
+    return message->crc == hash;
 }
diff --git a/shared/crc16.h b/shared/crc16.h
index dc28054..f399166 100644
--- a/shared/crc16.h
+++ b/shared/crc16.h
@@ -3,6 +3,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#include "slave_protocol.h"
 
 #define CRC16_HASH_LENGTH 2 // bytes
 
@@ -25,7 +26,7 @@ void crc16_update(crc16_data_t *crc16Config, const uint8_t *src, uint32_t length
 //! @param hash Pointer to the value returned for the final calculated crc value.
 void crc16_finalize(crc16_data_t *crc16Config, uint16_t *hash);
 
-void CRC16_AppendToMessage(uint8_t *message, uint32_t lengthInBytes);
-bool CRC16_IsMessageValid(uint8_t *message, uint32_t lengthInBytes);
+void CRC16_UpdateMessageChecksum(i2c_message_t *message);
+bool CRC16_IsMessageValid(i2c_message_t *message);
 
 #endif
diff --git a/shared/slave_protocol.h b/shared/slave_protocol.h
index 0c357da..2dedb69 100644
--- a/shared/slave_protocol.h
+++ b/shared/slave_protocol.h
@@ -1,6 +1,11 @@
 #ifndef __SLAVE_PROTOCOL_H__
 #define __SLAVE_PROTOCOL_H__
 
+// Macros:
+
+    #define I2C_MESSAGE_MAX_LENGTH 255
+    #define I2C_BUFFER_MAX_LENGTH (I2C_MESSAGE_MAX_LENGTH + 3)
+
 // Typedefs:
 
     typedef enum {
@@ -10,4 +15,10 @@
         SlaveCommand_JumpToBootloader,
     } slave_command_t;
 
+    typedef struct {
+        uint8_t length;
+        uint16_t crc;
+        uint8_t data[I2C_MESSAGE_MAX_LENGTH];
+    } __attribute__ ((packed)) i2c_message_t;
+
 #endif