drivers/net/caif/caif_hsi.c

   1 /*
   2  * Copyright (C) ST-Ericsson AB 2010
   3  * Author:  Daniel Martensson
   4  *          Dmitry.Tarnyagin  / dmitry.tarnyagin@lockless.no
   5  * License terms: GNU General Public License (GPL) version 2.
   6  */
   7
   8 #define pr_fmt(fmt) KBUILD_MODNAME fmt
   9
  10 #include <linux/init.h>
  11 #include <linux/module.h>
  12 #include <linux/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/rtnetlink.h>
  22 #include <linux/pkt_sched.h>
  23 #include <net/caif/caif_layer.h>
  24 #include <net/caif/caif_hsi.h>
  25
  26 MODULE_LICENSE("GPL");
  27 MODULE_AUTHOR("Daniel Martensson");
  28 MODULE_DESCRIPTION("CAIF HSI driver");
  29
  30 /* Returns the number of padding bytes for alignment. */
  31 #define PAD_POW2(x, pow) ((((x)&((pow)-1)) == 0) ? 0 :\
  32                                 (((pow)-((x)&((pow)-1)))))
  33
  34 static const struct cfhsi_config  hsi_default_config = {
  35
  36         /* Inactivity timeout on HSI, ms */
  37         .inactivity_timeout = HZ,
  38
  39         /* Aggregation timeout (ms) of zero means no aggregation is done*/
  40         .aggregation_timeout = 1,
  41
  42         /*
  43          * HSI link layer flow-control thresholds.
  44          * Threshold values for the HSI packet queue. Flow-control will be
  45          * asserted when the number of packets exceeds q_high_mark. It will
  46          * not be de-asserted before the number of packets drops below
  47          * q_low_mark.
  48          * Warning: A high threshold value might increase throughput but it
  49          * will at the same time prevent channel prioritization and increase
  50          * the risk of flooding the modem. The high threshold should be above
  51          * the low.
  52          */
  53         .q_high_mark = 100,
  54         .q_low_mark = 50,
  55
  56         /*
  57          * HSI padding options.
  58          * Warning: must be a base of 2 (& operation used) and can not be zero !
  59          */
  60         .head_align = 4,
  61         .tail_align = 4,
  62 };
  63
  64 #define ON 1
  65 #define OFF 0
  66
  67 static LIST_HEAD(cfhsi_list);
  68
  69 static void cfhsi_inactivity_tout(unsigned long arg)
  70 {
  71         struct cfhsi *cfhsi = (struct cfhsi *)arg;
  72
  73         netdev_dbg(cfhsi->ndev, "%s.\n",
  74                 __func__);
  75
  76         /* Schedule power down work queue. */
  77         if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  78                 queue_work(cfhsi->wq, &cfhsi->wake_down_work);
  79 }
  80
  81 static void cfhsi_update_aggregation_stats(struct cfhsi *cfhsi,
  82                                            const struct sk_buff *skb,
  83                                            int direction)
  84 {
  85         struct caif_payload_info *info;
  86         int hpad, tpad, len;
  87
  88         info = (struct caif_payload_info *)&skb->cb;
  89         hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align);
  90         tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align);
  91         len = skb->len + hpad + tpad;
  92
  93         if (direction > 0)
  94                 cfhsi->aggregation_len += len;
  95         else if (direction < 0)
  96                 cfhsi->aggregation_len -= len;
  97 }
  98
  99 static bool cfhsi_can_send_aggregate(struct cfhsi *cfhsi)
 100 {
 101         int i;
 102
 103         if (cfhsi->cfg.aggregation_timeout == 0)
 104                 return true;
 105
 106         for (i = 0; i < CFHSI_PRIO_BEBK; ++i) {
 107                 if (cfhsi->qhead[i].qlen)
 108                         return true;
 109         }
 110
 111         /* TODO: Use aggregation_len instead */
 112         if (cfhsi->qhead[CFHSI_PRIO_BEBK].qlen >= CFHSI_MAX_PKTS)
 113                 return true;
 114
 115         return false;
 116 }
 117
 118 static struct sk_buff *cfhsi_dequeue(struct cfhsi *cfhsi)
 119 {
 120         struct sk_buff *skb;
 121         int i;
 122
 123         for (i = 0; i < CFHSI_PRIO_LAST; ++i) {
 124                 skb = skb_dequeue(&cfhsi->qhead[i]);
 125                 if (skb)
 126                         break;
 127         }
 128
 129         return skb;
 130 }
 131
 132 static int cfhsi_tx_queue_len(struct cfhsi *cfhsi)
 133 {
 134         int i, len = 0;
 135         for (i = 0; i < CFHSI_PRIO_LAST; ++i)
 136                 len += skb_queue_len(&cfhsi->qhead[i]);
 137         return len;
 138 }
 139
 140 static void cfhsi_abort_tx(struct cfhsi *cfhsi)
 141 {
 142         struct sk_buff *skb;
 143
 144         for (;;) {
 145                 spin_lock_bh(&cfhsi->lock);
 146                 skb = cfhsi_dequeue(cfhsi);
 147                 if (!skb)
 148                         break;
 149
 150                 cfhsi->ndev->stats.tx_errors++;
 151                 cfhsi->ndev->stats.tx_dropped++;
 152                 cfhsi_update_aggregation_stats(cfhsi, skb, -1);
 153                 spin_unlock_bh(&cfhsi->lock);
 154                 kfree_skb(skb);
 155         }
 156         cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 157         if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 158                 mod_timer(&cfhsi->inactivity_timer,
 159                         jiffies + cfhsi->cfg.inactivity_timeout);
 160         spin_unlock_bh(&cfhsi->lock);
 161 }
 162
 163 static int cfhsi_flush_fifo(struct cfhsi *cfhsi)
 164 {
 165         char buffer[32]; /* Any reasonable value */
 166         size_t fifo_occupancy;
 167         int ret;
 168
 169         netdev_dbg(cfhsi->ndev, "%s.\n",
 170                 __func__);
 171
 172         do {
 173                 ret = cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops,
 174                                 &fifo_occupancy);
 175                 if (ret) {
 176                         netdev_warn(cfhsi->ndev,
 177                                 "%s: can't get FIFO occupancy: %d.\n",
 178                                 __func__, ret);
 179                         break;
 180                 } else if (!fifo_occupancy)
 181                         /* No more data, exitting normally */
 182                         break;
 183
 184                 fifo_occupancy = min(sizeof(buffer), fifo_occupancy);
 185                 set_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits);
 186                 ret = cfhsi->ops->cfhsi_rx(buffer, fifo_occupancy,
 187                                 cfhsi->ops);
 188                 if (ret) {
 189                         clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits);
 190                         netdev_warn(cfhsi->ndev,
 191                                 "%s: can't read data: %d.\n",
 192                                 __func__, ret);
 193                         break;
 194                 }
 195
 196                 ret = 5 * HZ;
 197                 ret = wait_event_interruptible_timeout(cfhsi->flush_fifo_wait,
 198                          !test_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits), ret);
 199
 200                 if (ret < 0) {
 201                         netdev_warn(cfhsi->ndev,
 202                                 "%s: can't wait for flush complete: %d.\n",
 203                                 __func__, ret);
 204                         break;
 205                 } else if (!ret) {
 206                         ret = -ETIMEDOUT;
 207                         netdev_warn(cfhsi->ndev,
 208                                 "%s: timeout waiting for flush complete.\n",
 209                                 __func__);
 210                         break;
 211                 }
 212         } while (1);
 213
 214         return ret;
 215 }
 216
 217 static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 218 {
 219         int nfrms = 0;
 220         int pld_len = 0;
 221         struct sk_buff *skb;
 222         u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 223
 224         skb = cfhsi_dequeue(cfhsi);
 225         if (!skb)
 226                 return 0;
 227
 228         /* Clear offset. */
 229         desc->offset = 0;
 230
 231         /* Check if we can embed a CAIF frame. */
 232         if (skb->len < CFHSI_MAX_EMB_FRM_SZ) {
 233                 struct caif_payload_info *info;
 234                 int hpad;
 235                 int tpad;
 236
 237                 /* Calculate needed head alignment and tail alignment. */
 238                 info = (struct caif_payload_info *)&skb->cb;
 239
 240                 hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align);
 241                 tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align);
 242
 243                 /* Check if frame still fits with added alignment. */
 244                 if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) {
 245                         u8 *pemb = desc->emb_frm;
 246                         desc->offset = CFHSI_DESC_SHORT_SZ;
 247                         *pemb = (u8)(hpad - 1);
 248                         pemb += hpad;
 249
 250                         /* Update network statistics. */
 251                         spin_lock_bh(&cfhsi->lock);
 252                         cfhsi->ndev->stats.tx_packets++;
 253                         cfhsi->ndev->stats.tx_bytes += skb->len;
 254                         cfhsi_update_aggregation_stats(cfhsi, skb, -1);
 255                         spin_unlock_bh(&cfhsi->lock);
 256
 257                         /* Copy in embedded CAIF frame. */
 258                         skb_copy_bits(skb, 0, pemb, skb->len);
 259
 260                         /* Consume the SKB */
 261                         consume_skb(skb);
 262                         skb = NULL;
 263                 }
 264         }
 265
 266         /* Create payload CAIF frames. */
 267         pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 268         while (nfrms < CFHSI_MAX_PKTS) {
 269                 struct caif_payload_info *info;
 270                 int hpad;
 271                 int tpad;
 272
 273                 if (!skb)
 274                         skb = cfhsi_dequeue(cfhsi);
 275
 276                 if (!skb)
 277                         break;
 278
 279                 /* Calculate needed head alignment and tail alignment. */
 280                 info = (struct caif_payload_info *)&skb->cb;
 281
 282                 hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align);
 283                 tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align);
 284
 285                 /* Fill in CAIF frame length in descriptor. */
 286                 desc->cffrm_len[nfrms] = hpad + skb->len + tpad;
 287
 288                 /* Fill head padding information. */
 289                 *pfrm = (u8)(hpad - 1);
 290                 pfrm += hpad;
 291
 292                 /* Update network statistics. */
 293                 spin_lock_bh(&cfhsi->lock);
 294                 cfhsi->ndev->stats.tx_packets++;
 295                 cfhsi->ndev->stats.tx_bytes += skb->len;
 296                 cfhsi_update_aggregation_stats(cfhsi, skb, -1);
 297                 spin_unlock_bh(&cfhsi->lock);
 298
 299                 /* Copy in CAIF frame. */
 300                 skb_copy_bits(skb, 0, pfrm, skb->len);
 301
 302                 /* Update payload length. */
 303                 pld_len += desc->cffrm_len[nfrms];
 304
 305                 /* Update frame pointer. */
 306                 pfrm += skb->len + tpad;
 307
 308                 /* Consume the SKB */
 309                 consume_skb(skb);
 310                 skb = NULL;
 311
 312                 /* Update number of frames. */
 313                 nfrms++;
 314         }
 315
 316         /* Unused length fields should be zero-filled (according to SPEC). */
 317         while (nfrms < CFHSI_MAX_PKTS) {
 318                 desc->cffrm_len[nfrms] = 0x0000;
 319                 nfrms++;
 320         }
 321
 322         /* Check if we can piggy-back another descriptor. */
 323         if (cfhsi_can_send_aggregate(cfhsi))
 324                 desc->header |= CFHSI_PIGGY_DESC;
 325         else
 326                 desc->header &= ~CFHSI_PIGGY_DESC;
 327
 328         return CFHSI_DESC_SZ + pld_len;
 329 }
 330
 331 static void cfhsi_start_tx(struct cfhsi *cfhsi)
 332 {
 333         struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
 334         int len, res;
 335
 336         netdev_dbg(cfhsi->ndev, "%s.\n", __func__);
 337
 338         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 339                 return;
 340
 341         do {
 342                 /* Create HSI frame. */
 343                 len = cfhsi_tx_frm(desc, cfhsi);
 344                 if (!len) {
 345                         spin_lock_bh(&cfhsi->lock);
 346                         if (unlikely(cfhsi_tx_queue_len(cfhsi))) {
 347                                 spin_unlock_bh(&cfhsi->lock);
 348                                 res = -EAGAIN;
 349                                 continue;
 350                         }
 351                         cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 352                         /* Start inactivity timer. */
 353                         mod_timer(&cfhsi->inactivity_timer,
 354                                 jiffies + cfhsi->cfg.inactivity_timeout);
 355                         spin_unlock_bh(&cfhsi->lock);
 356                         break;
 357                 }
 358
 359                 /* Set up new transfer. */
 360                 res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops);
 361                 if (WARN_ON(res < 0))
 362                         netdev_err(cfhsi->ndev, "%s: TX error %d.\n",
 363                                 __func__, res);
 364         } while (res < 0);
 365 }
 366
 367 static void cfhsi_tx_done(struct cfhsi *cfhsi)
 368 {
 369         netdev_dbg(cfhsi->ndev, "%s.\n", __func__);
 370
 371         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 372                 return;
 373
 374         /*
 375          * Send flow on if flow off has been previously signalled
 376          * and number of packets is below low water mark.
 377          */
 378         spin_lock_bh(&cfhsi->lock);
 379         if (cfhsi->flow_off_sent &&
 380                         cfhsi_tx_queue_len(cfhsi) <= cfhsi->cfg.q_low_mark &&
 381                         cfhsi->cfdev.flowctrl) {
 382
 383                 cfhsi->flow_off_sent = 0;
 384                 cfhsi->cfdev.flowctrl(cfhsi->ndev, ON);
 385         }
 386
 387         if (cfhsi_can_send_aggregate(cfhsi)) {
 388                 spin_unlock_bh(&cfhsi->lock);
 389                 cfhsi_start_tx(cfhsi);
 390         } else {
 391                 mod_timer(&cfhsi->aggregation_timer,
 392                         jiffies + cfhsi->cfg.aggregation_timeout);
 393                 spin_unlock_bh(&cfhsi->lock);
 394         }
 395
 396         return;
 397 }
 398
 399 static void cfhsi_tx_done_cb(struct cfhsi_cb_ops *cb_ops)
 400 {
 401         struct cfhsi *cfhsi;
 402
 403         cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
 404         netdev_dbg(cfhsi->ndev, "%s.\n",
 405                 __func__);
 406
 407         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 408                 return;
 409         cfhsi_tx_done(cfhsi);
 410 }
 411
 412 static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 413 {
 414         int xfer_sz = 0;
 415         int nfrms = 0;
 416         u16 *plen = NULL;
 417         u8 *pfrm = NULL;
 418
 419         if ((desc->header & ~CFHSI_PIGGY_DESC) ||
 420                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) {
 421                 netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n",
 422                         __func__);
 423                 return -EPROTO;
 424         }
 425
 426         /* Check for embedded CAIF frame. */
 427         if (desc->offset) {
 428                 struct sk_buff *skb;
 429                 u8 *dst = NULL;
 430                 int len = 0;
 431                 pfrm = ((u8 *)desc) + desc->offset;
 432
 433                 /* Remove offset padding. */
 434                 pfrm += *pfrm + 1;
 435
 436                 /* Read length of CAIF frame (little endian). */
 437                 len = *pfrm;
 438                 len |= ((*(pfrm+1)) << 8) & 0xFF00;
 439                 len += 2;       /* Add FCS fields. */
 440
 441                 /* Sanity check length of CAIF frame. */
 442                 if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) {
 443                         netdev_err(cfhsi->ndev, "%s: Invalid length.\n",
 444                                 __func__);
 445                         return -EPROTO;
 446                 }
 447
 448                 /* Allocate SKB (OK even in IRQ context). */
 449                 skb = alloc_skb(len + 1, GFP_ATOMIC);
 450                 if (!skb) {
 451                         netdev_err(cfhsi->ndev, "%s: Out of memory !\n",
 452                                 __func__);
 453                         return -ENOMEM;
 454                 }
 455                 caif_assert(skb != NULL);
 456
 457                 dst = skb_put(skb, len);
 458                 memcpy(dst, pfrm, len);
 459
 460                 skb->protocol = htons(ETH_P_CAIF);
 461                 skb_reset_mac_header(skb);
 462                 skb->dev = cfhsi->ndev;
 463
 464                 /*
 465                  * We are in a callback handler and
 466                  * unfortunately we don't know what context we're
 467                  * running in.
 468                  */
 469                 if (in_interrupt())
 470                         netif_rx(skb);
 471                 else
 472                         netif_rx_ni(skb);
 473
 474                 /* Update network statistics. */
 475                 cfhsi->ndev->stats.rx_packets++;
 476                 cfhsi->ndev->stats.rx_bytes += len;
 477         }
 478
 479         /* Calculate transfer length. */
 480         plen = desc->cffrm_len;
 481         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 482                 xfer_sz += *plen;
 483                 plen++;
 484                 nfrms++;
 485         }
 486
 487         /* Check for piggy-backed descriptor. */
 488         if (desc->header & CFHSI_PIGGY_DESC)
 489                 xfer_sz += CFHSI_DESC_SZ;
 490
 491         if ((xfer_sz % 4) || (xfer_sz > (CFHSI_BUF_SZ_RX - CFHSI_DESC_SZ))) {
 492                 netdev_err(cfhsi->ndev,
 493                                 "%s: Invalid payload len: %d, ignored.\n",
 494                         __func__, xfer_sz);
 495                 return -EPROTO;
 496         }
 497         return xfer_sz;
 498 }
 499
 500 static int cfhsi_rx_desc_len(struct cfhsi_desc *desc)
 501 {
 502         int xfer_sz = 0;
 503         int nfrms = 0;
 504         u16 *plen;
 505
 506         if ((desc->header & ~CFHSI_PIGGY_DESC) ||
 507                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) {
 508
 509                 pr_err("Invalid descriptor. %x %x\n", desc->header,
 510                                 desc->offset);
 511                 return -EPROTO;
 512         }
 513
 514         /* Calculate transfer length. */
 515         plen = desc->cffrm_len;
 516         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 517                 xfer_sz += *plen;
 518                 plen++;
 519                 nfrms++;
 520         }
 521
 522         if (xfer_sz % 4) {
 523                 pr_err("Invalid payload len: %d, ignored.\n", xfer_sz);
 524                 return -EPROTO;
 525         }
 526         return xfer_sz;
 527 }
 528
 529 static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 530 {
 531         int rx_sz = 0;
 532         int nfrms = 0;
 533         u16 *plen = NULL;
 534         u8 *pfrm = NULL;
 535
 536         /* Sanity check header and offset. */
 537         if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) ||
 538                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) {
 539                 netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n",
 540                         __func__);
 541                 return -EPROTO;
 542         }
 543
 544         /* Set frame pointer to start of payload. */
 545         pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 546         plen = desc->cffrm_len;
 547
 548         /* Skip already processed frames. */
 549         while (nfrms < cfhsi->rx_state.nfrms) {
 550                 pfrm += *plen;
 551                 rx_sz += *plen;
 552                 plen++;
 553                 nfrms++;
 554         }
 555
 556         /* Parse payload. */
 557         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 558                 struct sk_buff *skb;
 559                 u8 *dst = NULL;
 560                 u8 *pcffrm = NULL;
 561                 int len;
 562
 563                 /* CAIF frame starts after head padding. */
 564                 pcffrm = pfrm + *pfrm + 1;
 565
 566                 /* Read length of CAIF frame (little endian). */
 567                 len = *pcffrm;
 568                 len |= ((*(pcffrm + 1)) << 8) & 0xFF00;
 569                 len += 2;       /* Add FCS fields. */
 570
 571                 /* Sanity check length of CAIF frames. */
 572                 if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) {
 573                         netdev_err(cfhsi->ndev, "%s: Invalid length.\n",
 574                                 __func__);
 575                         return -EPROTO;
 576                 }
 577
 578                 /* Allocate SKB (OK even in IRQ context). */
 579                 skb = alloc_skb(len + 1, GFP_ATOMIC);
 580                 if (!skb) {
 581                         netdev_err(cfhsi->ndev, "%s: Out of memory !\n",
 582                                 __func__);
 583                         cfhsi->rx_state.nfrms = nfrms;
 584                         return -ENOMEM;
 585                 }
 586                 caif_assert(skb != NULL);
 587
 588                 dst = skb_put(skb, len);
 589                 memcpy(dst, pcffrm, len);
 590
 591                 skb->protocol = htons(ETH_P_CAIF);
 592                 skb_reset_mac_header(skb);
 593                 skb->dev = cfhsi->ndev;
 594
 595                 /*
 596                  * We're called in callback from HSI
 597                  * and don't know the context we're running in.
 598                  */
 599                 if (in_interrupt())
 600                         netif_rx(skb);
 601                 else
 602                         netif_rx_ni(skb);
 603
 604                 /* Update network statistics. */
 605                 cfhsi->ndev->stats.rx_packets++;
 606                 cfhsi->ndev->stats.rx_bytes += len;
 607
 608                 pfrm += *plen;
 609                 rx_sz += *plen;
 610                 plen++;
 611                 nfrms++;
 612         }
 613
 614         return rx_sz;
 615 }
 616
 617 static void cfhsi_rx_done(struct cfhsi *cfhsi)
 618 {
 619         int res;
 620         int desc_pld_len = 0, rx_len, rx_state;
 621         struct cfhsi_desc *desc = NULL;
 622         u8 *rx_ptr, *rx_buf;
 623         struct cfhsi_desc *piggy_desc = NULL;
 624
 625         desc = (struct cfhsi_desc *)cfhsi->rx_buf;
 626
 627         netdev_dbg(cfhsi->ndev, "%s\n", __func__);
 628
 629         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 630                 return;
 631
 632         /* Update inactivity timer if pending. */
 633         spin_lock_bh(&cfhsi->lock);
 634         mod_timer_pending(&cfhsi->inactivity_timer,
 635                         jiffies + cfhsi->cfg.inactivity_timeout);
 636         spin_unlock_bh(&cfhsi->lock);
 637
 638         if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) {
 639                 desc_pld_len = cfhsi_rx_desc_len(desc);
 640
 641                 if (desc_pld_len < 0)
 642                         goto out_of_sync;
 643
 644                 rx_buf = cfhsi->rx_buf;
 645                 rx_len = desc_pld_len;
 646                 if (desc_pld_len > 0 && (desc->header & CFHSI_PIGGY_DESC))
 647                         rx_len += CFHSI_DESC_SZ;
 648                 if (desc_pld_len == 0)
 649                         rx_buf = cfhsi->rx_flip_buf;
 650         } else {
 651                 rx_buf = cfhsi->rx_flip_buf;
 652
 653                 rx_len = CFHSI_DESC_SZ;
 654                 if (cfhsi->rx_state.pld_len > 0 &&
 655                                 (desc->header & CFHSI_PIGGY_DESC)) {
 656
 657                         piggy_desc = (struct cfhsi_desc *)
 658                                 (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ +
 659                                                 cfhsi->rx_state.pld_len);
 660
 661                         cfhsi->rx_state.piggy_desc = true;
 662
 663                         /* Extract payload len from piggy-backed descriptor. */
 664                         desc_pld_len = cfhsi_rx_desc_len(piggy_desc);
 665                         if (desc_pld_len < 0)
 666                                 goto out_of_sync;
 667
 668                         if (desc_pld_len > 0) {
 669                                 rx_len = desc_pld_len;
 670                                 if (piggy_desc->header & CFHSI_PIGGY_DESC)
 671                                         rx_len += CFHSI_DESC_SZ;
 672                         }
 673
 674                         /*
 675                          * Copy needed information from the piggy-backed
 676                          * descriptor to the descriptor in the start.
 677                          */
 678                         memcpy(rx_buf, (u8 *)piggy_desc,
 679                                         CFHSI_DESC_SHORT_SZ);
 680                 }
 681         }
 682
 683         if (desc_pld_len) {
 684                 rx_state = CFHSI_RX_STATE_PAYLOAD;
 685                 rx_ptr = rx_buf + CFHSI_DESC_SZ;
 686         } else {
 687                 rx_state = CFHSI_RX_STATE_DESC;
 688                 rx_ptr = rx_buf;
 689                 rx_len = CFHSI_DESC_SZ;
 690         }
 691
 692         /* Initiate next read */
 693         if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) {
 694                 /* Set up new transfer. */
 695                 netdev_dbg(cfhsi->ndev, "%s: Start RX.\n",
 696                                 __func__);
 697
 698                 res = cfhsi->ops->cfhsi_rx(rx_ptr, rx_len,
 699                                 cfhsi->ops);
 700                 if (WARN_ON(res < 0)) {
 701                         netdev_err(cfhsi->ndev, "%s: RX error %d.\n",
 702                                 __func__, res);
 703                         cfhsi->ndev->stats.rx_errors++;
 704                         cfhsi->ndev->stats.rx_dropped++;
 705                 }
 706         }
 707
 708         if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) {
 709                 /* Extract payload from descriptor */
 710                 if (cfhsi_rx_desc(desc, cfhsi) < 0)
 711                         goto out_of_sync;
 712         } else {
 713                 /* Extract payload */
 714                 if (cfhsi_rx_pld(desc, cfhsi) < 0)
 715                         goto out_of_sync;
 716                 if (piggy_desc) {
 717                         /* Extract any payload in piggyback descriptor. */
 718                         if (cfhsi_rx_desc(piggy_desc, cfhsi) < 0)
 719                                 goto out_of_sync;
 720                         /* Mark no embedded frame after extracting it */
 721                         piggy_desc->offset = 0;
 722                 }
 723         }
 724
 725         /* Update state info */
 726         memset(&cfhsi->rx_state, 0, sizeof(cfhsi->rx_state));
 727         cfhsi->rx_state.state = rx_state;
 728         cfhsi->rx_ptr = rx_ptr;
 729         cfhsi->rx_len = rx_len;
 730         cfhsi->rx_state.pld_len = desc_pld_len;
 731         cfhsi->rx_state.piggy_desc = desc->header & CFHSI_PIGGY_DESC;
 732
 733         if (rx_buf != cfhsi->rx_buf)
 734                 swap(cfhsi->rx_buf, cfhsi->rx_flip_buf);
 735         return;
 736
 737 out_of_sync:
 738         netdev_err(cfhsi->ndev, "%s: Out of sync.\n", __func__);
 739         print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE,
 740                         cfhsi->rx_buf, CFHSI_DESC_SZ);
 741         schedule_work(&cfhsi->out_of_sync_work);
 742 }
 743
 744 static void cfhsi_rx_slowpath(unsigned long arg)
 745 {
 746         struct cfhsi *cfhsi = (struct cfhsi *)arg;
 747
 748         netdev_dbg(cfhsi->ndev, "%s.\n",
 749                 __func__);
 750
 751         cfhsi_rx_done(cfhsi);
 752 }
 753
 754 static void cfhsi_rx_done_cb(struct cfhsi_cb_ops *cb_ops)
 755 {
 756         struct cfhsi *cfhsi;
 757
 758         cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
 759         netdev_dbg(cfhsi->ndev, "%s.\n",
 760                 __func__);
 761
 762         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 763                 return;
 764
 765         if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits))
 766                 wake_up_interruptible(&cfhsi->flush_fifo_wait);
 767         else
 768                 cfhsi_rx_done(cfhsi);
 769 }
 770
 771 static void cfhsi_wake_up(struct work_struct *work)
 772 {
 773         struct cfhsi *cfhsi = NULL;
 774         int res;
 775         int len;
 776         long ret;
 777
 778         cfhsi = container_of(work, struct cfhsi, wake_up_work);
 779
 780         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 781                 return;
 782
 783         if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) {
 784                 /* It happenes when wakeup is requested by
 785                  * both ends at the same time. */
 786                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 787                 clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 788                 return;
 789         }
 790
 791         /* Activate wake line. */
 792         cfhsi->ops->cfhsi_wake_up(cfhsi->ops);
 793
 794         netdev_dbg(cfhsi->ndev, "%s: Start waiting.\n",
 795                 __func__);
 796
 797         /* Wait for acknowledge. */
 798         ret = CFHSI_WAKE_TOUT;
 799         ret = wait_event_interruptible_timeout(cfhsi->wake_up_wait,
 800                                         test_and_clear_bit(CFHSI_WAKE_UP_ACK,
 801                                                         &cfhsi->bits), ret);
 802         if (unlikely(ret < 0)) {
 803                 /* Interrupted by signal. */
 804                 netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n",
 805                         __func__, ret);
 806
 807                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 808                 cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 809                 return;
 810         } else if (!ret) {
 811                 bool ca_wake = false;
 812                 size_t fifo_occupancy = 0;
 813
 814                 /* Wakeup timeout */
 815                 netdev_dbg(cfhsi->ndev, "%s: Timeout.\n",
 816                         __func__);
 817
 818                 /* Check FIFO to check if modem has sent something. */
 819                 WARN_ON(cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops,
 820                                         &fifo_occupancy));
 821
 822                 netdev_dbg(cfhsi->ndev, "%s: Bytes in FIFO: %u.\n",
 823                                 __func__, (unsigned) fifo_occupancy);
 824
 825                 /* Check if we misssed the interrupt. */
 826                 WARN_ON(cfhsi->ops->cfhsi_get_peer_wake(cfhsi->ops,
 827                                                         &ca_wake));
 828
 829                 if (ca_wake) {
 830                         netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n",
 831                                 __func__);
 832
 833                         /* Clear the CFHSI_WAKE_UP_ACK bit to prevent race. */
 834                         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 835
 836                         /* Continue execution. */
 837                         goto wake_ack;
 838                 }
 839
 840                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 841                 cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 842                 return;
 843         }
 844 wake_ack:
 845         netdev_dbg(cfhsi->ndev, "%s: Woken.\n",
 846                 __func__);
 847
 848         /* Clear power up bit. */
 849         set_bit(CFHSI_AWAKE, &cfhsi->bits);
 850         clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 851
 852         /* Resume read operation. */
 853         netdev_dbg(cfhsi->ndev, "%s: Start RX.\n", __func__);
 854         res = cfhsi->ops->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->ops);
 855
 856         if (WARN_ON(res < 0))
 857                 netdev_err(cfhsi->ndev, "%s: RX err %d.\n", __func__, res);
 858
 859         /* Clear power up acknowledment. */
 860         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 861
 862         spin_lock_bh(&cfhsi->lock);
 863
 864         /* Resume transmit if queues are not empty. */
 865         if (!cfhsi_tx_queue_len(cfhsi)) {
 866                 netdev_dbg(cfhsi->ndev, "%s: Peer wake, start timer.\n",
 867                         __func__);
 868                 /* Start inactivity timer. */
 869                 mod_timer(&cfhsi->inactivity_timer,
 870                                 jiffies + cfhsi->cfg.inactivity_timeout);
 871                 spin_unlock_bh(&cfhsi->lock);
 872                 return;
 873         }
 874
 875         netdev_dbg(cfhsi->ndev, "%s: Host wake.\n",
 876                 __func__);
 877
 878         spin_unlock_bh(&cfhsi->lock);
 879
 880         /* Create HSI frame. */
 881         len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi);
 882
 883         if (likely(len > 0)) {
 884                 /* Set up new transfer. */
 885                 res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops);
 886                 if (WARN_ON(res < 0)) {
 887                         netdev_err(cfhsi->ndev, "%s: TX error %d.\n",
 888                                 __func__, res);
 889                         cfhsi_abort_tx(cfhsi);
 890                 }
 891         } else {
 892                 netdev_err(cfhsi->ndev,
 893                                 "%s: Failed to create HSI frame: %d.\n",
 894                                 __func__, len);
 895         }
 896 }
 897
 898 static void cfhsi_wake_down(struct work_struct *work)
 899 {
 900         long ret;
 901         struct cfhsi *cfhsi = NULL;
 902         size_t fifo_occupancy = 0;
 903         int retry = CFHSI_WAKE_TOUT;
 904
 905         cfhsi = container_of(work, struct cfhsi, wake_down_work);
 906         netdev_dbg(cfhsi->ndev, "%s.\n", __func__);
 907
 908         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 909                 return;
 910
 911         /* Deactivate wake line. */
 912         cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 913
 914         /* Wait for acknowledge. */
 915         ret = CFHSI_WAKE_TOUT;
 916         ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait,
 917                                         test_and_clear_bit(CFHSI_WAKE_DOWN_ACK,
 918                                                         &cfhsi->bits), ret);
 919         if (ret < 0) {
 920                 /* Interrupted by signal. */
 921                 netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n",
 922                         __func__, ret);
 923                 return;
 924         } else if (!ret) {
 925                 bool ca_wake = true;
 926
 927                 /* Timeout */
 928                 netdev_err(cfhsi->ndev, "%s: Timeout.\n", __func__);
 929
 930                 /* Check if we misssed the interrupt. */
 931                 WARN_ON(cfhsi->ops->cfhsi_get_peer_wake(cfhsi->ops,
 932                                                         &ca_wake));
 933                 if (!ca_wake)
 934                         netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n",
 935                                 __func__);
 936         }
 937
 938         /* Check FIFO occupancy. */
 939         while (retry) {
 940                 WARN_ON(cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops,
 941                                                         &fifo_occupancy));
 942
 943                 if (!fifo_occupancy)
 944                         break;
 945
 946                 set_current_state(TASK_INTERRUPTIBLE);
 947                 schedule_timeout(1);
 948                 retry--;
 949         }
 950
 951         if (!retry)
 952                 netdev_err(cfhsi->ndev, "%s: FIFO Timeout.\n", __func__);
 953
 954         /* Clear AWAKE condition. */
 955         clear_bit(CFHSI_AWAKE, &cfhsi->bits);
 956
 957         /* Cancel pending RX requests. */
 958         cfhsi->ops->cfhsi_rx_cancel(cfhsi->ops);
 959 }
 960
 961 static void cfhsi_out_of_sync(struct work_struct *work)
 962 {
 963         struct cfhsi *cfhsi = NULL;
 964
 965         cfhsi = container_of(work, struct cfhsi, out_of_sync_work);
 966
 967         rtnl_lock();
 968         dev_close(cfhsi->ndev);
 969         rtnl_unlock();
 970 }
 971
 972 static void cfhsi_wake_up_cb(struct cfhsi_cb_ops *cb_ops)
 973 {
 974         struct cfhsi *cfhsi = NULL;
 975
 976         cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
 977         netdev_dbg(cfhsi->ndev, "%s.\n",
 978                 __func__);
 979
 980         set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 981         wake_up_interruptible(&cfhsi->wake_up_wait);
 982
 983         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 984                 return;
 985
 986         /* Schedule wake up work queue if the peer initiates. */
 987         if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
 988                 queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 989 }
 990
 991 static void cfhsi_wake_down_cb(struct cfhsi_cb_ops *cb_ops)
 992 {
 993         struct cfhsi *cfhsi = NULL;
 994
 995         cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
 996         netdev_dbg(cfhsi->ndev, "%s.\n",
 997                 __func__);
 998
 999         /* Initiating low power is only permitted by the host (us). */
1000         set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
1001         wake_up_interruptible(&cfhsi->wake_down_wait);
1002 }
1003
1004 static void cfhsi_aggregation_tout(unsigned long arg)
1005 {
1006         struct cfhsi *cfhsi = (struct cfhsi *)arg;
1007
1008         netdev_dbg(cfhsi->ndev, "%s.\n",
1009                 __func__);
1010
1011         cfhsi_start_tx(cfhsi);
1012 }
1013
1014 static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
1015 {
1016         struct cfhsi *cfhsi = NULL;
1017         int start_xfer = 0;
1018         int timer_active;
1019         int prio;
1020
1021         if (!dev)
1022                 return -EINVAL;
1023
1024         cfhsi = netdev_priv(dev);
1025
1026         switch (skb->priority) {
1027         case TC_PRIO_BESTEFFORT:
1028         case TC_PRIO_FILLER:
1029         case TC_PRIO_BULK:
1030                 prio = CFHSI_PRIO_BEBK;
1031                 break;
1032         case TC_PRIO_INTERACTIVE_BULK:
1033                 prio = CFHSI_PRIO_VI;
1034                 break;
1035         case TC_PRIO_INTERACTIVE:
1036                 prio = CFHSI_PRIO_VO;
1037                 break;
1038         case TC_PRIO_CONTROL:
1039         default:
1040                 prio = CFHSI_PRIO_CTL;
1041                 break;
1042         }
1043
1044         spin_lock_bh(&cfhsi->lock);
1045
1046         /* Update aggregation statistics  */
1047         cfhsi_update_aggregation_stats(cfhsi, skb, 1);
1048
1049         /* Queue the SKB */
1050         skb_queue_tail(&cfhsi->qhead[prio], skb);
1051
1052         /* Sanity check; xmit should not be called after unregister_netdev */
1053         if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) {
1054                 spin_unlock_bh(&cfhsi->lock);
1055                 cfhsi_abort_tx(cfhsi);
1056                 return -EINVAL;
1057         }
1058
1059         /* Send flow off if number of packets is above high water mark. */
1060         if (!cfhsi->flow_off_sent &&
1061                 cfhsi_tx_queue_len(cfhsi) > cfhsi->cfg.q_high_mark &&
1062                 cfhsi->cfdev.flowctrl) {
1063                 cfhsi->flow_off_sent = 1;
1064                 cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF);
1065         }
1066
1067         if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) {
1068                 cfhsi->tx_state = CFHSI_TX_STATE_XFER;
1069                 start_xfer = 1;
1070         }
1071
1072         if (!start_xfer) {
1073                 /* Send aggregate if it is possible */
1074                 bool aggregate_ready =
1075                         cfhsi_can_send_aggregate(cfhsi) &&
1076                         del_timer(&cfhsi->aggregation_timer) > 0;
1077                 spin_unlock_bh(&cfhsi->lock);
1078                 if (aggregate_ready)
1079                         cfhsi_start_tx(cfhsi);
1080                 return 0;
1081         }
1082
1083         /* Delete inactivity timer if started. */
1084         timer_active = del_timer_sync(&cfhsi->inactivity_timer);
1085
1086         spin_unlock_bh(&cfhsi->lock);
1087
1088         if (timer_active) {
1089                 struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
1090                 int len;
1091                 int res;
1092
1093                 /* Create HSI frame. */
1094                 len = cfhsi_tx_frm(desc, cfhsi);
1095                 WARN_ON(!len);
1096
1097                 /* Set up new transfer. */
1098                 res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops);
1099                 if (WARN_ON(res < 0)) {
1100                         netdev_err(cfhsi->ndev, "%s: TX error %d.\n",
1101                                 __func__, res);
1102                         cfhsi_abort_tx(cfhsi);
1103                 }
1104         } else {
1105                 /* Schedule wake up work queue if the we initiate. */
1106                 if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
1107                         queue_work(cfhsi->wq, &cfhsi->wake_up_work);
1108         }
1109
1110         return 0;
1111 }
1112
1113 static const struct net_device_ops cfhsi_netdevops;
1114
1115 static void cfhsi_setup(struct net_device *dev)
1116 {
1117         int i;
1118         struct cfhsi *cfhsi = netdev_priv(dev);
1119         dev->features = 0;
1120         dev->type = ARPHRD_CAIF;
1121         dev->flags = IFF_POINTOPOINT | IFF_NOARP;
1122         dev->mtu = CFHSI_MAX_CAIF_FRAME_SZ;
1123         dev->tx_queue_len = 0;
1124         dev->destructor = free_netdev;
1125         dev->netdev_ops = &cfhsi_netdevops;
1126         for (i = 0; i < CFHSI_PRIO_LAST; ++i)
1127                 skb_queue_head_init(&cfhsi->qhead[i]);
1128         cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
1129         cfhsi->cfdev.use_frag = false;
1130         cfhsi->cfdev.use_stx = false;
1131         cfhsi->cfdev.use_fcs = false;
1132         cfhsi->ndev = dev;
1133         cfhsi->cfg = hsi_default_config;
1134 }
1135
1136 static int cfhsi_open(struct net_device *ndev)
1137 {
1138         struct cfhsi *cfhsi = netdev_priv(ndev);
1139         int res;
1140
1141         clear_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
1142
1143         /* Initialize state vaiables. */
1144         cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
1145         cfhsi->rx_state.state = CFHSI_RX_STATE_DESC;
1146
1147         /* Set flow info */
1148         cfhsi->flow_off_sent = 0;
1149
1150         /*
1151          * Allocate a TX buffer with the size of a HSI packet descriptors
1152          * and the necessary room for CAIF payload frames.
1153          */
1154         cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL);
1155         if (!cfhsi->tx_buf) {
1156                 res = -ENODEV;
1157                 goto err_alloc_tx;
1158         }
1159
1160         /*
1161          * Allocate a RX buffer with the size of two HSI packet descriptors and
1162          * the necessary room for CAIF payload frames.
1163          */
1164         cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
1165         if (!cfhsi->rx_buf) {
1166                 res = -ENODEV;
1167                 goto err_alloc_rx;
1168         }
1169
1170         cfhsi->rx_flip_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
1171         if (!cfhsi->rx_flip_buf) {
1172                 res = -ENODEV;
1173                 goto err_alloc_rx_flip;
1174         }
1175
1176         /* Initialize aggregation timeout */
1177         cfhsi->cfg.aggregation_timeout = hsi_default_config.aggregation_timeout;
1178
1179         /* Initialize recieve vaiables. */
1180         cfhsi->rx_ptr = cfhsi->rx_buf;
1181         cfhsi->rx_len = CFHSI_DESC_SZ;
1182
1183         /* Initialize spin locks. */
1184         spin_lock_init(&cfhsi->lock);
1185
1186         /* Set up the driver. */
1187         cfhsi->cb_ops.tx_done_cb = cfhsi_tx_done_cb;
1188         cfhsi->cb_ops.rx_done_cb = cfhsi_rx_done_cb;
1189         cfhsi->cb_ops.wake_up_cb = cfhsi_wake_up_cb;
1190         cfhsi->cb_ops.wake_down_cb = cfhsi_wake_down_cb;
1191
1192         /* Initialize the work queues. */
1193         INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up);
1194         INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down);
1195         INIT_WORK(&cfhsi->out_of_sync_work, cfhsi_out_of_sync);
1196
1197         /* Clear all bit fields. */
1198         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
1199         clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
1200         clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
1201         clear_bit(CFHSI_AWAKE, &cfhsi->bits);
1202
1203         /* Create work thread. */
1204         cfhsi->wq = create_singlethread_workqueue(cfhsi->ndev->name);
1205         if (!cfhsi->wq) {
1206                 netdev_err(cfhsi->ndev, "%s: Failed to create work queue.\n",
1207                         __func__);
1208                 res = -ENODEV;
1209                 goto err_create_wq;
1210         }
1211
1212         /* Initialize wait queues. */
1213         init_waitqueue_head(&cfhsi->wake_up_wait);
1214         init_waitqueue_head(&cfhsi->wake_down_wait);
1215         init_waitqueue_head(&cfhsi->flush_fifo_wait);
1216
1217         /* Setup the inactivity timer. */
1218         init_timer(&cfhsi->inactivity_timer);
1219         cfhsi->inactivity_timer.data = (unsigned long)cfhsi;
1220         cfhsi->inactivity_timer.function = cfhsi_inactivity_tout;
1221         /* Setup the slowpath RX timer. */
1222         init_timer(&cfhsi->rx_slowpath_timer);
1223         cfhsi->rx_slowpath_timer.data = (unsigned long)cfhsi;
1224         cfhsi->rx_slowpath_timer.function = cfhsi_rx_slowpath;
1225         /* Setup the aggregation timer. */
1226         init_timer(&cfhsi->aggregation_timer);
1227         cfhsi->aggregation_timer.data = (unsigned long)cfhsi;
1228         cfhsi->aggregation_timer.function = cfhsi_aggregation_tout;
1229
1230         /* Activate HSI interface. */
1231         res = cfhsi->ops->cfhsi_up(cfhsi->ops);
1232         if (res) {
1233                 netdev_err(cfhsi->ndev,
1234                         "%s: can't activate HSI interface: %d.\n",
1235                         __func__, res);
1236                 goto err_activate;
1237         }
1238
1239         /* Flush FIFO */
1240         res = cfhsi_flush_fifo(cfhsi);
1241         if (res) {
1242                 netdev_err(cfhsi->ndev, "%s: Can't flush FIFO: %d.\n",
1243                         __func__, res);
1244                 goto err_net_reg;
1245         }
1246         return res;
1247
1248  err_net_reg:
1249         cfhsi->ops->cfhsi_down(cfhsi->ops);
1250  err_activate:
1251         destroy_workqueue(cfhsi->wq);
1252  err_create_wq:
1253         kfree(cfhsi->rx_flip_buf);
1254  err_alloc_rx_flip:
1255         kfree(cfhsi->rx_buf);
1256  err_alloc_rx:
1257         kfree(cfhsi->tx_buf);
1258  err_alloc_tx:
1259         return res;
1260 }
1261
1262 static int cfhsi_close(struct net_device *ndev)
1263 {
1264         struct cfhsi *cfhsi = netdev_priv(ndev);
1265         u8 *tx_buf, *rx_buf, *flip_buf;
1266
1267         /* going to shutdown driver */
1268         set_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
1269
1270         /* Flush workqueue */
1271         flush_workqueue(cfhsi->wq);
1272
1273         /* Delete timers if pending */
1274         del_timer_sync(&cfhsi->inactivity_timer);
1275         del_timer_sync(&cfhsi->rx_slowpath_timer);
1276         del_timer_sync(&cfhsi->aggregation_timer);
1277
1278         /* Cancel pending RX request (if any) */
1279         cfhsi->ops->cfhsi_rx_cancel(cfhsi->ops);
1280
1281         /* Destroy workqueue */
1282         destroy_workqueue(cfhsi->wq);
1283
1284         /* Store bufferes: will be freed later. */
1285         tx_buf = cfhsi->tx_buf;
1286         rx_buf = cfhsi->rx_buf;
1287         flip_buf = cfhsi->rx_flip_buf;
1288         /* Flush transmit queues. */
1289         cfhsi_abort_tx(cfhsi);
1290
1291         /* Deactivate interface */
1292         cfhsi->ops->cfhsi_down(cfhsi->ops);
1293
1294         /* Free buffers. */
1295         kfree(tx_buf);
1296         kfree(rx_buf);
1297         kfree(flip_buf);
1298         return 0;
1299 }
1300
1301 static void cfhsi_uninit(struct net_device *dev)
1302 {
1303         struct cfhsi *cfhsi = netdev_priv(dev);
1304         ASSERT_RTNL();
1305         symbol_put(cfhsi_get_device);
1306         list_del(&cfhsi->list);
1307 }
1308
1309 static const struct net_device_ops cfhsi_netdevops = {
1310         .ndo_uninit = cfhsi_uninit,
1311         .ndo_open = cfhsi_open,
1312         .ndo_stop = cfhsi_close,
1313         .ndo_start_xmit = cfhsi_xmit
1314 };
1315
1316 static void cfhsi_netlink_parms(struct nlattr *data[], struct cfhsi *cfhsi)
1317 {
1318         int i;
1319
1320         if (!data) {
1321                 pr_debug("no params data found\n");
1322                 return;
1323         }
1324
1325         i = __IFLA_CAIF_HSI_INACTIVITY_TOUT;
1326         /*
1327          * Inactivity timeout in millisecs. Lowest possible value is 1,
1328          * and highest possible is NEXT_TIMER_MAX_DELTA.
1329          */
1330         if (data[i]) {
1331                 u32 inactivity_timeout = nla_get_u32(data[i]);
1332                 /* Pre-calculate inactivity timeout. */
1333                 cfhsi->cfg.inactivity_timeout = inactivity_timeout * HZ / 1000;
1334                 if (cfhsi->cfg.inactivity_timeout == 0)
1335                         cfhsi->cfg.inactivity_timeout = 1;
1336                 else if (cfhsi->cfg.inactivity_timeout > NEXT_TIMER_MAX_DELTA)
1337                         cfhsi->cfg.inactivity_timeout = NEXT_TIMER_MAX_DELTA;
1338         }
1339
1340         i = __IFLA_CAIF_HSI_AGGREGATION_TOUT;
1341         if (data[i])
1342                 cfhsi->cfg.aggregation_timeout = nla_get_u32(data[i]);
1343
1344         i = __IFLA_CAIF_HSI_HEAD_ALIGN;
1345         if (data[i])
1346                 cfhsi->cfg.head_align = nla_get_u32(data[i]);
1347
1348         i = __IFLA_CAIF_HSI_TAIL_ALIGN;
1349         if (data[i])
1350                 cfhsi->cfg.tail_align = nla_get_u32(data[i]);
1351
1352         i = __IFLA_CAIF_HSI_QHIGH_WATERMARK;
1353         if (data[i])
1354                 cfhsi->cfg.q_high_mark = nla_get_u32(data[i]);
1355
1356         i = __IFLA_CAIF_HSI_QLOW_WATERMARK;
1357         if (data[i])
1358                 cfhsi->cfg.q_low_mark = nla_get_u32(data[i]);
1359 }
1360
1361 static int caif_hsi_changelink(struct net_device *dev, struct nlattr *tb[],
1362                                 struct nlattr *data[])
1363 {
1364         cfhsi_netlink_parms(data, netdev_priv(dev));
1365         netdev_state_change(dev);
1366         return 0;
1367 }
1368
1369 static const struct nla_policy caif_hsi_policy[__IFLA_CAIF_HSI_MAX + 1] = {
1370         [__IFLA_CAIF_HSI_INACTIVITY_TOUT] = { .type = NLA_U32, .len = 4 },
1371         [__IFLA_CAIF_HSI_AGGREGATION_TOUT] = { .type = NLA_U32, .len = 4 },
1372         [__IFLA_CAIF_HSI_HEAD_ALIGN] = { .type = NLA_U32, .len = 4 },
1373         [__IFLA_CAIF_HSI_TAIL_ALIGN] = { .type = NLA_U32, .len = 4 },
1374         [__IFLA_CAIF_HSI_QHIGH_WATERMARK] = { .type = NLA_U32, .len = 4 },
1375         [__IFLA_CAIF_HSI_QLOW_WATERMARK] = { .type = NLA_U32, .len = 4 },
1376 };
1377
1378 static size_t caif_hsi_get_size(const struct net_device *dev)
1379 {
1380         int i;
1381         size_t s = 0;
1382         for (i = __IFLA_CAIF_HSI_UNSPEC + 1; i < __IFLA_CAIF_HSI_MAX; i++)
1383                 s += nla_total_size(caif_hsi_policy[i].len);
1384         return s;
1385 }
1386
1387 static int caif_hsi_fill_info(struct sk_buff *skb, const struct net_device *dev)
1388 {
1389         struct cfhsi *cfhsi = netdev_priv(dev);
1390
1391         if (nla_put_u32(skb, __IFLA_CAIF_HSI_INACTIVITY_TOUT,
1392                         cfhsi->cfg.inactivity_timeout) ||
1393             nla_put_u32(skb, __IFLA_CAIF_HSI_AGGREGATION_TOUT,
1394                         cfhsi->cfg.aggregation_timeout) ||
1395             nla_put_u32(skb, __IFLA_CAIF_HSI_HEAD_ALIGN,
1396                         cfhsi->cfg.head_align) ||
1397             nla_put_u32(skb, __IFLA_CAIF_HSI_TAIL_ALIGN,
1398                         cfhsi->cfg.tail_align) ||
1399             nla_put_u32(skb, __IFLA_CAIF_HSI_QHIGH_WATERMARK,
1400                         cfhsi->cfg.q_high_mark) ||
1401             nla_put_u32(skb, __IFLA_CAIF_HSI_QLOW_WATERMARK,
1402                         cfhsi->cfg.q_low_mark))
1403                 return -EMSGSIZE;
1404
1405         return 0;
1406 }
1407
1408 static int caif_hsi_newlink(struct net *src_net, struct net_device *dev,
1409                           struct nlattr *tb[], struct nlattr *data[])
1410 {
1411         struct cfhsi *cfhsi = NULL;
1412         struct cfhsi_ops *(*get_ops)(void);
1413
1414         ASSERT_RTNL();
1415
1416         cfhsi = netdev_priv(dev);
1417         cfhsi_netlink_parms(data, cfhsi);
1418
1419         get_ops = symbol_get(cfhsi_get_ops);
1420         if (!get_ops) {
1421                 pr_err("%s: failed to get the cfhsi_ops\n", __func__);
1422                 return -ENODEV;
1423         }
1424
1425         /* Assign the HSI device. */
1426         cfhsi->ops = (*get_ops)();
1427         if (!cfhsi->ops) {
1428                 pr_err("%s: failed to get the cfhsi_ops\n", __func__);
1429                 goto err;
1430         }
1431
1432         /* Assign the driver to this HSI device. */
1433         cfhsi->ops->cb_ops = &cfhsi->cb_ops;
1434         if (register_netdevice(dev)) {
1435                 pr_warn("%s: caif_hsi device registration failed\n", __func__);
1436                 goto err;
1437         }
1438         /* Add CAIF HSI device to list. */
1439         list_add_tail(&cfhsi->list, &cfhsi_list);
1440
1441         return 0;
1442 err:
1443         symbol_put(cfhsi_get_ops);
1444         return -ENODEV;
1445 }
1446
1447 static struct rtnl_link_ops caif_hsi_link_ops __read_mostly = {
1448         .kind           = "cfhsi",
1449         .priv_size      = sizeof(struct cfhsi),
1450         .setup          = cfhsi_setup,
1451         .maxtype        = __IFLA_CAIF_HSI_MAX,
1452         .policy = caif_hsi_policy,
1453         .newlink        = caif_hsi_newlink,
1454         .changelink     = caif_hsi_changelink,
1455         .get_size       = caif_hsi_get_size,
1456         .fill_info      = caif_hsi_fill_info,
1457 };
1458
1459 static void __exit cfhsi_exit_module(void)
1460 {
1461         struct list_head *list_node;
1462         struct list_head *n;
1463         struct cfhsi *cfhsi;
1464
1465         rtnl_link_unregister(&caif_hsi_link_ops);
1466
1467         rtnl_lock();
1468         list_for_each_safe(list_node, n, &cfhsi_list) {
1469                 cfhsi = list_entry(list_node, struct cfhsi, list);
1470                 unregister_netdev(cfhsi->ndev);
1471         }
1472         rtnl_unlock();
1473 }
1474
1475 static int __init cfhsi_init_module(void)
1476 {
1477         return rtnl_link_register(&caif_hsi_link_ops);
1478 }
1479
1480 module_init(cfhsi_init_module);
1481 module_exit(cfhsi_exit_module);