diff --git a/Protocol/CAN.cpp b/Protocol/CAN.cpp
index c2b1777..ad521e8 100644
--- a/Protocol/CAN.cpp
+++ b/Protocol/CAN.cpp
@@ -1,4 +1,5 @@
 #include "CAN.h"
+#include "DSP2833x_Device.h"
 
 namespace canSpace {
 
@@ -119,6 +120,28 @@ void CAN::updateTXMessage(Uint16 boxNumber, const CANMessage& message){ // TODO
 }
 
 
+void CAN::updateTXMessage(Uint16 boxNumber, const Uint32& message){ // TODO check trs bit and set it at the end if it was here. Once the TRS bit is set for a mailbox and then data is changed in the mailbox using the CDR
+                                                                        // bit, the CAN module fails to transmit the new data and transmits the old data instead. To avoid this,
+                                                                        // reset transmission in that mailbox using the TRRn bit and set the TRSn bit again. The new data is
+                                                                        // then transmitted.
+    if (boxNumber > 31) return;
+
+    volatile MBOX* p_MailBox(0);
+    p_MailBox = &(p_CanMBoxes_->MBOX0) + boxNumber;
+
+    // Set change data request (CDR bit + MBOX number)
+    p_CanRegs_->CANMC.all |= (128 + boxNumber);
+
+    // TODO Add lenght changing?
+    p_MailBox->MDL.all = message;
+    p_MailBox->MDH.all = message >> 16;
+
+    CanShadow_.CANMC.all = p_CanRegs_->CANMC.all;
+    CanShadow_.CANMC.bit.CDR = 0;
+    p_CanRegs_->CANMC.all = CanShadow_.CANMC.all;
+}
+
+
 void CAN::sendRemoteRequest(Uint16 boxNumber){
     if (boxNumber > 31) return;
 
@@ -179,6 +202,37 @@ int16 CAN::receiveMsg(Uint16 boxNumber, CANMessage& rxMessage){ // TODO faults j
 }
 
 
+int16 CAN::receiveMsg(Uint16 boxNumber, Uint32& rxMessage){ // TODO faults just return -1
+    if (boxNumber > 31) { return -1; }
+
+    Uint32 mboxControl(0);
+    mboxControl = 1ul << boxNumber;
+
+    volatile MBOX* p_MailBox(0);
+    p_MailBox = &(p_CanMBoxes_->MBOX0) + boxNumber;
+
+    bool isNewMessageInBox = p_CanRegs_->CANRMP.all & mboxControl;
+    if(!isNewMessageInBox) return -2;
+
+    p_CanRegs_->CANRMP.all &= mboxControl;
+
+    rxMessage = p_MailBox->MDL.all;
+    rxMessage = (p_MailBox->MDH.all) << 16;
+
+    bool newMessage;
+    bool lostMessage;
+
+    newMessage = p_CanRegs_->CANRMP.all & mboxControl;
+    lostMessage = p_CanRegs_->CANRML.all & mboxControl;
+
+    if(newMessage || lostMessage) {
+        return -3;
+    }
+
+    return 0;
+}
+
+
 bool CAN::isNewMessage(){
     return static_cast<bool>(p_CanRegs_->CANRMP.all);
 }
diff --git a/Protocol/CAN.h b/Protocol/CAN.h
index f38226c..f230c14 100644
--- a/Protocol/CAN.h
+++ b/Protocol/CAN.h
@@ -117,8 +117,10 @@ public:
     void transmitMsg(Uint16 boxNumber, const Uint32& message);
     void transmitMsg(Uint16 boxNumber, const Uint32& message, const Uint16 dlc);
     void updateTXMessage(Uint16 boxNumber, const CANMessage& message);
+    void updateTXMessage(Uint16 boxNumber, const Uint32& message);
     void sendRemoteRequest(Uint16 boxNumber);
     int16 receiveMsg(Uint16 boxNumber, CANMessage& rxMessage);
+    int16 receiveMsg(Uint16 boxNumber, Uint32& rxMessage);
 
 private:
     CAN_VARIANT canPort;
diff --git a/Protocol/CANConfig.cpp b/Protocol/CANConfig.cpp
index 5eface1..f5ca931 100644
--- a/Protocol/CANConfig.cpp
+++ b/Protocol/CANConfig.cpp
@@ -212,9 +212,8 @@ void CAN::configTxMBox(Uint16 boxNumber, const MsgID& configID, const MsgCtrlReg
     CanShadow_.CANMD.all &= ~(mboxControl);
     p_CanRegs_->CANMD.all = CanShadow_.CANMD.all;
 
-    // Write to RTR and TPL field in control reg
-    p_MailBox->MSGCTRL.bit.RTR = configCtrlReg.bit.RTR;
-    p_MailBox->MSGCTRL.bit.TPL = configCtrlReg.bit.TPL;
+    // Config MBOX control reg
+    p_MailBox->MSGCTRL.all = configCtrlReg.all;
 
     // Mailbox enable
     CanShadow_.CANME.all = p_CanRegs_->CANME.all;
diff --git a/Protocol/CAN_data.h b/Protocol/CAN_data.h
new file mode 100644
index 0000000..932763a
--- /dev/null
+++ b/Protocol/CAN_data.h
@@ -0,0 +1,17 @@
+#pragma once
+
+namespace canSpace {
+
+enum MBOX_NUMBERS {
+    MODBUS_SETTINGS_MBOX = 0,
+    COMM_VERSION_MBOX = 1,
+    MODBUS_DATA_COMM_TO_CPU_RSV2_MBOX = 26,
+    MODBUS_DATA_COMM_TO_CPU_RSV1_MBOX = 27,
+    MODBUS_DATA_CPU_TO_COMM_MBOX = 28,
+    MODBUS_DATA_COMM_TO_CPU_MBOX = 29,
+    DIGITAL_INPUT_MBOX = 30,
+    DIGITAL_OUTPUT_MBOX = 31
+};
+
+
+}