ait_link.cpp

00001 //
00002 // HDLC based serial communication class.
00003 //
00004 // Adapted by Benjamin Hepp from ArduHDLC library (see LICENSE.txt)
00005 // Original work Copyright (c) 2015 Jarkko Hautakorpi et al.
00006 // Modified work Copyright (c) 2016 Benjamin Hepp.
00007 
00008 #include "ait_link/ait_link.h"
00009 
00010 using namespace ait;
00011 
00012 /* 16bit low and high bytes copier */
00013 #define low(x)    ((x) & 0xFF)
00014 #define high(x)   (((x)>>8) & 0xFF)
00015 
00016 AITLink::AITLink(size_t max_frame_length)
00017 {
00018   frame_checksum = CRC16_CCITT_INIT_VAL;
00019   escape_character = false;
00020   frame_position = 0;
00021   this->max_frame_length = max_frame_length;
00022   receive_frame_buffer = new uint8_t[max_frame_length+1]; // char* ab = (char*)malloc(12);
00023   handle_frame_callback = NULL;
00024   callback_user_data = NULL;
00025 }
00026 
00027 AITLink::~AITLink()
00028 {
00029   delete[] receive_frame_buffer;
00030 }
00031 
00032 /* Wrap given data in HDLC frame and send it out byte at a time*/
00033 bool AITLink::sendFrame(const uint8_t* frame_buffer, size_t frame_length)
00034 {
00035   if (frame_length > max_frame_length) {
00036     return false;
00037   }
00038 
00039   uint8_t data;
00040   uint16_t fcs = CRC16_CCITT_INIT_VAL;
00041 
00042   sendChar(FRAME_BOUNDARY_OCTET);
00043 
00044   while (frame_length)
00045   {
00046     data = *frame_buffer++;
00047     fcs = crcCcittUpdate(fcs, data);
00048     if ((data == CONTROL_ESCAPE_OCTET) || (data == FRAME_BOUNDARY_OCTET))
00049     {
00050       sendChar(CONTROL_ESCAPE_OCTET);
00051       data ^= INVERT_OCTET;
00052     }
00053     sendChar(data);
00054     --frame_length;
00055   }
00056   data = low(fcs);
00057   if ((data == CONTROL_ESCAPE_OCTET) || (data == FRAME_BOUNDARY_OCTET))
00058   {
00059     sendChar(CONTROL_ESCAPE_OCTET);
00060     data ^= INVERT_OCTET;
00061   }
00062   sendChar(data);
00063   data = high(fcs);
00064   if ((data == CONTROL_ESCAPE_OCTET) || (data == FRAME_BOUNDARY_OCTET))
00065   {
00066     sendChar(CONTROL_ESCAPE_OCTET);
00067     data ^= INVERT_OCTET;
00068   }
00069   sendChar(data);
00070   sendChar(FRAME_BOUNDARY_OCTET);
00071 
00072   return true;
00073 }
00074 
00075 /* Function to find valid HDLC frame from incoming data */
00076 void AITLink::inputReceivedChar(uint8_t data)
00077 {
00078   /* FRAME FLAG */
00079   if (data == FRAME_BOUNDARY_OCTET)
00080   {
00081     if (this->escape_character == true)
00082     {
00083       this->escape_character = false;
00084     }
00085       /* If a valid frame is detected */
00086     else if ( (this->frame_position >= 2) &&
00087               ( this->frame_checksum == ((this->receive_frame_buffer[this->frame_position-1] << 8 ) | (this->receive_frame_buffer[this->frame_position-2] & 0xff)) ) )  // (msb << 8 ) | (lsb & 0xff)
00088     {
00089       /* Call the user defined function and pass frame to it */
00090       handleFrame(receive_frame_buffer, this->frame_position - 2);
00091     }
00092     this->frame_position = 0;
00093     this->frame_checksum = CRC16_CCITT_INIT_VAL;
00094     return;
00095   }
00096 
00097   if (this->escape_character)
00098   {
00099     this->escape_character = false;
00100     data ^= INVERT_OCTET;
00101   }
00102   else if (data == CONTROL_ESCAPE_OCTET)
00103   {
00104     this->escape_character = true;
00105     return;
00106   }
00107 
00108   receive_frame_buffer[this->frame_position] = data;
00109 
00110   if (this->frame_position >= 2) {
00111     this->frame_checksum = crcCcittUpdate(this->frame_checksum, receive_frame_buffer[this->frame_position - 2]);
00112   }
00113 
00114   this->frame_position++;
00115 
00116   if (this->frame_position >= this->max_frame_length)
00117   {
00118     this->frame_position = 0;
00119     this->frame_checksum = CRC16_CCITT_INIT_VAL;
00120   }
00121 }
00122 
00123 void AITLink::registerFrameHandler(void (*callback)(void* user_data, const uint8_t* frame_buffer, size_t frame_length), void* user_data)
00124 {
00125   handle_frame_callback = callback;
00126   callback_user_data = user_data;
00127 }
00128 
00129 void AITLink::handleFrame(const uint8_t* frame_buffer, size_t frame_length)
00130 {
00131   if (handle_frame_callback != NULL) {
00132     (*handle_frame_callback)(callback_user_data, frame_buffer, frame_length);
00133   }
00134 
00135 }
00136 
00137 uint16_t AITLink::crcCcittUpdate(uint16_t crc, uint8_t data)
00138 {
00139   data ^= low(crc);
00140   data ^= data << 4;
00141 
00142   return (
00143           ( ((uint16_t)data << 8) | high(crc) ) ^ ((uint8_t)data >> 4) ^ ((uint16_t)data << 3)
00144   );
00145 }