Modbus 8 bit cpu

#include <SI_EFM8UB2_Register_Enums.h>

#define UART_BUFFERSIZE 64
uint8_t UART_Buffer[UART_BUFFERSIZE];
extern uint8_t UART_Buffer_Size;
uint8_t UART_Input_First;
uint8_t UART_Output_First;
extern uint8_t TX_Ready;
extern char Byte;

bool CrC_is_Error = false;

/* Supported function codes */
#define _READ_GPIO_REGISTERS 0x01
#define _WRITE_GPIO_REGISTERS 0x05

char Chip_Address;

// Port0 1-7 inputs
SI_SBIT(IN1, SFR_P0, 1);                // IN1 = '0' means on
SI_SBIT(IN2, SFR_P0, 2);
SI_SBIT(IN3, SFR_P0, 3);
SI_SBIT(IN4, SFR_P0, 4);
SI_SBIT(IN5, SFR_P0, 5);
SI_SBIT(IN6, SFR_P0, 6);
SI_SBIT(IN7, SFR_P0, 7);
// Port1 1-7 inputs
SI_SBIT(IN1, SFR_P1, 1);                // IN1 = '0' means on
SI_SBIT(IN2, SFR_P1, 2);
SI_SBIT(IN3, SFR_P1, 3);
SI_SBIT(IN4, SFR_P1, 4);
SI_SBIT(IN5, SFR_P1, 5);
SI_SBIT(IN6, SFR_P1, 6);
SI_SBIT(IN7, SFR_P1, 7);
// Port2 1-7 outputs
SI_SBIT(OUT1, SFR_P2, 1);
SI_SBIT(OUT2, SFR_P2, 2);
SI_SBIT(OUT3, SFR_P2, 3);
SI_SBIT(OUT4, SFR_P2, 4);
SI_SBIT(OUT5, SFR_P2, 5);
SI_SBIT(OUT6, SFR_P2, 6);
SI_SBIT(OUT7, SFR_P2, 7);
// Port3 1-7 outputs
SI_SBIT(OUT1, SFR_P3, 1);
SI_SBIT(OUT2, SFR_P3, 2);
SI_SBIT(OUT3, SFR_P3, 3);
SI_SBIT(OUT4, SFR_P3, 4);
SI_SBIT(OUT5, SFR_P3, 5);
SI_SBIT(OUT6, SFR_P3, 6);
SI_SBIT(OUT7, SFR_P3, 7);

static void DO_Request_Message(){   // I have to do a requet message based on the things i do?
    if(CrC_is_Error){
        // CrC Error response message
    }else{
        // GPIO Status message
    }
}
static void Check_GPIO_Status(uint8_t Data){
    uint8_t GPIO_STATUS_Input_P0 = SFR_P0;  // I have to put the gpio statuses together to be able to send back to master on request.
    uint8_t GPIO_STATUS_Input_P1 = SFR_P1;
    uint8_t GPIO_STATUS_Output_P2 = SFR_P2;
    uint8_t GPIO_STATUS_Output_P3 = SFR_P3;
    uint8_t From_Where = Data[0] +1;
    uint8_t How_Many = Data[1];
}

static void Toggle_IO_Pins(uint8_t Data){       // Need to toggle the I/O pins based on the master request
    //Data[0] tells us from where should i start to toggle the pins ( in theory )
    //Data[1] tells us how many pins should i toggle ( in theory )
}
static int Check_Chip_Address(uint8_t Msg_Address){         // ADRESS BASED ON THE P0.1-P0.4 GPIOS
    char Address1, Address2, Address3, Address4;            // This function is not okay.
    Address1 = IN1;                                         // I have to read the input gpio statuses and match it with a 8 bit variable
    Address2 = IN2;
    Address3 = IN3;
    Address4 = IN4;
    Chip_Address = Address1;
    Chip_Address += Address2;
    Chip_Address += Address3;
    Chip_Address += Address4;
    if(Chip_Address == Msg_Address){
        return 1;
    }else{
        return -1;
    }
}

static uint16_t Crc_Check(uint8_t *req, uint8_t req_length)
{
    uint8_t j;
    uint16_t crc;
    crc = 0xFFFF;
    while (req_length--) {
        crc = crc ^ *req++;
        for (j = 0; j < 8; j++) {
            if (crc & 0x0001)
                crc = (crc >> 1) ^ 0xA001;
            else
                crc = crc >> 1;
        }
    }
    return (crc << 8 | crc >> 8);
}

static int check_integrity(uint8_t *msg, uint8_t msg_length)
{
    uint16_t crc_calculated;
    uint16_t crc_received;
    if (msg_length < 2)
        return -1;
    crc_calculated = Crc_Check(msg, msg_length - 2);
    crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];

    /* Check CRC of msg */
    if (crc_calculated == crc_received) {
        return msg_length;
    } else {
        return -1;
    }
}

static void Check_Message(){
    uint8_t Address,Code;
    uint8_t Data[2];
    uint8_t Crc[2];
    int k = 0;
    int i = 0;
    // Get the whole message in separate variables for further analization
    for(i = 0; i < UART_Input_First;i++){
        if (i <= 2){
            Address = UART_Buffer[i];
        }
        else if (i <= 4){
            Code = UART_Buffer[i];
        }
        else if (i <= 8){
            if(i <= 6){
                Data[0] = UART_Buffer[i];
            }else{
                Data[1] = UART_Buffer[i];
            }
        }else{
            Crc[k] = UART_Buffer[i];
            k++;
        }
    }
    // Checking the separated variables
    if(Check_Chip_Address(Address) != -1){                  // Check if Message is ours.
        if(check_integrity(Crc,UART_Buffer_Size) != -1){    // Check if no errors on crc
            if(Code == _READ_GPIO_REGISTERS){
                /** MASTER WANTS TO READ THE GPIOS **/
                Check_GPIO_Status(Data);
            }else if(Code == _WRITE_GPIO_REGISTERS){
                /** MASTER WANTS TO TOGGLE THE GPIOS **/
                Toggle_IO_Pins(Data);
            }
            DO_Request_Message();
        }else{
            CrC_is_Error = true;
        }
    }
}

//-----------------------------------------------------------------------------
// SCON0::RI (Receive Interrupt Flag)
// SCON0::TI (Transmit Interrupt Flag)
//-----------------------------------------------------------------------------
SI_INTERRUPT(UART0_ISR, UART0_IRQn)
{
    if (SCON0_RI == 1)
       {
          if( UART_Buffer_Size == 0)  {         // If new word is entered
             UART_Input_First = 0;
          }
          SCON0_RI = 0;                         // Clear interrupt flag
          Byte = SBUF0;                         // Read a character from UART
          if (UART_Buffer_Size < UART_BUFFERSIZE)
          {
             UART_Buffer[UART_Input_First] = Byte; // Store in array

             UART_Buffer_Size++;                // Update array's size

             UART_Input_First++;                // Update counter
          }else{
              Check_Message();                  // Get the whole message and brake it into pieces
          }
       }


 /** ECHO BACK THE MESSAGES **/

if (SCON0_TI == 1)                              // Check if transmit flag is set
   {
      SCON0_TI = 0;                             // Clear interrupt flag
      if (UART_Buffer_Size != 1)                // If buffer not empty
      {
         // If a new word is being output
         if ( UART_Buffer_Size == UART_Input_First ) {
              UART_Output_First = 0;
         }
         // Store a character in the variable byte
         Byte = UART_Buffer[UART_Output_First];
         if ((Byte >= 0x61) && (Byte <= 0x7A)) { // If upper case letter
            Byte -= 32;
         }
         SBUF0 = Byte;                          // Transmit to Hyperterminal
         UART_Output_First++;                   // Update counter
         UART_Buffer_Size--;                    // Decrease array size
      }
      else
      {
         UART_Buffer_Size = 0;                  // Set the array size to 0
         TX_Ready = 1;                          // Indicate transmission complete
      }
   }
}