Pileus Git - ~andy/linux/blob - drivers/staging/brcm80211/brcmfmac/bcmsdh_sdmmc.c

   1 /*
   2  * Copyright (c) 2010 Broadcom Corporation
   3  *
   4  * Permission to use, copy, modify, and/or distribute this software for any
   5  * purpose with or without fee is hereby granted, provided that the above
   6  * copyright notice and this permission notice appear in all copies.
   7  *
   8  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
   9  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  10  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
  11  * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  12  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
  13  * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
  14  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  15  */
  16 #include <linux/types.h>
  17 #include <linux/netdevice.h>
  18 #include <linux/mmc/sdio.h>
  19 #include <linux/mmc/core.h>
  20 #include <linux/mmc/sdio_func.h>
  21 #include <linux/mmc/sdio_ids.h>
  22 #include <linux/mmc/card.h>
  23 #include <linux/suspend.h>
  24 #include <linux/errno.h>
  25 #include <linux/sched.h>        /* request_irq() */
  26 #include <net/cfg80211.h>
  27
  28 #include <defs.h>
  29 #include <brcm_hw_ids.h>
  30 #include <brcmu_utils.h>
  31 #include <brcmu_wifi.h>
  32 #include "sdio_host.h"
  33 #include "dhd.h"
  34 #include "dhd_dbg.h"
  35 #include "wl_cfg80211.h"
  36
  37 #define SDIO_VENDOR_ID_BROADCOM         0x02d0
  38
  39 #define DMA_ALIGN_MASK  0x03
  40
  41 #define SDIO_DEVICE_ID_BROADCOM_4329    0x4329
  42
  43 static int brcmf_sdioh_get_cisaddr(struct brcmf_sdio_dev *sdiodev, u32 regaddr);
  44 static int brcmf_ops_sdio_probe(struct sdio_func *func,
  45                                 const struct sdio_device_id *id);
  46 static void brcmf_ops_sdio_remove(struct sdio_func *func);
  47
  48 #ifdef CONFIG_PM
  49 static int brcmf_sdio_suspend(struct device *dev);
  50 static int brcmf_sdio_resume(struct device *dev);
  51 #endif /* CONFIG_PM */
  52
  53 uint sd_f2_blocksize = 512;     /* Default blocksize */
  54
  55 /* devices we support, null terminated */
  56 static const struct sdio_device_id brcmf_sdmmc_ids[] = {
  57         {SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4329)},
  58         { /* end: all zeroes */ },
  59 };
  60
  61 #ifdef CONFIG_PM_SLEEP
  62 static const struct dev_pm_ops brcmf_sdio_pm_ops = {
  63         .suspend        = brcmf_sdio_suspend,
  64         .resume         = brcmf_sdio_resume,
  65 };
  66 #endif  /* CONFIG_PM_SLEEP */
  67
  68 static struct sdio_driver brcmf_sdmmc_driver = {
  69         .probe = brcmf_ops_sdio_probe,
  70         .remove = brcmf_ops_sdio_remove,
  71         .name = "brcmfmac",
  72         .id_table = brcmf_sdmmc_ids,
  73 #ifdef CONFIG_PM_SLEEP
  74         .drv = {
  75                 .pm = &brcmf_sdio_pm_ops,
  76         },
  77 #endif  /* CONFIG_PM_SLEEP */
  78 };
  79
  80 MODULE_DEVICE_TABLE(sdio, brcmf_sdmmc_ids);
  81
  82 #ifdef CONFIG_PM_SLEEP
  83 DECLARE_WAIT_QUEUE_HEAD(sdioh_request_byte_wait);
  84 DECLARE_WAIT_QUEUE_HEAD(sdioh_request_word_wait);
  85 DECLARE_WAIT_QUEUE_HEAD(sdioh_request_packet_wait);
  86 DECLARE_WAIT_QUEUE_HEAD(sdioh_request_buffer_wait);
  87 #define BRCMF_PM_RESUME_WAIT(a, b) do { \
  88                 int retry = 0; \
  89                 while (atomic_read(&b->suspend) && retry++ != 30) { \
  90                         wait_event_timeout(a, false, HZ/100); \
  91                 } \
  92         }       while (0)
  93 #define BRCMF_PM_RESUME_RETURN_ERROR(a, b)      \
  94         do { if (atomic_read(&b->suspend)) return a; } while (0)
  95 #else
  96 #define BRCMF_PM_RESUME_WAIT(a, b)
  97 #define BRCMF_PM_RESUME_RETURN_ERROR(a, b)
  98 #endif          /* CONFIG_PM_SLEEP */
  99
 100 static int
 101 brcmf_sdioh_card_regread(struct brcmf_sdio_dev *sdiodev, int func, u32 regaddr,
 102                          int regsize, u32 *data);
 103
 104 static int brcmf_sdioh_enablefuncs(struct brcmf_sdio_dev *sdiodev)
 105 {
 106         int err_ret;
 107         u32 fbraddr;
 108         u8 func;
 109
 110         brcmf_dbg(TRACE, "\n");
 111
 112         /* Get the Card's common CIS address */
 113         sdiodev->func_cis_ptr[0] = brcmf_sdioh_get_cisaddr(sdiodev,
 114                                                            SDIO_CCCR_CIS);
 115         brcmf_dbg(INFO, "Card's Common CIS Ptr = 0x%x\n",
 116                   sdiodev->func_cis_ptr[0]);
 117
 118         /* Get the Card's function CIS (for each function) */
 119         for (fbraddr = SDIO_FBR_BASE(1), func = 1;
 120              func <= sdiodev->num_funcs; func++, fbraddr += SDIOD_FBR_SIZE) {
 121                 sdiodev->func_cis_ptr[func] =
 122                     brcmf_sdioh_get_cisaddr(sdiodev, SDIO_FBR_CIS + fbraddr);
 123                 brcmf_dbg(INFO, "Function %d CIS Ptr = 0x%x\n",
 124                           func, sdiodev->func_cis_ptr[func]);
 125         }
 126
 127         /* Enable Function 1 */
 128         sdio_claim_host(sdiodev->func[1]);
 129         err_ret = sdio_enable_func(sdiodev->func[1]);
 130         sdio_release_host(sdiodev->func[1]);
 131         if (err_ret)
 132                 brcmf_dbg(ERROR, "Failed to enable F1 Err: 0x%08x\n", err_ret);
 133
 134         return false;
 135 }
 136
 137 /*
 138  *      Public entry points & extern's
 139  */
 140 int brcmf_sdioh_attach(struct brcmf_sdio_dev *sdiodev)
 141 {
 142         int err_ret = 0;
 143
 144         brcmf_dbg(TRACE, "\n");
 145
 146         sdiodev->num_funcs = 2;
 147
 148         sdio_claim_host(sdiodev->func[1]);
 149         err_ret = sdio_set_block_size(sdiodev->func[1], 64);
 150         sdio_release_host(sdiodev->func[1]);
 151         if (err_ret) {
 152                 brcmf_dbg(ERROR, "Failed to set F1 blocksize\n");
 153                 goto out;
 154         }
 155
 156         sdio_claim_host(sdiodev->func[2]);
 157         err_ret = sdio_set_block_size(sdiodev->func[2], sd_f2_blocksize);
 158         sdio_release_host(sdiodev->func[2]);
 159         if (err_ret) {
 160                 brcmf_dbg(ERROR, "Failed to set F2 blocksize to %d\n",
 161                           sd_f2_blocksize);
 162                 goto out;
 163         }
 164
 165         brcmf_sdioh_enablefuncs(sdiodev);
 166
 167 out:
 168         brcmf_dbg(TRACE, "Done\n");
 169         return err_ret;
 170 }
 171
 172 void brcmf_sdioh_detach(struct brcmf_sdio_dev *sdiodev)
 173 {
 174         brcmf_dbg(TRACE, "\n");
 175
 176         /* Disable Function 2 */
 177         sdio_claim_host(sdiodev->func[2]);
 178         sdio_disable_func(sdiodev->func[2]);
 179         sdio_release_host(sdiodev->func[2]);
 180
 181         /* Disable Function 1 */
 182         sdio_claim_host(sdiodev->func[1]);
 183         sdio_disable_func(sdiodev->func[1]);
 184         sdio_release_host(sdiodev->func[1]);
 185
 186 }
 187
 188 static int brcmf_sdioh_get_cisaddr(struct brcmf_sdio_dev *sdiodev, u32 regaddr)
 189 {
 190         /* read 24 bits and return valid 17 bit addr */
 191         int i;
 192         u32 scratch, regdata;
 193         u8 *ptr = (u8 *)&scratch;
 194         for (i = 0; i < 3; i++) {
 195                 if ((brcmf_sdioh_card_regread(sdiodev, 0, regaddr, 1,
 196                                 &regdata)) != SUCCESS)
 197                         brcmf_dbg(ERROR, "Can't read!\n");
 198
 199                 *ptr++ = (u8) regdata;
 200                 regaddr++;
 201         }
 202
 203         /* Only the lower 17-bits are valid */
 204         scratch = le32_to_cpu(scratch);
 205         scratch &= 0x0001FFFF;
 206         return scratch;
 207 }
 208
 209 extern int
 210 brcmf_sdioh_cis_read(struct brcmf_sdio_dev *sdiodev, uint func,
 211                      u8 *cisd, u32 length)
 212 {
 213         u32 count;
 214         int offset;
 215         u32 foo;
 216         u8 *cis = cisd;
 217
 218         brcmf_dbg(TRACE, "Func = %d\n", func);
 219
 220         if (!sdiodev->func_cis_ptr[func]) {
 221                 memset(cis, 0, length);
 222                 brcmf_dbg(ERROR, "no func_cis_ptr[%d]\n", func);
 223                 return -ENOTSUPP;
 224         }
 225
 226         brcmf_dbg(ERROR, "func_cis_ptr[%d]=0x%04x\n",
 227                   func, sdiodev->func_cis_ptr[func]);
 228
 229         for (count = 0; count < length; count++) {
 230                 offset = sdiodev->func_cis_ptr[func] + count;
 231                 if (brcmf_sdioh_card_regread(sdiodev, 0, offset, 1, &foo) < 0) {
 232                         brcmf_dbg(ERROR, "regread failed: Can't read CIS\n");
 233                         return -EIO;
 234                 }
 235
 236                 *cis = (u8) (foo & 0xff);
 237                 cis++;
 238         }
 239
 240         return 0;
 241 }
 242
 243 extern int
 244 brcmf_sdioh_request_byte(struct brcmf_sdio_dev *sdiodev, uint rw, uint func,
 245                          uint regaddr, u8 *byte)
 246 {
 247         int err_ret;
 248
 249         brcmf_dbg(INFO, "rw=%d, func=%d, addr=0x%05x\n", rw, func, regaddr);
 250
 251         BRCMF_PM_RESUME_WAIT(sdioh_request_byte_wait, sdiodev);
 252         BRCMF_PM_RESUME_RETURN_ERROR(-EIO, sdiodev);
 253         if (rw) {               /* CMD52 Write */
 254                 if (func == 0) {
 255                         /* Can only directly write to some F0 registers.
 256                          * Handle F2 enable
 257                          * as a special case.
 258                          */
 259                         if (regaddr == SDIO_CCCR_IOEx) {
 260                                 if (sdiodev->func[2]) {
 261                                         sdio_claim_host(sdiodev->func[2]);
 262                                         if (*byte & SDIO_FUNC_ENABLE_2) {
 263                                                 /* Enable Function 2 */
 264                                                 err_ret =
 265                                                     sdio_enable_func
 266                                                     (sdiodev->func[2]);
 267                                                 if (err_ret)
 268                                                         brcmf_dbg(ERROR,
 269                                                                   "enable F2 failed:%d\n",
 270                                                                   err_ret);
 271                                         } else {
 272                                                 /* Disable Function 2 */
 273                                                 err_ret =
 274                                                     sdio_disable_func
 275                                                     (sdiodev->func[2]);
 276                                                 if (err_ret)
 277                                                         brcmf_dbg(ERROR,
 278                                                                   "Disable F2 failed:%d\n",
 279                                                                   err_ret);
 280                                         }
 281                                         sdio_release_host(sdiodev->func[2]);
 282                                 }
 283                         }
 284                         /* to allow abort command through F1 */
 285                         else if (regaddr == SDIO_CCCR_ABORT) {
 286                                 sdio_claim_host(sdiodev->func[func]);
 287                                 /*
 288                                  * this sdio_f0_writeb() can be replaced
 289                                  * with another api
 290                                  * depending upon MMC driver change.
 291                                  * As of this time, this is temporaray one
 292                                  */
 293                                 sdio_writeb(sdiodev->func[func], *byte,
 294                                             regaddr, &err_ret);
 295                                 sdio_release_host(sdiodev->func[func]);
 296                         } else if (regaddr < 0xF0) {
 297                                 brcmf_dbg(ERROR, "F0 Wr:0x%02x: write disallowed\n",
 298                                           regaddr);
 299                         } else {
 300                                 /* Claim host controller, perform F0 write,
 301                                  and release */
 302                                 sdio_claim_host(sdiodev->func[func]);
 303                                 sdio_f0_writeb(sdiodev->func[func], *byte,
 304                                                regaddr, &err_ret);
 305                                 sdio_release_host(sdiodev->func[func]);
 306                         }
 307                 } else {
 308                         /* Claim host controller, perform Fn write,
 309                          and release */
 310                         sdio_claim_host(sdiodev->func[func]);
 311                         sdio_writeb(sdiodev->func[func], *byte, regaddr,
 312                                     &err_ret);
 313                         sdio_release_host(sdiodev->func[func]);
 314                 }
 315         } else {                /* CMD52 Read */
 316                 /* Claim host controller, perform Fn read, and release */
 317                 sdio_claim_host(sdiodev->func[func]);
 318
 319                 if (func == 0) {
 320                         *byte =
 321                             sdio_f0_readb(sdiodev->func[func], regaddr,
 322                                           &err_ret);
 323                 } else {
 324                         *byte =
 325                             sdio_readb(sdiodev->func[func], regaddr,
 326                                        &err_ret);
 327                 }
 328
 329                 sdio_release_host(sdiodev->func[func]);
 330         }
 331
 332         if (err_ret)
 333                 brcmf_dbg(ERROR, "Failed to %s byte F%d:@0x%05x=%02x, Err: %d\n",
 334                           rw ? "write" : "read", func, regaddr, *byte, err_ret);
 335
 336         return err_ret;
 337 }
 338
 339 extern int
 340 brcmf_sdioh_request_word(struct brcmf_sdio_dev *sdiodev, uint cmd_type, uint rw,
 341                          uint func, uint addr, u32 *word, uint nbytes)
 342 {
 343         int err_ret = -EIO;
 344
 345         if (func == 0) {
 346                 brcmf_dbg(ERROR, "Only CMD52 allowed to F0\n");
 347                 return -EINVAL;
 348         }
 349
 350         brcmf_dbg(INFO, "cmd_type=%d, rw=%d, func=%d, addr=0x%05x, nbytes=%d\n",
 351                   cmd_type, rw, func, addr, nbytes);
 352
 353         BRCMF_PM_RESUME_WAIT(sdioh_request_word_wait, sdiodev);
 354         BRCMF_PM_RESUME_RETURN_ERROR(-EIO, sdiodev);
 355         /* Claim host controller */
 356         sdio_claim_host(sdiodev->func[func]);
 357
 358         if (rw) {               /* CMD52 Write */
 359                 if (nbytes == 4)
 360                         sdio_writel(sdiodev->func[func], *word, addr,
 361                                     &err_ret);
 362                 else if (nbytes == 2)
 363                         sdio_writew(sdiodev->func[func], (*word & 0xFFFF),
 364                                     addr, &err_ret);
 365                 else
 366                         brcmf_dbg(ERROR, "Invalid nbytes: %d\n", nbytes);
 367         } else {                /* CMD52 Read */
 368                 if (nbytes == 4)
 369                         *word =
 370                             sdio_readl(sdiodev->func[func], addr, &err_ret);
 371                 else if (nbytes == 2)
 372                         *word =
 373                             sdio_readw(sdiodev->func[func], addr,
 374                                        &err_ret) & 0xFFFF;
 375                 else
 376                         brcmf_dbg(ERROR, "Invalid nbytes: %d\n", nbytes);
 377         }
 378
 379         /* Release host controller */
 380         sdio_release_host(sdiodev->func[func]);
 381
 382         if (err_ret)
 383                 brcmf_dbg(ERROR, "Failed to %s word, Err: 0x%08x\n",
 384                           rw ? "write" : "read", err_ret);
 385
 386         return err_ret;
 387 }
 388
 389 static int
 390 brcmf_sdioh_request_packet(struct brcmf_sdio_dev *sdiodev, uint fix_inc,
 391                            uint write, uint func, uint addr,
 392                            struct sk_buff *pkt)
 393 {
 394         bool fifo = (fix_inc == SDIOH_DATA_FIX);
 395         u32 SGCount = 0;
 396         int err_ret = 0;
 397
 398         struct sk_buff *pnext;
 399
 400         brcmf_dbg(TRACE, "Enter\n");
 401
 402         BRCMF_PM_RESUME_WAIT(sdioh_request_packet_wait, sdiodev);
 403         BRCMF_PM_RESUME_RETURN_ERROR(-EIO, sdiodev);
 404
 405         /* Claim host controller */
 406         sdio_claim_host(sdiodev->func[func]);
 407         for (pnext = pkt; pnext; pnext = pnext->next) {
 408                 uint pkt_len = pnext->len;
 409                 pkt_len += 3;
 410                 pkt_len &= 0xFFFFFFFC;
 411
 412                 if ((write) && (!fifo)) {
 413                         err_ret = sdio_memcpy_toio(sdiodev->func[func], addr,
 414                                                    ((u8 *) (pnext->data)),
 415                                                    pkt_len);
 416                 } else if (write) {
 417                         err_ret = sdio_memcpy_toio(sdiodev->func[func], addr,
 418                                                    ((u8 *) (pnext->data)),
 419                                                    pkt_len);
 420                 } else if (fifo) {
 421                         err_ret = sdio_readsb(sdiodev->func[func],
 422                                               ((u8 *) (pnext->data)),
 423                                               addr, pkt_len);
 424                 } else {
 425                         err_ret = sdio_memcpy_fromio(sdiodev->func[func],
 426                                                      ((u8 *) (pnext->data)),
 427                                                      addr, pkt_len);
 428                 }
 429
 430                 if (err_ret) {
 431                         brcmf_dbg(ERROR, "%s FAILED %p[%d], addr=0x%05x, pkt_len=%d, ERR=0x%08x\n",
 432                                   write ? "TX" : "RX", pnext, SGCount, addr,
 433                                   pkt_len, err_ret);
 434                 } else {
 435                         brcmf_dbg(TRACE, "%s xfr'd %p[%d], addr=0x%05x, len=%d\n",
 436                                   write ? "TX" : "RX", pnext, SGCount, addr,
 437                                   pkt_len);
 438                 }
 439
 440                 if (!fifo)
 441                         addr += pkt_len;
 442                 SGCount++;
 443
 444         }
 445
 446         /* Release host controller */
 447         sdio_release_host(sdiodev->func[func]);
 448
 449         brcmf_dbg(TRACE, "Exit\n");
 450         return err_ret;
 451 }
 452
 453 /*
 454  * This function takes a buffer or packet, and fixes everything up
 455  * so that in the end, a DMA-able packet is created.
 456  *
 457  * A buffer does not have an associated packet pointer,
 458  * and may or may not be aligned.
 459  * A packet may consist of a single packet, or a packet chain.
 460  * If it is a packet chain, then all the packets in the chain
 461  * must be properly aligned.
 462  *
 463  * If the packet data is not aligned, then there may only be
 464  * one packet, and in this case,  it is copied to a new
 465  * aligned packet.
 466  *
 467  */
 468 extern int
 469 brcmf_sdioh_request_buffer(struct brcmf_sdio_dev *sdiodev, uint pio_dma,
 470                            uint fix_inc, uint write, uint func, uint addr,
 471                            uint reg_width, uint buflen_u, u8 *buffer,
 472                            struct sk_buff *pkt)
 473 {
 474         int Status;
 475         struct sk_buff *mypkt = NULL;
 476
 477         brcmf_dbg(TRACE, "Enter\n");
 478
 479         BRCMF_PM_RESUME_WAIT(sdioh_request_buffer_wait, sdiodev);
 480         BRCMF_PM_RESUME_RETURN_ERROR(-EIO, sdiodev);
 481         /* Case 1: we don't have a packet. */
 482         if (pkt == NULL) {
 483                 brcmf_dbg(DATA, "Creating new %s Packet, len=%d\n",
 484                           write ? "TX" : "RX", buflen_u);
 485                 mypkt = brcmu_pkt_buf_get_skb(buflen_u);
 486                 if (!mypkt) {
 487                         brcmf_dbg(ERROR, "brcmu_pkt_buf_get_skb failed: len %d\n",
 488                                   buflen_u);
 489                         return -EIO;
 490                 }
 491
 492                 /* For a write, copy the buffer data into the packet. */
 493                 if (write)
 494                         memcpy(mypkt->data, buffer, buflen_u);
 495
 496                 Status = brcmf_sdioh_request_packet(sdiodev, fix_inc, write,
 497                                                     func, addr, mypkt);
 498
 499                 /* For a read, copy the packet data back to the buffer. */
 500                 if (!write)
 501                         memcpy(buffer, mypkt->data, buflen_u);
 502
 503                 brcmu_pkt_buf_free_skb(mypkt);
 504         } else if (((ulong) (pkt->data) & DMA_ALIGN_MASK) != 0) {
 505                 /*
 506                  * Case 2: We have a packet, but it is unaligned.
 507                  * In this case, we cannot have a chain (pkt->next == NULL)
 508                  */
 509                 brcmf_dbg(DATA, "Creating aligned %s Packet, len=%d\n",
 510                           write ? "TX" : "RX", pkt->len);
 511                 mypkt = brcmu_pkt_buf_get_skb(pkt->len);
 512                 if (!mypkt) {
 513                         brcmf_dbg(ERROR, "brcmu_pkt_buf_get_skb failed: len %d\n",
 514                                   pkt->len);
 515                         return -EIO;
 516                 }
 517
 518                 /* For a write, copy the buffer data into the packet. */
 519                 if (write)
 520                         memcpy(mypkt->data, pkt->data, pkt->len);
 521
 522                 Status = brcmf_sdioh_request_packet(sdiodev, fix_inc, write,
 523                                                     func, addr, mypkt);
 524
 525                 /* For a read, copy the packet data back to the buffer. */
 526                 if (!write)
 527                         memcpy(pkt->data, mypkt->data, mypkt->len);
 528
 529                 brcmu_pkt_buf_free_skb(mypkt);
 530         } else {                /* case 3: We have a packet and
 531                                  it is aligned. */
 532                 brcmf_dbg(DATA, "Aligned %s Packet, direct DMA\n",
 533                           write ? "Tx" : "Rx");
 534                 Status = brcmf_sdioh_request_packet(sdiodev, fix_inc, write,
 535                                                     func, addr, pkt);
 536         }
 537
 538         return Status;
 539 }
 540
 541 /* Read client card reg */
 542 int
 543 brcmf_sdioh_card_regread(struct brcmf_sdio_dev *sdiodev, int func, u32 regaddr,
 544                          int regsize, u32 *data)
 545 {
 546
 547         if ((func == 0) || (regsize == 1)) {
 548                 u8 temp = 0;
 549
 550                 brcmf_sdioh_request_byte(sdiodev, SDIOH_READ, func, regaddr,
 551                                          &temp);
 552                 *data = temp;
 553                 *data &= 0xff;
 554                 brcmf_dbg(DATA, "byte read data=0x%02x\n", *data);
 555         } else {
 556                 brcmf_sdioh_request_word(sdiodev, 0, SDIOH_READ, func, regaddr,
 557                                          data, regsize);
 558                 if (regsize == 2)
 559                         *data &= 0xffff;
 560
 561                 brcmf_dbg(DATA, "word read data=0x%08x\n", *data);
 562         }
 563
 564         return SUCCESS;
 565 }
 566
 567 static int brcmf_ops_sdio_probe(struct sdio_func *func,
 568                               const struct sdio_device_id *id)
 569 {
 570         int ret = 0;
 571         struct brcmf_sdio_dev *sdiodev;
 572         brcmf_dbg(TRACE, "Enter\n");
 573         brcmf_dbg(TRACE, "func->class=%x\n", func->class);
 574         brcmf_dbg(TRACE, "sdio_vendor: 0x%04x\n", func->vendor);
 575         brcmf_dbg(TRACE, "sdio_device: 0x%04x\n", func->device);
 576         brcmf_dbg(TRACE, "Function#: 0x%04x\n", func->num);
 577
 578         if (func->num == 1) {
 579                 if (dev_get_drvdata(&func->card->dev)) {
 580                         brcmf_dbg(ERROR, "card private drvdata occupied\n");
 581                         return -ENXIO;
 582                 }
 583                 sdiodev = kzalloc(sizeof(struct brcmf_sdio_dev), GFP_KERNEL);
 584                 if (!sdiodev)
 585                         return -ENOMEM;
 586                 sdiodev->func[0] = func->card->sdio_func[0];
 587                 sdiodev->func[1] = func;
 588                 dev_set_drvdata(&func->card->dev, sdiodev);
 589
 590                 atomic_set(&sdiodev->suspend, false);
 591         }
 592
 593         if (func->num == 2) {
 594                 sdiodev = dev_get_drvdata(&func->card->dev);
 595                 if ((!sdiodev) || (sdiodev->func[1]->card != func->card))
 596                         return -ENODEV;
 597                 sdiodev->func[2] = func;
 598
 599                 brcmf_cfg80211_sdio_func(func);
 600                 brcmf_dbg(TRACE, "F2 found, calling brcmf_sdio_probe...\n");
 601                 ret = brcmf_sdio_probe(sdiodev);
 602         }
 603
 604         return ret;
 605 }
 606
 607 static void brcmf_ops_sdio_remove(struct sdio_func *func)
 608 {
 609         struct brcmf_sdio_dev *sdiodev;
 610         brcmf_dbg(TRACE, "Enter\n");
 611         brcmf_dbg(INFO, "func->class=%x\n", func->class);
 612         brcmf_dbg(INFO, "sdio_vendor: 0x%04x\n", func->vendor);
 613         brcmf_dbg(INFO, "sdio_device: 0x%04x\n", func->device);
 614         brcmf_dbg(INFO, "Function#: 0x%04x\n", func->num);
 615
 616         if (func->num == 2) {
 617                 sdiodev = dev_get_drvdata(&func->card->dev);
 618                 brcmf_dbg(TRACE, "F2 found, calling brcmf_sdio_remove...\n");
 619                 brcmf_sdio_remove(sdiodev);
 620                 dev_set_drvdata(&func->card->dev, NULL);
 621                 kfree(sdiodev);
 622         }
 623 }
 624
 625
 626 #ifdef CONFIG_PM_SLEEP
 627 static int brcmf_sdio_suspend(struct device *dev)
 628 {
 629         mmc_pm_flag_t sdio_flags;
 630         struct brcmf_sdio_dev *sdiodev;
 631         struct sdio_func *func = dev_to_sdio_func(dev);
 632         int ret = 0;
 633
 634         brcmf_dbg(TRACE, "\n");
 635
 636         sdiodev = dev_get_drvdata(&func->card->dev);
 637
 638         atomic_set(&sdiodev->suspend, true);
 639
 640         sdio_flags = sdio_get_host_pm_caps(sdiodev->func[1]);
 641         if (!(sdio_flags & MMC_PM_KEEP_POWER)) {
 642                 brcmf_dbg(ERROR, "Host can't keep power while suspended\n");
 643                 return -EINVAL;
 644         }
 645
 646         ret = sdio_set_host_pm_flags(sdiodev->func[1], MMC_PM_KEEP_POWER);
 647         if (ret) {
 648                 brcmf_dbg(ERROR, "Failed to set pm_flags\n");
 649                 return ret;
 650         }
 651
 652         brcmf_sdio_wdtmr_enable(sdiodev, false);
 653
 654         return ret;
 655 }
 656
 657 static int brcmf_sdio_resume(struct device *dev)
 658 {
 659         struct brcmf_sdio_dev *sdiodev;
 660         struct sdio_func *func = dev_to_sdio_func(dev);
 661
 662         sdiodev = dev_get_drvdata(&func->card->dev);
 663         brcmf_sdio_wdtmr_enable(sdiodev, true);
 664         atomic_set(&sdiodev->suspend, false);
 665         return 0;
 666 }
 667 #endif          /* CONFIG_PM_SLEEP */
 668
 669 /*
 670  * module init
 671 */
 672 int brcmf_sdio_function_init(void)
 673 {
 674         int error = 0;
 675         brcmf_dbg(TRACE, "Enter\n");
 676
 677         error = sdio_register_driver(&brcmf_sdmmc_driver);
 678
 679         return error;
 680 }
 681
 682 /*
 683  * module cleanup
 684 */
 685 void brcmf_sdio_function_cleanup(void)
 686 {
 687         brcmf_dbg(TRACE, "Enter\n");
 688
 689         sdio_unregister_driver(&brcmf_sdmmc_driver);
 690 }