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                 len = cfhsi_tx_frm(desc, cfhsi);
 308                 if (!len) {
 309                         cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 310                         /* Start inactivity timer. */
 311                         mod_timer(&cfhsi->timer,
 312                                         jiffies + CFHSI_INACTIVITY_TOUT);
 313                         break;
 314                 }
 315
 316                 /* Set up new transfer. */
 317                 res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
 318                 if (WARN_ON(res < 0)) {
 319                         dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
 320                                 __func__, res);
 321                 }
 322         } while (res < 0);
 323 }
 324
 325 static void cfhsi_tx_done_cb(struct cfhsi_drv *drv)
 326 {
 327         struct cfhsi *cfhsi;
 328
 329         cfhsi = container_of(drv, struct cfhsi, drv);
 330         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 331                 __func__);
 332
 333         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 334                 return;
 335
 336         queue_work(cfhsi->wq, &cfhsi->tx_done_work);
 337 }
 338
 339 static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 340 {
 341         int xfer_sz = 0;
 342         int nfrms = 0;
 343         u16 *plen = NULL;
 344         u8 *pfrm = NULL;
 345
 346         if ((desc->header & ~CFHSI_PIGGY_DESC) ||
 347                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) {
 348                 dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n",
 349                         __func__);
 350                 return 0;
 351         }
 352
 353         /* Check for embedded CAIF frame. */
 354         if (desc->offset) {
 355                 struct sk_buff *skb;
 356                 u8 *dst = NULL;
 357                 int len = 0, retries = 0;
 358                 pfrm = ((u8 *)desc) + desc->offset;
 359
 360                 /* Remove offset padding. */
 361                 pfrm += *pfrm + 1;
 362
 363                 /* Read length of CAIF frame (little endian). */
 364                 len = *pfrm;
 365                 len |= ((*(pfrm+1)) << 8) & 0xFF00;
 366                 len += 2;       /* Add FCS fields. */
 367
 368
 369                 /* Allocate SKB (OK even in IRQ context). */
 370                 skb = alloc_skb(len + 1, GFP_KERNEL);
 371                 while (!skb) {
 372                         retries++;
 373                         schedule_timeout(1);
 374                         skb = alloc_skb(len + 1, GFP_KERNEL);
 375                         if (skb) {
 376                                 printk(KERN_WARNING "%s: slept for %u "
 377                                                 "before getting memory\n",
 378                                                 __func__, retries);
 379                                 break;
 380                         }
 381                         if (retries > HZ) {
 382                                 printk(KERN_ERR "%s: slept for 1HZ and "
 383                                                 "did not get memory\n",
 384                                                 __func__);
 385                                 cfhsi->ndev->stats.rx_dropped++;
 386                                 goto drop_frame;
 387                         }
 388                 }
 389                 caif_assert(skb != NULL);
 390
 391                 dst = skb_put(skb, len);
 392                 memcpy(dst, pfrm, len);
 393
 394                 skb->protocol = htons(ETH_P_CAIF);
 395                 skb_reset_mac_header(skb);
 396                 skb->dev = cfhsi->ndev;
 397
 398                 /*
 399                  * We are called from a arch specific platform device.
 400                  * Unfortunately we don't know what context we're
 401                  * running in.
 402                  */
 403                 if (in_interrupt())
 404                         netif_rx(skb);
 405                 else
 406                         netif_rx_ni(skb);
 407
 408                 /* Update network statistics. */
 409                 cfhsi->ndev->stats.rx_packets++;
 410                 cfhsi->ndev->stats.rx_bytes += len;
 411         }
 412
 413 drop_frame:
 414         /* Calculate transfer length. */
 415         plen = desc->cffrm_len;
 416         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 417                 xfer_sz += *plen;
 418                 plen++;
 419                 nfrms++;
 420         }
 421
 422         /* Check for piggy-backed descriptor. */
 423         if (desc->header & CFHSI_PIGGY_DESC)
 424                 xfer_sz += CFHSI_DESC_SZ;
 425
 426         if (xfer_sz % 4) {
 427                 dev_err(&cfhsi->ndev->dev,
 428                                 "%s: Invalid payload len: %d, ignored.\n",
 429                         __func__, xfer_sz);
 430                 xfer_sz = 0;
 431         }
 432
 433         return xfer_sz;
 434 }
 435
 436 static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 437 {
 438         int rx_sz = 0;
 439         int nfrms = 0;
 440         u16 *plen = NULL;
 441         u8 *pfrm = NULL;
 442
 443         /* Sanity check header and offset. */
 444         if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) ||
 445                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) {
 446                 dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n",
 447                         __func__);
 448                 return -EINVAL;
 449         }
 450
 451         /* Set frame pointer to start of payload. */
 452         pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 453         plen = desc->cffrm_len;
 454         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 455                 struct sk_buff *skb;
 456                 u8 *dst = NULL;
 457                 u8 *pcffrm = NULL;
 458                 int len = 0, retries = 0;
 459
 460                 if (WARN_ON(desc->cffrm_len[nfrms] > CFHSI_MAX_PAYLOAD_SZ)) {
 461                         dev_err(&cfhsi->ndev->dev, "%s: Invalid payload.\n",
 462                                 __func__);
 463                         return -EINVAL;
 464                 }
 465
 466                 /* CAIF frame starts after head padding. */
 467                 pcffrm = pfrm + *pfrm + 1;
 468
 469                 /* Read length of CAIF frame (little endian). */
 470                 len = *pcffrm;
 471                 len |= ((*(pcffrm + 1)) << 8) & 0xFF00;
 472                 len += 2;       /* Add FCS fields. */
 473
 474                 /* Allocate SKB (OK even in IRQ context). */
 475                 skb = alloc_skb(len + 1, GFP_KERNEL);
 476                 while (!skb) {
 477                         retries++;
 478                         schedule_timeout(1);
 479                         skb = alloc_skb(len + 1, GFP_KERNEL);
 480                         if (skb) {
 481                                 printk(KERN_WARNING "%s: slept for %u "
 482                                                 "before getting memory\n",
 483                                                 __func__, retries);
 484                                 break;
 485                         }
 486                         if (retries > HZ) {
 487                                 printk(KERN_ERR "%s: slept for 1HZ "
 488                                                 "and did not get memory\n",
 489                                                 __func__);
 490                                 cfhsi->ndev->stats.rx_dropped++;
 491                                 goto drop_frame;
 492                         }
 493                 }
 494                 caif_assert(skb != NULL);
 495
 496                 dst = skb_put(skb, len);
 497                 memcpy(dst, pcffrm, len);
 498
 499                 skb->protocol = htons(ETH_P_CAIF);
 500                 skb_reset_mac_header(skb);
 501                 skb->dev = cfhsi->ndev;
 502
 503                 /*
 504                  * We're called from a platform device,
 505                  * and don't know the context we're running in.
 506                  */
 507                 if (in_interrupt())
 508                         netif_rx(skb);
 509                 else
 510                         netif_rx_ni(skb);
 511
 512                 /* Update network statistics. */
 513                 cfhsi->ndev->stats.rx_packets++;
 514                 cfhsi->ndev->stats.rx_bytes += len;
 515
 516 drop_frame:
 517                 pfrm += *plen;
 518                 rx_sz += *plen;
 519                 plen++;
 520                 nfrms++;
 521         }
 522
 523         return rx_sz;
 524 }
 525
 526 static void cfhsi_rx_done_work(struct work_struct *work)
 527 {
 528         int res;
 529         int desc_pld_len = 0;
 530         struct cfhsi *cfhsi = NULL;
 531         struct cfhsi_desc *desc = NULL;
 532
 533         cfhsi = container_of(work, struct cfhsi, rx_done_work);
 534         desc = (struct cfhsi_desc *)cfhsi->rx_buf;
 535
 536         dev_dbg(&cfhsi->ndev->dev, "%s: Kick timer if pending.\n",
 537                 __func__);
 538
 539         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 540                 return;
 541
 542         /* Update inactivity timer if pending. */
 543         mod_timer_pending(&cfhsi->timer, jiffies + CFHSI_INACTIVITY_TOUT);
 544
 545         if (cfhsi->rx_state == CFHSI_RX_STATE_DESC) {
 546                 desc_pld_len = cfhsi_rx_desc(desc, cfhsi);
 547         } else {
 548                 int pld_len;
 549
 550                 pld_len = cfhsi_rx_pld(desc, cfhsi);
 551
 552                 if ((pld_len > 0) && (desc->header & CFHSI_PIGGY_DESC)) {
 553                         struct cfhsi_desc *piggy_desc;
 554                         piggy_desc = (struct cfhsi_desc *)
 555                                 (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ +
 556                                                 pld_len);
 557
 558                         /* Extract piggy-backed descriptor. */
 559                         desc_pld_len = cfhsi_rx_desc(piggy_desc, cfhsi);
 560
 561                         /*
 562                          * Copy needed information from the piggy-backed
 563                          * descriptor to the descriptor in the start.
 564                          */
 565                         memcpy((u8 *)desc, (u8 *)piggy_desc,
 566                                         CFHSI_DESC_SHORT_SZ);
 567                 }
 568         }
 569
 570         if (desc_pld_len) {
 571                 cfhsi->rx_state = CFHSI_RX_STATE_PAYLOAD;
 572                 cfhsi->rx_ptr = cfhsi->rx_buf + CFHSI_DESC_SZ;
 573                 cfhsi->rx_len = desc_pld_len;
 574         } else {
 575                 cfhsi->rx_state = CFHSI_RX_STATE_DESC;
 576                 cfhsi->rx_ptr = cfhsi->rx_buf;
 577                 cfhsi->rx_len = CFHSI_DESC_SZ;
 578         }
 579         clear_bit(CFHSI_PENDING_RX, &cfhsi->bits);
 580
 581         if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) {
 582                 /* Set up new transfer. */
 583                 dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n",
 584                         __func__);
 585                 res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len,
 586                                 cfhsi->dev);
 587                 if (WARN_ON(res < 0)) {
 588                         dev_err(&cfhsi->ndev->dev, "%s: RX error %d.\n",
 589                                 __func__, res);
 590                         cfhsi->ndev->stats.rx_errors++;
 591                         cfhsi->ndev->stats.rx_dropped++;
 592                 }
 593         }
 594 }
 595
 596 static void cfhsi_rx_done_cb(struct cfhsi_drv *drv)
 597 {
 598         struct cfhsi *cfhsi;
 599
 600         cfhsi = container_of(drv, struct cfhsi, drv);
 601         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 602                 __func__);
 603
 604         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 605                 return;
 606
 607         set_bit(CFHSI_PENDING_RX, &cfhsi->bits);
 608
 609         if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits))
 610                 wake_up_interruptible(&cfhsi->flush_fifo_wait);
 611         else
 612                 queue_work(cfhsi->wq, &cfhsi->rx_done_work);
 613 }
 614
 615 static void cfhsi_wake_up(struct work_struct *work)
 616 {
 617         struct cfhsi *cfhsi = NULL;
 618         int res;
 619         int len;
 620         long ret;
 621
 622         cfhsi = container_of(work, struct cfhsi, wake_up_work);
 623
 624         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 625                 return;
 626
 627         if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) {
 628                 /* It happenes when wakeup is requested by
 629                  * both ends at the same time. */
 630                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 631                 return;
 632         }
 633
 634         /* Activate wake line. */
 635         cfhsi->dev->cfhsi_wake_up(cfhsi->dev);
 636
 637         dev_dbg(&cfhsi->ndev->dev, "%s: Start waiting.\n",
 638                 __func__);
 639
 640         /* Wait for acknowledge. */
 641         ret = CFHSI_WAKEUP_TOUT;
 642         wait_event_interruptible_timeout(cfhsi->wake_up_wait,
 643                                         test_bit(CFHSI_WAKE_UP_ACK,
 644                                                         &cfhsi->bits), ret);
 645         if (unlikely(ret < 0)) {
 646                 /* Interrupted by signal. */
 647                 dev_info(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n",
 648                         __func__, ret);
 649                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 650                 cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
 651                 return;
 652         } else if (!ret) {
 653                 /* Wakeup timeout */
 654                 dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n",
 655                         __func__);
 656                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 657                 cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
 658                 return;
 659         }
 660         dev_dbg(&cfhsi->ndev->dev, "%s: Woken.\n",
 661                 __func__);
 662
 663         /* Clear power up bit. */
 664         set_bit(CFHSI_AWAKE, &cfhsi->bits);
 665         clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 666
 667         /* Resume read operation. */
 668         if (!test_bit(CFHSI_PENDING_RX, &cfhsi->bits)) {
 669                 dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n",
 670                         __func__);
 671                 res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr,
 672                                 cfhsi->rx_len, cfhsi->dev);
 673                 if (WARN_ON(res < 0)) {
 674                         dev_err(&cfhsi->ndev->dev, "%s: RX error %d.\n",
 675                                 __func__, res);
 676                 }
 677         }
 678
 679         /* Clear power up acknowledment. */
 680         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 681
 682         spin_lock_bh(&cfhsi->lock);
 683
 684         /* Resume transmit if queue is not empty. */
 685         if (!skb_peek(&cfhsi->qhead)) {
 686                 dev_dbg(&cfhsi->ndev->dev, "%s: Peer wake, start timer.\n",
 687                         __func__);
 688                 /* Start inactivity timer. */
 689                 mod_timer(&cfhsi->timer,
 690                                 jiffies + CFHSI_INACTIVITY_TOUT);
 691                 spin_unlock_bh(&cfhsi->lock);
 692                 return;
 693         }
 694
 695         dev_dbg(&cfhsi->ndev->dev, "%s: Host wake.\n",
 696                 __func__);
 697
 698         spin_unlock_bh(&cfhsi->lock);
 699
 700         /* Create HSI frame. */
 701         len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi);
 702
 703         if (likely(len > 0)) {
 704                 /* Set up new transfer. */
 705                 res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
 706                 if (WARN_ON(res < 0)) {
 707                         dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
 708                                 __func__, res);
 709                         cfhsi_abort_tx(cfhsi);
 710                 }
 711         } else {
 712                 dev_err(&cfhsi->ndev->dev,
 713                                 "%s: Failed to create HSI frame: %d.\n",
 714                                 __func__, len);
 715         }
 716
 717 }
 718
 719 static void cfhsi_wake_down(struct work_struct *work)
 720 {
 721         long ret;
 722         struct cfhsi *cfhsi = NULL;
 723         size_t fifo_occupancy;
 724
 725         cfhsi = container_of(work, struct cfhsi, wake_down_work);
 726         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 727                 __func__);
 728
 729         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 730                 return;
 731
 732         /* Check if there is something in FIFO. */
 733         if (WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
 734                                                         &fifo_occupancy)))
 735                 fifo_occupancy = 0;
 736
 737         if (fifo_occupancy) {
 738                 dev_dbg(&cfhsi->ndev->dev,
 739                                 "%s: %u words in RX FIFO, restart timer.\n",
 740                                 __func__, (unsigned) fifo_occupancy);
 741                 spin_lock_bh(&cfhsi->lock);
 742                 mod_timer(&cfhsi->timer,
 743                                 jiffies + CFHSI_INACTIVITY_TOUT);
 744                 spin_unlock_bh(&cfhsi->lock);
 745                 return;
 746         }
 747
 748         /* Cancel pending RX requests */
 749         cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
 750
 751         /* Deactivate wake line. */
 752         cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
 753
 754         /* Wait for acknowledge. */
 755         ret = CFHSI_WAKEUP_TOUT;
 756         ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait,
 757                                         test_bit(CFHSI_WAKE_DOWN_ACK,
 758                                                         &cfhsi->bits),
 759                                         ret);
 760         if (ret < 0) {
 761                 /* Interrupted by signal. */
 762                 dev_info(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n",
 763                         __func__, ret);
 764                 return;
 765         } else if (!ret) {
 766                 /* Timeout */
 767                 dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n",
 768                         __func__);
 769         }
 770
 771         /* Clear power down acknowledment. */
 772         clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
 773         clear_bit(CFHSI_AWAKE, &cfhsi->bits);
 774
 775         /* Check if there is something in FIFO. */
 776         if (WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
 777                                                         &fifo_occupancy)))
 778                 fifo_occupancy = 0;
 779
 780         if (fifo_occupancy) {
 781                 dev_dbg(&cfhsi->ndev->dev,
 782                                 "%s: %u words in RX FIFO, wakeup forced.\n",
 783                                 __func__, (unsigned) fifo_occupancy);
 784                 if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
 785                         queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 786         } else
 787                 dev_dbg(&cfhsi->ndev->dev, "%s: Done.\n",
 788                         __func__);
 789 }
 790
 791 static void cfhsi_wake_up_cb(struct cfhsi_drv *drv)
 792 {
 793         struct cfhsi *cfhsi = NULL;
 794
 795         cfhsi = container_of(drv, struct cfhsi, drv);
 796         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 797                 __func__);
 798
 799         set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 800         wake_up_interruptible(&cfhsi->wake_up_wait);
 801
 802         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 803                 return;
 804
 805         /* Schedule wake up work queue if the peer initiates. */
 806         if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
 807                 queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 808 }
 809
 810 static void cfhsi_wake_down_cb(struct cfhsi_drv *drv)
 811 {
 812         struct cfhsi *cfhsi = NULL;
 813
 814         cfhsi = container_of(drv, struct cfhsi, drv);
 815         dev_dbg(&cfhsi->ndev->dev, "%s.\n",
 816                 __func__);
 817
 818         /* Initiating low power is only permitted by the host (us). */
 819         set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
 820         wake_up_interruptible(&cfhsi->wake_down_wait);
 821 }
 822
 823 static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 824 {
 825         struct cfhsi *cfhsi = NULL;
 826         int start_xfer = 0;
 827         int timer_active;
 828
 829         if (!dev)
 830                 return -EINVAL;
 831
 832         cfhsi = netdev_priv(dev);
 833
 834         spin_lock_bh(&cfhsi->lock);
 835
 836         skb_queue_tail(&cfhsi->qhead, skb);
 837
 838         /* Sanity check; xmit should not be called after unregister_netdev */
 839         if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) {
 840                 spin_unlock_bh(&cfhsi->lock);
 841                 cfhsi_abort_tx(cfhsi);
 842                 return -EINVAL;
 843         }
 844
 845         /* Send flow off if number of packets is above high water mark. */
 846         if (!cfhsi->flow_off_sent &&
 847                 cfhsi->qhead.qlen > cfhsi->q_high_mark &&
 848                 cfhsi->cfdev.flowctrl) {
 849                 cfhsi->flow_off_sent = 1;
 850                 cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF);
 851         }
 852
 853         if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) {
 854                 cfhsi->tx_state = CFHSI_TX_STATE_XFER;
 855                 start_xfer = 1;
 856         }
 857
 858         spin_unlock_bh(&cfhsi->lock);
 859
 860         if (!start_xfer)
 861                 return 0;
 862
 863         /* Delete inactivity timer if started. */
 864 #ifdef CONFIG_SMP
 865         timer_active = del_timer_sync(&cfhsi->timer);
 866 #else
 867         timer_active = del_timer(&cfhsi->timer);
 868 #endif /* CONFIG_SMP */
 869
 870         if (timer_active) {
 871                 struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
 872                 int len;
 873                 int res;
 874
 875                 /* Create HSI frame. */
 876                 len = cfhsi_tx_frm(desc, cfhsi);
 877                 BUG_ON(!len);
 878
 879                 /* Set up new transfer. */
 880                 res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
 881                 if (WARN_ON(res < 0)) {
 882                         dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
 883                                 __func__, res);
 884                         cfhsi_abort_tx(cfhsi);
 885                 }
 886         } else {
 887                 /* Schedule wake up work queue if the we initiate. */
 888                 if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
 889                         queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 890         }
 891
 892         return 0;
 893 }
 894
 895 static int cfhsi_open(struct net_device *dev)
 896 {
 897         netif_wake_queue(dev);
 898
 899         return 0;
 900 }
 901
 902 static int cfhsi_close(struct net_device *dev)
 903 {
 904         netif_stop_queue(dev);
 905
 906         return 0;
 907 }
 908
 909 static const struct net_device_ops cfhsi_ops = {
 910         .ndo_open = cfhsi_open,
 911         .ndo_stop = cfhsi_close,
 912         .ndo_start_xmit = cfhsi_xmit
 913 };
 914
 915 static void cfhsi_setup(struct net_device *dev)
 916 {
 917         struct cfhsi *cfhsi = netdev_priv(dev);
 918         dev->features = 0;
 919         dev->netdev_ops = &cfhsi_ops;
 920         dev->type = ARPHRD_CAIF;
 921         dev->flags = IFF_POINTOPOINT | IFF_NOARP;
 922         dev->mtu = CFHSI_MAX_PAYLOAD_SZ;
 923         dev->tx_queue_len = 0;
 924         dev->destructor = free_netdev;
 925         skb_queue_head_init(&cfhsi->qhead);
 926         cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
 927         cfhsi->cfdev.use_frag = false;
 928         cfhsi->cfdev.use_stx = false;
 929         cfhsi->cfdev.use_fcs = false;
 930         cfhsi->ndev = dev;
 931 }
 932
 933 int cfhsi_probe(struct platform_device *pdev)
 934 {
 935         struct cfhsi *cfhsi = NULL;
 936         struct net_device *ndev;
 937         struct cfhsi_dev *dev;
 938         int res;
 939
 940         ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup);
 941         if (!ndev)
 942                 return -ENODEV;
 943
 944         cfhsi = netdev_priv(ndev);
 945         cfhsi->ndev = ndev;
 946         cfhsi->pdev = pdev;
 947
 948         /* Initialize state vaiables. */
 949         cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 950         cfhsi->rx_state = CFHSI_RX_STATE_DESC;
 951
 952         /* Set flow info */
 953         cfhsi->flow_off_sent = 0;
 954         cfhsi->q_low_mark = LOW_WATER_MARK;
 955         cfhsi->q_high_mark = HIGH_WATER_MARK;
 956
 957         /* Assign the HSI device. */
 958         dev = (struct cfhsi_dev *)pdev->dev.platform_data;
 959         cfhsi->dev = dev;
 960
 961         /* Assign the driver to this HSI device. */
 962         dev->drv = &cfhsi->drv;
 963
 964         /*
 965          * Allocate a TX buffer with the size of a HSI packet descriptors
 966          * and the necessary room for CAIF payload frames.
 967          */
 968         cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL);
 969         if (!cfhsi->tx_buf) {
 970                 res = -ENODEV;
 971                 goto err_alloc_tx;
 972         }
 973
 974         /*
 975          * Allocate a RX buffer with the size of two HSI packet descriptors and
 976          * the necessary room for CAIF payload frames.
 977          */
 978         cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
 979         if (!cfhsi->rx_buf) {
 980                 res = -ENODEV;
 981                 goto err_alloc_rx;
 982         }
 983
 984         /* Initialize receive variables. */
 985         cfhsi->rx_ptr = cfhsi->rx_buf;
 986         cfhsi->rx_len = CFHSI_DESC_SZ;
 987
 988         /* Initialize spin locks. */
 989         spin_lock_init(&cfhsi->lock);
 990
 991         /* Set up the driver. */
 992         cfhsi->drv.tx_done_cb = cfhsi_tx_done_cb;
 993         cfhsi->drv.rx_done_cb = cfhsi_rx_done_cb;
 994         cfhsi->drv.wake_up_cb = cfhsi_wake_up_cb;
 995         cfhsi->drv.wake_down_cb = cfhsi_wake_down_cb;
 996
 997         /* Initialize the work queues. */
 998         INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up);
 999         INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down);
1000         INIT_WORK(&cfhsi->rx_done_work, cfhsi_rx_done_work);
1001         INIT_WORK(&cfhsi->tx_done_work, cfhsi_tx_done_work);
1002
1003         /* Clear all bit fields. */
1004         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
1005         clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
1006         clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
1007         clear_bit(CFHSI_AWAKE, &cfhsi->bits);
1008         clear_bit(CFHSI_PENDING_RX, &cfhsi->bits);
1009
1010         /* Create work thread. */
1011         cfhsi->wq = create_singlethread_workqueue(pdev->name);
1012         if (!cfhsi->wq) {
1013                 dev_err(&ndev->dev, "%s: Failed to create work queue.\n",
1014                         __func__);
1015                 res = -ENODEV;
1016                 goto err_create_wq;
1017         }
1018
1019         /* Initialize wait queues. */
1020         init_waitqueue_head(&cfhsi->wake_up_wait);
1021         init_waitqueue_head(&cfhsi->wake_down_wait);
1022         init_waitqueue_head(&cfhsi->flush_fifo_wait);
1023
1024         /* Setup the inactivity timer. */
1025         init_timer(&cfhsi->timer);
1026         cfhsi->timer.data = (unsigned long)cfhsi;
1027         cfhsi->timer.function = cfhsi_inactivity_tout;
1028
1029         /* Add CAIF HSI device to list. */
1030         spin_lock(&cfhsi_list_lock);
1031         list_add_tail(&cfhsi->list, &cfhsi_list);
1032         spin_unlock(&cfhsi_list_lock);
1033
1034         /* Activate HSI interface. */
1035         res = cfhsi->dev->cfhsi_up(cfhsi->dev);
1036         if (res) {
1037                 dev_err(&cfhsi->ndev->dev,
1038                         "%s: can't activate HSI interface: %d.\n",
1039                         __func__, res);
1040                 goto err_activate;
1041         }
1042
1043         /* Flush FIFO */
1044         res = cfhsi_flush_fifo(cfhsi);
1045         if (res) {
1046                 dev_err(&ndev->dev, "%s: Can't flush FIFO: %d.\n",
1047                         __func__, res);
1048                 goto err_net_reg;
1049         }
1050
1051         /* Register network device. */
1052         res = register_netdev(ndev);
1053         if (res) {
1054                 dev_err(&ndev->dev, "%s: Registration error: %d.\n",
1055                         __func__, res);
1056                 goto err_net_reg;
1057         }
1058
1059         netif_stop_queue(ndev);
1060
1061         return res;
1062
1063  err_net_reg:
1064         cfhsi->dev->cfhsi_down(cfhsi->dev);
1065  err_activate:
1066         destroy_workqueue(cfhsi->wq);
1067  err_create_wq:
1068         kfree(cfhsi->rx_buf);
1069  err_alloc_rx:
1070         kfree(cfhsi->tx_buf);
1071  err_alloc_tx:
1072         free_netdev(ndev);
1073
1074         return res;
1075 }
1076
1077 static void cfhsi_shutdown(struct cfhsi *cfhsi, bool remove_platform_dev)
1078 {
1079         u8 *tx_buf, *rx_buf;
1080
1081         /* Stop TXing */
1082         netif_tx_stop_all_queues(cfhsi->ndev);
1083
1084         /* going to shutdown driver */
1085         set_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
1086
1087         if (remove_platform_dev) {
1088                 /* Flush workqueue */
1089                 flush_workqueue(cfhsi->wq);
1090
1091                 /* Notify device. */
1092                 platform_device_unregister(cfhsi->pdev);
1093         }
1094
1095         /* Flush workqueue */
1096         flush_workqueue(cfhsi->wq);
1097
1098         /* Delete timer if pending */
1099 #ifdef CONFIG_SMP
1100         del_timer_sync(&cfhsi->timer);
1101 #else
1102         del_timer(&cfhsi->timer);
1103 #endif /* CONFIG_SMP */
1104
1105         /* Cancel pending RX request (if any) */
1106         cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
1107
1108         /* Flush again and destroy workqueue */
1109         destroy_workqueue(cfhsi->wq);
1110
1111         /* Store bufferes: will be freed later. */
1112         tx_buf = cfhsi->tx_buf;
1113         rx_buf = cfhsi->rx_buf;
1114
1115         /* Flush transmit queues. */
1116         cfhsi_abort_tx(cfhsi);
1117
1118         /* Deactivate interface */
1119         cfhsi->dev->cfhsi_down(cfhsi->dev);
1120
1121         /* Finally unregister the network device. */
1122         unregister_netdev(cfhsi->ndev);
1123
1124         /* Free buffers. */
1125         kfree(tx_buf);
1126         kfree(rx_buf);
1127 }
1128
1129 int cfhsi_remove(struct platform_device *pdev)
1130 {
1131         struct list_head *list_node;
1132         struct list_head *n;
1133         struct cfhsi *cfhsi = NULL;
1134         struct cfhsi_dev *dev;
1135
1136         dev = (struct cfhsi_dev *)pdev->dev.platform_data;
1137         spin_lock(&cfhsi_list_lock);
1138         list_for_each_safe(list_node, n, &cfhsi_list) {
1139                 cfhsi = list_entry(list_node, struct cfhsi, list);
1140                 /* Find the corresponding device. */
1141                 if (cfhsi->dev == dev) {
1142                         /* Remove from list. */
1143                         list_del(list_node);
1144                         spin_unlock(&cfhsi_list_lock);
1145
1146                         /* Shutdown driver. */
1147                         cfhsi_shutdown(cfhsi, false);
1148
1149                         return 0;
1150                 }
1151         }
1152         spin_unlock(&cfhsi_list_lock);
1153         return -ENODEV;
1154 }
1155
1156 struct platform_driver cfhsi_plat_drv = {
1157         .probe = cfhsi_probe,
1158         .remove = cfhsi_remove,
1159         .driver = {
1160                    .name = "cfhsi",
1161                    .owner = THIS_MODULE,
1162                    },
1163 };
1164
1165 static void __exit cfhsi_exit_module(void)
1166 {
1167         struct list_head *list_node;
1168         struct list_head *n;
1169         struct cfhsi *cfhsi = NULL;
1170
1171         spin_lock(&cfhsi_list_lock);
1172         list_for_each_safe(list_node, n, &cfhsi_list) {
1173                 cfhsi = list_entry(list_node, struct cfhsi, list);
1174
1175                 /* Remove from list. */
1176                 list_del(list_node);
1177                 spin_unlock(&cfhsi_list_lock);
1178
1179                 /* Shutdown driver. */
1180                 cfhsi_shutdown(cfhsi, true);
1181
1182                 spin_lock(&cfhsi_list_lock);
1183         }
1184         spin_unlock(&cfhsi_list_lock);
1185
1186         /* Unregister platform driver. */
1187         platform_driver_unregister(&cfhsi_plat_drv);
1188 }
1189
1190 static int __init cfhsi_init_module(void)
1191 {
1192         int result;
1193
1194         /* Initialize spin lock. */
1195         spin_lock_init(&cfhsi_list_lock);
1196
1197         /* Register platform driver. */
1198         result = platform_driver_register(&cfhsi_plat_drv);
1199         if (result) {
1200                 printk(KERN_ERR "Could not register platform HSI driver: %d.\n",
1201                         result);
1202                 goto err_dev_register;
1203         }
1204
1205         return result;
1206
1207  err_dev_register:
1208         return result;
1209 }
1210
1211 module_init(cfhsi_init_module);
1212 module_exit(cfhsi_exit_module);