Line data    Source code

       1             : #include "encoder.hpp"
       2             : #include "log.hpp"
       3             : 
       4             : 
       5           8 : void Encoder::set_data_format(int format)
       6             : {
       7             : #if REGMAP_VERSION < 0x0500
       8             :     m_header_size = 2;
       9             :     m_block_bytes = 32;
      10             :     m_length_mask = TOFLX_LENGTH_MASK_RM4;
      11             :     m_elink_shift = TOFLX_ELINK_SHIFT_RM4;
      12             : #else
      13           8 :     switch (format)
      14             :     {
      15           0 :         case FROMHOST_FORMAT_5BIT_LENGTH:
      16           0 :             m_header_size = 2;
      17           0 :             m_block_bytes = 32;
      18           0 :             m_length_mask = TOFLX_LENGTH_MASK;
      19           0 :             m_elink_shift = TOFLX_ELINK_SHIFT;
      20           0 :             break;
      21           0 :         case FROMHOST_FORMAT_HDR32_PACKET32:
      22           0 :             m_header_size = 4;
      23           0 :             m_block_bytes = 32;
      24           0 :             m_length_mask = TOFLX_LENGTH_MASK_HDR32;
      25           0 :             m_elink_shift = TOFLX_ELINK_SHIFT_HDR32;
      26           0 :             break;
      27           0 :         case FROMHOST_FORMAT_HDR32_PACKET64:
      28           0 :             m_block_bytes = 64;
      29           0 :             m_length_mask = TOFLX_LENGTH_MASK_HDR32;
      30           0 :             m_elink_shift = TOFLX_ELINK_SHIFT_HDR32;
      31           0 :             break;
      32           6 :         case FROMHOST_FORMAT_HDR32_PACKET32_8B:
      33           6 :             m_block_bytes = 32;
      34           6 :             break;
      35           1 :         case FROMHOST_FORMAT_HDR32_PACKET64_8B:
      36           1 :             m_block_bytes = 64;
      37           1 :             break;
      38           1 :         case FROMHOST_FORMAT_HDR32_PACKET128_8B:
      39           1 :             m_block_bytes = 128;
      40           1 :             break;
      41           0 :         default:
      42           0 :             LOG_ERR("Invalid FromHost data format %d. Assuming rm-5.1 for PCIe gen3.", format);
      43           0 :             break;
      44             :     }
      45             : #endif
      46           8 :     m_payload_bytes = m_block_bytes - m_header_size;
      47           8 : }
      48             : 
      49           8 : void Encoder::set_destination_parameters(uint8_t* dest_start, uint32_t dest_size)
      50             : {
      51           8 :     m_dest_start = dest_start;
      52           8 :     m_dest_size = dest_size;
      53           8 : }
      54             : 
      55             : 
      56       10004 : size_t Encoder::compute_max_msg_occupancy(size_t size) const
      57             : {
      58       10004 :     return (compute_full_blocks(size) + 1) * m_block_bytes;
      59             : }
      60             : 
      61             : 
      62       20011 : int Encoder::compute_full_blocks(size_t size) const
      63             : {
      64       20011 :     return ((size + m_payload_bytes - 1) / m_payload_bytes);
      65             : }
      66             : 
      67             : 
      68           3 : size_t Encoder::compute_final_bytes(size_t size) const
      69             : {
      70           3 :     size_t blocks = (size + m_payload_bytes - 1) / m_payload_bytes;
      71           3 :     size_t final_bytes = size - (blocks - 1) * m_payload_bytes;
      72           3 :     return final_bytes;
      73             : }
      74             : 
      75             : 
      76       10007 : uint64_t Encoder::encode_and_write_msg(uint64_t elink, const uint8_t *source, size_t size, uint32_t dest_offset)
      77             : {
      78       10007 :     if (m_dest_start == nullptr){
      79           0 :         LOG_ERR("FromHost encoder not initialized. The message for 0x%lx will not be written.", elink);
      80           0 :         return 0;
      81             :     }
      82       10007 :     uint32_t src_i(0);
      83       10007 :     uint32_t dest_i(dest_offset);
      84       10007 :     int blocks = compute_full_blocks(size);
      85       10007 :     int final_bytes = size - (blocks - 1) * m_payload_bytes;
      86             : #if REGMAP_VERSION < 0x0500
      87             :     int hdr = ((elink << m_elink_shift) | ((m_payload_bytes / 2) << TOFLX_LENGTH_SHIFT_RM4));
      88             :     int final_size = (final_bytes + 1) / 2; // Length in 2-byte units
      89             :     int final_hdr = hdr & ~m_length_mask;
      90             :     final_hdr |= ((final_size << TOFLX_LENGTH_SHIFT_RM4) | TOFLX_EOM_MASK_RM4);
      91             : #else
      92             :     // Regmap 0x0500 and beyond, see FLX-1355, FLX-1601 and FLX-1886
      93       10007 :     if (m_length_mask == TOFLX_LENGTH_MASK_HDR32_8B and m_elink_shift == TOFLX_ELINK_SHIFT_HDR32_8B){
      94             :         // Keeping the existing 4-byte header e-link numbering,
      95             :         // so map the e-link number to the '8-bit-fields' version
      96       10007 :         uint64_t linknr = (elink & 0x07C0) >> 6;
      97       10007 :         uint64_t eindex = (elink & 0x003F);
      98       10007 :         elink = (linknr << 8) | eindex;
      99             :     }
     100       10007 :     int hdr = ((elink << m_elink_shift) | m_length_mask);
     101       10007 :     int final_hdr = hdr & ~m_length_mask;
     102       10007 :     final_hdr |= final_bytes; // Length in bytes
     103             : #endif
     104             :     // Fully-filled FromHost data packets
     105       40016 :     for (int i = 0; i < blocks - 1; ++i)
     106             :     {
     107             :         // Invert payload byte order
     108      870421 :         for (int j = m_payload_bytes - 1; j >= 0; --j, ++dest_i)
     109             :         {
     110      840412 :             m_dest_start[dest_i] = source[src_i + j] & 0xFF;
     111             :         }
     112       30009 :         src_i += m_payload_bytes;
     113             : 
     114             :         // Header
     115      150045 :         for (int j = 0; j < m_header_size; ++j, ++dest_i)
     116      120036 :             m_dest_start[dest_i] = (uint8_t)((hdr >> j * 8) & 0xFF);
     117             : 
     118             :         // Wrap-around ?
     119       30009 :         if (dest_i >= m_dest_size)
     120           0 :             dest_i = 0;
     121             :     }
     122             : 
     123             :     // Final (likely not fully filled) FromHost data packet
     124             :     // Invert payload byte order
     125      130152 :     for (int j = m_payload_bytes - 1; j >= final_bytes; --j, ++dest_i)
     126             :     {
     127             :         // destination[dest_i] = (char) 0xFF; // Remaining payload bytes: set to 0xFF
     128      120145 :         m_dest_start[dest_i] = (uint8_t)0x00; // Remaining payload bytes: set to 0x00
     129             :     }
     130      170186 :     for (int j = final_bytes - 1; j >= 0; --j, ++dest_i)
     131             :     {
     132      160179 :         m_dest_start[dest_i] = source[src_i + j] & 0xFF; // Final data bytes
     133             :     }
     134             : 
     135             :     // Header
     136       50035 :     for (int j = 0; j < m_header_size; ++j, ++dest_i)
     137       40028 :         m_dest_start[dest_i] = (uint8_t)((final_hdr >> j * 8) & 0xFF);
     138             : 
     139             :     // Wrap-around ?
     140       10007 :     if (dest_i >= m_dest_size){
     141           2 :         dest_i = 0;
     142             :     }
     143             : 
     144       10007 :     return dest_i;
     145             : }