dma.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  *
00036  * Permission to use, copy, modify, and distribute this software for any
00037  * purpose with or without fee is hereby granted, provided that the above
00038  * copyright notice and this permission notice appear in all copies.
00039  *
00040  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
00041  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
00042  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
00043  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
00044  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
00045  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
00046  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
00047  *
00048  */
00049 
00050 /*************************************\
00051 * DMA and interrupt masking functions *
00052 \*************************************/
00053 
00054 /*
00055  * dma.c - DMA and interrupt masking functions
00056  *
00057  * Here we setup descriptor pointers (rxdp/txdp) start/stop dma engine and
00058  * handle queue setup for 5210 chipset (rest are handled on qcu.c).
00059  * Also we setup interrupt mask register (IMR) and read the various iterrupt
00060  * status registers (ISR).
00061  *
00062  * TODO: Handle SISR on 5211+ and introduce a function to return the queue
00063  * number that resulted the interrupt.
00064  */
00065 
00066 #include "ath5k.h"
00067 #include "reg.h"
00068 #include "debug.h"
00069 #include "base.h"
00070 
00071 /*********\
00072 * Receive *
00073 \*********/
00074 
00080 void ath5k_hw_start_rx_dma(struct ath5k_hw *ah)
00081 {
00082         ATH5K_TRACE(ah->ah_sc);
00083         ath5k_hw_reg_write(ah, AR5K_CR_RXE, AR5K_CR);
00084         ath5k_hw_reg_read(ah, AR5K_CR);
00085 }
00086 
00092 int ath5k_hw_stop_rx_dma(struct ath5k_hw *ah)
00093 {
00094         unsigned int i;
00095 
00096         ATH5K_TRACE(ah->ah_sc);
00097         ath5k_hw_reg_write(ah, AR5K_CR_RXD, AR5K_CR);
00098 
00099         /*
00100          * It may take some time to disable the DMA receive unit
00101          */
00102         for (i = 1000; i > 0 &&
00103                         (ath5k_hw_reg_read(ah, AR5K_CR) & AR5K_CR_RXE) != 0;
00104                         i--)
00105                 udelay(10);
00106 
00107         return i ? 0 : -EBUSY;
00108 }
00109 
00115 u32 ath5k_hw_get_rxdp(struct ath5k_hw *ah)
00116 {
00117         return ath5k_hw_reg_read(ah, AR5K_RXDP);
00118 }
00119 
00128 void ath5k_hw_set_rxdp(struct ath5k_hw *ah, u32 phys_addr)
00129 {
00130         ATH5K_TRACE(ah->ah_sc);
00131 
00132         ath5k_hw_reg_write(ah, phys_addr, AR5K_RXDP);
00133 }
00134 
00135 
00136 /**********\
00137 * Transmit *
00138 \**********/
00139 
00155 int ath5k_hw_start_tx_dma(struct ath5k_hw *ah, unsigned int queue)
00156 {
00157         u32 tx_queue;
00158 
00159         ATH5K_TRACE(ah->ah_sc);
00160         AR5K_ASSERT_ENTRY(queue, ah->ah_capabilities.cap_queues.q_tx_num);
00161 
00162         /* Return if queue is declared inactive */
00163         if (ah->ah_txq[queue].tqi_type == AR5K_TX_QUEUE_INACTIVE)
00164                 return -EIO;
00165 
00166         if (ah->ah_version == AR5K_AR5210) {
00167                 tx_queue = ath5k_hw_reg_read(ah, AR5K_CR);
00168 
00169                 /*
00170                  * Set the queue by type on 5210
00171                  */
00172                 switch (ah->ah_txq[queue].tqi_type) {
00173                 case AR5K_TX_QUEUE_DATA:
00174                         tx_queue |= AR5K_CR_TXE0 & ~AR5K_CR_TXD0;
00175                         break;
00176                 case AR5K_TX_QUEUE_BEACON:
00177                         tx_queue |= AR5K_CR_TXE1 & ~AR5K_CR_TXD1;
00178                         ath5k_hw_reg_write(ah, AR5K_BCR_TQ1V | AR5K_BCR_BDMAE,
00179                                         AR5K_BSR);
00180                         break;
00181                 case AR5K_TX_QUEUE_CAB:
00182                         tx_queue |= AR5K_CR_TXE1 & ~AR5K_CR_TXD1;
00183                         ath5k_hw_reg_write(ah, AR5K_BCR_TQ1FV | AR5K_BCR_TQ1V |
00184                                 AR5K_BCR_BDMAE, AR5K_BSR);
00185                         break;
00186                 default:
00187                         return -EINVAL;
00188                 }
00189                 /* Start queue */
00190                 ath5k_hw_reg_write(ah, tx_queue, AR5K_CR);
00191                 ath5k_hw_reg_read(ah, AR5K_CR);
00192         } else {
00193                 /* Return if queue is disabled */
00194                 if (AR5K_REG_READ_Q(ah, AR5K_QCU_TXD, queue))
00195                         return -EIO;
00196 
00197                 /* Start queue */
00198                 AR5K_REG_WRITE_Q(ah, AR5K_QCU_TXE, queue);
00199         }
00200 
00201         return 0;
00202 }
00203 
00215 int ath5k_hw_stop_tx_dma(struct ath5k_hw *ah, unsigned int queue)
00216 {
00217         unsigned int i = 40;
00218         u32 tx_queue, pending;
00219 
00220         ATH5K_TRACE(ah->ah_sc);
00221         AR5K_ASSERT_ENTRY(queue, ah->ah_capabilities.cap_queues.q_tx_num);
00222 
00223         /* Return if queue is declared inactive */
00224         if (ah->ah_txq[queue].tqi_type == AR5K_TX_QUEUE_INACTIVE)
00225                 return -EIO;
00226 
00227         if (ah->ah_version == AR5K_AR5210) {
00228                 tx_queue = ath5k_hw_reg_read(ah, AR5K_CR);
00229 
00230                 /*
00231                  * Set by queue type
00232                  */
00233                 switch (ah->ah_txq[queue].tqi_type) {
00234                 case AR5K_TX_QUEUE_DATA:
00235                         tx_queue |= AR5K_CR_TXD0 & ~AR5K_CR_TXE0;
00236                         break;
00237                 case AR5K_TX_QUEUE_BEACON:
00238                 case AR5K_TX_QUEUE_CAB:
00239                         /* XXX Fix me... */
00240                         tx_queue |= AR5K_CR_TXD1 & ~AR5K_CR_TXD1;
00241                         ath5k_hw_reg_write(ah, 0, AR5K_BSR);
00242                         break;
00243                 default:
00244                         return -EINVAL;
00245                 }
00246 
00247                 /* Stop queue */
00248                 ath5k_hw_reg_write(ah, tx_queue, AR5K_CR);
00249                 ath5k_hw_reg_read(ah, AR5K_CR);
00250         } else {
00251                 /*
00252                  * Schedule TX disable and wait until queue is empty
00253                  */
00254                 AR5K_REG_WRITE_Q(ah, AR5K_QCU_TXD, queue);
00255 
00256                 /*Check for pending frames*/
00257                 do {
00258                         pending = ath5k_hw_reg_read(ah,
00259                                 AR5K_QUEUE_STATUS(queue)) &
00260                                 AR5K_QCU_STS_FRMPENDCNT;
00261                         udelay(100);
00262                 } while (--i && pending);
00263 
00264                 /* For 2413+ order PCU to drop packets using
00265                  * QUIET mechanism */
00266                 if (ah->ah_mac_version >= (AR5K_SREV_AR2414 >> 4) &&
00267                 pending){
00268                         /* Set periodicity and duration */
00269                         ath5k_hw_reg_write(ah,
00270                                 AR5K_REG_SM(100, AR5K_QUIET_CTL2_QT_PER)|
00271                                 AR5K_REG_SM(10, AR5K_QUIET_CTL2_QT_DUR),
00272                                 AR5K_QUIET_CTL2);
00273 
00274                         /* Enable quiet period for current TSF */
00275                         ath5k_hw_reg_write(ah,
00276                                 AR5K_QUIET_CTL1_QT_EN |
00277                                 AR5K_REG_SM(ath5k_hw_reg_read(ah,
00278                                                 AR5K_TSF_L32_5211) >> 10,
00279                                                 AR5K_QUIET_CTL1_NEXT_QT_TSF),
00280                                 AR5K_QUIET_CTL1);
00281 
00282                         /* Force channel idle high */
00283                         AR5K_REG_ENABLE_BITS(ah, AR5K_DIAG_SW_5211,
00284                                         AR5K_DIAG_SW_CHANEL_IDLE_HIGH);
00285 
00286                         /* Wait a while and disable mechanism */
00287                         udelay(200);
00288                         AR5K_REG_DISABLE_BITS(ah, AR5K_QUIET_CTL1,
00289                                                 AR5K_QUIET_CTL1_QT_EN);
00290 
00291                         /* Re-check for pending frames */
00292                         i = 40;
00293                         do {
00294                                 pending = ath5k_hw_reg_read(ah,
00295                                         AR5K_QUEUE_STATUS(queue)) &
00296                                         AR5K_QCU_STS_FRMPENDCNT;
00297                                 udelay(100);
00298                         } while (--i && pending);
00299 
00300                         AR5K_REG_DISABLE_BITS(ah, AR5K_DIAG_SW_5211,
00301                                         AR5K_DIAG_SW_CHANEL_IDLE_HIGH);
00302                 }
00303 
00304                 /* Clear register */
00305                 ath5k_hw_reg_write(ah, 0, AR5K_QCU_TXD);
00306                 if (pending)
00307                         return -EBUSY;
00308         }
00309 
00310         /* TODO: Check for success on 5210 else return error */
00311         return 0;
00312 }
00313 
00327 u32 ath5k_hw_get_txdp(struct ath5k_hw *ah, unsigned int queue)
00328 {
00329         u16 tx_reg;
00330 
00331         ATH5K_TRACE(ah->ah_sc);
00332         AR5K_ASSERT_ENTRY(queue, ah->ah_capabilities.cap_queues.q_tx_num);
00333 
00334         /*
00335          * Get the transmit queue descriptor pointer from the selected queue
00336          */
00337         /*5210 doesn't have QCU*/
00338         if (ah->ah_version == AR5K_AR5210) {
00339                 switch (ah->ah_txq[queue].tqi_type) {
00340                 case AR5K_TX_QUEUE_DATA:
00341                         tx_reg = AR5K_NOQCU_TXDP0;
00342                         break;
00343                 case AR5K_TX_QUEUE_BEACON:
00344                 case AR5K_TX_QUEUE_CAB:
00345                         tx_reg = AR5K_NOQCU_TXDP1;
00346                         break;
00347                 default:
00348                         return 0xffffffff;
00349                 }
00350         } else {
00351                 tx_reg = AR5K_QUEUE_TXDP(queue);
00352         }
00353 
00354         return ath5k_hw_reg_read(ah, tx_reg);
00355 }
00356 
00370 int ath5k_hw_set_txdp(struct ath5k_hw *ah, unsigned int queue, u32 phys_addr)
00371 {
00372         u16 tx_reg;
00373 
00374         ATH5K_TRACE(ah->ah_sc);
00375         AR5K_ASSERT_ENTRY(queue, ah->ah_capabilities.cap_queues.q_tx_num);
00376 
00377         /*
00378          * Set the transmit queue descriptor pointer register by type
00379          * on 5210
00380          */
00381         if (ah->ah_version == AR5K_AR5210) {
00382                 switch (ah->ah_txq[queue].tqi_type) {
00383                 case AR5K_TX_QUEUE_DATA:
00384                         tx_reg = AR5K_NOQCU_TXDP0;
00385                         break;
00386                 case AR5K_TX_QUEUE_BEACON:
00387                 case AR5K_TX_QUEUE_CAB:
00388                         tx_reg = AR5K_NOQCU_TXDP1;
00389                         break;
00390                 default:
00391                         return -EINVAL;
00392                 }
00393         } else {
00394                 /*
00395                  * Set the transmit queue descriptor pointer for
00396                  * the selected queue on QCU for 5211+
00397                  * (this won't work if the queue is still active)
00398                  */
00399                 if (AR5K_REG_READ_Q(ah, AR5K_QCU_TXE, queue))
00400                         return -EIO;
00401 
00402                 tx_reg = AR5K_QUEUE_TXDP(queue);
00403         }
00404 
00405         /* Set descriptor pointer */
00406         ath5k_hw_reg_write(ah, phys_addr, tx_reg);
00407 
00408         return 0;
00409 }
00410 
00429 int ath5k_hw_update_tx_triglevel(struct ath5k_hw *ah, bool increase)
00430 {
00431         u32 trigger_level, imr;
00432         int ret = -EIO;
00433 
00434         ATH5K_TRACE(ah->ah_sc);
00435 
00436         /*
00437          * Disable interrupts by setting the mask
00438          */
00439         imr = ath5k_hw_set_imr(ah, ah->ah_imr & ~AR5K_INT_GLOBAL);
00440 
00441         trigger_level = AR5K_REG_MS(ath5k_hw_reg_read(ah, AR5K_TXCFG),
00442                         AR5K_TXCFG_TXFULL);
00443 
00444         if (!increase) {
00445                 if (--trigger_level < AR5K_TUNE_MIN_TX_FIFO_THRES)
00446                         goto done;
00447         } else
00448                 trigger_level +=
00449                         ((AR5K_TUNE_MAX_TX_FIFO_THRES - trigger_level) / 2);
00450 
00451         /*
00452          * Update trigger level on success
00453          */
00454         if (ah->ah_version == AR5K_AR5210)
00455                 ath5k_hw_reg_write(ah, trigger_level, AR5K_TRIG_LVL);
00456         else
00457                 AR5K_REG_WRITE_BITS(ah, AR5K_TXCFG,
00458                                 AR5K_TXCFG_TXFULL, trigger_level);
00459 
00460         ret = 0;
00461 
00462 done:
00463         /*
00464          * Restore interrupt mask
00465          */
00466         ath5k_hw_set_imr(ah, imr);
00467 
00468         return ret;
00469 }
00470 
00471 /*******************\
00472 * Interrupt masking *
00473 \*******************/
00474 
00483 bool ath5k_hw_is_intr_pending(struct ath5k_hw *ah)
00484 {
00485         ATH5K_TRACE(ah->ah_sc);
00486         return ath5k_hw_reg_read(ah, AR5K_INTPEND) == 1 ? 1 : 0;
00487 }
00488 
00505 int ath5k_hw_get_isr(struct ath5k_hw *ah, enum ath5k_int *interrupt_mask)
00506 {
00507         u32 data;
00508 
00509         ATH5K_TRACE(ah->ah_sc);
00510 
00511         /*
00512          * Read interrupt status from the Interrupt Status register
00513          * on 5210
00514          */
00515         if (ah->ah_version == AR5K_AR5210) {
00516                 data = ath5k_hw_reg_read(ah, AR5K_ISR);
00517                 if (unlikely(data == AR5K_INT_NOCARD)) {
00518                         *interrupt_mask = data;
00519                         return -ENODEV;
00520                 }
00521         } else {
00522                 /*
00523                  * Read interrupt status from Interrupt
00524                  * Status Register shadow copy (Read And Clear)
00525                  *
00526                  * Note: PISR/SISR Not available on 5210
00527                  */
00528                 data = ath5k_hw_reg_read(ah, AR5K_RAC_PISR);
00529                 if (unlikely(data == AR5K_INT_NOCARD)) {
00530                         *interrupt_mask = data;
00531                         return -ENODEV;
00532                 }
00533         }
00534 
00535         /*
00536          * Get abstract interrupt mask (driver-compatible)
00537          */
00538         *interrupt_mask = (data & AR5K_INT_COMMON) & ah->ah_imr;
00539 
00540         if (ah->ah_version != AR5K_AR5210) {
00541                 u32 sisr2 = ath5k_hw_reg_read(ah, AR5K_RAC_SISR2);
00542 
00543                 /*HIU = Host Interface Unit (PCI etc)*/
00544                 if (unlikely(data & (AR5K_ISR_HIUERR)))
00545                         *interrupt_mask |= AR5K_INT_FATAL;
00546 
00547                 /*Beacon Not Ready*/
00548                 if (unlikely(data & (AR5K_ISR_BNR)))
00549                         *interrupt_mask |= AR5K_INT_BNR;
00550 
00551                 if (unlikely(sisr2 & (AR5K_SISR2_SSERR |
00552                                         AR5K_SISR2_DPERR |
00553                                         AR5K_SISR2_MCABT)))
00554                         *interrupt_mask |= AR5K_INT_FATAL;
00555 
00556                 if (data & AR5K_ISR_TIM)
00557                         *interrupt_mask |= AR5K_INT_TIM;
00558 
00559                 if (data & AR5K_ISR_BCNMISC) {
00560                         if (sisr2 & AR5K_SISR2_TIM)
00561                                 *interrupt_mask |= AR5K_INT_TIM;
00562                         if (sisr2 & AR5K_SISR2_DTIM)
00563                                 *interrupt_mask |= AR5K_INT_DTIM;
00564                         if (sisr2 & AR5K_SISR2_DTIM_SYNC)
00565                                 *interrupt_mask |= AR5K_INT_DTIM_SYNC;
00566                         if (sisr2 & AR5K_SISR2_BCN_TIMEOUT)
00567                                 *interrupt_mask |= AR5K_INT_BCN_TIMEOUT;
00568                         if (sisr2 & AR5K_SISR2_CAB_TIMEOUT)
00569                                 *interrupt_mask |= AR5K_INT_CAB_TIMEOUT;
00570                 }
00571 
00572                 if (data & AR5K_ISR_RXDOPPLER)
00573                         *interrupt_mask |= AR5K_INT_RX_DOPPLER;
00574                 if (data & AR5K_ISR_QCBRORN) {
00575                         *interrupt_mask |= AR5K_INT_QCBRORN;
00576                         ah->ah_txq_isr |= AR5K_REG_MS(
00577                                         ath5k_hw_reg_read(ah, AR5K_RAC_SISR3),
00578                                         AR5K_SISR3_QCBRORN);
00579                 }
00580                 if (data & AR5K_ISR_QCBRURN) {
00581                         *interrupt_mask |= AR5K_INT_QCBRURN;
00582                         ah->ah_txq_isr |= AR5K_REG_MS(
00583                                         ath5k_hw_reg_read(ah, AR5K_RAC_SISR3),
00584                                         AR5K_SISR3_QCBRURN);
00585                 }
00586                 if (data & AR5K_ISR_QTRIG) {
00587                         *interrupt_mask |= AR5K_INT_QTRIG;
00588                         ah->ah_txq_isr |= AR5K_REG_MS(
00589                                         ath5k_hw_reg_read(ah, AR5K_RAC_SISR4),
00590                                         AR5K_SISR4_QTRIG);
00591                 }
00592 
00593                 if (data & AR5K_ISR_TXOK)
00594                         ah->ah_txq_isr |= AR5K_REG_MS(
00595                                         ath5k_hw_reg_read(ah, AR5K_RAC_SISR0),
00596                                         AR5K_SISR0_QCU_TXOK);
00597 
00598                 if (data & AR5K_ISR_TXDESC)
00599                         ah->ah_txq_isr |= AR5K_REG_MS(
00600                                         ath5k_hw_reg_read(ah, AR5K_RAC_SISR0),
00601                                         AR5K_SISR0_QCU_TXDESC);
00602 
00603                 if (data & AR5K_ISR_TXERR)
00604                         ah->ah_txq_isr |= AR5K_REG_MS(
00605                                         ath5k_hw_reg_read(ah, AR5K_RAC_SISR1),
00606                                         AR5K_SISR1_QCU_TXERR);
00607 
00608                 if (data & AR5K_ISR_TXEOL)
00609                         ah->ah_txq_isr |= AR5K_REG_MS(
00610                                         ath5k_hw_reg_read(ah, AR5K_RAC_SISR1),
00611                                         AR5K_SISR1_QCU_TXEOL);
00612 
00613                 if (data & AR5K_ISR_TXURN)
00614                         ah->ah_txq_isr |= AR5K_REG_MS(
00615                                         ath5k_hw_reg_read(ah, AR5K_RAC_SISR2),
00616                                         AR5K_SISR2_QCU_TXURN);
00617         } else {
00618                 if (unlikely(data & (AR5K_ISR_SSERR | AR5K_ISR_MCABT
00619                                 | AR5K_ISR_HIUERR | AR5K_ISR_DPERR)))
00620                         *interrupt_mask |= AR5K_INT_FATAL;
00621 
00622                 /*
00623                  * XXX: BMISS interrupts may occur after association.
00624                  * I found this on 5210 code but it needs testing. If this is
00625                  * true we should disable them before assoc and re-enable them
00626                  * after a successful assoc + some jiffies.
00627                         interrupt_mask &= ~AR5K_INT_BMISS;
00628                  */
00629         }
00630 
00631         /*
00632          * In case we didn't handle anything,
00633          * print the register value.
00634          */
00635         if (unlikely(*interrupt_mask == 0))
00636                 ATH5K_PRINTF("ISR: 0x%08x IMR: 0x%08x\n", data, ah->ah_imr);
00637 
00638         return 0;
00639 }
00640 
00651 enum ath5k_int ath5k_hw_set_imr(struct ath5k_hw *ah, enum ath5k_int new_mask)
00652 {
00653         enum ath5k_int old_mask, int_mask;
00654 
00655         old_mask = ah->ah_imr;
00656 
00657         /*
00658          * Disable card interrupts to prevent any race conditions
00659          * (they will be re-enabled afterwards if AR5K_INT GLOBAL
00660          * is set again on the new mask).
00661          */
00662         if (old_mask & AR5K_INT_GLOBAL) {
00663                 ath5k_hw_reg_write(ah, AR5K_IER_DISABLE, AR5K_IER);
00664                 ath5k_hw_reg_read(ah, AR5K_IER);
00665         }
00666 
00667         /*
00668          * Add additional, chipset-dependent interrupt mask flags
00669          * and write them to the IMR (interrupt mask register).
00670          */
00671         int_mask = new_mask & AR5K_INT_COMMON;
00672 
00673         if (ah->ah_version != AR5K_AR5210) {
00674                 /* Preserve per queue TXURN interrupt mask */
00675                 u32 simr2 = ath5k_hw_reg_read(ah, AR5K_SIMR2)
00676                                 & AR5K_SIMR2_QCU_TXURN;
00677 
00678                 if (new_mask & AR5K_INT_FATAL) {
00679                         int_mask |= AR5K_IMR_HIUERR;
00680                         simr2 |= (AR5K_SIMR2_MCABT | AR5K_SIMR2_SSERR
00681                                 | AR5K_SIMR2_DPERR);
00682                 }
00683 
00684                 /*Beacon Not Ready*/
00685                 if (new_mask & AR5K_INT_BNR)
00686                         int_mask |= AR5K_INT_BNR;
00687 
00688                 if (new_mask & AR5K_INT_TIM)
00689                         int_mask |= AR5K_IMR_TIM;
00690 
00691                 if (new_mask & AR5K_INT_TIM)
00692                         simr2 |= AR5K_SISR2_TIM;
00693                 if (new_mask & AR5K_INT_DTIM)
00694                         simr2 |= AR5K_SISR2_DTIM;
00695                 if (new_mask & AR5K_INT_DTIM_SYNC)
00696                         simr2 |= AR5K_SISR2_DTIM_SYNC;
00697                 if (new_mask & AR5K_INT_BCN_TIMEOUT)
00698                         simr2 |= AR5K_SISR2_BCN_TIMEOUT;
00699                 if (new_mask & AR5K_INT_CAB_TIMEOUT)
00700                         simr2 |= AR5K_SISR2_CAB_TIMEOUT;
00701 
00702                 if (new_mask & AR5K_INT_RX_DOPPLER)
00703                         int_mask |= AR5K_IMR_RXDOPPLER;
00704 
00705                 /* Note: Per queue interrupt masks
00706                  * are set via reset_tx_queue (qcu.c) */
00707                 ath5k_hw_reg_write(ah, int_mask, AR5K_PIMR);
00708                 ath5k_hw_reg_write(ah, simr2, AR5K_SIMR2);
00709 
00710         } else {
00711                 if (new_mask & AR5K_INT_FATAL)
00712                         int_mask |= (AR5K_IMR_SSERR | AR5K_IMR_MCABT
00713                                 | AR5K_IMR_HIUERR | AR5K_IMR_DPERR);
00714 
00715                 ath5k_hw_reg_write(ah, int_mask, AR5K_IMR);
00716         }
00717 
00718         /* If RXNOFRM interrupt is masked disable it
00719          * by setting AR5K_RXNOFRM to zero */
00720         if (!(new_mask & AR5K_INT_RXNOFRM))
00721                 ath5k_hw_reg_write(ah, 0, AR5K_RXNOFRM);
00722 
00723         /* Store new interrupt mask */
00724         ah->ah_imr = new_mask;
00725 
00726         /* ..re-enable interrupts if AR5K_INT_GLOBAL is set */
00727         if (new_mask & AR5K_INT_GLOBAL) {
00728                 ath5k_hw_reg_write(ah, AR5K_IER_ENABLE, AR5K_IER);
00729                 ath5k_hw_reg_read(ah, AR5K_IER);
00730         }
00731 
00732         return old_mask;
00733 }
00734 


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:09