Line data    Source code

       1             : #include <cstdio>
       2             : 
       3             : #include "flxcard/FlxCard.h"
       4             : #include "flxcard/FlxException.h"
       5             : 
       6             : extern "C" {
       7             : #include "flx_api.h"
       8             : #include "log.h"
       9             : }
      10             : 
      11             : #if REGMAP_VERSION < 0x500
      12             : 
      13         312 : size_t flx_encode_toflx_msg( const uint8_t *source,
      14             :                              size_t         size,
      15             :                              char          *destination,
      16             :                              size_t         dest_size,
      17             :                              size_t        *dest_index,
      18             :                              uint64_t       elink )
      19             : {
      20         312 :   int hdr = ((elink << TOGBT_EPATH_SHIFT_RM4) | ((TOGBT_PAYLOAD_BYTES_RM4/2) << TOGBT_LENGTH_SHIFT_RM4));
      21         312 :   int blocks = (size + TOGBT_PAYLOAD_BYTES_RM4-1) / TOGBT_PAYLOAD_BYTES_RM4;
      22         312 :   size_t encoded_size = blocks * TOGBT_BLOCK_BYTES_RM4;
      23         312 :   size_t dest_i = *dest_index;
      24         312 :   int src_i = 0;
      25             : 
      26             :   // Fully-filled To-GBT byte blocks
      27         462 :   for (int i = 0; i < blocks - 1; ++i) {
      28             :     // Invert byte order
      29        4650 :     for(int j = TOGBT_PAYLOAD_BYTES_RM4-1; j >= 0; --j, ++dest_i) {
      30        4500 :       destination[dest_i] = source[(src_i + j)] & 0xFF;
      31             :     }
      32         150 :     src_i += TOGBT_PAYLOAD_BYTES_RM4;
      33         150 :     destination[dest_i++] = (char)(hdr & 0xFF);
      34         150 :     destination[dest_i++] = (char)((hdr & 0xFF00) >> 8);
      35             : 
      36             :     // Wrap-around ?
      37         150 :     if( dest_i >= dest_size )
      38           0 :       dest_i = 0;
      39             :   }
      40             : 
      41             :   // Final (possibly not fully filled) To-GBT byte block
      42         312 :   int final_size, final_hdr, final_bytes;
      43         312 :   final_bytes = size - (blocks-1) * TOGBT_PAYLOAD_BYTES_RM4;
      44         312 :   final_size = (final_bytes + 1) / 2; // Length in 2-byte units
      45         312 :   final_hdr  = hdr & ~TOGBT_LENGTH_MASK_RM4;
      46         312 :   final_hdr |= ((final_size << TOGBT_LENGTH_SHIFT_RM4) | TOGBT_EOM_MASK_RM4);
      47             : 
      48             :   // Invert byte order
      49        6306 :   for (int j = TOGBT_PAYLOAD_BYTES_RM4-1; j >= final_bytes; --j, ++dest_i) {
      50        5994 :     destination[dest_i] = (char) 0xFF; // Remaining block bytes: 0xFF
      51             :   }
      52             : 
      53             :   // Write 0x00 as pad byte in case size is odd (normally not the case)
      54         312 :   if ((final_bytes % 2) > 0) {
      55           2 :     LOG_WARN("Toflx message is odd sized, no proper encoding possible, size %d", size);
      56           2 :     destination[dest_i-1] = (char) 0x00;
      57             :   }
      58        3678 :   for (int j = final_bytes - 1; j >= 0; --j, ++dest_i) {
      59        3366 :     destination[dest_i] = source[(src_i + j)] & 0xFF; // Final data bytes
      60             :   }
      61         312 :   destination[dest_i++] = (char)(final_hdr & 0xFF);
      62         312 :   destination[dest_i++] = (char)((final_hdr & 0xFF00) >> 8);
      63             : 
      64             :   // Wrap-around ?
      65         312 :   if( dest_i >= dest_size )
      66           0 :     dest_i = 0;
      67             : 
      68             :   // Return the updated index and size of the encoded message
      69         312 :   *dest_index = dest_i;
      70         312 :   return encoded_size;
      71             : }
      72             : 
      73             : #else
      74             : 
      75         298 : size_t flx_encode_toflx_msg( const uint8_t *source,
      76             :                              size_t         size,
      77             :                              char          *destination,
      78             :                              size_t         dest_size,
      79             :                              size_t        *dest_index,
      80             :                              uint64_t       elink )
      81             : {
      82             :   // Regmap 0x0500 and beyond, see FLX-1355
      83         298 :   int hdr = ((elink << TOGBT_EPATH_SHIFT) | TOGBT_LENGTH_MASK);
      84         298 :   int blocks = (size + TOGBT_PAYLOAD_BYTES-1) / TOGBT_PAYLOAD_BYTES;
      85         298 :   size_t encoded_size = blocks * TOGBT_BLOCK_BYTES;
      86         298 :   size_t dest_i = *dest_index;
      87         298 :   int src_i = 0;
      88             : 
      89             :   // Fully-filled To-GBT byte blocks
      90         441 :   for (int i = 0; i < blocks - 1; ++i) {
      91             :     // Invert byte order
      92        4433 :     for(int j = TOGBT_PAYLOAD_BYTES-1; j >= 0; --j, ++dest_i) {
      93        4290 :       destination[dest_i] = source[(src_i + j)] & 0xFF;
      94             :     }
      95         143 :     src_i += TOGBT_PAYLOAD_BYTES;
      96         143 :     destination[dest_i++] = (char)(hdr & 0xFF);
      97         143 :     destination[dest_i++] = (char)((hdr & 0xFF00) >> 8);
      98             : 
      99             :     // Wrap-around ?
     100         143 :     if( dest_i >= dest_size )
     101           0 :       dest_i = 0;
     102             :   }
     103             : 
     104             :   // Final (possibly not fully filled) To-GBT byte block
     105         298 :   int final_size, final_hdr, final_bytes;
     106         298 :   final_bytes = size - (blocks-1) * TOGBT_PAYLOAD_BYTES;
     107         298 :   final_size = final_bytes;
     108         298 :   final_hdr  = hdr & ~TOGBT_LENGTH_MASK;
     109         298 :   final_hdr |= final_size;
     110             : 
     111             :   // Invert byte order
     112        6026 :   for (int j = TOGBT_PAYLOAD_BYTES-1; j >= final_bytes; --j, ++dest_i) {
     113        5728 :     destination[dest_i] = (char) 0xFF; // Remaining block bytes: 0xFF
     114             :   }
     115             : 
     116        3510 :   for (int j = final_bytes - 1; j >= 0; --j, ++dest_i) {
     117        3212 :     destination[dest_i] = source[(src_i + j)] & 0xFF; // Final data bytes
     118             :   }
     119         298 :   destination[dest_i++] = (char)(final_hdr & 0xFF);
     120         298 :   destination[dest_i++] = (char)((final_hdr & 0xFF00) >> 8);
     121             : 
     122             :   // Wrap-around ?
     123         298 :   if( dest_i >= dest_size )
     124           0 :     dest_i = 0;
     125             : 
     126             :   // Return the updated index and size of the encoded message
     127         298 :   *dest_index = dest_i;
     128         298 :   return encoded_size;
     129             : }
     130             : 
     131             : #endif // REGMAP_VERSION