Pileus Git - ~andy/linux/blob - drivers/net/caif/caif_hsi.c

   1 /*
   2  * Copyright (C) ST-Ericsson AB 2010
   3  * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
   4  * Author:  Daniel Martensson / daniel.martensson@stericsson.com
   5  *          Dmitry.Tarnyagin  / dmitry.tarnyagin@stericsson.com
   6  * License terms: GNU General Public License (GPL) version 2.
   7  */
   8
   9 #include <linux/init.h>
  10 #include <linux/module.h>
  11 #include <linux/device.h>
  12 #include <linux/platform_device.h>
  13 #include <linux/netdevice.h>
  14 #include <linux/string.h>
  15 #include <linux/list.h>
  16 #include <linux/interrupt.h>
  17 #include <linux/delay.h>
  18 #include <linux/sched.h>
  19 #include <linux/if_arp.h>
  20 #include <linux/timer.h>
  21 #include <net/caif/caif_layer.h>
  22 #include <net/caif/caif_hsi.h>
  23
  24 MODULE_LICENSE("GPL");
  25 MODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>");
  26 MODULE_DESCRIPTION("CAIF HSI driver");
  27
  28 /* Returns the number of padding bytes for alignment. */
  29 #define PAD_POW2(x, pow) ((((x)&((pow)-1)) == 0) ? 0 :\
  30                                 (((pow)-((x)&((pow)-1)))))
  31
  32 /*
  33  * HSI padding options.
  34  * Warning: must be a base of 2 (& operation used) and can not be zero !
  35  */
  36 static int hsi_head_align = 4;
  37 module_param(hsi_head_align, int, S_IRUGO);
  38 MODULE_PARM_DESC(hsi_head_align, "HSI head alignment.");
  39
  40 static int hsi_tail_align = 4;
  41 module_param(hsi_tail_align, int, S_IRUGO);
  42 MODULE_PARM_DESC(hsi_tail_align, "HSI tail alignment.");
  43
  44 /*
  45  * HSI link layer flowcontrol thresholds.
  46  * Warning: A high threshold value migth increase throughput but it will at
  47  * the same time prevent channel prioritization and increase the risk of
  48  * flooding the modem. The high threshold should be above the low.
  49  */
  50 static int hsi_high_threshold = 100;
  51 module_param(hsi_high_threshold, int, S_IRUGO);
  52 MODULE_PARM_DESC(hsi_high_threshold, "HSI high threshold (FLOW OFF).");
  53
  54 static int hsi_low_threshold = 50;
  55 module_param(hsi_low_threshold, int, S_IRUGO);
  56 MODULE_PARM_DESC(hsi_low_threshold, "HSI high threshold (FLOW ON).");
  57
  58 #define ON 1
  59 #define OFF 0
  60
  61 /*
  62  * Threshold values for the HSI packet queue. Flowcontrol will be asserted
  63  * when the number of packets exceeds HIGH_WATER_MARK. It will not be
  64  * de-asserted before the number of packets drops below LOW_WATER_MARK.
  65  */
  66 #define LOW_WATER_MARK   hsi_low_threshold
  67 #define HIGH_WATER_MARK  hsi_high_threshold
  68
  69 static LIST_HEAD(cfhsi_list);
  70 static spinlock_t cfhsi_list_lock;
  71
  72 static void cfhsi_inactivity_tout(unsigned long arg)
  73 {
  74         struct cfhsi *cfhsi = (struct cfhsi *)arg;
  75
  76         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  77                 __func__);
  78
  79         /* Schedule power down work queue. */
  80         if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  81                 queue_work(cfhsi->wq, &cfhsi->wake_down_work);
  82 }
  83
  84 static void cfhsi_abort_tx(struct cfhsi *cfhsi)
  85 {
  86         struct sk_buff *skb;
  87
  88         for (;;) {
  89                 spin_lock_bh(&cfhsi->lock);
  90                 skb = skb_dequeue(&cfhsi->qhead);
  91                 if (!skb)
  92                         break;
  93
  94                 cfhsi->ndev->stats.tx_errors++;
  95                 cfhsi->ndev->stats.tx_dropped++;
  96                 spin_unlock_bh(&cfhsi->lock);
  97                 kfree_skb(skb);
  98         }
  99         cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 100         if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 101                 mod_timer(&cfhsi->timer, jiffies + CFHSI_INACTIVITY_TOUT);
 102         spin_unlock_bh(&cfhsi->lock);
 103 }
 104
 105 static int cfhsi_flush_fifo(struct cfhsi *cfhsi)
 106 {
 107         char buffer[32]; /* Any reasonable value */
 108         size_t fifo_occupancy;
 109         int ret;
 110
 111         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 112                 __func__);
 113
 114
 115         ret = cfhsi->dev->cfhsi_wake_up(cfhsi->dev);
 116         if (ret) {
 117                 dev_warn(&cfhsi->ndev->dev,
 118                         "%s: can't wake up HSI interface: %d.\n",
 119                         __func__, ret);
 120                 return ret;
 121         }
 122
 123         do {
 124                 ret = cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
 125                                 &fifo_occupancy);
 126                 if (ret) {
 127                         dev_warn(&cfhsi->ndev->dev,
 128                                 "%s: can't get FIFO occupancy: %d.\n",
 129                                 __func__, ret);
 130                         break;
 131                 } else if (!fifo_occupancy)
 132                         /* No more data, exitting normally */
 133                         break;
 134
 135                 fifo_occupancy = min(sizeof(buffer), fifo_occupancy);
 136                 set_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits);
 137                 ret = cfhsi->dev->cfhsi_rx(buffer, fifo_occupancy,
 138                                 cfhsi->dev);
 139                 if (ret) {
 140                         clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits);
 141                         dev_warn(&cfhsi->ndev->dev,
 142                                 "%s: can't read data: %d.\n",
 143                                 __func__, ret);
 144                         break;
 145                 }
 146
 147                 ret = 5 * HZ;
 148                 wait_event_interruptible_timeout(cfhsi->flush_fifo_wait,
 149                          !test_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits), ret);
 150
 151                 if (ret < 0) {
 152                         dev_warn(&cfhsi->ndev->dev,
 153                                 "%s: can't wait for flush complete: %d.\n",
 154                                 __func__, ret);
 155                         break;
 156                 } else if (!ret) {
 157                         ret = -ETIMEDOUT;
 158                         dev_warn(&cfhsi->ndev->dev,
 159                                 "%s: timeout waiting for flush complete.\n",
 160                                 __func__);
 161                         break;
 162                 }
 163         } while (1);
 164
 165         cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
 166
 167         return ret;
 168 }
 169
 170 static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 171 {
 172         int nfrms = 0;
 173         int pld_len = 0;
 174         struct sk_buff *skb;
 175         u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 176
 177         skb = skb_dequeue(&cfhsi->qhead);
 178         if (!skb)
 179                 return 0;
 180
 181         /* Clear offset. */
 182         desc->offset = 0;
 183
 184         /* Check if we can embed a CAIF frame. */
 185         if (skb->len < CFHSI_MAX_EMB_FRM_SZ) {
 186                 struct caif_payload_info *info;
 187                 int hpad = 0;
 188                 int tpad = 0;
 189
 190                 /* Calculate needed head alignment and tail alignment. */
 191                 info = (struct caif_payload_info *)&skb->cb;
 192
 193                 hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
 194                 tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
 195
 196                 /* Check if frame still fits with added alignment. */
 197                 if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) {
 198                         u8 *pemb = desc->emb_frm;
 199                         desc->offset = CFHSI_DESC_SHORT_SZ;
 200                         *pemb = (u8)(hpad - 1);
 201                         pemb += hpad;
 202
 203                         /* Update network statistics. */
 204                         cfhsi->ndev->stats.tx_packets++;
 205                         cfhsi->ndev->stats.tx_bytes += skb->len;
 206
 207                         /* Copy in embedded CAIF frame. */
 208                         skb_copy_bits(skb, 0, pemb, skb->len);
 209                         consume_skb(skb);
 210                         skb = NULL;
 211                 }
 212         }
 213
 214         /* Create payload CAIF frames. */
 215         pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 216         while (nfrms < CFHSI_MAX_PKTS) {
 217                 struct caif_payload_info *info;
 218                 int hpad = 0;
 219                 int tpad = 0;
 220
 221                 if (!skb)
 222                         skb = skb_dequeue(&cfhsi->qhead);
 223
 224                 if (!skb)
 225                         break;
 226
 227                 /* Calculate needed head alignment and tail alignment. */
 228                 info = (struct caif_payload_info *)&skb->cb;
 229
 230                 hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
 231                 tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
 232
 233                 /* Fill in CAIF frame length in descriptor. */
 234                 desc->cffrm_len[nfrms] = hpad + skb->len + tpad;
 235
 236                 /* Fill head padding information. */
 237                 *pfrm = (u8)(hpad - 1);
 238                 pfrm += hpad;
 239
 240                 /* Update network statistics. */
 241                 cfhsi->ndev->stats.tx_packets++;
 242                 cfhsi->ndev->stats.tx_bytes += skb->len;
 243
 244                 /* Copy in CAIF frame. */
 245                 skb_copy_bits(skb, 0, pfrm, skb->len);
 246
 247                 /* Update payload length. */
 248                 pld_len += desc->cffrm_len[nfrms];
 249
 250                 /* Update frame pointer. */
 251                 pfrm += skb->len + tpad;
 252                 consume_skb(skb);
 253                 skb = NULL;
 254
 255                 /* Update number of frames. */
 256                 nfrms++;
 257         }
 258
 259         /* Unused length fields should be zero-filled (according to SPEC). */
 260         while (nfrms < CFHSI_MAX_PKTS) {
 261                 desc->cffrm_len[nfrms] = 0x0000;
 262                 nfrms++;
 263         }
 264
 265         /* Check if we can piggy-back another descriptor. */
 266         skb = skb_peek(&cfhsi->qhead);
 267         if (skb)
 268                 desc->header |= CFHSI_PIGGY_DESC;
 269         else
 270                 desc->header &= ~CFHSI_PIGGY_DESC;
 271
 272         return CFHSI_DESC_SZ + pld_len;
 273 }
 274
 275 static void cfhsi_tx_done_work(struct work_struct *work)
 276 {
 277         struct cfhsi *cfhsi = NULL;
 278         struct cfhsi_desc *desc = NULL;
 279         int len = 0;
 280         int res;
 281
 282         cfhsi = container_of(work, struct cfhsi, tx_done_work);
 283         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 284                 __func__);
 285
 286         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 287                 return;
 288
 289         desc = (struct cfhsi_desc *)cfhsi->tx_buf;
 290
 291         do {
 292                 /*
 293                  * Send flow on if flow off has been previously signalled
 294                  * and number of packets is below low water mark.
 295                  */
 296                 spin_lock_bh(&cfhsi->lock);
 297                 if (cfhsi->flow_off_sent &&
 298                                 cfhsi->qhead.qlen <= cfhsi->q_low_mark &&
 299                                 cfhsi->cfdev.flowctrl) {
 300
 301                         cfhsi->flow_off_sent = 0;
 302                         cfhsi->cfdev.flowctrl(cfhsi->ndev, ON);
 303                 }
 304                 spin_unlock_bh(&cfhsi->lock);
 305
 306                 /* Create HSI frame. */
 307                 do {
 308                         len = cfhsi_tx_frm(desc, cfhsi);
 309                         if (!len) {
 310                                 spin_lock_bh(&cfhsi->lock);
 311                                 if (unlikely(skb_peek(&cfhsi->qhead))) {
 312                                         spin_unlock_bh(&cfhsi->lock);
 313                                         continue;
 314                                 }
 315                                 cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 316                                 /* Start inactivity timer. */
 317                                 mod_timer(&cfhsi->timer,
 318                                         jiffies + CFHSI_INACTIVITY_TOUT);
 319                                 spin_unlock_bh(&cfhsi->lock);
 320                                 goto done;
 321                         }
 322                 } while (!len);
 323
 324                 /* Set up new transfer. */
 325                 res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
 326                 if (WARN_ON(res < 0)) {
 327                         dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
 328                                 __func__, res);
 329                 }
 330         } while (res < 0);
 331
 332 done:
 333         return;
 334 }
 335
 336 static void cfhsi_tx_done_cb(struct cfhsi_drv *drv)
 337 {
 338         struct cfhsi *cfhsi;
 339
 340         cfhsi = container_of(drv, struct cfhsi, drv);
 341         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 342                 __func__);
 343
 344         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 345                 return;
 346
 347         queue_work(cfhsi->wq, &cfhsi->tx_done_work);
 348 }
 349
 350 static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 351 {
 352         int xfer_sz = 0;
 353         int nfrms = 0;
 354         u16 *plen = NULL;
 355         u8 *pfrm = NULL;
 356
 357         if ((desc->header & ~CFHSI_PIGGY_DESC) ||
 358                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) {
 359                 dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n",
 360                         __func__);
 361                 return 0;
 362         }
 363
 364         /* Check for embedded CAIF frame. */
 365         if (desc->offset) {
 366                 struct sk_buff *skb;
 367                 u8 *dst = NULL;
 368                 int len = 0, retries = 0;
 369                 pfrm = ((u8 *)desc) + desc->offset;
 370
 371                 /* Remove offset padding. */
 372                 pfrm += *pfrm + 1;
 373
 374                 /* Read length of CAIF frame (little endian). */
 375                 len = *pfrm;
 376                 len |= ((*(pfrm+1)) << 8) & 0xFF00;
 377                 len += 2;       /* Add FCS fields. */
 378
 379
 380                 /* Allocate SKB (OK even in IRQ context). */
 381                 skb = alloc_skb(len + 1, GFP_KERNEL);
 382                 while (!skb) {
 383                         retries++;
 384                         schedule_timeout(1);
 385                         skb = alloc_skb(len + 1, GFP_KERNEL);
 386                         if (skb) {
 387                                 printk(KERN_WARNING "%s: slept for %u "
 388                                                 "before getting memory\n",
 389                                                 __func__, retries);
 390                                 break;
 391                         }
 392                         if (retries > HZ) {
 393                                 printk(KERN_ERR "%s: slept for 1HZ and "
 394                                                 "did not get memory\n",
 395                                                 __func__);
 396                                 cfhsi->ndev->stats.rx_dropped++;
 397                                 goto drop_frame;
 398                         }
 399                 }
 400                 caif_assert(skb != NULL);
 401
 402                 dst = skb_put(skb, len);
 403                 memcpy(dst, pfrm, len);
 404
 405                 skb->protocol = htons(ETH_P_CAIF);
 406                 skb_reset_mac_header(skb);
 407                 skb->dev = cfhsi->ndev;
 408
 409                 /*
 410                  * We are called from a arch specific platform device.
 411                  * Unfortunately we don't know what context we're
 412                  * running in.
 413                  */
 414                 if (in_interrupt())
 415                         netif_rx(skb);
 416                 else
 417                         netif_rx_ni(skb);
 418
 419                 /* Update network statistics. */
 420                 cfhsi->ndev->stats.rx_packets++;
 421                 cfhsi->ndev->stats.rx_bytes += len;
 422         }
 423
 424 drop_frame:
 425         /* Calculate transfer length. */
 426         plen = desc->cffrm_len;
 427         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 428                 xfer_sz += *plen;
 429                 plen++;
 430                 nfrms++;
 431         }
 432
 433         /* Check for piggy-backed descriptor. */
 434         if (desc->header & CFHSI_PIGGY_DESC)
 435                 xfer_sz += CFHSI_DESC_SZ;
 436
 437         if (xfer_sz % 4) {
 438                 dev_err(&cfhsi->ndev->dev,
 439                                 "%s: Invalid payload len: %d, ignored.\n",
 440                         __func__, xfer_sz);
 441                 xfer_sz = 0;
 442         }
 443
 444         return xfer_sz;
 445 }
 446
 447 static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 448 {
 449         int rx_sz = 0;
 450         int nfrms = 0;
 451         u16 *plen = NULL;
 452         u8 *pfrm = NULL;
 453
 454         /* Sanity check header and offset. */
 455         if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) ||
 456                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) {
 457                 dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n",
 458                         __func__);
 459                 return -EINVAL;
 460         }
 461
 462         /* Set frame pointer to start of payload. */
 463         pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 464         plen = desc->cffrm_len;
 465         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 466                 struct sk_buff *skb;
 467                 u8 *dst = NULL;
 468                 u8 *pcffrm = NULL;
 469                 int len = 0, retries = 0;
 470
 471                 if (WARN_ON(desc->cffrm_len[nfrms] > CFHSI_MAX_PAYLOAD_SZ)) {
 472                         dev_err(&cfhsi->ndev->dev, "%s: Invalid payload.\n",
 473                                 __func__);
 474                         return -EINVAL;
 475                 }
 476
 477                 /* CAIF frame starts after head padding. */
 478                 pcffrm = pfrm + *pfrm + 1;
 479
 480                 /* Read length of CAIF frame (little endian). */
 481                 len = *pcffrm;
 482                 len |= ((*(pcffrm + 1)) << 8) & 0xFF00;
 483                 len += 2;       /* Add FCS fields. */
 484
 485                 /* Allocate SKB (OK even in IRQ context). */
 486                 skb = alloc_skb(len + 1, GFP_KERNEL);
 487                 while (!skb) {
 488                         retries++;
 489                         schedule_timeout(1);
 490                         skb = alloc_skb(len + 1, GFP_KERNEL);
 491                         if (skb) {
 492                                 printk(KERN_WARNING "%s: slept for %u "
 493                                                 "before getting memory\n",
 494                                                 __func__, retries);
 495                                 break;
 496                         }
 497                         if (retries > HZ) {
 498                                 printk(KERN_ERR "%s: slept for 1HZ "
 499                                                 "and did not get memory\n",
 500                                                 __func__);
 501                                 cfhsi->ndev->stats.rx_dropped++;
 502                                 goto drop_frame;
 503                         }
 504                 }
 505                 caif_assert(skb != NULL);
 506
 507                 dst = skb_put(skb, len);
 508                 memcpy(dst, pcffrm, len);
 509
 510                 skb->protocol = htons(ETH_P_CAIF);
 511                 skb_reset_mac_header(skb);
 512                 skb->dev = cfhsi->ndev;
 513
 514                 /*
 515                  * We're called from a platform device,
 516                  * and don't know the context we're running in.
 517                  */
 518                 if (in_interrupt())
 519                         netif_rx(skb);
 520                 else
 521                         netif_rx_ni(skb);
 522
 523                 /* Update network statistics. */
 524                 cfhsi->ndev->stats.rx_packets++;
 525                 cfhsi->ndev->stats.rx_bytes += len;
 526
 527 drop_frame:
 528                 pfrm += *plen;
 529                 rx_sz += *plen;
 530                 plen++;
 531                 nfrms++;
 532         }
 533
 534         return rx_sz;
 535 }
 536
 537 static void cfhsi_rx_done_work(struct work_struct *work)
 538 {
 539         int res;
 540         int desc_pld_len = 0;
 541         struct cfhsi *cfhsi = NULL;
 542         struct cfhsi_desc *desc = NULL;
 543
 544         cfhsi = container_of(work, struct cfhsi, rx_done_work);
 545         desc = (struct cfhsi_desc *)cfhsi->rx_buf;
 546
 547         dev_dbg(&cfhsi->ndev->dev, "%s: Kick timer if pending.\n",
 548                 __func__);
 549
 550         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 551                 return;
 552
 553         /* Update inactivity timer if pending. */
 554         mod_timer_pending(&cfhsi->timer, jiffies + CFHSI_INACTIVITY_TOUT);
 555
 556         if (cfhsi->rx_state == CFHSI_RX_STATE_DESC) {
 557                 desc_pld_len = cfhsi_rx_desc(desc, cfhsi);
 558         } else {
 559                 int pld_len;
 560
 561                 pld_len = cfhsi_rx_pld(desc, cfhsi);
 562
 563                 if ((pld_len > 0) && (desc->header & CFHSI_PIGGY_DESC)) {
 564                         struct cfhsi_desc *piggy_desc;
 565                         piggy_desc = (struct cfhsi_desc *)
 566                                 (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ +
 567                                                 pld_len);
 568
 569                         /* Extract piggy-backed descriptor. */
 570                         desc_pld_len = cfhsi_rx_desc(piggy_desc, cfhsi);
 571
 572                         /*
 573                          * Copy needed information from the piggy-backed
 574                          * descriptor to the descriptor in the start.
 575                          */
 576                         memcpy((u8 *)desc, (u8 *)piggy_desc,
 577                                         CFHSI_DESC_SHORT_SZ);
 578                 }
 579         }
 580
 581         if (desc_pld_len) {
 582                 cfhsi->rx_state = CFHSI_RX_STATE_PAYLOAD;
 583                 cfhsi->rx_ptr = cfhsi->rx_buf + CFHSI_DESC_SZ;
 584                 cfhsi->rx_len = desc_pld_len;
 585         } else {
 586                 cfhsi->rx_state = CFHSI_RX_STATE_DESC;
 587                 cfhsi->rx_ptr = cfhsi->rx_buf;
 588                 cfhsi->rx_len = CFHSI_DESC_SZ;
 589         }
 590         clear_bit(CFHSI_PENDING_RX, &cfhsi->bits);
 591
 592         if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) {
 593                 /* Set up new transfer. */
 594                 dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n",
 595                         __func__);
 596                 res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len,
 597                                 cfhsi->dev);
 598                 if (WARN_ON(res < 0)) {
 599                         dev_err(&cfhsi->ndev->dev, "%s: RX error %d.\n",
 600                                 __func__, res);
 601                         cfhsi->ndev->stats.rx_errors++;
 602                         cfhsi->ndev->stats.rx_dropped++;
 603                 }
 604         }
 605 }
 606
 607 static void cfhsi_rx_done_cb(struct cfhsi_drv *drv)
 608 {
 609         struct cfhsi *cfhsi;
 610
 611         cfhsi = container_of(drv, struct cfhsi, drv);
 612         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 613                 __func__);
 614
 615         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 616                 return;
 617
 618         set_bit(CFHSI_PENDING_RX, &cfhsi->bits);
 619
 620         if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits))
 621                 wake_up_interruptible(&cfhsi->flush_fifo_wait);
 622         else
 623                 queue_work(cfhsi->wq, &cfhsi->rx_done_work);
 624 }
 625
 626 static void cfhsi_wake_up(struct work_struct *work)
 627 {
 628         struct cfhsi *cfhsi = NULL;
 629         int res;
 630         int len;
 631         long ret;
 632
 633         cfhsi = container_of(work, struct cfhsi, wake_up_work);
 634
 635         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 636                 return;
 637
 638         if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) {
 639                 /* It happenes when wakeup is requested by
 640                  * both ends at the same time. */
 641                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 642                 return;
 643         }
 644
 645         /* Activate wake line. */
 646         cfhsi->dev->cfhsi_wake_up(cfhsi->dev);
 647
 648         dev_dbg(&cfhsi->ndev->dev, "%s: Start waiting.\n",
 649                 __func__);
 650
 651         /* Wait for acknowledge. */
 652         ret = CFHSI_WAKEUP_TOUT;
 653         wait_event_interruptible_timeout(cfhsi->wake_up_wait,
 654                                         test_bit(CFHSI_WAKE_UP_ACK,
 655                                                         &cfhsi->bits), ret);
 656         if (unlikely(ret < 0)) {
 657                 /* Interrupted by signal. */
 658                 dev_info(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n",
 659                         __func__, ret);
 660                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 661                 cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
 662                 return;
 663         } else if (!ret) {
 664                 /* Wakeup timeout */
 665                 dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n",
 666                         __func__);
 667                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 668                 cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
 669                 return;
 670         }
 671         dev_dbg(&cfhsi->ndev->dev, "%s: Woken.\n",
 672                 __func__);
 673
 674         /* Clear power up bit. */
 675         set_bit(CFHSI_AWAKE, &cfhsi->bits);
 676         clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 677
 678         /* Resume read operation. */
 679         if (!test_bit(CFHSI_PENDING_RX, &cfhsi->bits)) {
 680                 dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n",
 681                         __func__);
 682                 res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr,
 683                                 cfhsi->rx_len, cfhsi->dev);
 684                 if (WARN_ON(res < 0)) {
 685                         dev_err(&cfhsi->ndev->dev, "%s: RX error %d.\n",
 686                                 __func__, res);
 687                 }
 688         }
 689
 690         /* Clear power up acknowledment. */
 691         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 692
 693         spin_lock_bh(&cfhsi->lock);
 694
 695         /* Resume transmit if queue is not empty. */
 696         if (!skb_peek(&cfhsi->qhead)) {
 697                 dev_dbg(&cfhsi->ndev->dev, "%s: Peer wake, start timer.\n",
 698                         __func__);
 699                 /* Start inactivity timer. */
 700                 mod_timer(&cfhsi->timer,
 701                                 jiffies + CFHSI_INACTIVITY_TOUT);
 702                 spin_unlock_bh(&cfhsi->lock);
 703                 return;
 704         }
 705
 706         dev_dbg(&cfhsi->ndev->dev, "%s: Host wake.\n",
 707                 __func__);
 708
 709         spin_unlock_bh(&cfhsi->lock);
 710
 711         /* Create HSI frame. */
 712         len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi);
 713
 714         if (likely(len > 0)) {
 715                 /* Set up new transfer. */
 716                 res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
 717                 if (WARN_ON(res < 0)) {
 718                         dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
 719                                 __func__, res);
 720                         cfhsi_abort_tx(cfhsi);
 721                 }
 722         } else {
 723                 dev_err(&cfhsi->ndev->dev,
 724                                 "%s: Failed to create HSI frame: %d.\n",
 725                                 __func__, len);
 726         }
 727
 728 }
 729
 730 static void cfhsi_wake_down(struct work_struct *work)
 731 {
 732         long ret;
 733         struct cfhsi *cfhsi = NULL;
 734         size_t fifo_occupancy;
 735
 736         cfhsi = container_of(work, struct cfhsi, wake_down_work);
 737         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 738                 __func__);
 739
 740         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 741                 return;
 742
 743         /* Check if there is something in FIFO. */
 744         if (WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
 745                                                         &fifo_occupancy)))
 746                 fifo_occupancy = 0;
 747
 748         if (fifo_occupancy) {
 749                 dev_dbg(&cfhsi->ndev->dev,
 750                                 "%s: %u words in RX FIFO, restart timer.\n",
 751                                 __func__, (unsigned) fifo_occupancy);
 752                 spin_lock_bh(&cfhsi->lock);
 753                 mod_timer(&cfhsi->timer,
 754                                 jiffies + CFHSI_INACTIVITY_TOUT);
 755                 spin_unlock_bh(&cfhsi->lock);
 756                 return;
 757         }
 758
 759         /* Cancel pending RX requests */
 760         cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
 761
 762         /* Deactivate wake line. */
 763         cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
 764
 765         /* Wait for acknowledge. */
 766         ret = CFHSI_WAKEUP_TOUT;
 767         ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait,
 768                                         test_bit(CFHSI_WAKE_DOWN_ACK,
 769                                                         &cfhsi->bits),
 770                                         ret);
 771         if (ret < 0) {
 772                 /* Interrupted by signal. */
 773                 dev_info(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n",
 774                         __func__, ret);
 775                 return;
 776         } else if (!ret) {
 777                 /* Timeout */
 778                 dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n",
 779                         __func__);
 780         }
 781
 782         /* Clear power down acknowledment. */
 783         clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
 784         clear_bit(CFHSI_AWAKE, &cfhsi->bits);
 785
 786         /* Check if there is something in FIFO. */
 787         if (WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
 788                                                         &fifo_occupancy)))
 789                 fifo_occupancy = 0;
 790
 791         if (fifo_occupancy) {
 792                 dev_dbg(&cfhsi->ndev->dev,
 793                                 "%s: %u words in RX FIFO, wakeup forced.\n",
 794                                 __func__, (unsigned) fifo_occupancy);
 795                 if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
 796                         queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 797         } else
 798                 dev_dbg(&cfhsi->ndev->dev, "%s: Done.\n",
 799                         __func__);
 800 }
 801
 802 static void cfhsi_wake_up_cb(struct cfhsi_drv *drv)
 803 {
 804         struct cfhsi *cfhsi = NULL;
 805
 806         cfhsi = container_of(drv, struct cfhsi, drv);
 807         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 808                 __func__);
 809
 810         set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 811         wake_up_interruptible(&cfhsi->wake_up_wait);
 812
 813         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 814                 return;
 815
 816         /* Schedule wake up work queue if the peer initiates. */
 817         if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
 818                 queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 819 }
 820
 821 static void cfhsi_wake_down_cb(struct cfhsi_drv *drv)
 822 {
 823         struct cfhsi *cfhsi = NULL;
 824
 825         cfhsi = container_of(drv, struct cfhsi, drv);
 826         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 827                 __func__);
 828
 829         /* Initiating low power is only permitted by the host (us). */
 830         set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
 831         wake_up_interruptible(&cfhsi->wake_down_wait);
 832 }
 833
 834 static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 835 {
 836         struct cfhsi *cfhsi = NULL;
 837         int start_xfer = 0;
 838         int timer_active;
 839
 840         if (!dev)
 841                 return -EINVAL;
 842
 843         cfhsi = netdev_priv(dev);
 844
 845         spin_lock_bh(&cfhsi->lock);
 846
 847         skb_queue_tail(&cfhsi->qhead, skb);
 848
 849         /* Sanity check; xmit should not be called after unregister_netdev */
 850         if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) {
 851                 spin_unlock_bh(&cfhsi->lock);
 852                 cfhsi_abort_tx(cfhsi);
 853                 return -EINVAL;
 854         }
 855
 856         /* Send flow off if number of packets is above high water mark. */
 857         if (!cfhsi->flow_off_sent &&
 858                 cfhsi->qhead.qlen > cfhsi->q_high_mark &&
 859                 cfhsi->cfdev.flowctrl) {
 860                 cfhsi->flow_off_sent = 1;
 861                 cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF);
 862         }
 863
 864         if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) {
 865                 cfhsi->tx_state = CFHSI_TX_STATE_XFER;
 866                 start_xfer = 1;
 867         }
 868
 869         spin_unlock_bh(&cfhsi->lock);
 870
 871         if (!start_xfer)
 872                 return 0;
 873
 874         /* Delete inactivity timer if started. */
 875 #ifdef CONFIG_SMP
 876         timer_active = del_timer_sync(&cfhsi->timer);
 877 #else
 878         timer_active = del_timer(&cfhsi->timer);
 879 #endif /* CONFIG_SMP */
 880
 881         if (timer_active) {
 882                 struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
 883                 int len;
 884                 int res;
 885
 886                 /* Create HSI frame. */
 887                 len = cfhsi_tx_frm(desc, cfhsi);
 888                 BUG_ON(!len);
 889
 890                 /* Set up new transfer. */
 891                 res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
 892                 if (WARN_ON(res < 0)) {
 893                         dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
 894                                 __func__, res);
 895                         cfhsi_abort_tx(cfhsi);
 896                 }
 897         } else {
 898                 /* Schedule wake up work queue if the we initiate. */
 899                 if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
 900                         queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 901         }
 902
 903         return 0;
 904 }
 905
 906 static int cfhsi_open(struct net_device *dev)
 907 {
 908         netif_wake_queue(dev);
 909
 910         return 0;
 911 }
 912
 913 static int cfhsi_close(struct net_device *dev)
 914 {
 915         netif_stop_queue(dev);
 916
 917         return 0;
 918 }
 919
 920 static const struct net_device_ops cfhsi_ops = {
 921         .ndo_open = cfhsi_open,
 922         .ndo_stop = cfhsi_close,
 923         .ndo_start_xmit = cfhsi_xmit
 924 };
 925
 926 static void cfhsi_setup(struct net_device *dev)
 927 {
 928         struct cfhsi *cfhsi = netdev_priv(dev);
 929         dev->features = 0;
 930         dev->netdev_ops = &cfhsi_ops;
 931         dev->type = ARPHRD_CAIF;
 932         dev->flags = IFF_POINTOPOINT | IFF_NOARP;
 933         dev->mtu = CFHSI_MAX_PAYLOAD_SZ;
 934         dev->tx_queue_len = 0;
 935         dev->destructor = free_netdev;
 936         skb_queue_head_init(&cfhsi->qhead);
 937         cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
 938         cfhsi->cfdev.use_frag = false;
 939         cfhsi->cfdev.use_stx = false;
 940         cfhsi->cfdev.use_fcs = false;
 941         cfhsi->ndev = dev;
 942 }
 943
 944 int cfhsi_probe(struct platform_device *pdev)
 945 {
 946         struct cfhsi *cfhsi = NULL;
 947         struct net_device *ndev;
 948         struct cfhsi_dev *dev;
 949         int res;
 950
 951         ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup);
 952         if (!ndev)
 953                 return -ENODEV;
 954
 955         cfhsi = netdev_priv(ndev);
 956         cfhsi->ndev = ndev;
 957         cfhsi->pdev = pdev;
 958
 959         /* Initialize state vaiables. */
 960         cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 961         cfhsi->rx_state = CFHSI_RX_STATE_DESC;
 962
 963         /* Set flow info */
 964         cfhsi->flow_off_sent = 0;
 965         cfhsi->q_low_mark = LOW_WATER_MARK;
 966         cfhsi->q_high_mark = HIGH_WATER_MARK;
 967
 968         /* Assign the HSI device. */
 969         dev = (struct cfhsi_dev *)pdev->dev.platform_data;
 970         cfhsi->dev = dev;
 971
 972         /* Assign the driver to this HSI device. */
 973         dev->drv = &cfhsi->drv;
 974
 975         /*
 976          * Allocate a TX buffer with the size of a HSI packet descriptors
 977          * and the necessary room for CAIF payload frames.
 978          */
 979         cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL);
 980         if (!cfhsi->tx_buf) {
 981                 res = -ENODEV;
 982                 goto err_alloc_tx;
 983         }
 984
 985         /*
 986          * Allocate a RX buffer with the size of two HSI packet descriptors and
 987          * the necessary room for CAIF payload frames.
 988          */
 989         cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
 990         if (!cfhsi->rx_buf) {
 991                 res = -ENODEV;
 992                 goto err_alloc_rx;
 993         }
 994
 995         /* Initialize receive variables. */
 996         cfhsi->rx_ptr = cfhsi->rx_buf;
 997         cfhsi->rx_len = CFHSI_DESC_SZ;
 998
 999         /* Initialize spin locks. */
1000         spin_lock_init(&cfhsi->lock);
1001
1002         /* Set up the driver. */
1003         cfhsi->drv.tx_done_cb = cfhsi_tx_done_cb;
1004         cfhsi->drv.rx_done_cb = cfhsi_rx_done_cb;
1005         cfhsi->drv.wake_up_cb = cfhsi_wake_up_cb;
1006         cfhsi->drv.wake_down_cb = cfhsi_wake_down_cb;
1007
1008         /* Initialize the work queues. */
1009         INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up);
1010         INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down);
1011         INIT_WORK(&cfhsi->rx_done_work, cfhsi_rx_done_work);
1012         INIT_WORK(&cfhsi->tx_done_work, cfhsi_tx_done_work);
1013
1014         /* Clear all bit fields. */
1015         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
1016         clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
1017         clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
1018         clear_bit(CFHSI_AWAKE, &cfhsi->bits);
1019         clear_bit(CFHSI_PENDING_RX, &cfhsi->bits);
1020
1021         /* Create work thread. */
1022         cfhsi->wq = create_singlethread_workqueue(pdev->name);
1023         if (!cfhsi->wq) {
1024                 dev_err(&ndev->dev, "%s: Failed to create work queue.\n",
1025                         __func__);
1026                 res = -ENODEV;
1027                 goto err_create_wq;
1028         }
1029
1030         /* Initialize wait queues. */
1031         init_waitqueue_head(&cfhsi->wake_up_wait);
1032         init_waitqueue_head(&cfhsi->wake_down_wait);
1033         init_waitqueue_head(&cfhsi->flush_fifo_wait);
1034
1035         /* Setup the inactivity timer. */
1036         init_timer(&cfhsi->timer);
1037         cfhsi->timer.data = (unsigned long)cfhsi;
1038         cfhsi->timer.function = cfhsi_inactivity_tout;
1039
1040         /* Add CAIF HSI device to list. */
1041         spin_lock(&cfhsi_list_lock);
1042         list_add_tail(&cfhsi->list, &cfhsi_list);
1043         spin_unlock(&cfhsi_list_lock);
1044
1045         /* Activate HSI interface. */
1046         res = cfhsi->dev->cfhsi_up(cfhsi->dev);
1047         if (res) {
1048                 dev_err(&cfhsi->ndev->dev,
1049                         "%s: can't activate HSI interface: %d.\n",
1050                         __func__, res);
1051                 goto err_activate;
1052         }
1053
1054         /* Flush FIFO */
1055         res = cfhsi_flush_fifo(cfhsi);
1056         if (res) {
1057                 dev_err(&ndev->dev, "%s: Can't flush FIFO: %d.\n",
1058                         __func__, res);
1059                 goto err_net_reg;
1060         }
1061
1062         /* Register network device. */
1063         res = register_netdev(ndev);
1064         if (res) {
1065                 dev_err(&ndev->dev, "%s: Registration error: %d.\n",
1066                         __func__, res);
1067                 goto err_net_reg;
1068         }
1069
1070         netif_stop_queue(ndev);
1071
1072         return res;
1073
1074  err_net_reg:
1075         cfhsi->dev->cfhsi_down(cfhsi->dev);
1076  err_activate:
1077         destroy_workqueue(cfhsi->wq);
1078  err_create_wq:
1079         kfree(cfhsi->rx_buf);
1080  err_alloc_rx:
1081         kfree(cfhsi->tx_buf);
1082  err_alloc_tx:
1083         free_netdev(ndev);
1084
1085         return res;
1086 }
1087
1088 static void cfhsi_shutdown(struct cfhsi *cfhsi, bool remove_platform_dev)
1089 {
1090         u8 *tx_buf, *rx_buf;
1091
1092         /* Stop TXing */
1093         netif_tx_stop_all_queues(cfhsi->ndev);
1094
1095         /* going to shutdown driver */
1096         set_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
1097
1098         if (remove_platform_dev) {
1099                 /* Flush workqueue */
1100                 flush_workqueue(cfhsi->wq);
1101
1102                 /* Notify device. */
1103                 platform_device_unregister(cfhsi->pdev);
1104         }
1105
1106         /* Flush workqueue */
1107         flush_workqueue(cfhsi->wq);
1108
1109         /* Delete timer if pending */
1110 #ifdef CONFIG_SMP
1111         del_timer_sync(&cfhsi->timer);
1112 #else
1113         del_timer(&cfhsi->timer);
1114 #endif /* CONFIG_SMP */
1115
1116         /* Cancel pending RX request (if any) */
1117         cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
1118
1119         /* Flush again and destroy workqueue */
1120         destroy_workqueue(cfhsi->wq);
1121
1122         /* Store bufferes: will be freed later. */
1123         tx_buf = cfhsi->tx_buf;
1124         rx_buf = cfhsi->rx_buf;
1125
1126         /* Flush transmit queues. */
1127         cfhsi_abort_tx(cfhsi);
1128
1129         /* Deactivate interface */
1130         cfhsi->dev->cfhsi_down(cfhsi->dev);
1131
1132         /* Finally unregister the network device. */
1133         unregister_netdev(cfhsi->ndev);
1134
1135         /* Free buffers. */
1136         kfree(tx_buf);
1137         kfree(rx_buf);
1138 }
1139
1140 int cfhsi_remove(struct platform_device *pdev)
1141 {
1142         struct list_head *list_node;
1143         struct list_head *n;
1144         struct cfhsi *cfhsi = NULL;
1145         struct cfhsi_dev *dev;
1146
1147         dev = (struct cfhsi_dev *)pdev->dev.platform_data;
1148         spin_lock(&cfhsi_list_lock);
1149         list_for_each_safe(list_node, n, &cfhsi_list) {
1150                 cfhsi = list_entry(list_node, struct cfhsi, list);
1151                 /* Find the corresponding device. */
1152                 if (cfhsi->dev == dev) {
1153                         /* Remove from list. */
1154                         list_del(list_node);
1155                         spin_unlock(&cfhsi_list_lock);
1156
1157                         /* Shutdown driver. */
1158                         cfhsi_shutdown(cfhsi, false);
1159
1160                         return 0;
1161                 }
1162         }
1163         spin_unlock(&cfhsi_list_lock);
1164         return -ENODEV;
1165 }
1166
1167 struct platform_driver cfhsi_plat_drv = {
1168         .probe = cfhsi_probe,
1169         .remove = cfhsi_remove,
1170         .driver = {
1171                    .name = "cfhsi",
1172                    .owner = THIS_MODULE,
1173                    },
1174 };
1175
1176 static void __exit cfhsi_exit_module(void)
1177 {
1178         struct list_head *list_node;
1179         struct list_head *n;
1180         struct cfhsi *cfhsi = NULL;
1181
1182         spin_lock(&cfhsi_list_lock);
1183         list_for_each_safe(list_node, n, &cfhsi_list) {
1184                 cfhsi = list_entry(list_node, struct cfhsi, list);
1185
1186                 /* Remove from list. */
1187                 list_del(list_node);
1188                 spin_unlock(&cfhsi_list_lock);
1189
1190                 /* Shutdown driver. */
1191                 cfhsi_shutdown(cfhsi, true);
1192
1193                 spin_lock(&cfhsi_list_lock);
1194         }
1195         spin_unlock(&cfhsi_list_lock);
1196
1197         /* Unregister platform driver. */
1198         platform_driver_unregister(&cfhsi_plat_drv);
1199 }
1200
1201 static int __init cfhsi_init_module(void)
1202 {
1203         int result;
1204
1205         /* Initialize spin lock. */
1206         spin_lock_init(&cfhsi_list_lock);
1207
1208         /* Register platform driver. */
1209         result = platform_driver_register(&cfhsi_plat_drv);
1210         if (result) {
1211                 printk(KERN_ERR "Could not register platform HSI driver: %d.\n",
1212                         result);
1213                 goto err_dev_register;
1214         }
1215
1216         return result;
1217
1218  err_dev_register:
1219         return result;
1220 }
1221
1222 module_init(cfhsi_init_module);
1223 module_exit(cfhsi_exit_module);