Fix for TX in 40 MHz Bandwidth mode (#14)

Author: vertexodessa

src/RadioManagementModule.cpp

@@ -31,8 +31,8 @@ int get_40mhz_center_channel(int channel) {
         {132, 134}, {136, 134},
         {140, 142}, {144, 142},
         // 5GHz UNII-3
-        {149, 151}, {153, 151},
-        {157, 159}, {161, 159}
+        {149, 151}, {153, 155},
+        {157, 159}, {161, 163}
     };
 
     auto it = channel_map.find(channel);

src/Rtl8812aDevice.cpp

@@ -20,21 +20,12 @@ bool Rtl8812aDevice::send_packet(const uint8_t *packet, size_t length) {
   struct tx_desc *ptxdesc;
   bool resp;
   uint8_t *usb_frame;
-  // struct ieee80211_radiotap_header *rtap_hdr;
   int real_packet_length, usb_frame_length, radiotap_length;
 
   bool vht = false;
   int ret = 0;
   int qos_len = 0;
-  int dot11_hdr_len = 24;
-  int snap_len = 6;
-  unsigned char *pdata;
-  u16 frame_ctl;
-  unsigned char src_mac_addr[6];
-  unsigned char dst_mac_addr[6];
-  u8 fixed_rate = MGN_1M, sgi = 0,
-     bwidth = 0,
-     ldpc = 0, stbc = 0;
+  u8 fixed_rate = MGN_1M, sgi = 0, bwidth = 0, ldpc = 0, stbc = 0;
   u16 txflags = 0;
   int rate_id = 0;
   radiotap_length = int(packet[2]);
@@ -63,40 +54,34 @@ bool Rtl8812aDevice::send_packet(const uint8_t *packet, size_t length) {
     /* see if this argument is something we can use */
     switch (iterator.this_arg_index) {
 
-    case IEEE80211_RADIOTAP_RATE: /* u8 */
+    case IEEE80211_RADIOTAP_RATE:
       fixed_rate = *iterator.this_arg;
       break;
 
     case IEEE80211_RADIOTAP_TX_FLAGS:
       txflags = get_unaligned_le16(iterator.this_arg);
       break;
 
-    case IEEE80211_RADIOTAP_MCS: { /* u8,u8,u8 */
-      u8 mcs_have = iterator.this_arg[0];
-      printf("MCS value:%d %d %d\n", iterator.this_arg[0], iterator.this_arg[1],
-             iterator.this_arg[2]);
-      printf("mcs parse:%d\n", mcs_have);
-      if (mcs_have & IEEE80211_RADIOTAP_MCS_HAVE_MCS) {
-        fixed_rate = iterator.this_arg[2] & 0x7f;
-        if (fixed_rate > 31)
-          fixed_rate = 0;
-        fixed_rate += MGN_MCS0;
+    case IEEE80211_RADIOTAP_MCS: {
+      u8 mcs_flags = iterator.this_arg[1];
+
+      uint8_t mcs_bw_field = mcs_flags & IEEE80211_RADIOTAP_MCS_BW_MASK;
+      if (mcs_bw_field == IEEE80211_RADIOTAP_MCS_BW_40) {
+        bwidth = CHANNEL_WIDTH_40;
+      } else if (mcs_bw_field == IEEE80211_RADIOTAP_MCS_BW_20 ||
+                 mcs_bw_field == IEEE80211_RADIOTAP_MCS_BW_20L ||
+                 mcs_bw_field == IEEE80211_RADIOTAP_MCS_BW_20U) {
+        bwidth = CHANNEL_WIDTH_20;
       }
-      printf("mcs_have & 4 = %d,%d \n", (mcs_have & 4),
-             (iterator.this_arg[1] & 1));
-      if ((mcs_have & 4) && (iterator.this_arg[1] & 4))
+
+      if (mcs_flags & 0x04) {
         sgi = 1;
-      if ((mcs_have & 1) && (iterator.this_arg[1] & 1))
-        bwidth = 1;
-      if ((mcs_have & 0x10) && (iterator.this_arg[1] & 0x10))
-        ldpc = 1;
-      if ((mcs_have & 0x20))
-        stbc = (iterator.this_arg[1] >> 5) & 3;
+      } else {
+        sgi = 0;
+      }
     } break;
 
     case IEEE80211_RADIOTAP_VHT: {
-      /* u16 known, u8 flags, u8 bandwidth, u8 mcs_nss[4], u8 coding, u8
-       * group_id, u16 partial_aid */
       u8 known = iterator.this_arg[0];
       u8 flags = iterator.this_arg[2];
       unsigned int mcs, nss;
@@ -107,11 +92,11 @@ bool Rtl8812aDevice::send_packet(const uint8_t *packet, size_t length) {
       if (known & 0x40) {
         auto bw = iterator.this_arg[3] & 0x1f;
         if (bw >= 1 && bw <= 3)
-          bwidth = 40; // 40 MHz
+          bwidth = 40;
         else if (bw >= 4 && bw <= 10)
-          bwidth = 80; // 80 MHz
+          bwidth = 80;
         else
-          bwidth = 20; // 20 MHz
+          bwidth = 20;
       }
 
       if (iterator.this_arg[8] & 1)
@@ -132,13 +117,26 @@ bool Rtl8812aDevice::send_packet(const uint8_t *packet, size_t length) {
     }
   }
 
-  printf("fixed rate:%d\n", fixed_rate);
-  printf("sgi =%d,bandwdith=%d,ldpc=%d,stbc=%d\n", sgi, bwidth, ldpc, stbc);
-
   usb_frame = new uint8_t[usb_frame_length]();
 
   ptxdesc = (struct tx_desc *)usb_frame;
 
+  _logger->info("fixed rate:{}, sgi:{}, radiotap_bwidth:{}, ldpc:{}, stbc:{}",
+                (int)fixed_rate, (int)sgi, (int)bwidth, (int)ldpc, (int)stbc);
+
+  uint8_t BWSettingOfDesc;
+  if (bwidth == CHANNEL_WIDTH_40) {
+    BWSettingOfDesc = 1;
+  } else if (bwidth == CHANNEL_WIDTH_80) {
+    BWSettingOfDesc = 2;
+  } else {
+    BWSettingOfDesc = 0;
+  }
+  _logger->info("TX DESC BW decision: _channel.ChannelWidth(RX)={}, radiotap_bwidth(TX)={}, BWSettingOfDesc(TX_DESC)={}",
+                (int)_channel.ChannelWidth, (int)bwidth, (int)BWSettingOfDesc);
+
+  SET_TX_DESC_DATA_BW_8812(usb_frame, BWSettingOfDesc);
+
   SET_TX_DESC_FIRST_SEG_8812(usb_frame, 1);
   SET_TX_DESC_LAST_SEG_8812(usb_frame, 1);
   SET_TX_DESC_OWN_8812(usb_frame, 1);
@@ -160,15 +158,15 @@ bool Rtl8812aDevice::send_packet(const uint8_t *packet, size_t length) {
   SET_TX_DESC_BMC_8812(usb_frame, 1);
   SET_TX_DESC_RATE_ID_8812(
       usb_frame,
-      static_cast<uint8_t>(rate_id)); // Originally set to 7, need to reconsider
+      static_cast<uint8_t>(rate_id));
 
   SET_TX_DESC_QUEUE_SEL_8812(usb_frame, 0x12);
   SET_TX_DESC_HWSEQ_EN_8812(
-      usb_frame, static_cast<uint8_t>(0)); /* Hw do not set sequence number */
+      usb_frame, static_cast<uint8_t>(0));
   SET_TX_DESC_SEQ_8812(
       usb_frame,
       GetSequence(packet +
-                  radiotap_length)); /* Copy inject sequence number to TxDesc */
+                  radiotap_length));
   SET_TX_DESC_RETRY_LIMIT_ENABLE_8812(usb_frame, static_cast<uint8_t>(1));
 
   SET_TX_DESC_DATA_RETRY_LIMIT_8812(usb_frame, static_cast<uint8_t>(0));
@@ -180,64 +178,40 @@ bool Rtl8812aDevice::send_packet(const uint8_t *packet, size_t length) {
   SET_TX_DESC_USE_RATE_8812(usb_frame, 1);
   SET_TX_DESC_TX_RATE_8812(usb_frame,
                            static_cast<uint8_t>(MRateToHwRate(
-                               fixed_rate))); // Originally set to 6, also need
-                                              // to reconsider how to convert
+                               fixed_rate)));
 
   if (ldpc) {
     SET_TX_DESC_DATA_LDPC_8812(usb_frame, ldpc);
   }
 
   SET_TX_DESC_DATA_STBC_8812(usb_frame, stbc & 3);
 
-  uint8_t BWSettingOfDesc;
-  if (_channel.ChannelWidth == CHANNEL_WIDTH_80) {
-    if (bwidth == 80)
-      BWSettingOfDesc = 2;
-    else if (bwidth == 40)
-      BWSettingOfDesc = 1;
-    else
-      BWSettingOfDesc = 0;
-  } else if (_channel.ChannelWidth == CHANNEL_WIDTH_40) {
-    if ((bwidth == 40) || (bwidth == 80))
-      BWSettingOfDesc = 1;
-    else
-      BWSettingOfDesc = 0;
-  } else
-    BWSettingOfDesc = 0;
-
-  SET_TX_DESC_DATA_BW_8812(usb_frame, BWSettingOfDesc);
-
   rtl8812a_cal_txdesc_chksum(usb_frame);
   _logger->info("tx desc formed");
 #ifdef DEBUG
   for (size_t i = 0; i < usb_frame_length; ++i) {
-    // Print each byte as a two-digit hexadecimal number
     std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0')
               << static_cast<int>(usb_frame[i]);
 
-    // Print a space between bytes, but not after the last byte
     if (i < usb_frame_length - 1) {
       std::cout << ",";
     }
   }
-  std::cout << std::dec << std::endl; // Reset to decimal formatting
-                                      // ----- end of fill tx desc -----
+  std::cout << std::dec << std::endl;
 #endif
   uint8_t *addr = usb_frame + TXDESC_SIZE;
   memcpy(addr, packet + radiotap_length, real_packet_length);
   _logger->info("packet formed");
 #ifdef DEBUG
   for (size_t i = 0; i < usb_frame_length; ++i) {
-    // Print each byte as a two-digit hexadecimal number
     std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0')
               << static_cast<int>(usb_frame[i]);
 
-    // Print a space between bytes, but not after the last byte
     if (i < usb_frame_length - 1) {
       std::cout << ",";
     }
   }
-  std::cout << std::dec << std::endl; // Reset to decimal formatting
+  std::cout << std::dec << std::endl;
 #endif
   resp = _device.send_packet(usb_frame, usb_frame_length);
   delete[] usb_frame;