Line data    Source code

       1             : #include "hdlc_coder/hdlc.hpp"
       2             : 
       3             : 
       4             : static const uint8_t NibbleReversed[16] =
       5             :   {
       6             :    0x0, 0x8, 0x4, 0xc, 0x2, 0xa, 0x6, 0xe,
       7             :    0x1, 0x9, 0x5, 0xd, 0x3, 0xb, 0x7, 0xf
       8             :   };
       9             : 
      10             : // Return 8-bit value 'byte' bit-reversed
      11             : static uint8_t
      12         108 : bit_reversed(uint8_t byte)
      13             : {
      14             :   // Reverse the top and bottom nibble and swap them
      15         108 :   return( (NibbleReversed[byte & 0xF] << 4) | NibbleReversed[byte >> 4] );
      16             : }
      17             : 
      18             : 
      19             : /*
      20             : Calculate CRC-16 value; uses The CCITT-16 Polynomial,
      21             : expressed as X^16 + X^12 + X^5 + 1
      22             : NB: for GBT-SCA packets need to bit-reverse each byte
      23             :     because the bytes are transmitted LSB to MSB (?) (added 28 Oct 2016)
      24             : */
      25             : static uint16_t
      26          10 : crc16(int nbytes, const uint8_t *data)
      27             : {
      28             :   uint16_t crc = (uint16_t) 0xFFFF;
      29             :   int index, b;
      30          98 :   for( index=0; index<nbytes; ++index )
      31             :     {
      32             :       //crc ^= (((uint16_t) data[index]) << 8); // Non bit-reversed
      33          88 :       uint8_t tmp = bit_reversed( data[index] );
      34          88 :       crc ^= (((uint16_t) tmp) << 8);
      35         792 :       for( b=0; b<8; ++b )
      36             :         {
      37         704 :           if( crc & (uint16_t) 0x8000 )
      38         270 :             crc = (crc << 1) ^ (uint16_t) 0x1021;
      39             :           else
      40         434 :             crc = (crc << 1);
      41             :         }
      42             :     }
      43             :   uint8_t tmp1, tmp2;
      44          10 :   tmp1 = bit_reversed( (uint8_t) ((crc >> 8) & 0xFF) );
      45          10 :   tmp2 = bit_reversed( (uint8_t) (crc & 0xFF) );
      46          10 :   crc = (uint16_t) tmp2 | ((uint16_t) tmp1 << 8);
      47          10 :   return crc;
      48             : }
      49             : 
      50             : 
      51             : void
      52           3 : felix::hdlc::encode_hdlc_msg_header(uint8_t* header, int addr, uint64_t seqnr)
      53             : {
      54           3 :   header[0] = (uint8_t) (addr & 0xFF);
      55             :   // Control byte: acknowledge up to previous message, by default..(?)
      56           3 :   seqnr &= 0x7;
      57           3 :   header[1] = (uint8_t) ((seqnr << HDLC_CTRL_NRECVD_SHIFT)|(seqnr << HDLC_CTRL_NSEND_SHIFT));
      58           3 : }
      59             : 
      60             : 
      61             : void
      62           0 : felix::hdlc::encode_hdlc_ctrl_header(uint8_t* header, int addr, int control)
      63             : {
      64           0 :   header[0] = (uint8_t) (addr & 0xFF);
      65           0 :   header[1] = (uint8_t) (control & 0xFF);
      66           0 : }
      67             : 
      68             : 
      69             : void
      70           3 : felix::hdlc::encode_hdlc_trailer(uint8_t* trailer, const uint8_t* data, size_t len)
      71             : {
      72           3 :   uint16_t crc = crc16(len, data);
      73           3 :   trailer[0] = (uint8_t) ((crc >> 8) &  0xFF);
      74           3 :   trailer[1] = (uint8_t) (crc & 0xFF);
      75           3 : }
      76             : 
      77             : 
      78             : felix::hdlc::frame_t
      79           1 : felix::hdlc::frame_type(const uint8_t* data)
      80             : {
      81           1 :   if((data[1] & 0x3) == 0x3) {
      82             :     return UFRAME;
      83             :   }
      84           1 :   if((data[1] & 0x3) == 0x1) {
      85           0 :     return SFRAME;
      86             :   }
      87             :   return IFRAME;
      88             : }
      89             : 
      90             : 
      91             : void
      92           1 : felix::hdlc::decode_hdlc_msg(uint8_t** dest_msg, size_t* dest_len, uint8_t* data, size_t len)
      93             : {
      94           1 :   *dest_msg = data + 2;
      95           1 :   *dest_len = len - 4;
      96           1 : }
      97             : 
      98             : 
      99             : bool
     100           7 : felix::hdlc::verify_hdlc_trailer(const uint8_t* data, size_t len)
     101             : {
     102             :   // Verify that the FCS (2-byte trailer) is correct
     103           7 :   return( crc16(len, data) == 0 );
     104             : }