Merge branch 'master' of /home/davem/src/GIT/linux-2.6/
[linux-2.6] / drivers / net / wireless / iwlwifi / iwl-3945-rs.c
1 /******************************************************************************
2  *
3  * Copyright(c) 2005 - 2009 Intel Corporation. All rights reserved.
4  *
5  * This program is free software; you can redistribute it and/or modify it
6  * under the terms of version 2 of the GNU General Public License as
7  * published by the Free Software Foundation.
8  *
9  * This program is distributed in the hope that it will be useful, but WITHOUT
10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
12  * more details.
13  *
14  * You should have received a copy of the GNU General Public License along with
15  * this program; if not, write to the Free Software Foundation, Inc.,
16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
17  *
18  * The full GNU General Public License is included in this distribution in the
19  * file called LICENSE.
20  *
21  * Contact Information:
22  *  Intel Linux Wireless <ilw@linux.intel.com>
23  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
24  *
25  *****************************************************************************/
26
27 #include <linux/kernel.h>
28 #include <linux/init.h>
29 #include <linux/skbuff.h>
30 #include <linux/wireless.h>
31 #include <net/mac80211.h>
32
33 #include <linux/netdevice.h>
34 #include <linux/etherdevice.h>
35 #include <linux/delay.h>
36
37 #include <linux/workqueue.h>
38
39 #include "iwl-commands.h"
40 #include "iwl-3945.h"
41
42 #define RS_NAME "iwl-3945-rs"
43
44 struct iwl3945_rate_scale_data {
45         u64 data;
46         s32 success_counter;
47         s32 success_ratio;
48         s32 counter;
49         s32 average_tpt;
50         unsigned long stamp;
51 };
52
53 struct iwl3945_rs_sta {
54         spinlock_t lock;
55         struct iwl_priv *priv;
56         s32 *expected_tpt;
57         unsigned long last_partial_flush;
58         unsigned long last_flush;
59         u32 flush_time;
60         u32 last_tx_packets;
61         u32 tx_packets;
62         u8 tgg;
63         u8 flush_pending;
64         u8 start_rate;
65         u8 ibss_sta_added;
66         struct timer_list rate_scale_flush;
67         struct iwl3945_rate_scale_data win[IWL_RATE_COUNT_3945];
68 #ifdef CONFIG_MAC80211_DEBUGFS
69         struct dentry *rs_sta_dbgfs_stats_table_file;
70 #endif
71
72         /* used to be in sta_info */
73         int last_txrate_idx;
74 };
75
76 static s32 iwl3945_expected_tpt_g[IWL_RATE_COUNT_3945] = {
77         7, 13, 35, 58, 0, 0, 76, 104, 130, 168, 191, 202
78 };
79
80 static s32 iwl3945_expected_tpt_g_prot[IWL_RATE_COUNT_3945] = {
81         7, 13, 35, 58, 0, 0, 0, 80, 93, 113, 123, 125
82 };
83
84 static s32 iwl3945_expected_tpt_a[IWL_RATE_COUNT_3945] = {
85         0, 0, 0, 0, 40, 57, 72, 98, 121, 154, 177, 186
86 };
87
88 static s32 iwl3945_expected_tpt_b[IWL_RATE_COUNT_3945] = {
89         7, 13, 35, 58, 0, 0, 0, 0, 0, 0, 0, 0
90 };
91
92 struct iwl3945_tpt_entry {
93         s8 min_rssi;
94         u8 index;
95 };
96
97 static struct iwl3945_tpt_entry iwl3945_tpt_table_a[] = {
98         {-60, IWL_RATE_54M_INDEX},
99         {-64, IWL_RATE_48M_INDEX},
100         {-72, IWL_RATE_36M_INDEX},
101         {-80, IWL_RATE_24M_INDEX},
102         {-84, IWL_RATE_18M_INDEX},
103         {-85, IWL_RATE_12M_INDEX},
104         {-87, IWL_RATE_9M_INDEX},
105         {-89, IWL_RATE_6M_INDEX}
106 };
107
108 static struct iwl3945_tpt_entry iwl3945_tpt_table_g[] = {
109         {-60, IWL_RATE_54M_INDEX},
110         {-64, IWL_RATE_48M_INDEX},
111         {-68, IWL_RATE_36M_INDEX},
112         {-80, IWL_RATE_24M_INDEX},
113         {-84, IWL_RATE_18M_INDEX},
114         {-85, IWL_RATE_12M_INDEX},
115         {-86, IWL_RATE_11M_INDEX},
116         {-88, IWL_RATE_5M_INDEX},
117         {-90, IWL_RATE_2M_INDEX},
118         {-92, IWL_RATE_1M_INDEX}
119 };
120
121 #define IWL_RATE_MAX_WINDOW          62
122 #define IWL_RATE_FLUSH           (3*HZ)
123 #define IWL_RATE_WIN_FLUSH       (HZ/2)
124 #define IWL39_RATE_HIGH_TH          11520
125 #define IWL_SUCCESS_UP_TH          8960
126 #define IWL_SUCCESS_DOWN_TH       10880
127 #define IWL_RATE_MIN_FAILURE_TH       8
128 #define IWL_RATE_MIN_SUCCESS_TH       8
129 #define IWL_RATE_DECREASE_TH       1920
130
131 static u8 iwl3945_get_rate_index_by_rssi(s32 rssi, enum ieee80211_band band)
132 {
133         u32 index = 0;
134         u32 table_size = 0;
135         struct iwl3945_tpt_entry *tpt_table = NULL;
136
137         if ((rssi < IWL_MIN_RSSI_VAL) || (rssi > IWL_MAX_RSSI_VAL))
138                 rssi = IWL_MIN_RSSI_VAL;
139
140         switch (band) {
141         case IEEE80211_BAND_2GHZ:
142                 tpt_table = iwl3945_tpt_table_g;
143                 table_size = ARRAY_SIZE(iwl3945_tpt_table_g);
144                 break;
145
146         case IEEE80211_BAND_5GHZ:
147                 tpt_table = iwl3945_tpt_table_a;
148                 table_size = ARRAY_SIZE(iwl3945_tpt_table_a);
149                 break;
150
151         default:
152                 BUG();
153                 break;
154         }
155
156         while ((index < table_size) && (rssi < tpt_table[index].min_rssi))
157                 index++;
158
159         index = min(index, (table_size - 1));
160
161         return tpt_table[index].index;
162 }
163
164 static void iwl3945_clear_window(struct iwl3945_rate_scale_data *window)
165 {
166         window->data = 0;
167         window->success_counter = 0;
168         window->success_ratio = -1;
169         window->counter = 0;
170         window->average_tpt = IWL_INVALID_VALUE;
171         window->stamp = 0;
172 }
173
174 /**
175  * iwl3945_rate_scale_flush_windows - flush out the rate scale windows
176  *
177  * Returns the number of windows that have gathered data but were
178  * not flushed.  If there were any that were not flushed, then
179  * reschedule the rate flushing routine.
180  */
181 static int iwl3945_rate_scale_flush_windows(struct iwl3945_rs_sta *rs_sta)
182 {
183         int unflushed = 0;
184         int i;
185         unsigned long flags;
186         struct iwl_priv *priv __maybe_unused = rs_sta->priv;
187
188         /*
189          * For each rate, if we have collected data on that rate
190          * and it has been more than IWL_RATE_WIN_FLUSH
191          * since we flushed, clear out the gathered statistics
192          */
193         for (i = 0; i < IWL_RATE_COUNT_3945; i++) {
194                 if (!rs_sta->win[i].counter)
195                         continue;
196
197                 spin_lock_irqsave(&rs_sta->lock, flags);
198                 if (time_after(jiffies, rs_sta->win[i].stamp +
199                                IWL_RATE_WIN_FLUSH)) {
200                         IWL_DEBUG_RATE(priv, "flushing %d samples of rate "
201                                        "index %d\n",
202                                        rs_sta->win[i].counter, i);
203                         iwl3945_clear_window(&rs_sta->win[i]);
204                 } else
205                         unflushed++;
206                 spin_unlock_irqrestore(&rs_sta->lock, flags);
207         }
208
209         return unflushed;
210 }
211
212 #define IWL_RATE_FLUSH_MAX              5000    /* msec */
213 #define IWL_RATE_FLUSH_MIN              50      /* msec */
214 #define IWL_AVERAGE_PACKETS             1500
215
216 static void iwl3945_bg_rate_scale_flush(unsigned long data)
217 {
218         struct iwl3945_rs_sta *rs_sta = (void *)data;
219         struct iwl_priv *priv __maybe_unused = rs_sta->priv;
220         int unflushed = 0;
221         unsigned long flags;
222         u32 packet_count, duration, pps;
223
224         IWL_DEBUG_RATE(priv, "enter\n");
225
226         unflushed = iwl3945_rate_scale_flush_windows(rs_sta);
227
228         spin_lock_irqsave(&rs_sta->lock, flags);
229
230         /* Number of packets Rx'd since last time this timer ran */
231         packet_count = (rs_sta->tx_packets - rs_sta->last_tx_packets) + 1;
232
233         rs_sta->last_tx_packets = rs_sta->tx_packets + 1;
234
235         if (unflushed) {
236                 duration =
237                     jiffies_to_msecs(jiffies - rs_sta->last_partial_flush);
238
239                 IWL_DEBUG_RATE(priv, "Tx'd %d packets in %dms\n",
240                                packet_count, duration);
241
242                 /* Determine packets per second */
243                 if (duration)
244                         pps = (packet_count * 1000) / duration;
245                 else
246                         pps = 0;
247
248                 if (pps) {
249                         duration = (IWL_AVERAGE_PACKETS * 1000) / pps;
250                         if (duration < IWL_RATE_FLUSH_MIN)
251                                 duration = IWL_RATE_FLUSH_MIN;
252                         else if (duration > IWL_RATE_FLUSH_MAX)
253                                 duration = IWL_RATE_FLUSH_MAX;
254                 } else
255                         duration = IWL_RATE_FLUSH_MAX;
256
257                 rs_sta->flush_time = msecs_to_jiffies(duration);
258
259                 IWL_DEBUG_RATE(priv, "new flush period: %d msec ave %d\n",
260                                duration, packet_count);
261
262                 mod_timer(&rs_sta->rate_scale_flush, jiffies +
263                           rs_sta->flush_time);
264
265                 rs_sta->last_partial_flush = jiffies;
266         } else {
267                 rs_sta->flush_time = IWL_RATE_FLUSH;
268                 rs_sta->flush_pending = 0;
269         }
270         /* If there weren't any unflushed entries, we don't schedule the timer
271          * to run again */
272
273         rs_sta->last_flush = jiffies;
274
275         spin_unlock_irqrestore(&rs_sta->lock, flags);
276
277         IWL_DEBUG_RATE(priv, "leave\n");
278 }
279
280 /**
281  * iwl3945_collect_tx_data - Update the success/failure sliding window
282  *
283  * We keep a sliding window of the last 64 packets transmitted
284  * at this rate.  window->data contains the bitmask of successful
285  * packets.
286  */
287 static void iwl3945_collect_tx_data(struct iwl3945_rs_sta *rs_sta,
288                                 struct iwl3945_rate_scale_data *window,
289                                 int success, int retries, int index)
290 {
291         unsigned long flags;
292         s32 fail_count;
293         struct iwl_priv *priv __maybe_unused = rs_sta->priv;
294
295         if (!retries) {
296                 IWL_DEBUG_RATE(priv, "leave: retries == 0 -- should be at least 1\n");
297                 return;
298         }
299
300         spin_lock_irqsave(&rs_sta->lock, flags);
301         while (retries--) {
302
303                 /* If we have filled up the window then subtract one from the
304                  * success counter if the high-bit is counting toward
305                  * success */
306                 if (window->counter == IWL_RATE_MAX_WINDOW) {
307                         if (window->data & (1ULL << (IWL_RATE_MAX_WINDOW - 1)))
308                                 window->success_counter--;
309                 } else
310                         window->counter++;
311
312                 /* Slide the window to the left one bit */
313                 window->data = (window->data << 1);
314
315                 /* If this packet was a success then set the low bit high */
316                 if (success) {
317                         window->success_counter++;
318                         window->data |= 1;
319                 }
320
321                 /* window->counter can't be 0 -- it is either >0 or
322                  * IWL_RATE_MAX_WINDOW */
323                 window->success_ratio = 12800 * window->success_counter /
324                     window->counter;
325
326                 /* Tag this window as having been updated */
327                 window->stamp = jiffies;
328
329         }
330
331         fail_count = window->counter - window->success_counter;
332         if ((fail_count >= IWL_RATE_MIN_FAILURE_TH) ||
333             (window->success_counter >= IWL_RATE_MIN_SUCCESS_TH))
334                 window->average_tpt = ((window->success_ratio *
335                                 rs_sta->expected_tpt[index] + 64) / 128);
336         else
337                 window->average_tpt = IWL_INVALID_VALUE;
338
339         spin_unlock_irqrestore(&rs_sta->lock, flags);
340
341 }
342
343 static void rs_rate_init(void *priv_r, struct ieee80211_supported_band *sband,
344                          struct ieee80211_sta *sta, void *priv_sta)
345 {
346         struct iwl3945_rs_sta *rs_sta = priv_sta;
347         struct iwl_priv *priv = (struct iwl_priv *)priv_r;
348         int i;
349
350         IWL_DEBUG_RATE(priv, "enter\n");
351
352         /* TODO: what is a good starting rate for STA? About middle? Maybe not
353          * the lowest or the highest rate.. Could consider using RSSI from
354          * previous packets? Need to have IEEE 802.1X auth succeed immediately
355          * after assoc.. */
356
357         for (i = sband->n_bitrates - 1; i >= 0; i--) {
358                 if (sta->supp_rates[sband->band] & (1 << i)) {
359                         rs_sta->last_txrate_idx = i;
360                         break;
361                 }
362         }
363
364         priv->sta_supp_rates = sta->supp_rates[sband->band];
365         /* For 5 GHz band it start at IWL_FIRST_OFDM_RATE */
366         if (sband->band == IEEE80211_BAND_5GHZ) {
367                 rs_sta->last_txrate_idx += IWL_FIRST_OFDM_RATE;
368                 priv->sta_supp_rates = priv->sta_supp_rates <<
369                                                 IWL_FIRST_OFDM_RATE;
370         }
371
372
373         IWL_DEBUG_RATE(priv, "leave\n");
374 }
375
376 static void *rs_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
377 {
378         return hw->priv;
379 }
380
381 /* rate scale requires free function to be implemented */
382 static void rs_free(void *priv)
383 {
384         return;
385 }
386
387 static void *rs_alloc_sta(void *iwl_priv, struct ieee80211_sta *sta, gfp_t gfp)
388 {
389         struct iwl3945_rs_sta *rs_sta;
390         struct iwl3945_sta_priv *psta = (void *) sta->drv_priv;
391         struct iwl_priv *priv = iwl_priv;
392         int i;
393
394         /*
395          * XXX: If it's using sta->drv_priv anyway, it might
396          *      as well just put all the information there.
397          */
398
399         IWL_DEBUG_RATE(priv, "enter\n");
400
401         rs_sta = kzalloc(sizeof(struct iwl3945_rs_sta), gfp);
402         if (!rs_sta) {
403                 IWL_DEBUG_RATE(priv, "leave: ENOMEM\n");
404                 return NULL;
405         }
406
407         psta->rs_sta = rs_sta;
408
409         spin_lock_init(&rs_sta->lock);
410
411         rs_sta->priv = priv;
412
413         rs_sta->start_rate = IWL_RATE_INVALID;
414
415         /* default to just 802.11b */
416         rs_sta->expected_tpt = iwl3945_expected_tpt_b;
417
418         rs_sta->last_partial_flush = jiffies;
419         rs_sta->last_flush = jiffies;
420         rs_sta->flush_time = IWL_RATE_FLUSH;
421         rs_sta->last_tx_packets = 0;
422         rs_sta->ibss_sta_added = 0;
423
424         init_timer(&rs_sta->rate_scale_flush);
425         rs_sta->rate_scale_flush.data = (unsigned long)rs_sta;
426         rs_sta->rate_scale_flush.function = &iwl3945_bg_rate_scale_flush;
427
428         for (i = 0; i < IWL_RATE_COUNT_3945; i++)
429                 iwl3945_clear_window(&rs_sta->win[i]);
430
431         IWL_DEBUG_RATE(priv, "leave\n");
432
433         return rs_sta;
434 }
435
436 static void rs_free_sta(void *iwl_priv, struct ieee80211_sta *sta,
437                         void *priv_sta)
438 {
439         struct iwl3945_sta_priv *psta = (void *) sta->drv_priv;
440         struct iwl3945_rs_sta *rs_sta = priv_sta;
441         struct iwl_priv *priv __maybe_unused = rs_sta->priv;
442
443         psta->rs_sta = NULL;
444
445         IWL_DEBUG_RATE(priv, "enter\n");
446         del_timer_sync(&rs_sta->rate_scale_flush);
447         kfree(rs_sta);
448         IWL_DEBUG_RATE(priv, "leave\n");
449 }
450
451
452 /**
453  * rs_tx_status - Update rate control values based on Tx results
454  *
455  * NOTE: Uses iwl_priv->retry_rate for the # of retries attempted by
456  * the hardware for each rate.
457  */
458 static void rs_tx_status(void *priv_rate, struct ieee80211_supported_band *sband,
459                          struct ieee80211_sta *sta, void *priv_sta,
460                          struct sk_buff *skb)
461 {
462         s8 retries = 0, current_count;
463         int scale_rate_index, first_index, last_index;
464         unsigned long flags;
465         struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
466         struct iwl3945_rs_sta *rs_sta = priv_sta;
467         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
468
469         IWL_DEBUG_RATE(priv, "enter\n");
470
471         retries = info->status.rates[0].count;
472
473         first_index = sband->bitrates[info->status.rates[0].idx].hw_value;
474         if ((first_index < 0) || (first_index >= IWL_RATE_COUNT_3945)) {
475                 IWL_DEBUG_RATE(priv, "leave: Rate out of bounds: %d\n", first_index);
476                 return;
477         }
478
479         if (!priv_sta) {
480                 IWL_DEBUG_RATE(priv, "leave: No STA priv data to update!\n");
481                 return;
482         }
483
484         rs_sta->tx_packets++;
485
486         scale_rate_index = first_index;
487         last_index = first_index;
488
489         /*
490          * Update the window for each rate.  We determine which rates
491          * were Tx'd based on the total number of retries vs. the number
492          * of retries configured for each rate -- currently set to the
493          * priv value 'retry_rate' vs. rate specific
494          *
495          * On exit from this while loop last_index indicates the rate
496          * at which the frame was finally transmitted (or failed if no
497          * ACK)
498          */
499         while (retries > 1) {
500                 if ((retries - 1) < priv->retry_rate) {
501                         current_count = (retries - 1);
502                         last_index = scale_rate_index;
503                 } else {
504                         current_count = priv->retry_rate;
505                         last_index = iwl3945_rs_next_rate(priv,
506                                                          scale_rate_index);
507                 }
508
509                 /* Update this rate accounting for as many retries
510                  * as was used for it (per current_count) */
511                 iwl3945_collect_tx_data(rs_sta,
512                                     &rs_sta->win[scale_rate_index],
513                                     0, current_count, scale_rate_index);
514                 IWL_DEBUG_RATE(priv, "Update rate %d for %d retries.\n",
515                                scale_rate_index, current_count);
516
517                 retries -= current_count;
518
519                 scale_rate_index = last_index;
520         }
521
522
523         /* Update the last index window with success/failure based on ACK */
524         IWL_DEBUG_RATE(priv, "Update rate %d with %s.\n",
525                        last_index,
526                        (info->flags & IEEE80211_TX_STAT_ACK) ?
527                        "success" : "failure");
528         iwl3945_collect_tx_data(rs_sta,
529                             &rs_sta->win[last_index],
530                             info->flags & IEEE80211_TX_STAT_ACK, 1, last_index);
531
532         /* We updated the rate scale window -- if its been more than
533          * flush_time since the last run, schedule the flush
534          * again */
535         spin_lock_irqsave(&rs_sta->lock, flags);
536
537         if (!rs_sta->flush_pending &&
538             time_after(jiffies, rs_sta->last_flush +
539                        rs_sta->flush_time)) {
540
541                 rs_sta->last_partial_flush = jiffies;
542                 rs_sta->flush_pending = 1;
543                 mod_timer(&rs_sta->rate_scale_flush,
544                           jiffies + rs_sta->flush_time);
545         }
546
547         spin_unlock_irqrestore(&rs_sta->lock, flags);
548
549         IWL_DEBUG_RATE(priv, "leave\n");
550
551         return;
552 }
553
554 static u16 iwl3945_get_adjacent_rate(struct iwl3945_rs_sta *rs_sta,
555                                  u8 index, u16 rate_mask, enum ieee80211_band band)
556 {
557         u8 high = IWL_RATE_INVALID;
558         u8 low = IWL_RATE_INVALID;
559         struct iwl_priv *priv __maybe_unused = rs_sta->priv;
560
561         /* 802.11A walks to the next literal adjacent rate in
562          * the rate table */
563         if (unlikely(band == IEEE80211_BAND_5GHZ)) {
564                 int i;
565                 u32 mask;
566
567                 /* Find the previous rate that is in the rate mask */
568                 i = index - 1;
569                 for (mask = (1 << i); i >= 0; i--, mask >>= 1) {
570                         if (rate_mask & mask) {
571                                 low = i;
572                                 break;
573                         }
574                 }
575
576                 /* Find the next rate that is in the rate mask */
577                 i = index + 1;
578                 for (mask = (1 << i); i < IWL_RATE_COUNT_3945;
579                      i++, mask <<= 1) {
580                         if (rate_mask & mask) {
581                                 high = i;
582                                 break;
583                         }
584                 }
585
586                 return (high << 8) | low;
587         }
588
589         low = index;
590         while (low != IWL_RATE_INVALID) {
591                 if (rs_sta->tgg)
592                         low = iwl3945_rates[low].prev_rs_tgg;
593                 else
594                         low = iwl3945_rates[low].prev_rs;
595                 if (low == IWL_RATE_INVALID)
596                         break;
597                 if (rate_mask & (1 << low))
598                         break;
599                 IWL_DEBUG_RATE(priv, "Skipping masked lower rate: %d\n", low);
600         }
601
602         high = index;
603         while (high != IWL_RATE_INVALID) {
604                 if (rs_sta->tgg)
605                         high = iwl3945_rates[high].next_rs_tgg;
606                 else
607                         high = iwl3945_rates[high].next_rs;
608                 if (high == IWL_RATE_INVALID)
609                         break;
610                 if (rate_mask & (1 << high))
611                         break;
612                 IWL_DEBUG_RATE(priv, "Skipping masked higher rate: %d\n", high);
613         }
614
615         return (high << 8) | low;
616 }
617
618 /**
619  * rs_get_rate - find the rate for the requested packet
620  *
621  * Returns the ieee80211_rate structure allocated by the driver.
622  *
623  * The rate control algorithm has no internal mapping between hw_mode's
624  * rate ordering and the rate ordering used by the rate control algorithm.
625  *
626  * The rate control algorithm uses a single table of rates that goes across
627  * the entire A/B/G spectrum vs. being limited to just one particular
628  * hw_mode.
629  *
630  * As such, we can't convert the index obtained below into the hw_mode's
631  * rate table and must reference the driver allocated rate table
632  *
633  */
634 static void rs_get_rate(void *priv_r, struct ieee80211_sta *sta,
635                         void *priv_sta, struct ieee80211_tx_rate_control *txrc)
636 {
637         struct ieee80211_supported_band *sband = txrc->sband;
638         struct sk_buff *skb = txrc->skb;
639         u8 low = IWL_RATE_INVALID;
640         u8 high = IWL_RATE_INVALID;
641         u16 high_low;
642         int index;
643         struct iwl3945_rs_sta *rs_sta = priv_sta;
644         struct iwl3945_rate_scale_data *window = NULL;
645         int current_tpt = IWL_INVALID_VALUE;
646         int low_tpt = IWL_INVALID_VALUE;
647         int high_tpt = IWL_INVALID_VALUE;
648         u32 fail_count;
649         s8 scale_action = 0;
650         unsigned long flags;
651         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
652         u16 fc;
653         u16 rate_mask = 0;
654         s8 max_rate_idx = -1;
655         struct iwl_priv *priv = (struct iwl_priv *)priv_r;
656         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
657
658         IWL_DEBUG_RATE(priv, "enter\n");
659
660         if (sta)
661                 rate_mask = sta->supp_rates[sband->band];
662
663         /* Send management frames and broadcast/multicast data using lowest
664          * rate. */
665         fc = le16_to_cpu(hdr->frame_control);
666         if ((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA ||
667             is_multicast_ether_addr(hdr->addr1) ||
668             !sta || !priv_sta) {
669                 IWL_DEBUG_RATE(priv, "leave: No STA priv data to update!\n");
670                 if (!rate_mask)
671                         info->control.rates[0].idx =
672                                         rate_lowest_index(sband, NULL);
673                 else
674                         info->control.rates[0].idx =
675                                         rate_lowest_index(sband, sta);
676                 return;
677         }
678
679         /* get user max rate if set */
680         max_rate_idx = txrc->max_rate_idx;
681         if ((sband->band == IEEE80211_BAND_5GHZ) && (max_rate_idx != -1))
682                 max_rate_idx += IWL_FIRST_OFDM_RATE;
683         if ((max_rate_idx < 0) || (max_rate_idx >= IWL_RATE_COUNT))
684                 max_rate_idx = -1;
685
686         index = min(rs_sta->last_txrate_idx & 0xffff, IWL_RATE_COUNT_3945 - 1);
687
688         if (sband->band == IEEE80211_BAND_5GHZ)
689                 rate_mask = rate_mask << IWL_FIRST_OFDM_RATE;
690
691         if ((priv->iw_mode == NL80211_IFTYPE_ADHOC) &&
692             !rs_sta->ibss_sta_added) {
693                 u8 sta_id = iwl3945_hw_find_station(priv, hdr->addr1);
694
695                 if (sta_id == IWL_INVALID_STATION) {
696                         IWL_DEBUG_RATE(priv, "LQ: ADD station %pm\n",
697                                        hdr->addr1);
698                         sta_id = iwl3945_add_station(priv,
699                                     hdr->addr1, 0, CMD_ASYNC);
700                 }
701                 if (sta_id != IWL_INVALID_STATION)
702                         rs_sta->ibss_sta_added = 1;
703         }
704
705         spin_lock_irqsave(&rs_sta->lock, flags);
706
707         /* for recent assoc, choose best rate regarding
708          * to rssi value
709          */
710         if (rs_sta->start_rate != IWL_RATE_INVALID) {
711                 if (rs_sta->start_rate < index &&
712                    (rate_mask & (1 << rs_sta->start_rate)))
713                         index = rs_sta->start_rate;
714                 rs_sta->start_rate = IWL_RATE_INVALID;
715         }
716
717         /* force user max rate if set by user */
718         if ((max_rate_idx != -1) && (max_rate_idx < index)) {
719                 if (rate_mask & (1 << max_rate_idx))
720                         index = max_rate_idx;
721         }
722
723         window = &(rs_sta->win[index]);
724
725         fail_count = window->counter - window->success_counter;
726
727         if (((fail_count <= IWL_RATE_MIN_FAILURE_TH) &&
728              (window->success_counter < IWL_RATE_MIN_SUCCESS_TH))) {
729                 spin_unlock_irqrestore(&rs_sta->lock, flags);
730
731                 IWL_DEBUG_RATE(priv, "Invalid average_tpt on rate %d: "
732                                "counter: %d, success_counter: %d, "
733                                "expected_tpt is %sNULL\n",
734                                index,
735                                window->counter,
736                                window->success_counter,
737                                rs_sta->expected_tpt ? "not " : "");
738                 goto out;
739
740         }
741
742         current_tpt = window->average_tpt;
743
744         high_low = iwl3945_get_adjacent_rate(rs_sta, index, rate_mask,
745                                              sband->band);
746         low = high_low & 0xff;
747         high = (high_low >> 8) & 0xff;
748
749         /* If user set max rate, dont allow higher than user constrain */
750         if ((max_rate_idx != -1) && (max_rate_idx < high))
751                 high = IWL_RATE_INVALID;
752
753         if (low != IWL_RATE_INVALID)
754                 low_tpt = rs_sta->win[low].average_tpt;
755
756         if (high != IWL_RATE_INVALID)
757                 high_tpt = rs_sta->win[high].average_tpt;
758
759         spin_unlock_irqrestore(&rs_sta->lock, flags);
760
761         scale_action = 1;
762
763         if ((window->success_ratio < IWL_RATE_DECREASE_TH) || !current_tpt) {
764                 IWL_DEBUG_RATE(priv, "decrease rate because of low success_ratio\n");
765                 scale_action = -1;
766         } else if ((low_tpt == IWL_INVALID_VALUE) &&
767                    (high_tpt == IWL_INVALID_VALUE))
768                 scale_action = 1;
769         else if ((low_tpt != IWL_INVALID_VALUE) &&
770                  (high_tpt != IWL_INVALID_VALUE) &&
771                  (low_tpt < current_tpt) && (high_tpt < current_tpt)) {
772                 IWL_DEBUG_RATE(priv, "No action -- low [%d] & high [%d] < "
773                                "current_tpt [%d]\n",
774                                low_tpt, high_tpt, current_tpt);
775                 scale_action = 0;
776         } else {
777                 if (high_tpt != IWL_INVALID_VALUE) {
778                         if (high_tpt > current_tpt)
779                                 scale_action = 1;
780                         else {
781                                 IWL_DEBUG_RATE(priv,
782                                     "decrease rate because of high tpt\n");
783                                 scale_action = -1;
784                         }
785                 } else if (low_tpt != IWL_INVALID_VALUE) {
786                         if (low_tpt > current_tpt) {
787                                 IWL_DEBUG_RATE(priv,
788                                     "decrease rate because of low tpt\n");
789                                 scale_action = -1;
790                         } else
791                                 scale_action = 1;
792                 }
793         }
794
795         if (scale_action == -1) {
796                 if (window->success_ratio > IWL_SUCCESS_DOWN_TH)
797                         scale_action = 0;
798         } else if (scale_action == 1) {
799                 if (window->success_ratio < IWL_SUCCESS_UP_TH) {
800                         IWL_DEBUG_RATE(priv, "No action -- success_ratio [%d] < "
801                                "SUCCESS UP\n", window->success_ratio);
802                         scale_action = 0;
803                 }
804         }
805
806         switch (scale_action) {
807         case -1:
808                 if (low != IWL_RATE_INVALID)
809                         index = low;
810                 break;
811
812         case 1:
813                 if (high != IWL_RATE_INVALID)
814                         index = high;
815
816                 break;
817
818         case 0:
819         default:
820                 break;
821         }
822
823         IWL_DEBUG_RATE(priv, "Selected %d (action %d) - low %d high %d\n",
824                        index, scale_action, low, high);
825
826  out:
827
828         rs_sta->last_txrate_idx = index;
829         if (sband->band == IEEE80211_BAND_5GHZ)
830                 info->control.rates[0].idx = rs_sta->last_txrate_idx -
831                                 IWL_FIRST_OFDM_RATE;
832         else
833                 info->control.rates[0].idx = rs_sta->last_txrate_idx;
834
835         IWL_DEBUG_RATE(priv, "leave: %d\n", index);
836 }
837
838 #ifdef CONFIG_MAC80211_DEBUGFS
839 static int iwl3945_open_file_generic(struct inode *inode, struct file *file)
840 {
841         file->private_data = inode->i_private;
842         return 0;
843 }
844
845 static ssize_t iwl3945_sta_dbgfs_stats_table_read(struct file *file,
846                                                   char __user *user_buf,
847                                                   size_t count, loff_t *ppos)
848 {
849         char buff[1024];
850         int desc = 0;
851         int j;
852         struct iwl3945_rs_sta *lq_sta = file->private_data;
853
854         desc += sprintf(buff + desc, "tx packets=%d last rate index=%d\n"
855                         "rate=0x%X flush time %d\n",
856                         lq_sta->tx_packets,
857                         lq_sta->last_txrate_idx,
858                         lq_sta->start_rate, jiffies_to_msecs(lq_sta->flush_time));
859         for (j = 0; j < IWL_RATE_COUNT_3945; j++) {
860                 desc += sprintf(buff+desc,
861                                 "counter=%d success=%d %%=%d\n",
862                                 lq_sta->win[j].counter,
863                                 lq_sta->win[j].success_counter,
864                                 lq_sta->win[j].success_ratio);
865         }
866         return simple_read_from_buffer(user_buf, count, ppos, buff, desc);
867 }
868
869 static const struct file_operations rs_sta_dbgfs_stats_table_ops = {
870         .read = iwl3945_sta_dbgfs_stats_table_read,
871         .open = iwl3945_open_file_generic,
872 };
873
874 static void iwl3945_add_debugfs(void *priv, void *priv_sta,
875                                 struct dentry *dir)
876 {
877         struct iwl3945_rs_sta *lq_sta = priv_sta;
878
879         lq_sta->rs_sta_dbgfs_stats_table_file =
880                 debugfs_create_file("rate_stats_table", 0600, dir,
881                 lq_sta, &rs_sta_dbgfs_stats_table_ops);
882
883 }
884
885 static void iwl3945_remove_debugfs(void *priv, void *priv_sta)
886 {
887         struct iwl3945_rs_sta *lq_sta = priv_sta;
888         debugfs_remove(lq_sta->rs_sta_dbgfs_stats_table_file);
889 }
890 #endif
891
892 static struct rate_control_ops rs_ops = {
893         .module = NULL,
894         .name = RS_NAME,
895         .tx_status = rs_tx_status,
896         .get_rate = rs_get_rate,
897         .rate_init = rs_rate_init,
898         .alloc = rs_alloc,
899         .free = rs_free,
900         .alloc_sta = rs_alloc_sta,
901         .free_sta = rs_free_sta,
902 #ifdef CONFIG_MAC80211_DEBUGFS
903         .add_sta_debugfs = iwl3945_add_debugfs,
904         .remove_sta_debugfs = iwl3945_remove_debugfs,
905 #endif
906
907 };
908
909 void iwl3945_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id)
910 {
911         struct iwl_priv *priv = hw->priv;
912         s32 rssi = 0;
913         unsigned long flags;
914         struct iwl3945_rs_sta *rs_sta;
915         struct ieee80211_sta *sta;
916         struct iwl3945_sta_priv *psta;
917
918         IWL_DEBUG_RATE(priv, "enter\n");
919
920         rcu_read_lock();
921
922         sta = ieee80211_find_sta(hw, priv->stations_39[sta_id].sta.sta.addr);
923         if (!sta) {
924                 rcu_read_unlock();
925                 return;
926         }
927
928         psta = (void *) sta->drv_priv;
929         rs_sta = psta->rs_sta;
930
931         spin_lock_irqsave(&rs_sta->lock, flags);
932
933         rs_sta->tgg = 0;
934         switch (priv->band) {
935         case IEEE80211_BAND_2GHZ:
936                 /* TODO: this always does G, not a regression */
937                 if (priv->active_rxon.flags & RXON_FLG_TGG_PROTECT_MSK) {
938                         rs_sta->tgg = 1;
939                         rs_sta->expected_tpt = iwl3945_expected_tpt_g_prot;
940                 } else
941                         rs_sta->expected_tpt = iwl3945_expected_tpt_g;
942                 break;
943
944         case IEEE80211_BAND_5GHZ:
945                 rs_sta->expected_tpt = iwl3945_expected_tpt_a;
946                 break;
947         case IEEE80211_NUM_BANDS:
948                 BUG();
949                 break;
950         }
951
952         spin_unlock_irqrestore(&rs_sta->lock, flags);
953
954         rssi = priv->last_rx_rssi;
955         if (rssi == 0)
956                 rssi = IWL_MIN_RSSI_VAL;
957
958         IWL_DEBUG_RATE(priv, "Network RSSI: %d\n", rssi);
959
960         rs_sta->start_rate = iwl3945_get_rate_index_by_rssi(rssi, priv->band);
961
962         IWL_DEBUG_RATE(priv, "leave: rssi %d assign rate index: "
963                        "%d (plcp 0x%x)\n", rssi, rs_sta->start_rate,
964                        iwl3945_rates[rs_sta->start_rate].plcp);
965         rcu_read_unlock();
966 }
967
968 int iwl3945_rate_control_register(void)
969 {
970         return ieee80211_rate_control_register(&rs_ops);
971 }
972
973 void iwl3945_rate_control_unregister(void)
974 {
975         ieee80211_rate_control_unregister(&rs_ops);
976 }
977
978