1 files changed, 269 insertions, 596 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.c b/drivers/net/wireless/iwlwifi/iwl-4965.c
index d727de8b96fe..a9c30bcb65b8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965.c
@@ -79,6 +79,30 @@ const struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT] = {
 	IWL_DECLARE_RATE_INFO(60, 60, 48, INV, 48, INV, 48, INV),/* 60mbps */
 };
 
+#ifdef CONFIG_IWL4965_HT
+
+static const u16 default_tid_to_tx_fifo[] = {
+	IWL_TX_FIFO_AC1,
+	IWL_TX_FIFO_AC0,
+	IWL_TX_FIFO_AC0,
+	IWL_TX_FIFO_AC1,
+	IWL_TX_FIFO_AC2,
+	IWL_TX_FIFO_AC2,
+	IWL_TX_FIFO_AC3,
+	IWL_TX_FIFO_AC3,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_AC3
+};
+
+#endif	/*CONFIG_IWL4965_HT */
+
 static int is_fat_channel(__le32 rxon_flags)
 {
 	return (rxon_flags & RXON_FLG_CHANNEL_MODE_PURE_40_MSK) ||
@@ -315,14 +339,15 @@ static int iwl4965_kw_alloc(struct iwl4965_priv *priv)
  *
  * Does not set up a command, or touch hardware.
  */
-int iwl4965_set_fat_chan_info(struct iwl4965_priv *priv, int phymode, u16 channel,
+int iwl4965_set_fat_chan_info(struct iwl4965_priv *priv,
+			      enum ieee80211_band band, u16 channel,
 			      const struct iwl4965_eeprom_channel *eeprom_ch,
 			      u8 fat_extension_channel)
 {
 	struct iwl4965_channel_info *ch_info;
 
 	ch_info = (struct iwl4965_channel_info *)
-			iwl4965_get_channel_info(priv, phymode, channel);
+			iwl4965_get_channel_info(priv, band, channel);
 
 	if (!is_channel_valid(ch_info))
 		return -1;
@@ -520,9 +545,10 @@ int iwl4965_hw_nic_init(struct iwl4965_priv *priv)
 
 	/* set CSR_HW_CONFIG_REG for uCode use */
 
-	iwl4965_set_bit(priv, CSR_SW_VER, CSR_HW_IF_CONFIG_REG_BIT_KEDRON_R |
-		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
-		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
+	iwl4965_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			CSR_HW_IF_CONFIG_REG_BIT_KEDRON_R |
+			CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
+			CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
 
 	rc = iwl4965_grab_nic_access(priv);
 	if (rc < 0) {
@@ -1769,7 +1795,6 @@ int iwl4965_hw_set_hw_setting(struct iwl4965_priv *priv)
 	memset(priv->hw_setting.shared_virt, 0, sizeof(struct iwl4965_shared));
 
 	priv->hw_setting.max_txq_num = iwl4965_param_queues_num;
-	priv->hw_setting.ac_queue_count = AC_NUM;
 	priv->hw_setting.tx_cmd_len = sizeof(struct iwl4965_tx_cmd);
 	priv->hw_setting.max_rxq_size = RX_QUEUE_SIZE;
 	priv->hw_setting.max_rxq_log = RX_QUEUE_SIZE_LOG;
@@ -1780,6 +1805,9 @@ int iwl4965_hw_set_hw_setting(struct iwl4965_priv *priv)
 	priv->hw_setting.max_pkt_size = priv->hw_setting.rx_buf_size - 256;
 	priv->hw_setting.max_stations = IWL4965_STATION_COUNT;
 	priv->hw_setting.bcast_sta_id = IWL4965_BROADCAST_ID;
+
+	priv->hw_setting.tx_ant_num = 2;
+
 	return 0;
 }
 
@@ -1915,11 +1943,12 @@ static s32 iwl4965_get_voltage_compensation(s32 eeprom_voltage,
 }
 
 static const struct iwl4965_channel_info *
-iwl4965_get_channel_txpower_info(struct iwl4965_priv *priv, u8 phymode, u16 channel)
+iwl4965_get_channel_txpower_info(struct iwl4965_priv *priv,
+				 enum ieee80211_band band, u16 channel)
 {
 	const struct iwl4965_channel_info *ch_info;
 
-	ch_info = iwl4965_get_channel_info(priv, phymode, channel);
+	ch_info = iwl4965_get_channel_info(priv, band, channel);
 
 	if (!is_channel_valid(ch_info))
 		return NULL;
@@ -2368,7 +2397,7 @@ static int iwl4965_fill_txpower_tbl(struct iwl4965_priv *priv, u8 band, u16 chan
 
 	/* Get current (RXON) channel, band, width */
 	ch_info =
-		iwl4965_get_channel_txpower_info(priv, priv->phymode, channel);
+		iwl4965_get_channel_txpower_info(priv, priv->band, channel);
 
 	IWL_DEBUG_TXPOWER("chan %d band %d is_fat %d\n", channel, band,
 			  is_fat);
@@ -2595,8 +2624,7 @@ int iwl4965_hw_reg_send_txpower(struct iwl4965_priv *priv)
 		return -EAGAIN;
 	}
 
-	band = ((priv->phymode == MODE_IEEE80211B) ||
-		(priv->phymode == MODE_IEEE80211G));
+	band = priv->band == IEEE80211_BAND_2GHZ;
 
 	is_fat =  is_fat_channel(priv->active_rxon.flags);
 
@@ -2626,10 +2654,9 @@ int iwl4965_hw_channel_switch(struct iwl4965_priv *priv, u16 channel)
 	struct iwl4965_channel_switch_cmd cmd = { 0 };
 	const struct iwl4965_channel_info *ch_info;
 
-	band = ((priv->phymode == MODE_IEEE80211B) ||
-		(priv->phymode == MODE_IEEE80211G));
+	band = priv->band == IEEE80211_BAND_2GHZ;
 
-	ch_info = iwl4965_get_channel_info(priv, priv->phymode, channel);
+	ch_info = iwl4965_get_channel_info(priv, priv->band, channel);
 
 	is_fat = is_fat_channel(priv->staging_rxon.flags);
 
@@ -2674,7 +2701,7 @@ void iwl4965_hw_build_tx_cmd_rate(struct iwl4965_priv *priv,
 	u16 fc = le16_to_cpu(hdr->frame_control);
 	u8 rate_plcp;
 	u16 rate_flags = 0;
-	int rate_idx = min(ctrl->tx_rate & 0xffff, IWL_RATE_COUNT - 1);
+	int rate_idx = min(ctrl->tx_rate->hw_value & 0xffff, IWL_RATE_COUNT - 1);
 
 	rate_plcp = iwl4965_rates[rate_idx].plcp;
 
@@ -2922,378 +2949,6 @@ void iwl4965_set_rxon_chain(struct iwl4965_priv *priv)
 	IWL_DEBUG_ASSOC("rx chain %X\n", priv->staging_rxon.rx_chain);
 }
 
-#ifdef CONFIG_IWL4965_HT
-#ifdef CONFIG_IWL4965_HT_AGG
-/*
-	get the traffic load value for tid
-*/
-static u32 iwl4965_tl_get_load(struct iwl4965_priv *priv, u8 tid)
-{
-	u32 load = 0;
-	u32 current_time = jiffies_to_msecs(jiffies);
-	u32 time_diff;
-	s32 index;
-	unsigned long flags;
-	struct iwl4965_traffic_load *tid_ptr = NULL;
-
-	if (tid >= TID_MAX_LOAD_COUNT)
-		return 0;
-
-	tid_ptr = &(priv->lq_mngr.agg_ctrl.traffic_load[tid]);
-
-	current_time -= current_time % TID_ROUND_VALUE;
-
-	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-	if (!(tid_ptr->queue_count))
-		goto out;
-
-	time_diff = TIME_WRAP_AROUND(tid_ptr->time_stamp, current_time);
-	index = time_diff / TID_QUEUE_CELL_SPACING;
-
-	if (index >= TID_QUEUE_MAX_SIZE) {
-		u32 oldest_time = current_time - TID_MAX_TIME_DIFF;
-
-		while (tid_ptr->queue_count &&
-		       (tid_ptr->time_stamp < oldest_time)) {
-			tid_ptr->total -= tid_ptr->packet_count[tid_ptr->head];
-			tid_ptr->packet_count[tid_ptr->head] = 0;
-			tid_ptr->time_stamp += TID_QUEUE_CELL_SPACING;
-			tid_ptr->queue_count--;
-			tid_ptr->head++;
-			if (tid_ptr->head >= TID_QUEUE_MAX_SIZE)
-				tid_ptr->head = 0;
-		}
-	}
-	load = tid_ptr->total;
-
- out:
-	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-	return load;
-}
-
-/*
-	increment traffic load value for tid and also remove
-	any old values if passed the certian time period
-*/
-static void iwl4965_tl_add_packet(struct iwl4965_priv *priv, u8 tid)
-{
-	u32 current_time = jiffies_to_msecs(jiffies);
-	u32 time_diff;
-	s32 index;
-	unsigned long flags;
-	struct iwl4965_traffic_load *tid_ptr = NULL;
-
-	if (tid >= TID_MAX_LOAD_COUNT)
-		return;
-
-	tid_ptr = &(priv->lq_mngr.agg_ctrl.traffic_load[tid]);
-
-	current_time -= current_time % TID_ROUND_VALUE;
-
-	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-	if (!(tid_ptr->queue_count)) {
-		tid_ptr->total = 1;
-		tid_ptr->time_stamp = current_time;
-		tid_ptr->queue_count = 1;
-		tid_ptr->head = 0;
-		tid_ptr->packet_count[0] = 1;
-		goto out;
-	}
-
-	time_diff = TIME_WRAP_AROUND(tid_ptr->time_stamp, current_time);
-	index = time_diff / TID_QUEUE_CELL_SPACING;
-
-	if (index >= TID_QUEUE_MAX_SIZE) {
-		u32 oldest_time = current_time - TID_MAX_TIME_DIFF;
-
-		while (tid_ptr->queue_count &&
-		       (tid_ptr->time_stamp < oldest_time)) {
-			tid_ptr->total -= tid_ptr->packet_count[tid_ptr->head];
-			tid_ptr->packet_count[tid_ptr->head] = 0;
-			tid_ptr->time_stamp += TID_QUEUE_CELL_SPACING;
-			tid_ptr->queue_count--;
-			tid_ptr->head++;
-			if (tid_ptr->head >= TID_QUEUE_MAX_SIZE)
-				tid_ptr->head = 0;
-		}
-	}
-
-	index = (tid_ptr->head + index) % TID_QUEUE_MAX_SIZE;
-	tid_ptr->packet_count[index] = tid_ptr->packet_count[index] + 1;
-	tid_ptr->total = tid_ptr->total + 1;
-
-	if ((index + 1) > tid_ptr->queue_count)
-		tid_ptr->queue_count = index + 1;
- out:
-	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-
-}
-
-#define MMAC_SCHED_MAX_NUMBER_OF_HT_BACK_FLOWS   7
-enum HT_STATUS {
-	BA_STATUS_FAILURE = 0,
-	BA_STATUS_INITIATOR_DELBA,
-	BA_STATUS_RECIPIENT_DELBA,
-	BA_STATUS_RENEW_ADDBA_REQUEST,
-	BA_STATUS_ACTIVE,
-};
-
-/**
- * iwl4964_tl_ba_avail - Find out if an unused aggregation queue is available
- */
-static u8 iwl4964_tl_ba_avail(struct iwl4965_priv *priv)
-{
-	int i;
-	struct iwl4965_lq_mngr *lq;
-	u8 count = 0;
-	u16 msk;
-
-	lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
-
-	/* Find out how many agg queues are in use */
-	for (i = 0; i < TID_MAX_LOAD_COUNT ; i++) {
-		msk = 1 << i;
-		if ((lq->agg_ctrl.granted_ba & msk) ||
-		    (lq->agg_ctrl.wait_for_agg_status & msk))
-			count++;
-	}
-
-	if (count < MMAC_SCHED_MAX_NUMBER_OF_HT_BACK_FLOWS)
-		return 1;
-
-	return 0;
-}
-
-static void iwl4965_ba_status(struct iwl4965_priv *priv,
-			      u8 tid, enum HT_STATUS status);
-
-static int iwl4965_perform_addba(struct iwl4965_priv *priv, u8 tid, u32 length,
-				 u32 ba_timeout)
-{
-	int rc;
-
-	rc = ieee80211_start_BA_session(priv->hw, priv->bssid, tid);
-	if (rc)
-		iwl4965_ba_status(priv, tid, BA_STATUS_FAILURE);
-
-	return rc;
-}
-
-static int iwl4965_perform_delba(struct iwl4965_priv *priv, u8 tid)
-{
-	int rc;
-
-	rc = ieee80211_stop_BA_session(priv->hw, priv->bssid, tid);
-	if (rc)
-		iwl4965_ba_status(priv, tid, BA_STATUS_FAILURE);
-
-	return rc;
-}
-
-static void iwl4965_turn_on_agg_for_tid(struct iwl4965_priv *priv,
-					struct iwl4965_lq_mngr *lq,
-					u8 auto_agg, u8 tid)
-{
-	u32 tid_msk = (1 << tid);
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-/*
-	if ((auto_agg) && (!lq->enable_counter)){
-		lq->agg_ctrl.next_retry = 0;
-		lq->agg_ctrl.tid_retry = 0;
-		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-		return;
-	}
-*/
-	if (!(lq->agg_ctrl.granted_ba & tid_msk) &&
-	    (lq->agg_ctrl.requested_ba & tid_msk)) {
-		u8 available_queues;
-		u32 load;
-
-		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-		available_queues = iwl4964_tl_ba_avail(priv);
-		load = iwl4965_tl_get_load(priv, tid);
-
-		spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-		if (!available_queues) {
-			if (auto_agg)
-				lq->agg_ctrl.tid_retry |= tid_msk;
-			else {
-				lq->agg_ctrl.requested_ba &= ~tid_msk;
-				lq->agg_ctrl.wait_for_agg_status &= ~tid_msk;
-			}
-		} else if ((auto_agg) &&
-			   ((load <= lq->agg_ctrl.tid_traffic_load_threshold) ||
-			    ((lq->agg_ctrl.wait_for_agg_status & tid_msk))))
-			lq->agg_ctrl.tid_retry |= tid_msk;
-		else {
-			lq->agg_ctrl.wait_for_agg_status |= tid_msk;
-			spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-			iwl4965_perform_addba(priv, tid, 0x40,
-					      lq->agg_ctrl.ba_timeout);
-			spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-		}
-	}
-	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-}
-
-static void iwl4965_turn_on_agg(struct iwl4965_priv *priv, u8 tid)
-{
-	struct iwl4965_lq_mngr *lq;
-	unsigned long flags;
-
-	lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
-
-	if ((tid < TID_MAX_LOAD_COUNT))
-		iwl4965_turn_on_agg_for_tid(priv, lq, lq->agg_ctrl.auto_agg,
-					    tid);
-	else if (tid == TID_ALL_SPECIFIED) {
-		if (lq->agg_ctrl.requested_ba) {
-			for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++)
-				iwl4965_turn_on_agg_for_tid(priv, lq,
-					lq->agg_ctrl.auto_agg, tid);
-		} else {
-			spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-			lq->agg_ctrl.tid_retry = 0;
-			lq->agg_ctrl.next_retry = 0;
-			spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-		}
-	}
-
-}
-
-void iwl4965_turn_off_agg(struct iwl4965_priv *priv, u8 tid)
-{
-	u32 tid_msk;
-	struct iwl4965_lq_mngr *lq;
-	unsigned long flags;
-
-	lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
-
-	if ((tid < TID_MAX_LOAD_COUNT)) {
-		tid_msk = 1 << tid;
-		spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-		lq->agg_ctrl.wait_for_agg_status |= tid_msk;
-		lq->agg_ctrl.requested_ba &= ~tid_msk;
-		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-		iwl4965_perform_delba(priv, tid);
-	} else if (tid == TID_ALL_SPECIFIED) {
-		spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-		for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++) {
-			tid_msk = 1 << tid;
-			lq->agg_ctrl.wait_for_agg_status |= tid_msk;
-			spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-			iwl4965_perform_delba(priv, tid);
-			spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-		}
-		lq->agg_ctrl.requested_ba = 0;
-		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-	}
-}
-
-/**
- * iwl4965_ba_status - Update driver's link quality mgr with tid's HT status
- */
-static void iwl4965_ba_status(struct iwl4965_priv *priv,
-				u8 tid, enum HT_STATUS status)
-{
-	struct iwl4965_lq_mngr *lq;
-	u32 tid_msk = (1 << tid);
-	unsigned long flags;
-
-	lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
-
-	if ((tid >= TID_MAX_LOAD_COUNT))
-		goto out;
-
-	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-	switch (status) {
-	case BA_STATUS_ACTIVE:
-		if (!(lq->agg_ctrl.granted_ba & tid_msk))
-			lq->agg_ctrl.granted_ba |= tid_msk;
-		break;
-	default:
-		if ((lq->agg_ctrl.granted_ba & tid_msk))
-			lq->agg_ctrl.granted_ba &= ~tid_msk;
-		break;
-	}
-
-	lq->agg_ctrl.wait_for_agg_status &= ~tid_msk;
-	if (status != BA_STATUS_ACTIVE) {
-		if (lq->agg_ctrl.auto_agg) {
-			lq->agg_ctrl.tid_retry |= tid_msk;
-			lq->agg_ctrl.next_retry =
-			    jiffies + msecs_to_jiffies(500);
-		} else
-			lq->agg_ctrl.requested_ba &= ~tid_msk;
-	}
-	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
- out:
-	return;
-}
-
-static void iwl4965_bg_agg_work(struct work_struct *work)
-{
-	struct iwl4965_priv *priv = container_of(work, struct iwl4965_priv,
-					agg_work);
-
-	u32 tid;
-	u32 retry_tid;
-	u32 tid_msk;
-	unsigned long flags;
-	struct iwl4965_lq_mngr *lq = (struct iwl4965_lq_mngr *)&(priv->lq_mngr);
-
-	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-	retry_tid = lq->agg_ctrl.tid_retry;
-	lq->agg_ctrl.tid_retry = 0;
-	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-
-	if (retry_tid == TID_ALL_SPECIFIED)
-		iwl4965_turn_on_agg(priv, TID_ALL_SPECIFIED);
-	else {
-		for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++) {
-			tid_msk = (1 << tid);
-			if (retry_tid & tid_msk)
-				iwl4965_turn_on_agg(priv, tid);
-		}
-	}
-
-	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-	if (lq->agg_ctrl.tid_retry)
-		lq->agg_ctrl.next_retry = jiffies + msecs_to_jiffies(500);
-	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-	return;
-}
-
-/* TODO: move this functionality to rate scaling */
-void iwl4965_tl_get_stats(struct iwl4965_priv *priv,
-		   struct ieee80211_hdr *hdr)
-{
-	__le16 *qc = ieee80211_get_qos_ctrl(hdr);
-
-	if (qc &&
-	    (priv->iw_mode != IEEE80211_IF_TYPE_IBSS)) {
-		u8 tid = 0;
-		tid = (u8) (le16_to_cpu(*qc) & 0xF);
-		if (tid < TID_MAX_LOAD_COUNT)
-			iwl4965_tl_add_packet(priv, tid);
-	}
-
-	if (priv->lq_mngr.agg_ctrl.next_retry &&
-	    (time_after(priv->lq_mngr.agg_ctrl.next_retry, jiffies))) {
-		unsigned long flags;
-
-		spin_lock_irqsave(&priv->lq_mngr.lock, flags);
-		priv->lq_mngr.agg_ctrl.next_retry = 0;
-		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
-		schedule_work(&priv->agg_work);
-	}
-}
-
-#endif /*CONFIG_IWL4965_HT_AGG */
-#endif /* CONFIG_IWL4965_HT */
-
 /**
  * sign_extend - Sign extend a value using specified bit as sign-bit
  *
@@ -3526,7 +3181,7 @@ static void iwl4965_add_radiotap(struct iwl4965_priv *priv,
 {
 	s8 signal = stats->ssi;
 	s8 noise = 0;
-	int rate = stats->rate;
+	int rate = stats->rate_idx;
 	u64 tsf = stats->mactime;
 	__le16 phy_flags_hw = rx_start->phy_flags;
 	struct iwl4965_rt_rx_hdr {
@@ -3594,7 +3249,6 @@ static void iwl4965_add_radiotap(struct iwl4965_priv *priv,
 					  IEEE80211_CHAN_2GHZ),
 			      &iwl4965_rt->rt_chbitmask);
 
-	rate = iwl4965_rate_index_from_plcp(rate);
 	if (rate == -1)
 		iwl4965_rt->rt_rate = 0;
 	else
@@ -3808,14 +3462,15 @@ static int parse_elems(u8 *start, size_t len, struct ieee802_11_elems *elems)
 	return 0;
 }
 
-void iwl4965_init_ht_hw_capab(struct ieee80211_ht_info *ht_info, int mode)
+void iwl4965_init_ht_hw_capab(struct ieee80211_ht_info *ht_info,
+			      enum ieee80211_band band)
 {
 	ht_info->cap = 0;
 	memset(ht_info->supp_mcs_set, 0, 16);
 
 	ht_info->ht_supported = 1;
 
-	if (mode == MODE_IEEE80211A) {
+	if (band == IEEE80211_BAND_5GHZ) {
 		ht_info->cap |= (u16)IEEE80211_HT_CAP_SUP_WIDTH;
 		ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_40;
 		ht_info->supp_mcs_set[4] = 0x01;
@@ -3890,12 +3545,13 @@ static void iwl4965_rx_reply_rx(struct iwl4965_priv *priv,
 	u16 fc;
 	struct ieee80211_rx_status stats = {
 		.mactime = le64_to_cpu(rx_start->timestamp),
-		.channel = le16_to_cpu(rx_start->channel),
-		.phymode =
+		.freq = ieee80211chan2mhz(le16_to_cpu(rx_start->channel)),
+		.band =
 			(rx_start->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
-			MODE_IEEE80211G : MODE_IEEE80211A,
+			IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ,
 		.antenna = 0,
-		.rate = iwl4965_hw_get_rate(rx_start->rate_n_flags),
+		.rate_idx = iwl4965_rate_index_from_plcp(
+				le32_to_cpu(rx_start->rate_n_flags)),
 		.flag = 0,
 	};
 	u8 network_packet;
@@ -3946,8 +3602,6 @@ static void iwl4965_rx_reply_rx(struct iwl4965_priv *priv,
 
 	priv->ucode_beacon_time = le32_to_cpu(rx_start->beacon_time_stamp);
 
-	stats.freq = ieee80211chan2mhz(stats.channel);
-
 	/* Find max signal strength (dBm) among 3 antenna/receiver chains */
 	stats.ssi = iwl4965_calc_rssi(rx_start);
 
@@ -4167,24 +3821,6 @@ static void iwl4965_rx_missed_beacon_notif(struct iwl4965_priv *priv,
 }
 
 #ifdef CONFIG_IWL4965_HT
-#ifdef CONFIG_IWL4965_HT_AGG
-
-/**
- * iwl4965_set_tx_status - Update driver's record of one Tx frame's status
- *
- * This will get sent to mac80211.
- */
-static void iwl4965_set_tx_status(struct iwl4965_priv *priv, int txq_id, int idx,
-				  u32 status, u32 retry_count, u32 rate)
-{
-	struct ieee80211_tx_status *tx_status =
-		&(priv->txq[txq_id].txb[idx].status);
-
-	tx_status->flags = status ? IEEE80211_TX_STATUS_ACK : 0;
-	tx_status->retry_count += retry_count;
-	tx_status->control.tx_rate = rate;
-}
-
 
 /**
  * iwl4965_sta_modify_enable_tid_tx - Enable Tx for this TID in station table
@@ -4204,7 +3840,6 @@ static void iwl4965_sta_modify_enable_tid_tx(struct iwl4965_priv *priv,
 	iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
 }
 
-
 /**
  * iwl4965_tx_status_reply_compressed_ba - Update tx status from block-ack
  *
@@ -4218,10 +3853,11 @@ static int iwl4965_tx_status_reply_compressed_ba(struct iwl4965_priv *priv,
 
 {
 	int i, sh, ack;
-	u16 ba_seq_ctl = le16_to_cpu(ba_resp->ba_seq_ctl);
-	u32 bitmap0, bitmap1;
-	u32 resp_bitmap0 = le32_to_cpu(ba_resp->ba_bitmap0);
-	u32 resp_bitmap1 = le32_to_cpu(ba_resp->ba_bitmap1);
+	u16 seq_ctl = le16_to_cpu(ba_resp->seq_ctl);
+	u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
+	u64 bitmap;
+	int successes = 0;
+	struct ieee80211_tx_status *tx_status;
 
 	if (unlikely(!agg->wait_for_ba))  {
 		IWL_ERROR("Received BA when not expected\n");
@@ -4230,17 +3866,15 @@ static int iwl4965_tx_status_reply_compressed_ba(struct iwl4965_priv *priv,
 
 	/* Mark that the expected block-ack response arrived */
 	agg->wait_for_ba = 0;
-	IWL_DEBUG_TX_REPLY("BA %d %d\n", agg->start_idx, ba_resp->ba_seq_ctl);
+	IWL_DEBUG_TX_REPLY("BA %d %d\n", agg->start_idx, ba_resp->seq_ctl);
 
 	/* Calculate shift to align block-ack bits with our Tx window bits */
-	sh = agg->start_idx - SEQ_TO_INDEX(ba_seq_ctl >> 4);
+	sh = agg->start_idx - SEQ_TO_INDEX(seq_ctl>>4);
 	if (sh < 0) /* tbw something is wrong with indices */
 		sh += 0x100;
 
 	/* don't use 64-bit values for now */
-	bitmap0 = resp_bitmap0 >> sh;
-	bitmap1 = resp_bitmap1 >> sh;
-	bitmap0 |= (resp_bitmap1 & ((1 << sh) | ((1 << sh) - 1))) << (32 - sh);
+	bitmap = le64_to_cpu(ba_resp->bitmap) >> sh;
 
 	if (agg->frame_count > (64 - sh)) {
 		IWL_DEBUG_TX_REPLY("more frames than bitmap size");
@@ -4249,26 +3883,108 @@ static int iwl4965_tx_status_reply_compressed_ba(struct iwl4965_priv *priv,
 
 	/* check for success or failure according to the
 	 * transmitted bitmap and block-ack bitmap */
-	bitmap0 &= agg->bitmap0;
-	bitmap1 &= agg->bitmap1;
+	bitmap &= agg->bitmap;
 
 	/* For each frame attempted in aggregation,
 	 * update driver's record of tx frame's status. */
 	for (i = 0; i < agg->frame_count ; i++) {
-		int idx = (agg->start_idx + i) & 0xff;
-		ack = bitmap0 & (1 << i);
+		ack = bitmap & (1 << i);
+		successes += !!ack;
 		IWL_DEBUG_TX_REPLY("%s ON i=%d idx=%d raw=%d\n",
-			ack? "ACK":"NACK", i, idx, agg->start_idx + i);
-		iwl4965_set_tx_status(priv, agg->txq_id, idx, ack, 0,
-			agg->rate_n_flags);
+			ack? "ACK":"NACK", i, (agg->start_idx + i) & 0xff,
+			agg->start_idx + i);
+	}
+
+	tx_status = &priv->txq[scd_flow].txb[agg->start_idx].status;
+	tx_status->flags = IEEE80211_TX_STATUS_ACK;
+	tx_status->flags |= IEEE80211_TX_STATUS_AMPDU;
+	tx_status->ampdu_ack_map = successes;
+	tx_status->ampdu_ack_len = agg->frame_count;
+	/* FIXME Wrong rate
+	tx_status->control.tx_rate = agg->rate_n_flags;
+	*/
+
+	IWL_DEBUG_TX_REPLY("Bitmap %llx\n", bitmap);
+
+	return 0;
+}
+
+/**
+ * iwl4965_tx_queue_stop_scheduler - Stop queue, but keep configuration
+ */
+static void iwl4965_tx_queue_stop_scheduler(struct iwl4965_priv *priv,
+					    u16 txq_id)
+{
+	/* Simply stop the queue, but don't change any configuration;
+	 * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
+	iwl4965_write_prph(priv,
+		KDR_SCD_QUEUE_STATUS_BITS(txq_id),
+		(0 << SCD_QUEUE_STTS_REG_POS_ACTIVE)|
+		(1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
+}
 
+/**
+ * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
+ */
+static int iwl4965_tx_queue_agg_disable(struct iwl4965_priv *priv, u16 txq_id,
+					u16 ssn_idx, u8 tx_fifo)
+{
+	if (IWL_BACK_QUEUE_FIRST_ID > txq_id) {
+		IWL_WARNING("queue number too small: %d, must be > %d\n",
+				txq_id, IWL_BACK_QUEUE_FIRST_ID);
+		return -EINVAL;
 	}
 
-	IWL_DEBUG_TX_REPLY("Bitmap %x%x\n", bitmap0, bitmap1);
+	iwl4965_tx_queue_stop_scheduler(priv, txq_id);
+
+	iwl4965_clear_bits_prph(priv, KDR_SCD_QUEUECHAIN_SEL, (1 << txq_id));
+
+	priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
+	priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
+	/* supposes that ssn_idx is valid (!= 0xFFF) */
+	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
+
+	iwl4965_clear_bits_prph(priv, KDR_SCD_INTERRUPT_MASK, (1 << txq_id));
+	iwl4965_txq_ctx_deactivate(priv, txq_id);
+	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
 
 	return 0;
 }
 
+int iwl4965_check_empty_hw_queue(struct iwl4965_priv *priv, int sta_id,
+					 u8 tid, int txq_id)
+{
+	struct iwl4965_queue *q = &priv->txq[txq_id].q;
+	u8 *addr = priv->stations[sta_id].sta.sta.addr;
+	struct iwl4965_tid_data *tid_data = &priv->stations[sta_id].tid[tid];
+
+	switch (priv->stations[sta_id].tid[tid].agg.state) {
+	case IWL_EMPTYING_HW_QUEUE_DELBA:
+		/* We are reclaiming the last packet of the */
+		/* aggregated HW queue */
+		if (txq_id  == tid_data->agg.txq_id &&
+		    q->read_ptr == q->write_ptr) {
+			u16 ssn = SEQ_TO_SN(tid_data->seq_number);
+			int tx_fifo = default_tid_to_tx_fifo[tid];
+			IWL_DEBUG_HT("HW queue empty: continue DELBA flow\n");
+			iwl4965_tx_queue_agg_disable(priv, txq_id,
+						     ssn, tx_fifo);
+			tid_data->agg.state = IWL_AGG_OFF;
+			ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, addr, tid);
+		}
+		break;
+	case IWL_EMPTYING_HW_QUEUE_ADDBA:
+		/* We are reclaiming the last packet of the queue */
+		if (tid_data->tfds_in_queue == 0) {
+			IWL_DEBUG_HT("HW queue empty: continue ADDBA flow\n");
+			tid_data->agg.state = IWL_AGG_ON;
+			ieee80211_start_tx_ba_cb_irqsafe(priv->hw, addr, tid);
+		}
+		break;
+	}
+	return 0;
+}
+
 /**
  * iwl4965_queue_dec_wrap - Decrement queue index, wrap back to end if needed
  * @index -- current index
@@ -4293,48 +4009,43 @@ static void iwl4965_rx_reply_compressed_ba(struct iwl4965_priv *priv,
 	int index;
 	struct iwl4965_tx_queue *txq = NULL;
 	struct iwl4965_ht_agg *agg;
+	DECLARE_MAC_BUF(mac);
 
 	/* "flow" corresponds to Tx queue */
-	u16 ba_resp_scd_flow = le16_to_cpu(ba_resp->scd_flow);
+	u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
 
 	/* "ssn" is start of block-ack Tx window, corresponds to index
 	 * (in Tx queue's circular buffer) of first TFD/frame in window */
 	u16 ba_resp_scd_ssn = le16_to_cpu(ba_resp->scd_ssn);
 
-	if (ba_resp_scd_flow >= ARRAY_SIZE(priv->txq)) {
+	if (scd_flow >= ARRAY_SIZE(priv->txq)) {
 		IWL_ERROR("BUG_ON scd_flow is bigger than number of queues");
 		return;
 	}
 
-	txq = &priv->txq[ba_resp_scd_flow];
+	txq = &priv->txq[scd_flow];
 	agg = &priv->stations[ba_resp->sta_id].tid[ba_resp->tid].agg;
 
 	/* Find index just before block-ack window */
 	index = iwl4965_queue_dec_wrap(ba_resp_scd_ssn & 0xff, txq->q.n_bd);
 
 	/* TODO: Need to get this copy more safely - now good for debug */
-/*
-	{
-	DECLARE_MAC_BUF(mac);
+
 	IWL_DEBUG_TX_REPLY("REPLY_COMPRESSED_BA [%d]Received from %s, "
 			   "sta_id = %d\n",
 			   agg->wait_for_ba,
 			   print_mac(mac, (u8*) &ba_resp->sta_addr_lo32),
 			   ba_resp->sta_id);
-	IWL_DEBUG_TX_REPLY("TID = %d, SeqCtl = %d, bitmap = 0x%X%X, scd_flow = "
+	IWL_DEBUG_TX_REPLY("TID = %d, SeqCtl = %d, bitmap = 0x%llx, scd_flow = "
 			   "%d, scd_ssn = %d\n",
 			   ba_resp->tid,
-			   ba_resp->ba_seq_ctl,
-			   ba_resp->ba_bitmap1,
-			   ba_resp->ba_bitmap0,
+			   ba_resp->seq_ctl,
+			   ba_resp->bitmap,
 			   ba_resp->scd_flow,
 			   ba_resp->scd_ssn);
-	IWL_DEBUG_TX_REPLY("DAT start_idx = %d, bitmap = 0x%X%X \n",
+	IWL_DEBUG_TX_REPLY("DAT start_idx = %d, bitmap = 0x%llx \n",
 			   agg->start_idx,
-			   agg->bitmap1,
-			   agg->bitmap0);
-	}
-*/
+			   agg->bitmap);
 
 	/* Update driver's record of ACK vs. not for each frame in window */
 	iwl4965_tx_status_reply_compressed_ba(priv, agg, ba_resp);
@@ -4342,23 +4053,17 @@ static void iwl4965_rx_reply_compressed_ba(struct iwl4965_priv *priv,
 	/* Release all TFDs before the SSN, i.e. all TFDs in front of
 	 * block-ack window (we assume that they've been successfully
 	 * transmitted ... if not, it's too late anyway). */
-	if (txq->q.read_ptr != (ba_resp_scd_ssn & 0xff))
-		iwl4965_tx_queue_reclaim(priv, ba_resp_scd_flow, index);
-
-}
-
-
-/**
- * iwl4965_tx_queue_stop_scheduler - Stop queue, but keep configuration
- */
-static void iwl4965_tx_queue_stop_scheduler(struct iwl4965_priv *priv, u16 txq_id)
-{
-	/* Simply stop the queue, but don't change any configuration;
-	 * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
-	iwl4965_write_prph(priv,
-		KDR_SCD_QUEUE_STATUS_BITS(txq_id),
-		(0 << SCD_QUEUE_STTS_REG_POS_ACTIVE)|
-		(1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
+	if (txq->q.read_ptr != (ba_resp_scd_ssn & 0xff)) {
+		int freed = iwl4965_tx_queue_reclaim(priv, scd_flow, index);
+		priv->stations[ba_resp->sta_id].
+			tid[ba_resp->tid].tfds_in_queue -= freed;
+		if (iwl4965_queue_space(&txq->q) > txq->q.low_mark &&
+			priv->mac80211_registered &&
+			agg->state != IWL_EMPTYING_HW_QUEUE_DELBA)
+			ieee80211_wake_queue(priv->hw, scd_flow);
+		iwl4965_check_empty_hw_queue(priv, ba_resp->sta_id,
+			ba_resp->tid, scd_flow);
+	}
 }
 
 /**
@@ -4388,6 +4093,7 @@ static int iwl4965_tx_queue_set_q2ratid(struct iwl4965_priv *priv, u16 ra_tid,
 	return 0;
 }
 
+
 /**
  * iwl4965_tx_queue_agg_enable - Set up & enable aggregation for selected queue
  *
@@ -4455,48 +4161,6 @@ static int iwl4965_tx_queue_agg_enable(struct iwl4965_priv *priv, int txq_id,
 	return 0;
 }
 
-/**
- * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
- */
-static int iwl4965_tx_queue_agg_disable(struct iwl4965_priv *priv, u16 txq_id,
-					u16 ssn_idx, u8 tx_fifo)
-{
-	unsigned long flags;
-	int rc;
-
-	if (IWL_BACK_QUEUE_FIRST_ID > txq_id) {
-		IWL_WARNING("queue number too small: %d, must be > %d\n",
-				txq_id, IWL_BACK_QUEUE_FIRST_ID);
-		return -EINVAL;
-	}
-
-	spin_lock_irqsave(&priv->lock, flags);
-	rc = iwl4965_grab_nic_access(priv);
-	if (rc) {
-		spin_unlock_irqrestore(&priv->lock, flags);
-		return rc;
-	}
-
-	iwl4965_tx_queue_stop_scheduler(priv, txq_id);
-
-	iwl4965_clear_bits_prph(priv, KDR_SCD_QUEUECHAIN_SEL, (1 << txq_id));
-
-	priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
-	priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
-	/* supposes that ssn_idx is valid (!= 0xFFF) */
-	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
-
-	iwl4965_clear_bits_prph(priv, KDR_SCD_INTERRUPT_MASK, (1 << txq_id));
-	iwl4965_txq_ctx_deactivate(priv, txq_id);
-	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
-
-	iwl4965_release_nic_access(priv);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	return 0;
-}
-
-#endif/* CONFIG_IWL4965_HT_AGG */
 #endif /* CONFIG_IWL4965_HT */
 
 /**
@@ -4525,7 +4189,7 @@ void iwl4965_add_station(struct iwl4965_priv *priv, const u8 *addr, int is_ap)
 	 * all the way down to 1M in IEEE order, and then spin on 1M */
 	if (is_ap)
 		r = IWL_RATE_54M_INDEX;
-	else if (priv->phymode == MODE_IEEE80211A)
+	else if (priv->band == IEEE80211_BAND_5GHZ)
 		r = IWL_RATE_6M_INDEX;
 	else
 		r = IWL_RATE_1M_INDEX;
@@ -4558,12 +4222,13 @@ void iwl4965_add_station(struct iwl4965_priv *priv, const u8 *addr, int is_ap)
 
 #ifdef CONFIG_IWL4965_HT
 
-static u8 iwl4965_is_channel_extension(struct iwl4965_priv *priv, int phymode,
-				   u16 channel, u8 extension_chan_offset)
+static u8 iwl4965_is_channel_extension(struct iwl4965_priv *priv,
+				       enum ieee80211_band band,
+				       u16 channel, u8 extension_chan_offset)
 {
 	const struct iwl4965_channel_info *ch_info;
 
-	ch_info = iwl4965_get_channel_info(priv, phymode, channel);
+	ch_info = iwl4965_get_channel_info(priv, band, channel);
 	if (!is_channel_valid(ch_info))
 		return 0;
 
@@ -4589,11 +4254,11 @@ static u8 iwl4965_is_fat_tx_allowed(struct iwl4965_priv *priv,
 
 	if (sta_ht_inf) {
 		if ((!sta_ht_inf->ht_supported) ||
-		   (!sta_ht_inf->cap & IEEE80211_HT_CAP_SUP_WIDTH))
+		   (!(sta_ht_inf->cap & IEEE80211_HT_CAP_SUP_WIDTH)))
 			return 0;
 	}
 
-	return (iwl4965_is_channel_extension(priv, priv->phymode,
+	return (iwl4965_is_channel_extension(priv, priv->band,
 					 iwl_ht_conf->control_channel,
 					 iwl_ht_conf->extension_chan_offset));
 }
@@ -4730,56 +4395,6 @@ static void iwl4965_sta_modify_del_ba_tid(struct iwl4965_priv *priv,
 	iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
 }
 
-int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
-			     enum ieee80211_ampdu_mlme_action action,
-			     const u8 *addr, u16 tid, u16 ssn)
-{
-	struct iwl4965_priv *priv = hw->priv;
-	int sta_id;
-	DECLARE_MAC_BUF(mac);
-
-	IWL_DEBUG_HT("A-MPDU action on da=%s tid=%d ",
-			print_mac(mac, addr), tid);
-	sta_id = iwl4965_hw_find_station(priv, addr);
-	switch (action) {
-	case IEEE80211_AMPDU_RX_START:
-		IWL_DEBUG_HT("start Rx\n");
-		iwl4965_sta_modify_add_ba_tid(priv, sta_id, tid, ssn);
-		break;
-	case IEEE80211_AMPDU_RX_STOP:
-		IWL_DEBUG_HT("stop Rx\n");
-		iwl4965_sta_modify_del_ba_tid(priv, sta_id, tid);
-		break;
-	default:
-		IWL_DEBUG_HT("unknown\n");
-		return -EINVAL;
-		break;
-	}
-	return 0;
-}
-
-#ifdef CONFIG_IWL4965_HT_AGG
-
-static const u16 default_tid_to_tx_fifo[] = {
-	IWL_TX_FIFO_AC1,
-	IWL_TX_FIFO_AC0,
-	IWL_TX_FIFO_AC0,
-	IWL_TX_FIFO_AC1,
-	IWL_TX_FIFO_AC2,
-	IWL_TX_FIFO_AC2,
-	IWL_TX_FIFO_AC3,
-	IWL_TX_FIFO_AC3,
-	IWL_TX_FIFO_NONE,
-	IWL_TX_FIFO_NONE,
-	IWL_TX_FIFO_NONE,
-	IWL_TX_FIFO_NONE,
-	IWL_TX_FIFO_NONE,
-	IWL_TX_FIFO_NONE,
-	IWL_TX_FIFO_NONE,
-	IWL_TX_FIFO_NONE,
-	IWL_TX_FIFO_AC3
-};
-
 /*
  * Find first available (lowest unused) Tx Queue, mark it "active".
  * Called only when finding queue for aggregation.
@@ -4796,70 +4411,78 @@ static int iwl4965_txq_ctx_activate_free(struct iwl4965_priv *priv)
 	return -1;
 }
 
-int iwl4965_mac_ht_tx_agg_start(struct ieee80211_hw *hw, u8 *da, u16 tid,
-			    u16 *start_seq_num)
+static int iwl4965_mac_ht_tx_agg_start(struct ieee80211_hw *hw, const u8 *da,
+				       u16 tid, u16 *start_seq_num)
 {
-
 	struct iwl4965_priv *priv = hw->priv;
 	int sta_id;
 	int tx_fifo;
 	int txq_id;
 	int ssn = -1;
+	int rc = 0;
 	unsigned long flags;
 	struct iwl4965_tid_data *tid_data;
 	DECLARE_MAC_BUF(mac);
 
-	/* Determine Tx DMA/FIFO channel for this Traffic ID */
 	if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
 		tx_fifo = default_tid_to_tx_fifo[tid];
 	else
 		return -EINVAL;
 
-	IWL_WARNING("iwl-AGG iwl4965_mac_ht_tx_agg_start on da=%s"
-		    " tid=%d\n", print_mac(mac, da), tid);
+	IWL_WARNING("%s on da = %s tid = %d\n",
+			__func__, print_mac(mac, da), tid);
 
-	/* Get index into station table */
 	sta_id = iwl4965_hw_find_station(priv, da);
 	if (sta_id == IWL_INVALID_STATION)
 		return -ENXIO;
 
-	/* Find available Tx queue for aggregation */
+	if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_OFF) {
+		IWL_ERROR("Start AGG when state is not IWL_AGG_OFF !\n");
+		return -ENXIO;
+	}
+
 	txq_id = iwl4965_txq_ctx_activate_free(priv);
 	if (txq_id == -1)
 		return -ENXIO;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
 	tid_data = &priv->stations[sta_id].tid[tid];
-
-	/* Get starting sequence number for 1st frame in block ack window.
-	 * We'll use least signif byte as 1st frame's index into Tx queue. */
 	ssn = SEQ_TO_SN(tid_data->seq_number);
 	tid_data->agg.txq_id = txq_id;
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 
 	*start_seq_num = ssn;
-
-	/* Update driver's link quality manager */
-	iwl4965_ba_status(priv, tid, BA_STATUS_ACTIVE);
-
-	/* Set up and enable aggregation for selected Tx queue and FIFO */
-	return iwl4965_tx_queue_agg_enable(priv, txq_id, tx_fifo,
+	rc = iwl4965_tx_queue_agg_enable(priv, txq_id, tx_fifo,
 					   sta_id, tid, ssn);
-}
+	if (rc)
+		return rc;
 
+	rc = 0;
+	if (tid_data->tfds_in_queue == 0) {
+		printk(KERN_ERR "HW queue is empty\n");
+		tid_data->agg.state = IWL_AGG_ON;
+		ieee80211_start_tx_ba_cb_irqsafe(hw, da, tid);
+	} else {
+		IWL_DEBUG_HT("HW queue is NOT empty: %d packets in HW queue\n",
+				tid_data->tfds_in_queue);
+		tid_data->agg.state = IWL_EMPTYING_HW_QUEUE_ADDBA;
+	}
+	return rc;
+}
 
-int iwl4965_mac_ht_tx_agg_stop(struct ieee80211_hw *hw, u8 *da, u16 tid,
-			   int generator)
+static int iwl4965_mac_ht_tx_agg_stop(struct ieee80211_hw *hw, const u8 *da,
+				      u16 tid)
 {
 
 	struct iwl4965_priv *priv = hw->priv;
 	int tx_fifo_id, txq_id, sta_id, ssn = -1;
 	struct iwl4965_tid_data *tid_data;
-	int rc;
+	int rc, write_ptr, read_ptr;
+	unsigned long flags;
 	DECLARE_MAC_BUF(mac);
 
 	if (!da) {
-		IWL_ERROR("%s: da = NULL\n", __func__);
+		IWL_ERROR("da = NULL\n");
 		return -EINVAL;
 	}
 
@@ -4873,24 +4496,81 @@ int iwl4965_mac_ht_tx_agg_stop(struct ieee80211_hw *hw, u8 *da, u16 tid,
 	if (sta_id == IWL_INVALID_STATION)
 		return -ENXIO;
 
+	if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_ON)
+		IWL_WARNING("Stopping AGG while state not IWL_AGG_ON\n");
+
 	tid_data = &priv->stations[sta_id].tid[tid];
 	ssn = (tid_data->seq_number & IEEE80211_SCTL_SEQ) >> 4;
 	txq_id = tid_data->agg.txq_id;
+	write_ptr = priv->txq[txq_id].q.write_ptr;
+	read_ptr = priv->txq[txq_id].q.read_ptr;
+
+	/* The queue is not empty */
+	if (write_ptr != read_ptr) {
+		IWL_DEBUG_HT("Stopping a non empty AGG HW QUEUE\n");
+		priv->stations[sta_id].tid[tid].agg.state =
+				IWL_EMPTYING_HW_QUEUE_DELBA;
+		return 0;
+	}
+
+	IWL_DEBUG_HT("HW queue empty\n");;
+	priv->stations[sta_id].tid[tid].agg.state = IWL_AGG_OFF;
 
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl4965_grab_nic_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
 	rc = iwl4965_tx_queue_agg_disable(priv, txq_id, ssn, tx_fifo_id);
-	/* FIXME: need more safe way to handle error condition */
+	iwl4965_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
 	if (rc)
 		return rc;
 
-	iwl4965_ba_status(priv, tid, BA_STATUS_INITIATOR_DELBA);
+	ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, da, tid);
+
 	IWL_DEBUG_INFO("iwl4965_mac_ht_tx_agg_stop on da=%s tid=%d\n",
-		       print_mac(mac, da), tid);
+			print_mac(mac, da), tid);
 
 	return 0;
 }
 
+int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
+			     enum ieee80211_ampdu_mlme_action action,
+			     const u8 *addr, u16 tid, u16 *ssn)
+{
+	struct iwl4965_priv *priv = hw->priv;
+	int sta_id;
+	DECLARE_MAC_BUF(mac);
+
+	IWL_DEBUG_HT("A-MPDU action on da=%s tid=%d ",
+			print_mac(mac, addr), tid);
+	sta_id = iwl4965_hw_find_station(priv, addr);
+	switch (action) {
+	case IEEE80211_AMPDU_RX_START:
+		IWL_DEBUG_HT("start Rx\n");
+		iwl4965_sta_modify_add_ba_tid(priv, sta_id, tid, *ssn);
+		break;
+	case IEEE80211_AMPDU_RX_STOP:
+		IWL_DEBUG_HT("stop Rx\n");
+		iwl4965_sta_modify_del_ba_tid(priv, sta_id, tid);
+		break;
+	case IEEE80211_AMPDU_TX_START:
+		IWL_DEBUG_HT("start Tx\n");
+		return iwl4965_mac_ht_tx_agg_start(hw, addr, tid, ssn);
+	case IEEE80211_AMPDU_TX_STOP:
+		IWL_DEBUG_HT("stop Tx\n");
+		return iwl4965_mac_ht_tx_agg_stop(hw, addr, tid);
+	default:
+		IWL_DEBUG_HT("unknown\n");
+		return -EINVAL;
+		break;
+	}
+	return 0;
+}
 
-#endif /* CONFIG_IWL4965_HT_AGG */
 #endif /* CONFIG_IWL4965_HT */
 
 /* Set up 4965-specific Rx frame reply handlers */
@@ -4907,9 +4587,7 @@ void iwl4965_hw_rx_handler_setup(struct iwl4965_priv *priv)
 	    iwl4965_rx_missed_beacon_notif;
 
 #ifdef CONFIG_IWL4965_HT
-#ifdef CONFIG_IWL4965_HT_AGG
 	priv->rx_handlers[REPLY_COMPRESSED_BA] = iwl4965_rx_reply_compressed_ba;
-#endif /* CONFIG_IWL4965_HT_AGG */
 #endif /* CONFIG_IWL4965_HT */
 }
 
@@ -4920,11 +4598,6 @@ void iwl4965_hw_setup_deferred_work(struct iwl4965_priv *priv)
 #ifdef CONFIG_IWL4965_SENSITIVITY
 	INIT_WORK(&priv->sensitivity_work, iwl4965_bg_sensitivity_work);
 #endif
-#ifdef CONFIG_IWL4965_HT
-#ifdef CONFIG_IWL4965_HT_AGG
-	INIT_WORK(&priv->agg_work, iwl4965_bg_agg_work);
-#endif /* CONFIG_IWL4965_HT_AGG */
-#endif /* CONFIG_IWL4965_HT */
 	init_timer(&priv->statistics_periodic);
 	priv->statistics_periodic.data = (unsigned long)priv;
 	priv->statistics_periodic.function = iwl4965_bg_statistics_periodic;