- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

# /space/home/slher/rel/Beken/v6_4_5/macsw/ip/lmac/src/bfr/bfr.c

Go to the documentation of this file.

    00001
    00020 /*
    00021  * INCLUDE FILES
    00022  ****************************************************************************************
    00023  */
    00024
    00025 #include "bfr.h"
    00026
    00027 #if (RW_BFMER_EN)
    00028
    00029 // Station Definition
    00030 #include "sta_mgmt.h"
    00031 // VIF Definition
    00032 #include "vif_mgmt.h"
    00033 // Status Codes Definition
    00034 #include "co_status.h"
    00035 // General Purpose DMA Definition
    00036 #include "hal_dma.h"
    00037 // For use of rxl_mpdu_free function
    00038 #include "rxl_hwdesc.h"
    00039 // For TPC related definitions
    00040 #include "tpc.h"
    00041 #if NX_MFP
    00042 // For MFP related definitions
    00043 #include "mfp.h"
    00044 // For TXU related definitions
    00045 #include "txu_cntrl.h"
    00046 #endif
    00047 // For TD related definitions
    00048 #include "td.h"
    00049 #include "ke_timer.h"
    00050
    00051 /*
    00052  * DEFINES
    00053  ****************************************************************************************
    00054  */
    00055
    00060 #define BFR_NB_BIT_PHY_SU               (4)
    00061
    00065 #define BFR_NB_BIT_PHY_MU               (7)
    00066
    00070 #define BFR_NB_BIT_PSY_SU               (6)
    00071
    00075 #define BFR_NB_BIT_PSY_MU               (9)
    00076
    00078 #define BFR_NDPA_SU_DURATION            (56)
    00080 #define BFR_NDP_DURATION                (47)
    00082 #define BFR_BRP_DURATION                (52)
    00084 #define BFR_BCR_DURATION                (8)
    00086 #define BFR_SIFS_DURATION               (14)
    00087
    00088 /*
    00089  * MACROS
    00090  ****************************************************************************************
    00091  */
    00092
    00100 #define BFR_GET_REPORT_DURATION(length)                                         \
    00101              (((length + 20) / 20) * BFR_BCR_DURATION)                          \
    00102
    00103
    00111 #define BFR_STA_INFO_SET_AID12(sta_info, aid)                                  \
    00112             (sta_info = (aid & BFR_NDPA_AID12_MASK))                           \
    00113
    00114
    00122 #define BFR_STA_INFO_SET_FBACK_TYPE(sta_info, fback_type)                      \
    00123             (sta_info |= (fback_type << BFR_NPDA_FBACK_POS))                   \
    00124
    00125
    00133 #define BFR_STA_INFO_SET_NC_INDEX(sta_info, nc_index)                          \
    00134             (sta_info |= (nc_index << BFR_NPDA_NC_IDX_POS))                    \
    00135
    00136
    00153 #define BFR_VHT_MIMO_GET_VALUE(vht_mimo_ctrl, field)                           \
    00154             ((vht_mimo_ctrl >> BFR_MIMO_CTRL_ ## field ## _OFFSET) &           \
    00155                                BFR_MIMO_CTRL_ ## field ## _MASK)               \
    00156
    00157
    00164 #define BFR_IS_STATUS_BIT_SET(status, bit_pos)                                 \
    00165             (status & (1 << bit_pos))                                          \
    00166
    00167
    00174 #define BFR_SET_STATUS_BIT(status, bit_pos)                                    \
    00175             (status |= (1 << bit_pos))                                         \
    00176
    00177
    00184 #define BFR_RESET_STATUS_BIT(status, bit_pos)                                  \
    00185             (status &= ~(1 << bit_pos))                                        \
    00186
    00187
    00196 #define BFR_GET_NA(nc, nr)                                                     \
    00197             (nc * ((2 * nr) - nc - 1))                                         \
    00198
    00199 /*
    00200  * "PRIVATE" VARIABLES
    00201  ****************************************************************************************
    00202  */
    00203
    00205 struct bfr_mem_node bfr_nodes[BFR_MEM_NODE_NB];
    00207 struct bfr_info_sta bfr_sta_infos[NX_REMOTE_STA_MAX];
    00209 uint16_t bfr_smm_ptrs[BFR_MEM_REP_NB_MAX];
    00211 struct bfr_env_tag bfr_env;
    00212
    00220 const uint16_t bfr_ns[] = {
    00221     [PHY_CHNL_BW_20] = 52,
    00222     [PHY_CHNL_BW_40] = 108,
    00223     [PHY_CHNL_BW_80] = 234,
    00224     [PHY_CHNL_BW_160] = 468,
    00225     [PHY_CHNL_BW_80P80] = 468,
    00226 };
    00227
    00235 const uint8_t bfr_nsp[] = {
    00236     [PHY_CHNL_BW_20] = 30,
    00237     [PHY_CHNL_BW_40] = 58,
    00238     [PHY_CHNL_BW_80] = 122,
    00239     [PHY_CHNL_BW_160] = 244,
    00240     [PHY_CHNL_BW_80P80] = 244,
    00241 };
    00242
    00243 /*
    00244  * "PRIVATE" FUNCTION DECLARATIONS
    00245  ****************************************************************************************
    00246  */
    00247
    00248 static void bfr_start_next_calibration(void);
    00249 static void bfr_calibrate(void);
    00250
    00251 /*
    00252  * "PRIVATE" FUNCTION DEFINITIONS
    00253  ****************************************************************************************
    00254  */
    00261 __INLINE uint8_t bfr_get_bw(struct vif_info_tag *vif)
    00262 {
    00263     uint8_t bw;
    00264     #if (!NX_CHNL_CTXT)
    00265     struct phy_channel_info phy_info;
    00266     phy_get_channel(&phy_info, PHY_PRIM);
    00267     bw = (uint8_t) PHY_INFO_BW(phy_info);
    00268     #else
    00269     ASSERT_ERR(vif->chan_ctxt);
    00270     bw = vif->chan_ctxt->channel.type;
    00271     #endif
    00272     ASSERT_ERR(bw < PHY_CHNL_BW_OTHER);
    00273     return bw;
    00274 }
    00275
    00282 static void bfr_calib_tmr_exp_cb(void *env)
    00283 {
    00284     // Start calibration
    00285     bfr_calibrate();
    00286
    00287     // Restart the timer
    00288     mm_timer_set(&bfr_env.calib_tmr, ke_time() + bfr_env.calib_tmr_dur);
    00289 }
    00290
    00303 static void bfr_end_calibration_for_sta(struct bfr_info_sta *bfr, bool updated)
    00304 {
    00305     // Update STA state
    00306     BFR_RESET_STATUS_BIT(bfr->status, BFR_STA_CALIBRATE);
    00307
    00308     if (updated)
    00309     {
    00310         // Keep in mind that report has been updated, it will be downloaded upon next transmission.
    00311         BFR_SET_STATUS_BIT(bfr->status, BFR_STA_REPORT_UPDATED);
    00312         // Report in host memory is valid
    00313         BFR_SET_STATUS_BIT(bfr->status, BFR_STA_HAS_VALID_REPORT);
    00314
    00315         // Calibration is successful, store calibration time
    00316         bfr->last_calib_time = hal_machw_time();
    00317         // Update the index of the report currently used
    00318         bfr->host_report_idx ^= 1;
    00319     }
    00320
    00321     // If SU calibration, go back to IDLE state
    00322     if (bfr_env.status == BFR_STATUS_SU_CALIB)
    00323     {
    00324         // For profiling
    00325         PROF_BFR_SU_CALIB_CLR();
    00326
    00327         bfr_env.status = BFR_STATUS_IDLE;
    00328     }
    00329     else // MU Calibration
    00330     {
    00331         // Decrease number of expected reports
    00332         bfr_env.nb_reports--;
    00333
    00334         // If all reports have been received, stop MU Calibration
    00335         if (bfr_env.nb_reports == 0)
    00336         {
    00337             // For profiling
    00338             PROF_BFR_MU_CALIB_CLR();
    00339
    00340             bfr_env.status = BFR_STATUS_IDLE;
    00341         }
    00342     }
    00343
    00344     // If status is IDLE, can try to start a new calibration
    00345     if (bfr_env.status == BFR_STATUS_IDLE)
    00346     {
    00347         bfr_start_next_calibration();
    00348     }
    00349 }
    00350
    00362 static void bfr_tx_frame_cfm(void *env, uint32_t status)
    00363 {
    00364     // Cast the environment
    00365     struct bfr_info_sta *bfr = (struct bfr_info_sta *)env;
    00366
    00367     // Verify that the connection still exists
    00368     if (bfr->sta_idx != INVALID_STA_IDX)
    00369     {
    00370         /*
    00371          * Extract from RW-WLAN-nX-MAC-HW-UM v3.00, section 2.6.2.2.1.1
    00372          * "In case of unsuccessful reception of beamforming report response, the NDP is
    00373          * marked as done with retry limit reached".
    00374          * Same behavior implemented for Beamforming Report Poll frame.
    00375          */
    00376         if (!(status & DESC_DONE_TX_BIT) || (status & RETRY_LIMIT_REACHED_BIT))
    00377         {
    00378             // Abort calibration for this STA
    00379             bfr_end_calibration_for_sta(bfr, false);
    00380             // A new calibration will be restarted later
    00381         }
    00382         // else continue until report indication.
    00383     }
    00384 }
    00385
    00396 static void bfr_dma_upload_cfm(void *env)
    00397 {
    00398     // Get BFR information
    00399     struct bfr_info_sta *bfr = (struct bfr_info_sta *)env;
    00400
    00401     // For profiling
    00402     PROF_BFR_UPLOAD_CLR();
    00403
    00404     // Verify that the connection still exists
    00405     if (bfr->sta_idx != INVALID_STA_IDX)
    00406     {
    00407         // Decrease expected number of confirmations for this STA
    00408         bfr->nb_segments--;
    00409
    00410         // If all segments have been uploaded, end calibration procedure for this STA.
    00411         if (bfr->nb_segments == 0)
    00412         {
    00413             // Calibration is done for this STA
    00414             bfr_end_calibration_for_sta(bfr, true);
    00415         }
    00416     }
    00417 }
    00418
    00426 static bool bfr_is_vht_bfr(uint8_t *frame)
    00427 {
    00428     // Return if frame is VHT Compressend Beamforming frame
    00429     bool is_vht_bfr = false;
    00430
    00431     do
    00432     {
    00433         // Frame MAC Header
    00434         struct mac_hdr *mac_hdr = (struct mac_hdr *)frame;
    00435
    00436         // Check if frame is a Management Action No Ack frame
    00437         if (!((mac_hdr->fctl & MAC_FCTRL_TYPESUBTYPE_MASK) == MAC_FCTRL_ACTION_NO_ACK))
    00438         {
    00439             break;
    00440         }
    00441
    00442         // Check if frame is a VHT Action frame
    00443         if (co_read8p(CPU2HW(frame) + BFR_VHT_BFR_CATEGORY_OFFSET) != MAC_VHT_ACTION_CATEGORY)
    00444         {
    00445             break;
    00446         }
    00447
    00448         // Check if frame is a VHT Compressed Beamforming frame
    00449         if (co_read8p(CPU2HW(frame) + BFR_VHT_BFR_VHT_ACTION_OFFSET) != MAC_COMP_BEAMFORMING_VHT_ACTION)
    00450         {
    00451             break;
    00452         }
    00453
    00454         is_vht_bfr = true;
    00455     } while (0);
    00456
    00457     return (is_vht_bfr);
    00458 }
    00459
    00471 static bool bfr_is_frame_bf_compliant(struct txdesc *txdesc, struct bfr_info_sta *bfr)
    00472 {
    00473     // Indicate if frame can be transmitted using beamforming
    00474     bool bf_compliant = false;
    00475
    00476     do
    00477     {
    00478         #if (RW_MUMIMO_TX_EN)
    00479         // Check if the MPDU is inside a MU-MIMO PPDU, in that case we have to beamform
    00480         if (is_in_mumimo_ppdu(txdesc))
    00481         {
    00482             bf_compliant = true;
    00483             break;
    00484         }
    00485         else
    00486         #endif //(RW_MUMIMO_TX_EN)
    00487         {
    00488             #if (NX_AMPDU_TX)
    00489             if (is_mpdu_agg(txdesc))
    00490             {
    00491                 if (is_mpdu_first(txdesc))
    00492                 {
    00493                     // Number of spatial streams for the transmission
    00494                     uint32_t tx_nss;
    00495
    00496                     // Check if frame is a VHT frame
    00497                     if (((txdesc->umac.phy_flags & FORMAT_MOD_TX_RCX_MASK) >> FORMAT_MOD_TX_RCX_OFT)
    00498                                                                                     != FORMATMOD_VHT)
    00499                     {
    00500                         break;
    00501                     }
    00502
    00503                     tx_nss = ((((txdesc->umac.phy_flags & MCS_INDEX_TX_RCX_MASK) >> MCS_INDEX_TX_RCX_OFT)
    00504                                         & VHT_NSS_MASK) >> VHT_NSS_OFT);
    00505
    00506                     // Do not beamform if number of used spatial streams is the number of antennas
    00507                     if (tx_nss == phy_get_ntx())
    00508                     {
    00509                         break;
    00510                     }
    00511
    00512                     // Check that number of spatial streams is compliant with the received report
    00513                     if ((uint8_t)tx_nss > bfr->last_rx_nc)
    00514                     {
    00515                         break;
    00516                     }
    00517                 }
    00518                 else
    00519                 {
    00520                     break;
    00521                 }
    00522             }
    00523             else
    00524             #endif //(NX_AMPDU_TX)
    00525             {
    00526                 // Check if the frame can be beamformed as a unicast
    00527                 if (!bfr_is_bfmed_sglt_allowed(txdesc))
    00528                 {
    00529                     break;
    00530                 }
    00531             }
    00532         }
    00533
    00534         // Transmission can be done using beamforming
    00535         bf_compliant = true;
    00536     } while (0);
    00537
    00538     return (bf_compliant);
    00539 }
    00540
    00549 static void bfr_increase_nb_frames(struct bfr_mem_node *node)
    00550 {
    00551     if (node->nb_frames == 0)
    00552     {
    00553         bfr_env.nb_used_nodes++;
    00554     }
    00555
    00556     node->nb_frames++;
    00557 }
    00558
    00566 static void bfr_remove_node(struct bfr_mem_node *node)
    00567 {
    00568     // Extract node
    00569     co_list_extract(&bfr_env.nodes, &node->hdr);
    00570
    00571     // Check if node is linked with a STA
    00572     if (node->sta_idx != INVALID_STA_IDX)
    00573     {
    00574         // Get BFR information
    00575         struct bfr_info_sta *bfr = &bfr_sta_infos[node->sta_idx];
    00576
    00577         // Sanity check
    00578         ASSERT_ERR(bfr->node == node);
    00579
    00580         // Unlink the STA and the node
    00581         bfr->node = NULL;
    00582     }
    00583
    00584     ASSERT_WARN(node->smm_index < BFR_MEM_REP_NB_MAX);
    00585
    00586     // Free the SMM index that was used for this node
    00587     bfr_smm_ptrs[node->smm_index] = 0;
    00588     node->smm_index = BFR_INVALID_SMM_INDEX;
    00589     node->sta_idx = INVALID_STA_IDX;
    00590
    00591     // Push it in list of free nodes
    00592     co_list_push_back(&bfr_env.free_nodes, &node->hdr);
    00593 }
    00594
    00609 static bool bfr_check_next_nodes(uint16_t needed_length, struct bfr_mem_node **start_node,
    00610                                  uint8_t *nb_nodes)
    00611 {
    00612     // Remaining length to be found
    00613     uint16_t rem_len = needed_length;
    00614     // Get node after the indicated nodes
    00615     struct bfr_mem_node *cnode = (*start_node);
    00616
    00617     // Initialize number of considered nodes
    00618     *nb_nodes = 0;
    00619
    00620     // Loop until no more nodes are available or all needed length has been found
    00621     while (cnode && rem_len)
    00622     {
    00623         // Get next node
    00624         struct bfr_mem_node *next_node = (struct bfr_mem_node *)cnode->hdr.next;
    00625
    00626         // Increase number of considered nodes
    00627         (*nb_nodes)++;
    00628
    00629         // Check if node can be overwritten
    00630         if (cnode->nb_frames)
    00631         {
    00632             // Stop to search
    00633             *start_node = cnode;
    00634             break;
    00635         }
    00636
    00637         // Update remaining length to find
    00638         rem_len -= co_min(rem_len, cnode->length);
    00639
    00640         if (!rem_len)
    00641         {
    00642             break;
    00643         }
    00644
    00645         // Check if we reached the last node
    00646         if (next_node)
    00647         {
    00648             // We can use the free memory between the two nodes
    00649             rem_len -= co_min(rem_len, next_node->start_addr - (cnode->start_addr + cnode->length));
    00650         }
    00651         else
    00652         {
    00653             // We can use the memory between the last used node and end of the memory
    00654             rem_len -= co_min(rem_len, bfr_env.bfr_mem_eaddr - (cnode->start_addr + cnode->length));
    00655         }
    00656
    00657         // Get next node
    00658         cnode = next_node;
    00659     }
    00660
    00661     // Return if the required length has been found or not
    00662     return (rem_len == 0);
    00663 }
    00664
    00674 static struct bfr_mem_node *bfr_get_node(uint16_t length)
    00675 {
    00676     // Length of report blocks shall be alignment on words
    00677     uint16_t pad_length = (length & 0xFFFC) + 4;
    00678
    00679     // Get first used node
    00680     struct bfr_mem_node *cnode = (struct bfr_mem_node *)co_list_pick(&bfr_env.nodes);
    00681     // New node that can be used
    00682     struct bfr_mem_node *nnode = NULL;
    00683     // Keep previous node
    00684     struct bfr_mem_node *pnode = NULL;
    00685
    00686     // Best previous node for insertion
    00687     struct bfr_mem_node *bnode = NULL;
    00688     // Start address for new node
    00689     uint16_t best_addr = 0;
    00690     // Number of report overwritten for the best found case
    00691     uint8_t nb_nodes = 0xFF;
    00692
    00693     if (!cnode)
    00694     {
    00695         // Whole memory is free, we can put the report at the beginning of the BFR Memory
    00696         best_addr = BFR_MEM_START_ADDR;
    00697         nb_nodes = 0;
    00698     }
    00699     else
    00700     {
    00701         // Start Address
    00702         uint16_t start_addr = BFR_MEM_START_ADDR;
    00703
    00704         // Go through the memory and find the best place for the new report
    00705         while (cnode)
    00706         {
    00707             // Compute available length before the used node
    00708             uint16_t ava_length = cnode->start_addr - start_addr;
    00709
    00710             // Check if enough room can be found
    00711             if (ava_length >= pad_length)
    00712             {
    00713                 // Can directly use this place
    00714                 nb_nodes = 0;
    00715                 bnode = pnode;
    00716                 best_addr = start_addr;
    00717                 break;
    00718             }
    00719             else
    00720             {
    00721                 do
    00722                 {
    00723                     // Number of node to be removed from cnode
    00724                     uint8_t new_nb_nodes = 0xFF;
    00725
    00726                     // Check if a good position replacing a report has already been found
    00727                     if (nb_nodes == 1)
    00728                     {
    00729                         break;
    00730                     }
    00731
    00732                     // Check number of nodes to be removed if we insert the report here
    00733                     if (!bfr_check_next_nodes(pad_length - ava_length, &cnode, &new_nb_nodes))
    00734                     {
    00735                         break;
    00736                     }
    00737
    00738                     if (new_nb_nodes < nb_nodes)
    00739                     {
    00740                         // Keep this position in mind
    00741                         nb_nodes = new_nb_nodes;
    00742                         best_addr = start_addr;
    00743                         bnode = pnode;
    00744                     }
    00745
    00746                 } while (0);
    00747             }
    00748
    00749             // Update start_addr with end of current block
    00750             start_addr = cnode->start_addr + cnode->length;
    00751
    00752             // Get next node
    00753             pnode = cnode;
    00754             cnode = (struct bfr_mem_node *)cnode->hdr.next;
    00755         }
    00756
    00757         // If no free place has been found check end of memory, after last node
    00758         if (best_addr == 0)
    00759         {
    00760             // Compute available length between end of last node and end of memory
    00761             uint16_t ava_length = bfr_env.bfr_mem_eaddr - start_addr;
    00762
    00763             // Check if enough room can be found
    00764             if (ava_length >= pad_length)
    00765             {
    00766                 // Can directly use this place
    00767                 nb_nodes = 0;
    00768                 bnode = pnode;
    00769                 best_addr = start_addr;
    00770             }
    00771         }
    00772     }
    00773
    00774     // Check if a position has been found
    00775     if (best_addr != 0)
    00776     {
    00777         // Remove nodes
    00778         while (nb_nodes)
    00779         {
    00780             // Get the node to extract
    00781             cnode = (bnode)
    00782                     ? (struct bfr_mem_node *)bnode->hdr.next
    00783                     : (struct bfr_mem_node *)co_list_pick(&bfr_env.nodes);
    00784
    00785             // Extract the node
    00786             bfr_remove_node(cnode);
    00787
    00788             nb_nodes--;
    00789         }
    00790
    00791         // Get a new memory node if no node was available before
    00792         nnode =  (struct bfr_mem_node *)co_list_pop_front(&bfr_env.free_nodes);
    00793
    00794         // Sanity check
    00795         ASSERT_ERR(nnode);
    00796
    00797         // And start to fill the information
    00798         nnode->start_addr = best_addr;
    00799         nnode->length = pad_length;
    00800         nnode->nb_frames = 0;
    00801
    00802         // Insert in list of used nodes
    00803         co_list_insert_after(&bfr_env.nodes, (bnode) ? &bnode->hdr : NULL, &nnode->hdr);
    00804     }
    00805
    00806     // Return new node
    00807     return (nnode);
    00808 }
    00809
    00818 static uint8_t bfr_get_smm_index(struct bfr_mem_node *node)
    00819 {
    00820     uint8_t smm_index = BFR_INVALID_SMM_INDEX;
    00821
    00822     // Go through the list of smm pointer in order to find an available SMM index
    00823     for (int i = 0; i < BFR_MEM_REP_NB_MAX; i++)
    00824     {
    00825         uint16_t *smm_ptr = &bfr_smm_ptrs[i];
    00826
    00827         if (*smm_ptr == 0)
    00828         {
    00829             // NOTE : BFR Memory is write-only and accesses are 32-bit only
    00830             uint32_t a_bfr;
    00831             uint32_t index_val;
    00832
    00833             *smm_ptr = node->start_addr;
    00834             smm_index = i;
    00835
    00836             // Get report pointer in BFR Memory
    00837             a_bfr = BFR_MEM_BASE_ADDR + ((smm_index >> 1) * 4);
    00838
    00839             if (smm_index & 1)
    00840             {
    00841                 uint16_t *adj_smm_ptr = &bfr_smm_ptrs[i - 1];
    00842
    00843                 index_val = (uint32_t)(*adj_smm_ptr) | ((uint32_t)(*smm_ptr) << 16);
    00844             }
    00845             else
    00846             {
    00847                 uint16_t *adj_smm_ptr = &bfr_smm_ptrs[i + 1];
    00848
    00849                 index_val = (uint32_t)(*smm_ptr) | ((uint32_t)(*adj_smm_ptr) << 16);
    00850             }
    00851
    00852             // Set new value
    00853             REG_PL_WR(a_bfr, index_val);
    00854
    00855             break;
    00856         }
    00857     }
    00858
    00859     return (smm_index);
    00860 }
    00861
    00875 static uint16_t bfr_get_report_max_length(struct bfr_info_sta *bfr, bool mu)
    00876 {
    00877     // Length in bits
    00878     uint32_t length;
    00879     // Number of rows, number of columns, number of bits for phy and psy values, number of angles
    00880     uint8_t nr, nc, b_phy, b_psy, na;
    00881     // Number of carriers in Compressed Beamforming Feedback
    00882     uint16_t ns;
    00883     // Retrieve bandwitdh
    00884     uint8_t bw = bfr_get_bw(&vif_info_tab[bfr->vif_idx]);
    00885
    00886     if (mu)
    00887     {
    00888         nc = bfr->bfee_nss + 1;
    00889         b_phy = BFR_NB_BIT_PHY_MU;
    00890         b_psy = BFR_NB_BIT_PSY_MU;
    00891     }
    00892     else
    00893     {
    00894         nc = 1;
    00895         b_phy = BFR_NB_BIT_PHY_SU;
    00896         b_psy = BFR_NB_BIT_PSY_SU;
    00897     }
    00898
    00899     nr = bfr_env.bfer_nss + 1;
    00900     na = BFR_GET_NA(nc, nr);
    00901     ns = bfr_ns[bw];
    00902
    00903     // Deduce length of Compressend Beamforming Feedback
    00904     length = (8 * nc) + (ns * na * ((b_phy + b_psy) >> 1));
    00905
    00906     // If MU calibration, add length of MU Exclusive Beamforming Report
    00907     if (mu)
    00908     {
    00909         // Number of carriers in MU Exclusive Beamforming Report
    00910         uint8_t nsp = bfr_nsp[bw];
    00911
    00912         length += 4 * nc * nsp;
    00913     }
    00914
    00915     // Return length in bytes
    00916     return (uint16_t)(length / 8);
    00917 }
    00918
    00934 static struct txdesc *bfr_prep_ndpa(struct bfr_info_sta *bfr, uint8_t *nb_devs)
    00935 {
    00936     // HW TX Header Descriptor
    00937     struct tx_hd *thd = NULL;
    00938     // Next BFR device information
    00939     struct bfr_info_sta *next_bfr;
    00940     // Get VIF Information
    00941     struct vif_info_tag *vif;
    00942     // Allocated frame
    00943     struct txl_frame_desc_tag *frame = NULL;
    00944     struct mac_hdr_ctrl *mac_hdr;
    00945     // Policy table
    00946     struct tx_policy_tbl *pol_tbl;
    00947     uint32_t paid_gid;
    00948
    00949
    00950     // Check if MU-capable devices can be grouped together on the VIF
    00951     do
    00952     {
    00953         // Initialize number of calibrations
    00954         *nb_devs = 1;
    00955
    00956         // Clear STA Info field, except the AID. Feedback type is then SU.
    00957         bfr->sta_info &= BFR_NDPA_AID12_MASK;
    00958
    00959         vif = &vif_info_tab[bfr->vif_idx];
    00960
    00961         // Extract bandwidth for the calibration
    00962         bfr_env.calib_bw = bfr_get_bw(vif);
    00963
    00964         if (!BFR_IS_STATUS_BIT_SET(bfr->status, BFR_STA_MU_CAPABLE))
    00965         {
    00966             break;
    00967         }
    00968
    00969         // Update STA Info field. Feedback type is MU.
    00970         BFR_STA_INFO_SET_FBACK_TYPE(bfr->sta_info, BFR_MU_TYPE);
    00971         BFR_STA_INFO_SET_NC_INDEX(bfr->sta_info, 0);
    00972
    00973         // If peer device is not STA, only one STA info can be inserted in the NDPA
    00974         if (vif->type == VIF_STA)
    00975         {
    00976             break;
    00977         }
    00978
    00979         next_bfr = (struct bfr_info_sta *)bfr->hdr.next;
    00980
    00981         /*
    00982          * If next devices are also MU capable and are on the same VIF, we can group them together
    00983          * for the calibration.
    00984          */
    00985         while (next_bfr)
    00986         {
    00987             #if (NX_CHNL_CTXT)
    00988             // Get next VIF information
    00989             struct vif_info_tag *next_vif;
    00990             #endif //(NX_CHNL_CTXT)
    00991
    00992             // Check if peer STA is MU capable
    00993             if ((bfr->vif_idx != next_bfr->vif_idx) ||
    00994                 !BFR_IS_STATUS_BIT_SET(next_bfr->status, BFR_STA_MU_CAPABLE))
    00995             {
    00996                 break;
    00997             }
    00998
    00999             #if (NX_CHNL_CTXT)
    01000             next_vif = &vif_info_tab[next_bfr->vif_idx];
    01001
    01002             // Verify that we are on the same channel context
    01003             if (next_vif->chan_ctxt != vif->chan_ctxt)
    01004             {
    01005                 break;
    01006             }
    01007             #endif //(NX_CHNL_CTXT)
    01008
    01009             // Increase number of calibration
    01010             (*nb_devs)++;
    01011
    01012             // Clear STA Info field, except the AID.
    01013             next_bfr->sta_info &= BFR_NDPA_AID12_MASK;
    01014
    01015             // Update STA Info field. Feedback type is MU.
    01016             BFR_STA_INFO_SET_FBACK_TYPE(next_bfr->sta_info, BFR_MU_TYPE);
    01017             BFR_STA_INFO_SET_NC_INDEX(next_bfr->sta_info, 0);
    01018
    01019             next_bfr = (struct bfr_info_sta *)next_bfr->hdr.next;
    01020         }
    01021     } while (0);
    01022
    01023     next_bfr = bfr;
    01024
    01025     // If we calibrate only 1 device, we switch to SU feedback
    01026     if (*nb_devs == 1)
    01027     {
    01028         // Clear STA Info field, except the AID. Feedback type is then SU.
    01029         bfr->sta_info &= BFR_NDPA_AID12_MASK;
    01030     }
    01031
    01032     // Allocate a frame descriptor from the TX path
    01033     frame = txl_frame_get(TX_DEFAULT_NDPA_BRP,
    01034                           BFR_NDPA_SU_LEN + ((*nb_devs - 1) * BFR_NDPA_STA_INFO_LEN));
    01035
    01036     // Verify that enough memory has been found for the frame
    01037     if (frame == NULL)
    01038         return NULL;
    01039
    01040     // Get the buffer pointer
    01041     mac_hdr = txl_buffer_payload_get(&frame->txdesc);
    01042
    01043     // Fill-in the MAC Header
    01044     mac_hdr->fctl = MAC_FCTRL_VHT_NDPA;
    01045     // Duration includes NDPA duration, NDP duration, SIFS and report durations (worst case)
    01046     mac_hdr->durid = BFR_NDPA_SU_DURATION + BFR_SIFS_DURATION + BFR_NDP_DURATION;
    01047
    01048     // Add expected report durations
    01049     for (int i = 0; i < *nb_devs; i++)
    01050     {
    01051         mac_hdr->durid += (BFR_SIFS_DURATION
    01052                     + BFR_GET_REPORT_DURATION(bfr_get_report_max_length(next_bfr, (*nb_devs > 1))));
    01053
    01054         next_bfr = (struct bfr_info_sta *)next_bfr->hdr.next;
    01055     }
    01056
    01057     next_bfr = bfr;
    01058
    01059     /*
    01060      * If the VHT NDP Announcement frame includes more than one STA Info field, the RA shall be
    01061      * set to the broadcast address, else the RA shall be set to the MAC address of the VHT
    01062      * beamformee.
    01063      */
    01064     if (*nb_devs == 1)
    01065     {
    01066         struct sta_info_tag *sta = &sta_info_tab[bfr->sta_idx];
    01067         mac_hdr->addr1 = *sta_mgmt_get_peer_addr(bfr->sta_idx);
    01068         paid_gid = sta->paid_gid;
    01069     }
    01070     else
    01071     {
    01072         mac_hdr->addr1 = mac_addr_bcst;
    01073         paid_gid = 63 << GID_TX_OFT;
    01074     }
    01075     mac_hdr->addr2 = *vif_mgmt_get_addr(bfr->vif_idx);
    01076
    01077     // Fill-in the Frame Content
    01078     for (int i = 0; i < *nb_devs; i++)
    01079     {
    01080         // Set STA Info
    01081         co_write16p(CPU2HW(mac_hdr) + BFR_NDPA_STA_INFO_OFFSET + (i * BFR_NDPA_STA_INFO_LENGTH),
    01082                     next_bfr->sta_info);
    01083
    01084         // Store the token
    01085         next_bfr->last_token = bfr_env.token;
    01086
    01087         next_bfr = (struct bfr_info_sta *)next_bfr->hdr.next;
    01088     }
    01089
    01090     // Set token -- !!! Bit 0 and Bit 1 are reserved !!!
    01091     co_write8p(CPU2HW(mac_hdr) + BFR_NDPA_DIALOG_TOKEN_OFFSET,
    01092                bfr_env.token << BFR_NPDA_TOKEN_POS);
    01093
    01094     // Update the token for the next calibration
    01095     bfr_env.token = (bfr_env.token + 1) & BFR_NDPA_TOKEN_MAX;
    01096
    01097     /*
    01098      * Configure HW Descriptor
    01099      * The expectedAck field must be set to 0 - No acknowledgement.
    01100      * protFrmDur must be provided and dontTouchDur bit shall be set.
    01101      */
    01102     thd = &frame->txdesc.lmac.hw_desc->thd;
    01103     thd->macctrlinfo1 |= (mac_hdr->durid << PROT_FRM_DURATION_OFT);
    01104     thd->macctrlinfo1 &= ~EXPECTED_ACK_MSK;
    01105     thd->macctrlinfo2 |= DONT_TOUCH_DUR;
    01106
    01107     thd->nextfrmexseq_ptr = 0;
    01108     thd->nextmpdudesc_ptr = 0;
    01109     thd->macctrlinfo2 &= ~(WHICHDESC_MSK | UNDER_BA_SETUP_BIT);
    01110     thd->phyctrlinfo = paid_gid;
    01111     thd->statinfo = 0;
    01112
    01113     // Get policy table
    01114     pol_tbl = (struct tx_policy_tbl *)HW2CPU(thd->policyentryaddr);
    01115
    01116     // Set BW
    01117     pol_tbl->ratecntrlinfo[0] &= ~BW_TX_RCX_MASK;
    01118     pol_tbl->ratecntrlinfo[0] |= ((uint32_t)bfr_env.calib_bw << BW_TX_RCX_OFT);
    01119
    01120     #if (NX_CHNL_CTXT || NX_P2P)
    01121     // Set VIF and STA indexes
    01122     frame->txdesc.host.vif_idx = bfr->vif_idx;
    01123     frame->txdesc.host.staid   = bfr->sta_idx;
    01124     #endif //(NX_CHNL_CTXT || NX_P2P)
    01125
    01126     return (&(frame->txdesc));
    01127 }
    01128
    01141 static struct txdesc *bfr_prep_ndp(struct bfr_info_sta *bfr, uint8_t nb_devs)
    01142 {
    01143     // Allocate a frame descriptor from the TX path
    01144     struct txl_frame_desc_tag *frame = txl_frame_get(TX_DEFAULT_NDP, 0);
    01145     // HW TX Header Descriptor
    01146     struct tx_hd *thd;
    01147     // Policy table
    01148     struct tx_policy_tbl *pol_tbl;
    01149
    01150     if (frame == NULL)
    01151         return NULL;
    01152
    01153     // Fill-in the confirmation structure
    01154     frame->cfm.cfm_func = bfr_tx_frame_cfm;
    01155     frame->cfm.env = bfr;
    01156
    01157     /*
    01158      * Configure HW Descriptor
    01159      * The expectedAck field must be set to 1 - Normal ACK.
    01160      * dontTouchDur bit shall be reset.
    01161      */
    01162     thd = &frame->txdesc.lmac.hw_desc->thd;
    01163     thd->macctrlinfo1 &= ~EXPECTED_ACK_MSK;
    01164     thd->macctrlinfo1 |= EXPECTED_ACK_NORMAL_ACK;
    01165     thd->macctrlinfo2 &= ~DONT_TOUCH_DUR;
    01166     thd->phyctrlinfo &= ~(GID_TX_MASK | PAID_TX_MASK);
    01167     if (nb_devs > 1)
    01168     {
    01169         thd->phyctrlinfo = 63 << GID_TX_OFT;
    01170     }
    01171     else
    01172     {
    01173         struct sta_info_tag *sta = &sta_info_tab[bfr->sta_idx];
    01174         thd->phyctrlinfo = sta->paid_gid;
    01175     }
    01176
    01177     thd->nextfrmexseq_ptr = 0;
    01178     thd->nextmpdudesc_ptr = 0;
    01179     thd->macctrlinfo2 &= ~(WHICHDESC_MSK | UNDER_BA_SETUP_BIT);
    01180     thd->statinfo = 0;
    01181
    01182     // Get policy table
    01183     pol_tbl = (struct tx_policy_tbl *)HW2CPU(thd->policyentryaddr);
    01184
    01185     // Set BW
    01186     pol_tbl->ratecntrlinfo[0] &= ~BW_TX_RCX_MASK;
    01187     pol_tbl->ratecntrlinfo[0] |= ((uint32_t)bfr_env.calib_bw << BW_TX_RCX_OFT);
    01188
    01189     #if (NX_CHNL_CTXT || NX_P2P)
    01190     // Set VIF and STA indexes
    01191     frame->txdesc.host.vif_idx = bfr->vif_idx;
    01192     frame->txdesc.host.staid   = bfr->sta_idx;
    01193     #endif //(NX_CHNL_CTXT || NX_P2P)
    01194
    01195     return (&(frame->txdesc));
    01196 }
    01197
    01209 static struct txdesc *bfr_prep_bfr_poll(struct bfr_info_sta *bfr)
    01210 {
    01211     // Allocate a frame descriptor from the TX path
    01212     struct txl_frame_desc_tag *frame = txl_frame_get(TX_DEFAULT_NDPA_BRP, BFR_BRP_LEN);
    01213     // HW TX Header Descriptor
    01214     struct tx_hd *thd;
    01215     struct mac_hdr_ctrl *mac_hdr;
    01216     // Policy table
    01217     struct tx_policy_tbl *pol_tbl;
    01218     struct sta_info_tag *sta = &sta_info_tab[bfr->sta_idx];
    01219
    01220     // Verify that enough memory has been found for the frame
    01221     if (frame == NULL)
    01222     {
    01223         return NULL;
    01224     }
    01225
    01226     // Get the buffer pointer
    01227     mac_hdr = txl_buffer_payload_get(&frame->txdesc);
    01228
    01229     // Fill-in the MAC Header
    01230     mac_hdr->fctl = MAC_FCTRL_BFM_REPORT_POLL;
    01231     mac_hdr->durid = 0;
    01232     mac_hdr->addr1 = *sta_mgmt_get_peer_addr(bfr->sta_idx);
    01233     mac_hdr->addr2 = *vif_mgmt_get_addr(bfr->vif_idx);
    01234
    01235     // Fill-in the Frame Content - Request for all feedback segments
    01236     co_write8p(CPU2HW(mac_hdr) + BFR_BRP_RETX_BITMAP_OFFSET, 0xFF);
    01237
    01238     /*
    01239      * Configure HW Descriptor
    01240      * The expectedAck field must be set to 1 - Normal ACK.
    01241      * dontTouchDur bit shall be reset.
    01242      */
    01243     thd = &frame->txdesc.lmac.hw_desc->thd;
    01244     thd->macctrlinfo1 &= ~EXPECTED_ACK_MSK;
    01245     thd->macctrlinfo1 |= EXPECTED_ACK_NORMAL_ACK;
    01246     thd->macctrlinfo2 &= ~DONT_TOUCH_DUR;
    01247
    01248     thd->nextfrmexseq_ptr = 0;
    01249     thd->nextmpdudesc_ptr = 0;
    01250     thd->macctrlinfo2 &= ~(WHICHDESC_MSK | UNDER_BA_SETUP_BIT);
    01251     thd->statinfo = 0;
    01252     thd->phyctrlinfo = sta->paid_gid;
    01253
    01254     // Get policy table
    01255     pol_tbl = (struct tx_policy_tbl *)HW2CPU(thd->policyentryaddr);
    01256
    01257     // Set BW
    01258     pol_tbl->ratecntrlinfo[0] &= ~BW_TX_RCX_MASK;
    01259     pol_tbl->ratecntrlinfo[0] |= ((uint32_t)bfr_env.calib_bw << BW_TX_RCX_OFT);
    01260
    01261     // Fill-in the confirmation structure
    01262     frame->cfm.cfm_func = bfr_tx_frame_cfm;
    01263     frame->cfm.env = bfr;
    01264
    01265     #if (NX_CHNL_CTXT || NX_P2P)
    01266     // Set VIF and STA indexes
    01267     frame->txdesc.host.vif_idx = bfr->vif_idx;
    01268     frame->txdesc.host.staid   = bfr->sta_idx;
    01269     #endif //(NX_CHNL_CTXT || NX_P2P)
    01270
    01271     return (&(frame->txdesc));
    01272 }
    01273
    01301 static uint8_t bfr_upload_report(struct bfr_info_sta *bfr, struct rxdesc *rxdesc,
    01302                                  uint8_t *frame)
    01303 {
    01304     // Returned status, consider the upload is valid
    01305     uint8_t status = BFR_RX_STATUS_VALID;
    01306
    01307     // Get MAC HW descriptor
    01308     struct rx_dmadesc *dma_hdrdesc = rxl_dmadesc_get(rxdesc);
    01309
    01310     // Index of the currently unused host report, where the upload is done
    01311     uint16_t report_idx = bfr->host_report_idx ^ 1;
    01312     // Address in host memory for report upload
    01313     uint32_t host_addr = bfr->host_report_addr[report_idx];
    01314     // Keep VHT MIMO Control value, value in frame will be overwritten in order to insert BFR Header
    01315     uint32_t vht_mimo_ctrl = co_read32p(CPU2HW(frame) + BFR_VHT_BFR_VHT_MIMO_CTRL_OFFSET);
    01316     /*
    01317      * Payload offset (for first payload descriptor, no need to copy the MAC Header and needless fields
    01318      * before the report.
    01319      */
    01320     uint16_t payload_offset = BFR_VHT_REPORT_OFFSET;
    01321     // Total length of the received MPDU
    01322     uint16_t upload_len = dma_hdrdesc->hd.frmlen;
    01323
    01324     // If report is not a 80MHz report or is not for 2 spatial streams, reject it
    01325     if ((BFR_VHT_MIMO_GET_VALUE(vht_mimo_ctrl, CHAN_WIDTH) != bfr_env.calib_bw)
    01326            /*|| (BFR_VHT_MIMO_GET_VALUE(vht_mimo_ctrl, NC) != 1)*/)
    01327     {
    01328         return (BFR_RX_STATUS_ERROR);
    01329     }
    01330
    01331     // For profiling
    01332     PROF_BFR_UPLOAD_SET();
    01333
    01334     if (!BFR_VHT_MIMO_GET_VALUE(vht_mimo_ctrl, FIRST_SEGM))
    01335     {
    01336         // Do not overwrite previously uploaded segments
    01337         host_addr += bfr->host_report_len[report_idx];
    01338     }
    01339     else
    01340     {
    01341         // Reset report length
    01342         bfr->host_report_len[report_idx] = 0;
    01343         // And store number of reports to be confirmed
    01344         bfr->nb_segments = 1 + BFR_VHT_MIMO_GET_VALUE(vht_mimo_ctrl, REM_SEGM);
    01345
    01346         // Move back payload offset for the BFR Header whose length is 2 bytes
    01347         payload_offset -= 2;
    01348
    01349         // Set the BFR Header, match with the first 16 bits of the received VHT MIMO Control value
    01350         co_write16p(CPU2HW(frame) + BFR_VHT_BFR_VHT_MIMO_CTRL_OFFSET + 1,
    01351                     (uint16_t)vht_mimo_ctrl);
    01352
    01353         // Keep received Nc
    01354         bfr->last_rx_nc = BFR_VHT_MIMO_GET_VALUE(vht_mimo_ctrl, NC);
    01355     }
    01356
    01357     upload_len -= payload_offset;
    01358
    01359     bfr->host_report_len[report_idx] += upload_len;
    01360     if (bfr->host_report_len[report_idx] > bfr->host_max_report_len)
    01361     {
    01362         // Finally end calibration for the STA
    01363         bfr_end_calibration_for_sta(bfr, false);
    01364
    01365         // Return an error status
    01366         return BFR_RX_STATUS_ERROR;
    01367     }
    01368
    01369     // Program upload of report
    01370     rxl_mpdu_partial_transfer(rxdesc, upload_len, host_addr, payload_offset,
    01371                               bfr_dma_upload_cfm, bfr);
    01372
    01373     return (status);
    01374 }
    01375
    01390 static uint8_t bfr_download_report(struct bfr_info_sta *bfr, struct dma_desc *dma_desc_bfr)
    01391 {
    01392     // Look for available memory in the BFR memory
    01393     struct bfr_mem_node *node = NULL;
    01394     // Pointer to the node currently used by the STA
    01395     struct bfr_mem_node *sta_node = bfr->node;
    01396     // Index of the host report that is downloaded
    01397     uint16_t report_idx = bfr->host_report_idx;
    01398
    01399     // For profiling
    01400     PROF_BFR_DOWNLOAD_SET();
    01401
    01402     // Check if we can quickly reuse the node currently used by the STA or if node is now useless
    01403     if (sta_node)
    01404     {
    01405         do
    01406         {
    01407             if (sta_node->nb_frames)
    01408             {
    01409                 // Unlink the node, will be removed once all confirmation are received
    01410                 sta_node->sta_idx = INVALID_STA_IDX;
    01411                 break;
    01412             }
    01413
    01414             // Check if report length has been modified, apply padding for host report length
    01415             if (((bfr->host_report_len[report_idx] & 0xFFFC) + 4) != sta_node->length)
    01416             {
    01417                 // Remove the node
    01418                 bfr_remove_node(sta_node);
    01419                 break;
    01420             }
    01421
    01422             // Can directly reuse this node
    01423             node = sta_node;
    01424         } while (0);
    01425     }
    01426
    01427     // Check if a new node has to be used and if a new one can be found
    01428     if (!node && (bfr_env.nb_used_nodes < BFR_MEM_NODE_NB))
    01429     {
    01430         node = bfr_get_node(bfr->host_report_len[report_idx]);
    01431
    01432         // Check if a free node has been found
    01433         if (node)
    01434         {
    01435             // Link the node with the STA
    01436             bfr->node = node;
    01437             node->sta_idx = bfr->sta_idx;
    01438
    01439             // Get and store the available smm_index
    01440             node->smm_index = bfr_get_smm_index(node);
    01441
    01442             // Sanity check
    01443             ASSERT_ERR(node->smm_index != BFR_INVALID_SMM_INDEX);
    01444         }
    01445     }
    01446
    01447     // Check if free memory has been found for the download
    01448     if (node)
    01449     {
    01450         // Configure the DMA descriptor for the report
    01451         dma_desc_bfr->src = bfr->host_report_addr[report_idx];
    01452         dma_desc_bfr->dest = BFR_MEM_BASE_ADDR + node->start_addr;
    01453         dma_desc_bfr->length = bfr->host_report_len[report_idx];
    01454         dma_desc_bfr->ctrl = 0;
    01455     }
    01456
    01457     // For profiling
    01458     PROF_BFR_DOWNLOAD_CLR();
    01459
    01460     return ((node) ? node->smm_index : BFR_INVALID_SMM_INDEX);
    01461 }
    01462
    01471 static void bfr_start_next_calibration(void)
    01472 {
    01473     // Device to calibrate with
    01474     struct bfr_info_sta *bfr;
    01475     // Number of devices for which calibration is started (if > 1, MU Calibration is used)
    01476     uint8_t nb_devs;
    01477     // Number of packets transmitted during that calibration procedure
    01478     int i, nb_pkts = 0;
    01479     // Flag indicating if all BFR poll frames have been correctly prepared
    01480     bool bfr_poll_ok;
    01481     // TX Descriptor
    01482     struct txdesc *txdesc;
    01483     // Table of TX descriptor pointers used for the calibration (NDPA+NDP+1 per MU dev)
    01484     struct txdesc *calib_txdesc[2 + RW_USER_MAX];
    01485
    01486     do
    01487     {
    01488         // Check that BFR module is not handling another calibration
    01489         if (bfr_env.status != BFR_STATUS_IDLE)
    01490             break;
    01491
    01492         // Get first calibration request
    01493         bfr = (struct bfr_info_sta *)co_list_pick(&bfr_env.calib_sched);
    01494         if (!bfr)
    01495             break;
    01496
    01497         // Sanity check
    01498         ASSERT_ERR(bfr->sta_idx != INVALID_STA_IDX);
    01499
    01500         // Prepare the NDPA frame, will decide if MU or SU calibration has to be performed
    01501         txdesc = bfr_prep_ndpa(bfr, &nb_devs);
    01502         if (!txdesc)
    01503             break;
    01504
    01505         // Store the pointer to the TX descriptor in the table
    01506         calib_txdesc[nb_pkts++] = txdesc;
    01507
    01508         // Prepare the NDP frame
    01509         txdesc = bfr_prep_ndp(bfr, nb_devs);
    01510         if (!txdesc)
    01511             break;
    01512
    01513         // Store the pointer to the TX descriptor in the table
    01514         calib_txdesc[nb_pkts++] = txdesc;
    01515
    01516         // Prepare the BFR poll frames
    01517         bfr_poll_ok = true;
    01518         for (i = 1; i < nb_devs; i++)
    01519         {
    01520             // No BFR poll sent to the first STA so go directly to next one
    01521             bfr = (struct bfr_info_sta *)co_list_next(&bfr->hdr);
    01522             ASSERT_ERR(bfr != NULL);
    01523
    01524             // Prepare the BFR poll
    01525             txdesc = bfr_prep_bfr_poll(bfr);
    01526             if (!txdesc)
    01527             {
    01528                 bfr_poll_ok = false;
    01529                 break;
    01530             }
    01531
    01532             // Store the pointer to the TX descriptor in the table
    01533             calib_txdesc[nb_pkts++] = txdesc;
    01534         }
    01535
    01536         // Check if all necessary BFR poll were prepared
    01537         if (!bfr_poll_ok)
    01538             break;
    01539
    01540         // Check whether we are starting a MU calibration or not
    01541         if (nb_devs > 1)
    01542         {
    01543             // For profiling
    01544             PROF_BFR_MU_CALIB_SET();
    01545
    01546             // Set number of expected reports
    01547             bfr_env.nb_reports = nb_devs;
    01548
    01549             // Update BFR Environment status
    01550             bfr_env.status = BFR_STATUS_MU_CALIB;
    01551         }
    01552         else
    01553         {
    01554             // For profiling
    01555             PROF_BFR_SU_CALIB_SET();
    01556
    01557             // Update BFR Environment status
    01558             bfr_env.status = BFR_STATUS_SU_CALIB;
    01559         }
    01560
    01561         for (i = 0; i < nb_devs; i++)
    01562         {
    01563             // Get device information from the lists of pending calibrations
    01564             bfr = (struct bfr_info_sta *)co_list_pop_front(&bfr_env.calib_sched);
    01565             co_list_push_back(&bfr_env.bfee_sta_list, &bfr->hdr);
    01566
    01567             // Keep in mind we are calibrating for this STA
    01568             BFR_SET_STATUS_BIT(bfr->status, BFR_STA_CALIBRATE);
    01569         }
    01570
    01571         /*
    01572          * Frames are pushed to the MACHW only after a DMA transfer interrupt. This critical
    01573          * section allows to be sure to have enough time for chaining NDPA, NDP and BFR
    01574          * poll packets without any HW fetching.
    01575          */
    01576         GLOBAL_INT_DISABLE();
    01577         for (i = 0; i < nb_pkts; i++)
    01578         {
    01579             txl_cntrl_push_int(calib_txdesc[i], AC_VO);
    01580         }
    01581         GLOBAL_INT_RESTORE();
    01582
    01583         // Calibration is successfully started, return
    01584         return;
    01585     } while (0);
    01586
    01587     // If we fall into this part of code it means that some of the TX packet
    01588     // preparation has failed
    01589     // Free allocated frames
    01590     for (i = 0; i < nb_pkts; i++)
    01591     {
    01592         // Push the NDPA frame for TX
    01593         txl_frame_release(calib_txdesc[i], false);
    01594     }
    01595 }
    01596
    01603 static void bfr_calibrate(void)
    01604 {
    01605     // Get current time
    01606     uint32_t current_time = ke_time();
    01607     // Pointer to the first SU-Beamformee device inserted
    01608     struct bfr_info_sta *su_bfr = NULL;
    01609     // Get first STA to calibrate
    01610     struct bfr_info_sta *bfr = (struct bfr_info_sta *)co_list_pick(&bfr_env.bfee_sta_list);
    01611
    01612     // Go through all BFR-oriented STA information not yet scheduled for calibration
    01613     while (bfr)
    01614     {
    01615         // Get next STA
    01616         struct bfr_info_sta *next_bfr = (struct bfr_info_sta *)bfr->hdr.next;
    01617
    01618         do
    01619         {
    01620             // Check if calibration not already in progress for this device
    01621             if (BFR_IS_STATUS_BIT_SET(bfr->status, BFR_STA_CALIBRATE))
    01622             {
    01623                 break;
    01624             }
    01625
    01626             /*
    01627              * If traffic has been detected or if last
    01628              * calibration has been performed more than BFR_CALIB_MAX_DELAY us ago, start a
    01629              * calibration with the STA.
    01630              */
    01631             if (!td_sta_has_tx_traffic(bfr->sta_idx))
    01632             {
    01633                 if (ke_time_abs_cmp(current_time, bfr->last_calib_time + BFR_CALIB_MAX_DELAY))
    01634                 {
    01635                     break;
    01636                 }
    01637             }
    01638
    01639             #if (NX_UMAC_PRESENT)
    01640             // Don't calibrate stations that are sleeping
    01641             if (sta_mgmt_is_in_ps(bfr->sta_idx))
    01642             {
    01643                 break;
    01644             }
    01645             #endif //(NX_UMAC_PRESENT)
    01646
    01647             // Remove from list of not scheduled beamformee-capable STAs
    01648             co_list_extract(&bfr_env.bfee_sta_list, &bfr->hdr);
    01649
    01650             /*
    01651              * Insert the device in the list of pending calibrations. We group device that are MU
    01652              * Beamformee capable together. These devices are inserted before the list of device
    01653              * which are SU-Beamformee only.
    01654              */
    01655             if (!BFR_IS_STATUS_BIT_SET(bfr->status, BFR_STA_MU_CAPABLE))
    01656             {
    01657                 if (!su_bfr)
    01658                 {
    01659                     su_bfr = bfr;
    01660                 }
    01661
    01662                 // Add the device to the list of devices to calibrate
    01663                 co_list_push_back(&bfr_env.calib_sched, &bfr->hdr);
    01664             }
    01665             else
    01666             {
    01667                 // Insert before SU-Beamformee only device
    01668                 co_list_insert_before(&bfr_env.calib_sched, (struct co_list_hdr *)su_bfr,
    01669                                       &bfr->hdr);
    01670             }
    01671         } while (0);
    01672
    01673         // Get next STA
    01674         bfr = next_bfr;
    01675     }
    01676
    01677     // Start next calibration if possible
    01678     bfr_start_next_calibration();
    01679 }
    01680
    01687 static void bfr_init_sta_info(struct bfr_info_sta *bfr)
    01688 {
    01689     // Fully reset BFR information for the STA
    01690     memset(bfr, 0, sizeof(struct bfr_info_sta));
    01691
    01692     // Reset STA index
    01693     bfr->sta_idx = INVALID_STA_IDX;
    01694 }
    01695
    01701 static void bfr_init_index_table(void)
    01702 {
    01703     for (int i = 0; i < (BFR_MEM_REP_NB_MAX / 2); i++)
    01704     {
    01705         uint32_t *bfr_ptr = HW2CPU(BFR_MEM_BASE_ADDR + (i * 4));
    01706
    01707         *bfr_ptr = 0;
    01708     }
    01709 }
    01710
    01711 /*
    01712  * PUBLIC FUNCTION DEFINITIONS
    01713  ****************************************************************************************
    01714  */
    01715
    01716 void bfr_init(void)
    01717 {
    01718     struct bfr_mem_node *node;
    01719
    01720     // Fully reset the BFR environment variable
    01721     memset(&bfr_env, 0, sizeof(bfr_env));
    01722
    01723     // Verify we can be beamformer
    01724     bfr_env.bfr_enabled = hal_machw_bfmer_support();
    01725
    01726     if (bfr_is_enabled())
    01727     {
    01728         // Read memory size in a number of bytes
    01729         bfr_env.bfr_mem_eaddr = (phy_get_bfr_mem_size() << 10);
    01730
    01731         // Fully reset content of the memory nodes
    01732         memset(&bfr_nodes[0], 0, BFR_MEM_NODE_NB * sizeof(struct bfr_mem_node));
    01733
    01734         // Initialize list of available memory nodes
    01735         for (int i = 0; i < BFR_MEM_NODE_NB; i++)
    01736         {
    01737             // Get node
    01738             node = &bfr_nodes[i];
    01739
    01740             // Set the node index
    01741             node->idx = i;
    01742             // Insert the node
    01743             co_list_push_back(&bfr_env.free_nodes, &node->hdr);
    01744         }
    01745
    01746         // Initialize BFR information stored for each STA
    01747         for (int i = 0; i < NX_REMOTE_STA_MAX; i++)
    01748         {
    01749             bfr_init_sta_info(&bfr_sta_infos[i]);
    01750         }
    01751
    01752         // Initialize SMM Pointers values
    01753         for (int i = 0; i < BFR_MEM_REP_NB_MAX; i++)
    01754         {
    01755             bfr_smm_ptrs[i] = 0;
    01756         }
    01757
    01758         // Initialize callback for calibration timer
    01759         bfr_env.calib_tmr.cb = bfr_calib_tmr_exp_cb;
    01760         // Set calibration timer duration
    01761         bfr_env.calib_tmr_dur = BFR_CALIB_TMR_DFLT_DUR;
    01762
    01763         // Initialize Index Table in BFR memory
    01764         bfr_init_index_table();
    01765
    01766         // Get our number of spatial stream
    01767         bfr_env.bfer_nss = phy_get_nss();
    01768
    01769         PROF_SMM_IDX_SET(0xF);
    01770     }
    01771     else
    01772     {
    01773         dbg(D_CRT "Disable Beamformer as MAC and/or PHY does not support it\n");
    01774     }
    01775 }
    01776
    01777 void bfr_add_sta_ind(uint8_t sta_idx, bool mu_capable, uint16_t aid, uint32_t host_addr,
    01778                      uint16_t host_length, uint8_t nss)
    01779 {
    01780     // Get VIF Info
    01781     struct vif_info_tag *vif = &vif_info_tab[sta_mgmt_get_vif_idx(sta_idx)];
    01782     uint8_t band = vif_mgmt_get_band(vif);
    01783
    01784     // Check if channel used for the connection is a 5GHz channel
    01785     if (band == PHY_BAND_5G)
    01786     {
    01787         // Get BFR information for this STA
    01788         struct bfr_info_sta *bfr = &bfr_sta_infos[sta_idx];
    01789
    01790         // Set BFR parameters
    01791         bfr->sta_idx = sta_idx;
    01792         bfr->host_max_report_len = host_length/2;
    01793         bfr->host_report_addr[0] = host_addr;
    01794         bfr->host_report_addr[1] = host_addr + bfr->host_max_report_len;
    01795         bfr->host_report_idx = 0;
    01796         bfr->bfee_nss = nss;
    01797         bfr->vif_idx = sta_mgmt_get_vif_idx(sta_idx);
    01798         bfr->last_calib_time = hal_machw_time();
    01799
    01800         if (vif->type == VIF_AP)
    01801         {
    01802             BFR_STA_INFO_SET_AID12(bfr->sta_info, aid);
    01803         }
    01804         else
    01805         {
    01806             // Force AID to 0
    01807             BFR_STA_INFO_SET_AID12(bfr->sta_info, 0);
    01808         }
    01809
    01810         if (mu_capable)
    01811         {
    01812             BFR_SET_STATUS_BIT(bfr->status, BFR_STA_MU_CAPABLE);
    01813         }
    01814
    01815         // If first STA, start calibration timer
    01816         if (co_list_is_empty(&bfr_env.bfee_sta_list) && co_list_is_empty(&bfr_env.calib_sched))
    01817         {
    01818             mm_timer_set(&bfr_env.calib_tmr, hal_machw_time() + bfr_env.calib_tmr_dur);
    01819         }
    01820
    01821         // Add STA in list of beamformee capable STAs
    01822         co_list_push_back(&bfr_env.bfee_sta_list, &bfr->hdr);
    01823     }
    01824 }
    01825
    01826 void bfr_del_sta_ind(uint8_t sta_idx)
    01827 {
    01828     // Get BFR information for this STA
    01829     struct bfr_info_sta *bfr = &bfr_sta_infos[sta_idx];
    01830
    01831     do
    01832     {
    01833         if (bfr->sta_idx == INVALID_STA_IDX)
    01834         {
    01835             break;
    01836         }
    01837
    01838         // Remove the STA from its list (either bfee_sta_list or calib_sched)
    01839         co_list_extract(&bfr_env.bfee_sta_list, &bfr->hdr);
    01840         co_list_extract(&bfr_env.calib_sched, &bfr->hdr);
    01841
    01842         // Check if we were calibrating
    01843         if (BFR_IS_STATUS_BIT_SET(bfr->status, BFR_STA_CALIBRATE))
    01844         {
    01845             bfr_end_calibration_for_sta(bfr, false);
    01846         }
    01847
    01848         // Stop the calibration timer if no more STA to interact with
    01849         if (co_list_is_empty(&bfr_env.bfee_sta_list) && co_list_is_empty(&bfr_env.calib_sched))
    01850         {
    01851             mm_timer_clear(&bfr_env.calib_tmr);
    01852         }
    01853
    01854         if (bfr->node)
    01855         {
    01856             if (bfr->node->nb_frames)
    01857             {
    01858                 bfr->node->sta_idx = INVALID_STA_IDX;
    01859             }
    01860             else
    01861             {
    01862                 // Remove the node
    01863                 bfr_remove_node(bfr->node);
    01864             }
    01865         }
    01866
    01867         // Initialize beamforming structure
    01868         bfr_init_sta_info(bfr);
    01869     } while (0);
    01870 }
    01871
    01872 uint8_t bfr_rx_frame_ind(uint8_t sta_idx, struct rxdesc *rxdesc, uint8_t *frame)
    01873 {
    01874     // Returned status, consider frame is not a VHT Compressed Beamforming frame by default
    01875     uint8_t status = BFR_RX_STATUS_NOT_VALID;
    01876
    01877     do
    01878     {
    01879         // Get BFR information for this STA
    01880         struct bfr_info_sta *bfr = &bfr_sta_infos[sta_idx];
    01881         // VHT Mimo Control
    01882         uint32_t vht_mimo_ctrl;
    01883
    01884         // Check if STA is beamformee capable
    01885         if (bfr->sta_idx == INVALID_STA_IDX)
    01886         {
    01887             break;
    01888         }
    01889
    01890         // Check if calibration is in progress from this STA
    01891         if (!BFR_IS_STATUS_BIT_SET(bfr->status, BFR_STA_CALIBRATE))
    01892         {
    01893             break;
    01894         }
    01895
    01896         // Check if frame is a VHT Compressed Beamforming Report
    01897         if (!bfr_is_vht_bfr(frame))
    01898         {
    01899             break;
    01900         }
    01901
    01902         // For profiling
    01903         PROF_BFR_RX_BFR_SET();
    01904
    01905         vht_mimo_ctrl = co_read32p(CPU2HW(frame) + BFR_VHT_BFR_VHT_MIMO_CTRL_OFFSET);
    01906
    01907         // Check if received dialog token is the expected one
    01908         if (bfr->last_token != BFR_VHT_MIMO_GET_VALUE(vht_mimo_ctrl, TOKEN))
    01909         {
    01910             // End calibration
    01911             bfr_end_calibration_for_sta(bfr, false);
    01912             break;
    01913         }
    01914
    01915         // BFR module is now responsible for SW Descriptor
    01916         status = bfr_upload_report(bfr, rxdesc, frame);
    01917
    01918         if (status == BFR_RX_STATUS_ERROR)
    01919         {
    01920             // End calibration
    01921             bfr_end_calibration_for_sta(bfr, false);
    01922         }
    01923
    01924         // For profiling
    01925         PROF_BFR_RX_BFR_CLR();
    01926     } while (0);
    01927
    01928     return (status);
    01929 }
    01930
    01931 bool bfr_tx_frame_ind(struct txdesc *txdesc, struct dma_desc *dma_desc_bfr)
    01932 {
    01933     // Get BFR information for this STA
    01934     struct bfr_info_sta *bfr;
    01935     // Indicate if a report have to be downloaded from host memory
    01936     bool download = false;
    01937     // SMM Index
    01938     uint8_t smm_index = BFR_INVALID_SMM_INDEX;
    01939
    01940     do
    01941     {
    01942         if (txdesc->host.staid == INVALID_STA_IDX)
    01943         {
    01944             break;
    01945         }
    01946
    01947         bfr = &bfr_sta_infos[txdesc->host.staid];
    01948
    01949         // Check if STA is beamformee capable
    01950         if (bfr->sta_idx == INVALID_STA_IDX)
    01951         {
    01952             break;
    01953         }
    01954
    01955         // Check if frame can be beamformed
    01956         if (!bfr_is_frame_bf_compliant(txdesc, bfr))
    01957         {
    01958             break;
    01959         }
    01960
    01961         // For profiling
    01962         PROF_BFR_TX_IND_SET();
    01963
    01964         // Check if STA report is already present in phy memory
    01965         if (bfr->node)
    01966         {
    01967             /*
    01968              * Check if a new report has been received.
    01969              * In that case, do not use current report but try to download a new one
    01970              */
    01971             if (!BFR_IS_STATUS_BIT_SET(bfr->status, BFR_STA_REPORT_UPDATED))
    01972             {
    01973                 struct bfr_mem_node *node = bfr->node;
    01974
    01975                 smm_index = node->smm_index;
    01976
    01977                 // Increase the number of packets handling
    01978                 bfr_increase_nb_frames(bfr->node);
    01979
    01980                 // Link the BFR Node with the TX Descriptor so that we can decrease the nb_frames value upon confirmation
    01981                 txdesc->lmac.bfr_node = bfr->node;
    01982                 break;
    01983             }
    01984         }
    01985
    01986         // Check if report can be downloaded from host memory
    01987         if (!BFR_IS_STATUS_BIT_SET(bfr->status, BFR_STA_HAS_VALID_REPORT))
    01988         {
    01989             break;
    01990         }
    01991
    01992         // Try to download the report from host memory
    01993         smm_index = bfr_download_report(bfr, dma_desc_bfr);
    01994
    01995         if (smm_index != BFR_INVALID_SMM_INDEX)
    01996         {
    01997             // If a new report has been downloaded, clear the update bit
    01998             BFR_RESET_STATUS_BIT(bfr->status, BFR_STA_REPORT_UPDATED);
    01999
    02000             download = true;
    02001
    02002             // Increase the number of packets handling
    02003             bfr_increase_nb_frames(bfr->node);
    02004
    02005             // Link the BFR Node with the TX Descriptor so that we can decrease the nb_frames value upon confirmation
    02006             txdesc->lmac.bfr_node = bfr->node;
    02007         }
    02008     } while (0);
    02009
    02010     // For profiling
    02011     PROF_BFR_TX_IND_CLR();
    02012     PROF_SMM_IDX_SET(smm_index);
    02013
    02014     return (download);
    02015 }
    02016
    02017 void bfr_tx_cfm(struct txdesc *txdesc)
    02018 {
    02019     do
    02020     {
    02021         // Get node used for the transmission
    02022         struct bfr_mem_node *node = txdesc->lmac.bfr_node;
    02023
    02024         if (!node)
    02025         {
    02026             // Beamforming has not been used for this transmission
    02027             break;
    02028         }
    02029
    02030         // For profiling
    02031         PROF_BFR_TX_CFM_SET();
    02032
    02033         // Critical section because nb_frames and nb_used_nodes are incremented
    02034         // in bfr_tx_frame_ind() which is called under interrupt
    02035         GLOBAL_INT_DISABLE();
    02036
    02037         // Sanity check
    02038         ASSERT_WARN(node->nb_frames);
    02039
    02040         // Decrease number of frames to be handled
    02041         node->nb_frames--;
    02042
    02043         if (!node->nb_frames)
    02044         {
    02045             bfr_env.nb_used_nodes--;
    02046         }
    02047
    02048         /*
    02049          * If node is no more linked with a STA and no more frame has to be sent with this report,
    02050          * free the part of memory
    02051          */
    02052         if ((node->sta_idx == INVALID_STA_IDX) && (!node->nb_frames))
    02053         {
    02054             bfr_remove_node(node);
    02055         }
    02056
    02057         GLOBAL_INT_RESTORE();
    02058
    02059         // For profiling
    02060         PROF_BFR_TX_CFM_CLR();
    02061     } while (0);
    02062 }
    02063
    02064 #if (NX_UMAC_PRESENT)
    02065 uint8_t bfr_get_last_nc(uint8_t sta_idx)
    02066 {
    02067     // Get BFR information for this STA
    02068     struct bfr_info_sta *bfr = &bfr_sta_infos[sta_idx];
    02069     // NC
    02070     uint8_t nc = BFR_INVALID_NC;
    02071
    02072     do
    02073     {
    02074         // Check if STA is beamformee capable
    02075         if (bfr->sta_idx == INVALID_STA_IDX)
    02076         {
    02077             break;
    02078         }
    02079
    02080         nc = bfr->last_rx_nc;
    02081     } while (0);
    02082
    02083     return (nc);
    02084 }
    02085 #endif //(NX_UMAC_PRESENT)
    02086
    02087 #if RW_MUMIMO_TX_EN
    02088
    02097 static void bfr_group_update_cfm(void *env, uint32_t status)
    02098 {
    02099     // Cast the environment
    02100     struct mm_mu_group_update_req const *param = env;
    02101     struct ke_msg *msg = ke_param2msg(param);
    02102
    02103     // Send the confirmation to the upper layers
    02104     ke_msg_send_basic(MM_MU_GROUP_UPDATE_CFM, msg->src_id, msg->dest_id);
    02105
    02106     // Free the message structure
    02107     ke_msg_free(msg);
    02108 }
    02109
    02110 void bfr_group_update_req(struct mm_mu_group_update_req const *param)
    02111 {
    02112     uint8_t sta_idx = param->sta_idx;
    02113     struct txl_frame_desc_tag *frame;
    02114     struct mac_hdr *buf;
    02115     struct tx_hd *thd;
    02116     uint32_t length = 0;
    02117     struct sta_info_tag *sta = &sta_info_tab[sta_idx];
    02118     struct vif_info_tag *vif = &vif_info_tab[sta->inst_nbr];
    02119     uint32_t payload;
    02120     int txtype;
    02121     int i;
    02122
    02123     // Allocate a frame descriptor from the TX path
    02124     txtype = vif_mgmt_get_txtype(vif);
    02125     frame = txl_frame_get(txtype, NX_TXFRAME_LEN);
    02126     if (frame == NULL)
    02127     {
    02128         bfr_group_update_cfm((void *)param, 0);
    02129         return;
    02130     }
    02131
    02132     // update Tx power in policy table
    02133     tpc_update_frame_tx_power(vif, frame);
    02134
    02135     // Get the buffer pointer
    02136     buf = txl_buffer_payload_get(&frame->txdesc);
    02137
    02138     // Fill-in the frame
    02139     buf->fctl  = MAC_FCTRL_ACTION;
    02140     // TODO[LT] - Set duration
    02141     buf->durid = 0;
    02142     // Set RA
    02143     MAC_ADDR_CPY(&buf->addr1, sta_mgmt_get_peer_addr(sta_idx));
    02144     // Set TA
    02145     MAC_ADDR_CPY(&buf->addr2, vif_mgmt_get_addr(sta->inst_nbr));
    02146     // Set BSSID
    02147     MAC_ADDR_CPY(&buf->addr3, vif_mgmt_get_addr(sta->inst_nbr));
    02148     buf->seq = txl_get_seq_ctrl();
    02149
    02150     // Fill-in the confirmation structure
    02151     frame->cfm.cfm_func = bfr_group_update_cfm;
    02152     frame->cfm.env = (void *)param;
    02153
    02154     #if (NX_CHNL_CTXT || NX_P2P)
    02155     // Set VIF and STA indexes
    02156     frame->txdesc.host.vif_idx = sta->inst_nbr;
    02157     frame->txdesc.host.staid   = sta_idx;
    02158     #endif //(NX_CHNL_CTXT || NX_P2P)
    02159
    02160     length = MAC_SHORT_MAC_HDR_LEN;
    02161
    02162     #if NX_MFP
    02163     frame->txdesc.umac.head_len = 0;
    02164     frame->txdesc.umac.tail_len = 0;
    02165     if (MFP_UNICAST_PROT == mfp_protect_mgmt_frame(&frame->txdesc, buf->fctl,
    02166                                                    MAC_VHT_ACTION_CATEGORY))
    02167     {
    02168         txu_cntrl_protect_mgmt_frame(&frame->txdesc, buf, MAC_SHORT_MAC_HDR_LEN);
    02169         length += frame->txdesc.umac.head_len;
    02170     }
    02171     #endif
    02172
    02173     payload = CPU2HW(buf) + length;
    02174     length += MAC_GROUP_ID_MGT_PAYL_LENGTH;
    02175
    02176     co_write8p(payload, MAC_VHT_ACTION_CATEGORY);
    02177     co_write8p(payload + MAC_ACTION_ACTION_OFT, MAC_GROUP_ID_MGMT_VHT_ACTION);
    02178
    02179     // Erase the membership status array
    02180     for (i = 0; i < 8; i++)
    02181     {
    02182         co_write8p(payload + MAC_GROUP_ID_MGT_MEMBERSHIP_OFT + i, 0);
    02183     }
    02184
    02185     // Erase the user position array
    02186     for (i = 0; i < 16; i++)
    02187     {
    02188         co_write8p(payload + MAC_GROUP_ID_MGT_USER_POS_OFT + i, 0);
    02189     }
    02190
    02191     // Set the correct bits in the membership status array and user position
    02192     for (i = 0; i < param->group_cnt; i++)
    02193     {
    02194         int group_id = param->groups[i].group_id;
    02195         uint8_t user_pos = param->groups[i].user_pos;
    02196         int group_byte_idx = group_id / 8;
    02197         int group_bit = group_id % 8;
    02198         int user_byte_idx = group_id / 4;
    02199         int user_bit = (group_id % 4) * 2;
    02200         uint32_t membership_addr = payload + MAC_GROUP_ID_MGT_MEMBERSHIP_OFT + group_byte_idx;
    02201         uint32_t userpos_addr = payload + MAC_GROUP_ID_MGT_USER_POS_OFT + user_byte_idx;
    02202
    02203         co_write8p(membership_addr, co_read8p(membership_addr) | (1 << group_bit));
    02204         co_write8p(userpos_addr, co_read8p(userpos_addr) | (user_pos << user_bit));
    02205     }
    02206
    02207     #if NX_MFP
    02208     length += frame->txdesc.umac.tail_len;
    02209     #endif
    02210     thd             = &frame->txdesc.lmac.hw_desc->thd;
    02211     thd->dataendptr = (uint32_t)thd->datastartptr + length - 1;
    02212     thd->frmlen     = length + MAC_FCS_LEN;
    02213
    02214     // Push the frame for TX
    02215     txl_frame_push(frame, AC_VO);
    02216 }
    02217 #endif //RW_MUMIMO_TX_EN
    02218
    02219 #endif //(RW_BFMER_EN)
    02220