iwlwifi: fix bugs in beacon configuration

[deliverable/linux.git] / drivers / net / wireless / iwlwifi / iwl-agn.c

diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/iwl-agn.c
index f586e7e83131354912a35710ef6853b8f6fb6701..29f3b73c7560e067ad47aa9b67fcb3954911f124 100644 (file)
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.c
@@ -310,7 +310,7 @@ static void iwl_free_frame(struct iwl_priv *priv, struct iwl_frame *frame)
        list_add(&frame->list, &priv->free_frames);
 }
 
-static unsigned int iwl_fill_beacon_frame(struct iwl_priv *priv,
+static u32 iwl_fill_beacon_frame(struct iwl_priv *priv,
                                          struct ieee80211_hdr *hdr,
                                          int left)
 {
@@ -327,34 +327,74 @@ static unsigned int iwl_fill_beacon_frame(struct iwl_priv *priv,
        return priv->ibss_beacon->len;
 }
 
+/* Parse the beacon frame to find the TIM element and set tim_idx & tim_size */
+static void iwl_set_beacon_tim(struct iwl_priv *priv,
+               struct iwl_tx_beacon_cmd *tx_beacon_cmd,
+               u8 *beacon, u32 frame_size)
+{
+       u16 tim_idx;
+       struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)beacon;
+
+       /*
+        * The index is relative to frame start but we start looking at the
+        * variable-length part of the beacon.
+        */
+       tim_idx = mgmt->u.beacon.variable - beacon;
+
+       /* Parse variable-length elements of beacon to find WLAN_EID_TIM */
+       while ((tim_idx < (frame_size - 2)) &&
+                       (beacon[tim_idx] != WLAN_EID_TIM))
+               tim_idx += beacon[tim_idx+1] + 2;
+
+       /* If TIM field was found, set variables */
+       if ((tim_idx < (frame_size - 1)) && (beacon[tim_idx] == WLAN_EID_TIM)) {
+               tx_beacon_cmd->tim_idx = cpu_to_le16(tim_idx);
+               tx_beacon_cmd->tim_size = beacon[tim_idx+1];
+       } else
+               IWL_WARN(priv, "Unable to find TIM Element in beacon\n");
+}
+
 static unsigned int iwl_hw_get_beacon_cmd(struct iwl_priv *priv,
-                                      struct iwl_frame *frame, u8 rate)
+                                      struct iwl_frame *frame)
 {
        struct iwl_tx_beacon_cmd *tx_beacon_cmd;
-       unsigned int frame_size;
+       u32 frame_size;
+       u32 rate_flags;
+       u32 rate;
+       /*
+        * We have to set up the TX command, the TX Beacon command, and the
+        * beacon contents.
+        */
 
+       /* Initialize memory */
        tx_beacon_cmd = &frame->u.beacon;
        memset(tx_beacon_cmd, 0, sizeof(*tx_beacon_cmd));
 
-       tx_beacon_cmd->tx.sta_id = priv->hw_params.bcast_sta_id;
-       tx_beacon_cmd->tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
-
+       /* Set up TX beacon contents */
        frame_size = iwl_fill_beacon_frame(priv, tx_beacon_cmd->frame,
                                sizeof(frame->u) - sizeof(*tx_beacon_cmd));
+       if (WARN_ON_ONCE(frame_size > MAX_MPDU_SIZE))
+               return 0;
 
-       BUG_ON(frame_size > MAX_MPDU_SIZE);
+       /* Set up TX command fields */
        tx_beacon_cmd->tx.len = cpu_to_le16((u16)frame_size);
+       tx_beacon_cmd->tx.sta_id = priv->hw_params.bcast_sta_id;
+       tx_beacon_cmd->tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+       tx_beacon_cmd->tx.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK |
+               TX_CMD_FLG_TSF_MSK | TX_CMD_FLG_STA_RATE_MSK;
 
-       if ((rate == IWL_RATE_1M_PLCP) || (rate >= IWL_RATE_2M_PLCP))
-               tx_beacon_cmd->tx.rate_n_flags =
-                       iwl_hw_set_rate_n_flags(rate, RATE_MCS_CCK_MSK);
-       else
-               tx_beacon_cmd->tx.rate_n_flags =
-                       iwl_hw_set_rate_n_flags(rate, 0);
+       /* Set up TX beacon command fields */
+       iwl_set_beacon_tim(priv, tx_beacon_cmd, (u8 *)tx_beacon_cmd->frame,
+                       frame_size);
 
-       tx_beacon_cmd->tx.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK |
-                                    TX_CMD_FLG_TSF_MSK |
-                                    TX_CMD_FLG_STA_RATE_MSK;
+       /* Set up packet rate and flags */
+       rate = iwl_rate_get_lowest_plcp(priv);
+       priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant);
+       rate_flags = iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
+       if ((rate >= IWL_FIRST_CCK_RATE) && (rate <= IWL_LAST_CCK_RATE))
+               rate_flags |= RATE_MCS_CCK_MSK;
+       tx_beacon_cmd->tx.rate_n_flags = iwl_hw_set_rate_n_flags(rate,
+                       rate_flags);
 
        return sizeof(*tx_beacon_cmd) + frame_size;
 }
@@ -363,19 +403,20 @@ static int iwl_send_beacon_cmd(struct iwl_priv *priv)
        struct iwl_frame *frame;
        unsigned int frame_size;
        int rc;
-       u8 rate;
 
        frame = iwl_get_free_frame(priv);
-
        if (!frame) {
                IWL_ERR(priv, "Could not obtain free frame buffer for beacon "
                          "command.\n");
                return -ENOMEM;
        }
 
-       rate = iwl_rate_get_lowest_plcp(priv);
-
-       frame_size = iwl_hw_get_beacon_cmd(priv, frame, rate);
+       frame_size = iwl_hw_get_beacon_cmd(priv, frame);
+       if (!frame_size) {
+               IWL_ERR(priv, "Error configuring the beacon command\n");
+               iwl_free_frame(priv, frame);
+               return -EINVAL;
+       }
 
        rc = iwl_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
                              &frame->u.cmd[0]);