Ethernet CAN Bus Gateway

// below if is required to process if defined statements properly
#if 1
__asm volatile ("nop");
#endif
// The CAN bus gateway relais CAN bus packets over Ethernet to the central application. 
// No local functionality or sensors are connected. Only packet routing is performed.
// The gateway does only listen to CAN packets when a request has been received over 
// Ethernet.
#include <mcp_can.h>
#include <mcp_can_dfs.h>
#include <Timer.h> 
#include <SPI.h>
#include <Ethernet.h>
#include <EthernetUdp.h>                                 // UDP library from: bjoern@cs.stanford.edu 12/30/2008

#define DEBUG
/* ------------------------------------------------------------------------------------------------------------------------------------
 *  Ethernet definitions
 * ------------------------------------------------------------------------------------------------------------------------------------ */
byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xF1};
IPAddress ip(192,168,0,170);
unsigned int localPort = 8000;                            // local port to listen on
// buffers for receiving and sending data
char  packetBuffer[24];                                   // buffer to hold incoming packet,
char  replyBuffer[24] ;                                   // a string to send back
EthernetUDP Udp;                                          // An EthernetUDP instance to let us send and receive packets over UDP
/* ------------------------------------------------------------------------------------------------------------------------------------
 *  CAN bus definitions
 * ------------------------------------------------------------------------------------------------------------------------------------ */
MCP_CAN CAN(9);                                           // Set CS to pin 9
byte ourId=0xB0;                                          // fixed CAN bus id for the Gateway
unsigned char len = 8;
unsigned char buf[8];
//char str[20];
int heartBeat=0;
int Flag_Restart=0;
// output buffer for CAN write 
unsigned char stmpOut[8] = {100, 0x01, 0, 0, 0, 0, 0, 0};
unsigned char stmpIn[8] = {100, 0x01, 0, 0, 0, 0, 0, 0};
/* ------------------------------------------------------------------------------------------------------------------------------------
 *  Timer definitions
 * ------------------------------------------------------------------------------------------------------------------------------------ */
Timer watchDogTimer;
boolean responseExpected=false;
boolean canTimeout = false;
/* ------------------------------------------------------------------------------------------------------------------------------------
 *  Initialise
 * ------------------------------------------------------------------------------------------------------------------------------------ */
void setup() {
  Serial.begin(9600);
  while (CAN_OK != CAN.begin(CAN_500KBPS)){                 // init can bus : baudrate = 500k
     Serial.println("CAN BUS Shield init fail");
     delay(1000);
  }
  Serial.println("CAN BUS Shield init ok!");
  // start the Ethernet and UDP:
  Ethernet.begin(mac, ip);
  Udp.begin(localPort);
  watchDogTimer.every(2000, checkTransmissionTimeout);
}
/* ------------------------------------------------------------------------------------------------------------------------------------
 *  Software Reset 
 * ------------------------------------------------------------------------------------------------------------------------------------ */
void(* resetFunc) (void) = 0;                                 //declare reset function @ address 0
/* ------------------------------------------------------------------------------------------------------------------------------------
 *  Ethernet Functions 
 * ------------------------------------------------------------------------------------------------------------------------------------ */
// send a reply, to the IP address and port that sent us the packet we received
void sendReplyPacketOverEthernet( char* outBuffer) {
    Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
    Udp.write(outBuffer);
    Udp.endPacket();
}
// just acknowledge receipt of the test packet
void replyToHeartBeat() {
    replyBuffer[0] = 'A';
    replyBuffer[1] = 'c';
    replyBuffer[2] = 'k';
    sendReplyPacketOverEthernet(replyBuffer);
}
void forwardCANbusMessage(unsigned char* CANbuffer) {
    for (int i=0; i<24;i++) {
        replyBuffer[i]=0;
    }  
    replyBuffer[0] = 'A';      // Answer
    replyBuffer[1] = 'C';      // to CAN request
    for (int i=0; i<len;i++) {
       replyBuffer[i+8]= CANbuffer[i];                         // CAN bus message to byte 9 for reply
    }
    sendReplyPacketOverEthernet(replyBuffer);
}
void CANbusTimeOutMessage() {
    replyBuffer[0] = 'A';      // Answer
    replyBuffer[1] = 'C';      // CAN request
    replyBuffer[2] = 'O';      // Client Off-Line
    sendReplyPacketOverEthernet(replyBuffer);
}
/* ------------------------------------------------------------------------------------------------------------------------------------
 *  CAN bus functions 
 * ------------------------------------------------------------------------------------------------------------------------------------ */
void sendCanMessage(int clientId,int format, int length,  unsigned char* buf) {
#if defined(DEBUG)
   Serial.println("Send CAN package ");
#endif   
   int canStatus = CAN.sendMsgBuf(clientId, format, length, buf); 
   if (canStatus != 0) {
     Serial.print("CAN bus status ");
     Serial.print(canStatus);
     Serial.println();    
   }else {
     heartBeat++;
   }
}
// CAN message is 8 bytes, just copy from sender message to output
void prepareCANmessage( char* inBuffer, unsigned char* outBuffer) {
    for (int i=0; i<len;i++) {
       outBuffer[i]= inBuffer[i+8];                         // CAN bus message starts at byte 9
    }
}
void readCANbusReply(unsigned char* canInBuffer){
   while (CAN_MSGAVAIL == CAN.checkReceive()) {
      // read data,  len: data length, buf: data buf
      int canStatus = CAN.readMsgBuf(&len, canInBuffer);
      if (canStatus != 0 && canStatus != 8) {
         Serial.print("CAN bus status ");
         Serial.print(canStatus);
         Serial.println();    
       }else {
#if defined(DEBUG)
          Serial.println(" CAN response package received");
#endif     
          responseExpected=false;
          // CAN bus message was read successfully - now send it back over the Ethernet
          forwardCANbusMessage(canInBuffer);
          heartBeat++;
       }
       watchDogTimer.update();
    } 
}
void checkTransmissionTimeout() {
      if  (responseExpected) {
          canTimeout=true; 
      }
}
/* ------------------------------------------------------------------------------------------------------------------------------------
 *  Main loop
 *  Receive datagrams from a sender, pass on CAN bus commands and wait for answer.
 *  If CAN bus clienet does not answer and timout reply is sent to the sender.
 *  Datagram format: 
 *  Byte 0: "R"  for Request
 *  Byte 1: Command    "T" = Test (Heartbeat)    "C" = CAN bus message to client
 * ------------------------------------------------------------------------------------------------------------------------------------ */

void loop() {

  int packetSize = Udp.parsePacket();                        // check presence of UDB packet
  if (packetSize>0){
    Udp.read(packetBuffer, 24);                              // read the packet into packetBufffer
    switch ( packetBuffer[1] )  { 
     case 'T':   
#if defined(DEBUG)
                 Serial.println("Test Packet Received"); 
#endif
                 replyToHeartBeat();
                 break;
     case 'C':   
#if defined(DEBUG)
                 Serial.println("CAN Bus Packet Received"); 
#endif
                 prepareCANmessage(packetBuffer,stmpOut);
                 sendCanMessage(ourId,0,8,stmpOut);
                 responseExpected=true;
                 delay(50);
                 readCANbusReply(stmpIn);
                 break;
    }
   }
   if (canTimeout) {
       responseExpected= false;
       canTimeout = false;
#if defined(DEBUG)
       Serial.println("CAN bus timeout"); 
       CANbusTimeOutMessage();
#endif
   }
   watchDogTimer.update();
}