pcu.c
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------------
00002  *-------------------------        ATH5K Driver          -----------------------
00003  *------------------------------------------------------------------------------
00004  *                                                           V1.0  08/02/2010
00005  *
00006  *
00007  *  Feb 2010 - Samuel Cabrero <samuelcabrero@gmail.com>
00008  *              Initial release
00009  *
00010  *  ----------------------------------------------------------------------------
00011  *  Copyright (C) 2000-2010, Universidad de Zaragoza, SPAIN
00012  *
00013  *  Autors:
00014  *              Samuel Cabrero        <samuelcabrero@gmail.com>
00015  *              Danilo Tardioli       <dantard@unizar.es>
00016  *              Jose Luis Villarroel  <jlvilla@unizar.es>
00017  *
00018  *  This is a simplified version of the original ath5k driver. It should work 
00019  *  with all Atheros 5xxx WLAN cards. The 802.11 layer have been removed so it
00020  *  just send and receive frames over the air, as if it were an Ethernet bus
00021  *  interface.
00022  *
00023  *  Please read ath5k_interface.h for instructions.
00024  *
00025  *  This program is distributed under the terms of GPL version 2 and in the 
00026  *  hope that it will be useful, but WITHOUT ANY WARRANTY; without even the 
00027  *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
00028  *  See the GNU General Public License for more details.
00029  *
00030  *----------------------------------------------------------------------------*/
00031 
00032 /*
00033  * Copyright (c) 2004-2008 Reyk Floeter <reyk@openbsd.org>
00034  * Copyright (c) 2006-2008 Nick Kossifidis <mickflemm@gmail.com>
00035  * Copyright (c) 2007-2008 Matthew W. S. Bell  <mentor@madwifi.org>
00036  * Copyright (c) 2007-2008 Luis Rodriguez <mcgrof@winlab.rutgers.edu>
00037  * Copyright (c) 2007-2008 Pavel Roskin <proski@gnu.org>
00038  * Copyright (c) 2007-2008 Jiri Slaby <jirislaby@gmail.com>
00039  *
00040  * Permission to use, copy, modify, and distribute this software for any
00041  * purpose with or without fee is hereby granted, provided that the above
00042  * copyright notice and this permission notice appear in all copies.
00043  *
00044  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
00045  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
00046  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
00047  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
00048  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
00049  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
00050  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
00051  *
00052  */
00053 
00054 /*********************************\
00055 * Protocol Control Unit Functions *
00056 \*********************************/
00057 
00058 #include "ath5k.h"
00059 #include "reg.h"
00060 #include "debug.h"
00061 #include "base.h"
00062 
00063 /*******************\
00064 * Generic functions *
00065 \*******************/
00066 
00076 int ath5k_hw_set_opmode(struct ath5k_hw *ah)
00077 {
00078         u32 pcu_reg, beacon_reg, low_id, high_id;
00079 
00080 
00081         /* Preserve rest settings */
00082         pcu_reg = ath5k_hw_reg_read(ah, AR5K_STA_ID1) & 0xffff0000;
00083         pcu_reg &= ~(AR5K_STA_ID1_ADHOC | AR5K_STA_ID1_AP
00084                         | AR5K_STA_ID1_KEYSRCH_MODE
00085                         | (ah->ah_version == AR5K_AR5210 ?
00086                         (AR5K_STA_ID1_PWR_SV | AR5K_STA_ID1_NO_PSPOLL) : 0));
00087 
00088         beacon_reg = 0;
00089 
00090         ATH5K_TRACE(ah->ah_sc);
00091 
00092         /* Modo monitor */
00093         pcu_reg |= AR5K_STA_ID1_NO_KEYSRCH
00094                 | (ah->ah_version == AR5K_AR5210 ?
00095                         AR5K_STA_ID1_NO_PSPOLL : 0);
00096 
00097         /*
00098          * Set PCU registers
00099          */
00100         low_id = AR5K_LOW_ID(ah->ah_sta_id);
00101         high_id = AR5K_HIGH_ID(ah->ah_sta_id);
00102         ath5k_hw_reg_write(ah, low_id, AR5K_STA_ID0);
00103         ath5k_hw_reg_write(ah, pcu_reg | high_id, AR5K_STA_ID1);
00104 
00105         /*
00106          * Set Beacon Control Register on 5210
00107          */
00108         if (ah->ah_version == AR5K_AR5210)
00109                 ath5k_hw_reg_write(ah, beacon_reg, AR5K_BCR);
00110 
00111         return 0;
00112 }
00113 
00124 void ath5k_hw_update_mib_counters(struct ath5k_hw *ah,
00125                 struct ieee80211_low_level_stats  *stats)
00126 {
00127         ATH5K_TRACE(ah->ah_sc);
00128 
00129         /* Read-And-Clear */
00130         stats->dot11ACKFailureCount += ath5k_hw_reg_read(ah, AR5K_ACK_FAIL);
00131         stats->dot11RTSFailureCount += ath5k_hw_reg_read(ah, AR5K_RTS_FAIL);
00132         stats->dot11RTSSuccessCount += ath5k_hw_reg_read(ah, AR5K_RTS_OK);
00133         stats->dot11FCSErrorCount += ath5k_hw_reg_read(ah, AR5K_FCS_FAIL);
00134 
00135         /* XXX: Should we use this to track beacon count ?
00136          * -we read it anyway to clear the register */
00137         ath5k_hw_reg_read(ah, AR5K_BEACON_CNT);
00138 
00139         /* Reset profile count registers on 5212*/
00140         if (ah->ah_version == AR5K_AR5212) {
00141                 ath5k_hw_reg_write(ah, 0, AR5K_PROFCNT_TX);
00142                 ath5k_hw_reg_write(ah, 0, AR5K_PROFCNT_RX);
00143                 ath5k_hw_reg_write(ah, 0, AR5K_PROFCNT_RXCLR);
00144                 ath5k_hw_reg_write(ah, 0, AR5K_PROFCNT_CYCLE);
00145         }
00146 
00147         /* TODO: Handle ANI stats */
00148 }
00149 
00162 void ath5k_hw_set_ack_bitrate_high(struct ath5k_hw *ah, bool high)
00163 {
00164         if (ah->ah_version != AR5K_AR5212)
00165                 return;
00166         else {
00167                 u32 val = AR5K_STA_ID1_BASE_RATE_11B | AR5K_STA_ID1_ACKCTS_6MB;
00168                 if (high)
00169                         AR5K_REG_ENABLE_BITS(ah, AR5K_STA_ID1, val);
00170                 else
00171                         AR5K_REG_DISABLE_BITS(ah, AR5K_STA_ID1, val);
00172         }
00173 }
00174 
00175 
00176 /******************\
00177 * ACK/CTS Timeouts *
00178 \******************/
00179 
00185 unsigned int ath5k_hw_get_ack_timeout(struct ath5k_hw *ah)
00186 {
00187         ATH5K_TRACE(ah->ah_sc);
00188 
00189         return ath5k_hw_clocktoh(AR5K_REG_MS(ath5k_hw_reg_read(ah,
00190                         AR5K_TIME_OUT), AR5K_TIME_OUT_ACK), ah->ah_turbo);
00191 }
00192 
00199 int ath5k_hw_set_ack_timeout(struct ath5k_hw *ah, unsigned int timeout)
00200 {
00201         ATH5K_TRACE(ah->ah_sc);
00202         if (ath5k_hw_clocktoh(AR5K_REG_MS(0xffffffff, AR5K_TIME_OUT_ACK),
00203                         ah->ah_turbo) <= timeout)
00204                 return -EINVAL;
00205 
00206         AR5K_REG_WRITE_BITS(ah, AR5K_TIME_OUT, AR5K_TIME_OUT_ACK,
00207                 ath5k_hw_htoclock(timeout, ah->ah_turbo));
00208 
00209         return 0;
00210 }
00211 
00217 unsigned int ath5k_hw_get_cts_timeout(struct ath5k_hw *ah)
00218 {
00219         ATH5K_TRACE(ah->ah_sc);
00220         return ath5k_hw_clocktoh(AR5K_REG_MS(ath5k_hw_reg_read(ah,
00221                         AR5K_TIME_OUT), AR5K_TIME_OUT_CTS), ah->ah_turbo);
00222 }
00223 
00230 int ath5k_hw_set_cts_timeout(struct ath5k_hw *ah, unsigned int timeout)
00231 {
00232         ATH5K_TRACE(ah->ah_sc);
00233         if (ath5k_hw_clocktoh(AR5K_REG_MS(0xffffffff, AR5K_TIME_OUT_CTS),
00234                         ah->ah_turbo) <= timeout)
00235                 return -EINVAL;
00236 
00237         AR5K_REG_WRITE_BITS(ah, AR5K_TIME_OUT, AR5K_TIME_OUT_CTS,
00238                         ath5k_hw_htoclock(timeout, ah->ah_turbo));
00239 
00240         return 0;
00241 }
00242 
00243 
00244 /****************\
00245 * BSSID handling *
00246 \****************/
00247 
00259 void ath5k_hw_get_lladdr(struct ath5k_hw *ah, u8 *mac)
00260 {
00261         ATH5K_TRACE(ah->ah_sc);
00262         memcpy(mac, ah->ah_sta_id, ETH_ALEN);
00263 }
00264 
00273 int ath5k_hw_set_lladdr(struct ath5k_hw *ah, const u8 *mac)
00274 {
00275         u32 low_id, high_id;
00276         u32 pcu_reg;
00277 
00278         ATH5K_TRACE(ah->ah_sc);
00279         /* Set new station ID */
00280         memcpy(ah->ah_sta_id, mac, ETH_ALEN);
00281 
00282         pcu_reg = ath5k_hw_reg_read(ah, AR5K_STA_ID1) & 0xffff0000;
00283 
00284         low_id = AR5K_LOW_ID(mac);
00285         high_id = AR5K_HIGH_ID(mac);
00286 
00287         ath5k_hw_reg_write(ah, low_id, AR5K_STA_ID0);
00288         ath5k_hw_reg_write(ah, pcu_reg | high_id, AR5K_STA_ID1);
00289 
00290         return 0;
00291 }
00292 
00302 void ath5k_hw_set_associd(struct ath5k_hw *ah, const u8 *bssid, u16 assoc_id)
00303 {
00304         u32 low_id, high_id;
00305         u16 tim_offset = 0;
00306 
00307         /*
00308          * Set simple BSSID mask on 5212
00309          */
00310         if (ah->ah_version == AR5K_AR5212) {
00311                 ath5k_hw_reg_write(ah, AR5K_LOW_ID(ah->ah_bssid_mask),
00312                                                         AR5K_BSS_IDM0);
00313                 ath5k_hw_reg_write(ah, AR5K_HIGH_ID(ah->ah_bssid_mask),
00314                                                         AR5K_BSS_IDM1);
00315         }
00316 
00317         /*
00318          * Set BSSID which triggers the "SME Join" operation
00319          */
00320         low_id = AR5K_LOW_ID(bssid);
00321         high_id = AR5K_HIGH_ID(bssid);
00322         ath5k_hw_reg_write(ah, low_id, AR5K_BSS_ID0);
00323         ath5k_hw_reg_write(ah, high_id | ((assoc_id & 0x3fff) <<
00324                                 AR5K_BSS_ID1_AID_S), AR5K_BSS_ID1);
00325 
00326         if (assoc_id == 0) {
00327                 ath5k_hw_disable_pspoll(ah);
00328                 return;
00329         }
00330 
00331         AR5K_REG_WRITE_BITS(ah, AR5K_BEACON, AR5K_BEACON_TIM,
00332                         tim_offset ? tim_offset + 4 : 0);
00333 
00334         ath5k_hw_enable_pspoll(ah, NULL, 0);
00335 }
00336 
00363 /*
00364  * Simple example: on your card you have have two BSSes you have created with
00365  * BSSID-01 and BSSID-02. Lets assume BSSID-01 will not use the MAC address.
00366  * There is another BSSID-03 but you are not part of it. For simplicity's sake,
00367  * assuming only 4 bits for a mac address and for BSSIDs you can then have:
00368  *
00369  *                  \
00370  * MAC:                0001 |
00371  * BSSID-01:   0100 | --> Belongs to us
00372  * BSSID-02:   1001 |
00373  *                  /
00374  * -------------------
00375  * BSSID-03:   0110  | --> External
00376  * -------------------
00377  *
00378  * Our bssid_mask would then be:
00379  *
00380  *             On loop iteration for BSSID-01:
00381  *             ~(0001 ^ 0100)  -> ~(0101)
00382  *                             ->   1010
00383  *             bssid_mask      =    1010
00384  *
00385  *             On loop iteration for BSSID-02:
00386  *             bssid_mask &= ~(0001   ^   1001)
00387  *             bssid_mask =   (1010)  & ~(0001 ^ 1001)
00388  *             bssid_mask =   (1010)  & ~(1001)
00389  *             bssid_mask =   (1010)  &  (0110)
00390  *             bssid_mask =   0010
00391  *
00392  * A bssid_mask of 0010 means "only pay attention to the second least
00393  * significant bit". This is because its the only bit common
00394  * amongst the MAC and all BSSIDs we support. To findout what the real
00395  * common bit is we can simply "&" the bssid_mask now with any BSSID we have
00396  * or our MAC address (we assume the hardware uses the MAC address).
00397  *
00398  * Now, suppose there's an incoming frame for BSSID-03:
00399  *
00400  * IFRAME-01:  0110
00401  *
00402  * An easy eye-inspeciton of this already should tell you that this frame
00403  * will not pass our check. This is beacuse the bssid_mask tells the
00404  * hardware to only look at the second least significant bit and the
00405  * common bit amongst the MAC and BSSIDs is 0, this frame has the 2nd LSB
00406  * as 1, which does not match 0.
00407  *
00408  * So with IFRAME-01 we *assume* the hardware will do:
00409  *
00410  *     allow = (IFRAME-01 & bssid_mask) == (bssid_mask & MAC) ? 1 : 0;
00411  *  --> allow = (0110 & 0010) == (0010 & 0001) ? 1 : 0;
00412  *  --> allow = (0010) == 0000 ? 1 : 0;
00413  *  --> allow = 0
00414  *
00415  *  Lets now test a frame that should work:
00416  *
00417  * IFRAME-02:  0001 (we should allow)
00418  *
00419  *     allow = (0001 & 1010) == 1010
00420  *
00421  *     allow = (IFRAME-02 & bssid_mask) == (bssid_mask & MAC) ? 1 : 0;
00422  *  --> allow = (0001 & 0010) ==  (0010 & 0001) ? 1 :0;
00423  *  --> allow = (0010) == (0010)
00424  *  --> allow = 1
00425  *
00426  * Other examples:
00427  *
00428  * IFRAME-03:  0100 --> allowed
00429  * IFRAME-04:  1001 --> allowed
00430  * IFRAME-05:  1101 --> allowed but its not for us!!!
00431  *
00432  */
00433 int ath5k_hw_set_bssid_mask(struct ath5k_hw *ah, const u8 *mask)
00434 {
00435         u32 low_id, high_id;
00436         ATH5K_TRACE(ah->ah_sc);
00437 
00438         /* Cache bssid mask so that we can restore it
00439          * on reset */
00440         memcpy(ah->ah_bssid_mask, mask, ETH_ALEN);
00441         if (ah->ah_version == AR5K_AR5212) {
00442                 low_id = AR5K_LOW_ID(mask);
00443                 high_id = AR5K_HIGH_ID(mask);
00444 
00445                 ath5k_hw_reg_write(ah, low_id, AR5K_BSS_IDM0);
00446                 ath5k_hw_reg_write(ah, high_id, AR5K_BSS_IDM1);
00447 
00448                 return 0;
00449         }
00450 
00451         return -EIO;
00452 }
00453 
00454 
00455 /************\
00456 * RX Control *
00457 \************/
00458 
00470 void ath5k_hw_start_rx_pcu(struct ath5k_hw *ah)
00471 {
00472         ATH5K_TRACE(ah->ah_sc);
00473         AR5K_REG_DISABLE_BITS(ah, AR5K_DIAG_SW, AR5K_DIAG_SW_DIS_RX);
00474 }
00475 
00485 void ath5k_hw_stop_rx_pcu(struct ath5k_hw *ah)
00486 {
00487         ATH5K_TRACE(ah->ah_sc);
00488         AR5K_REG_ENABLE_BITS(ah, AR5K_DIAG_SW, AR5K_DIAG_SW_DIS_RX);
00489 }
00490 
00491 /*
00492  * Set multicast filter
00493  */
00494 void ath5k_hw_set_mcast_filter(struct ath5k_hw *ah, u32 filter0, u32 filter1)
00495 {
00496         ATH5K_TRACE(ah->ah_sc);
00497         /* Set the multicat filter */
00498         ath5k_hw_reg_write(ah, filter0, AR5K_MCAST_FILTER0);
00499         ath5k_hw_reg_write(ah, filter1, AR5K_MCAST_FILTER1);
00500 }
00501 
00502 /*
00503  * Set multicast filter by index
00504  */
00505 int ath5k_hw_set_mcast_filter_idx(struct ath5k_hw *ah, u32 index)
00506 {
00507 
00508         ATH5K_TRACE(ah->ah_sc);
00509         if (index >= 64)
00510                 return -EINVAL;
00511         else if (index >= 32)
00512                 AR5K_REG_ENABLE_BITS(ah, AR5K_MCAST_FILTER1,
00513                                 (1 << (index - 32)));
00514         else
00515                 AR5K_REG_ENABLE_BITS(ah, AR5K_MCAST_FILTER0, (1 << index));
00516 
00517         return 0;
00518 }
00519 
00520 /*
00521  * Clear Multicast filter by index
00522  */
00523 int ath5k_hw_clear_mcast_filter_idx(struct ath5k_hw *ah, u32 index)
00524 {
00525 
00526         ATH5K_TRACE(ah->ah_sc);
00527         if (index >= 64)
00528                 return -EINVAL;
00529         else if (index >= 32)
00530                 AR5K_REG_DISABLE_BITS(ah, AR5K_MCAST_FILTER1,
00531                                 (1 << (index - 32)));
00532         else
00533                 AR5K_REG_DISABLE_BITS(ah, AR5K_MCAST_FILTER0, (1 << index));
00534 
00535         return 0;
00536 }
00537 
00549 u32 ath5k_hw_get_rx_filter(struct ath5k_hw *ah)
00550 {
00551         u32 data, filter = 0;
00552 
00553         ATH5K_TRACE(ah->ah_sc);
00554         filter = ath5k_hw_reg_read(ah, AR5K_RX_FILTER);
00555 
00556         /*Radar detection for 5212*/
00557         if (ah->ah_version == AR5K_AR5212) {
00558                 data = ath5k_hw_reg_read(ah, AR5K_PHY_ERR_FIL);
00559 
00560                 if (data & AR5K_PHY_ERR_FIL_RADAR)
00561                         filter |= AR5K_RX_FILTER_RADARERR;
00562                 if (data & (AR5K_PHY_ERR_FIL_OFDM | AR5K_PHY_ERR_FIL_CCK))
00563                         filter |= AR5K_RX_FILTER_PHYERR;
00564         }
00565 
00566         return filter;
00567 }
00568 
00579 void ath5k_hw_set_rx_filter(struct ath5k_hw *ah, u32 filter)
00580 {
00581         u32 data = 0;
00582 
00583         ATH5K_TRACE(ah->ah_sc);
00584 
00585         /* Set PHY error filter register on 5212*/
00586         if (ah->ah_version == AR5K_AR5212) {
00587                 if (filter & AR5K_RX_FILTER_RADARERR)
00588                         data |= AR5K_PHY_ERR_FIL_RADAR;
00589                 if (filter & AR5K_RX_FILTER_PHYERR)
00590                         data |= AR5K_PHY_ERR_FIL_OFDM | AR5K_PHY_ERR_FIL_CCK;
00591         }
00592 
00593         /*
00594          * The AR5210 uses promiscous mode to detect radar activity
00595          */
00596         if (ah->ah_version == AR5K_AR5210 &&
00597                         (filter & AR5K_RX_FILTER_RADARERR)) {
00598                 filter &= ~AR5K_RX_FILTER_RADARERR;
00599                 filter |= AR5K_RX_FILTER_PROM;
00600         }
00601 
00602         /*Zero length DMA (phy error reporting) */
00603         if (data)
00604                 AR5K_REG_ENABLE_BITS(ah, AR5K_RXCFG, AR5K_RXCFG_ZLFDMA);
00605         else
00606                 AR5K_REG_DISABLE_BITS(ah, AR5K_RXCFG, AR5K_RXCFG_ZLFDMA);
00607 
00608         /*Write RX Filter register*/
00609         ath5k_hw_reg_write(ah, filter & 0xff, AR5K_RX_FILTER);
00610 
00611         /*Write PHY error filter register on 5212*/
00612         if (ah->ah_version == AR5K_AR5212)
00613                 ath5k_hw_reg_write(ah, data, AR5K_PHY_ERR_FIL);
00614 
00615 }
00616 


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Fri Jan 3 2014 12:07:55