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                 int len = 0;
 430                 pfrm = ((u8 *)desc) + desc->offset;
 431
 432                 /* Remove offset padding. */
 433                 pfrm += *pfrm + 1;
 434
 435                 /* Read length of CAIF frame (little endian). */
 436                 len = *pfrm;
 437                 len |= ((*(pfrm+1)) << 8) & 0xFF00;
 438                 len += 2;       /* Add FCS fields. */
 439
 440                 /* Sanity check length of CAIF frame. */
 441                 if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) {
 442                         netdev_err(cfhsi->ndev, "%s: Invalid length.\n",
 443                                 __func__);
 444                         return -EPROTO;
 445                 }
 446
 447                 /* Allocate SKB (OK even in IRQ context). */
 448                 skb = alloc_skb(len + 1, GFP_ATOMIC);
 449                 if (!skb) {
 450                         netdev_err(cfhsi->ndev, "%s: Out of memory !\n",
 451                                 __func__);
 452                         return -ENOMEM;
 453                 }
 454                 caif_assert(skb != NULL);
 455
 456                 skb_put_data(skb, pfrm, len);
 457
 458                 skb->protocol = htons(ETH_P_CAIF);
 459                 skb_reset_mac_header(skb);
 460                 skb->dev = cfhsi->ndev;
 461
 462                 /*
 463                  * We are in a callback handler and
 464                  * unfortunately we don't know what context we're
 465                  * running in.
 466                  */
 467                 if (in_interrupt())
 468                         netif_rx(skb);
 469                 else
 470                         netif_rx_ni(skb);
 471
 472                 /* Update network statistics. */
 473                 cfhsi->ndev->stats.rx_packets++;
 474                 cfhsi->ndev->stats.rx_bytes += len;
 475         }
 476
 477         /* Calculate transfer length. */
 478         plen = desc->cffrm_len;
 479         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 480                 xfer_sz += *plen;
 481                 plen++;
 482                 nfrms++;
 483         }
 484
 485         /* Check for piggy-backed descriptor. */
 486         if (desc->header & CFHSI_PIGGY_DESC)
 487                 xfer_sz += CFHSI_DESC_SZ;
 488
 489         if ((xfer_sz % 4) || (xfer_sz > (CFHSI_BUF_SZ_RX - CFHSI_DESC_SZ))) {
 490                 netdev_err(cfhsi->ndev,
 491                                 "%s: Invalid payload len: %d, ignored.\n",
 492                         __func__, xfer_sz);
 493                 return -EPROTO;
 494         }
 495         return xfer_sz;
 496 }
 497
 498 static int cfhsi_rx_desc_len(struct cfhsi_desc *desc)
 499 {
 500         int xfer_sz = 0;
 501         int nfrms = 0;
 502         u16 *plen;
 503
 504         if ((desc->header & ~CFHSI_PIGGY_DESC) ||
 505                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) {
 506
 507                 pr_err("Invalid descriptor. %x %x\n", desc->header,
 508                                 desc->offset);
 509                 return -EPROTO;
 510         }
 511
 512         /* Calculate transfer length. */
 513         plen = desc->cffrm_len;
 514         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 515                 xfer_sz += *plen;
 516                 plen++;
 517                 nfrms++;
 518         }
 519
 520         if (xfer_sz % 4) {
 521                 pr_err("Invalid payload len: %d, ignored.\n", xfer_sz);
 522                 return -EPROTO;
 523         }
 524         return xfer_sz;
 525 }
 526
 527 static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 528 {
 529         int rx_sz = 0;
 530         int nfrms = 0;
 531         u16 *plen = NULL;
 532         u8 *pfrm = NULL;
 533
 534         /* Sanity check header and offset. */
 535         if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) ||
 536                         (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) {
 537                 netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n",
 538                         __func__);
 539                 return -EPROTO;
 540         }
 541
 542         /* Set frame pointer to start of payload. */
 543         pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 544         plen = desc->cffrm_len;
 545
 546         /* Skip already processed frames. */
 547         while (nfrms < cfhsi->rx_state.nfrms) {
 548                 pfrm += *plen;
 549                 rx_sz += *plen;
 550                 plen++;
 551                 nfrms++;
 552         }
 553
 554         /* Parse payload. */
 555         while (nfrms < CFHSI_MAX_PKTS && *plen) {
 556                 struct sk_buff *skb;
 557                 u8 *pcffrm = NULL;
 558                 int len;
 559
 560                 /* CAIF frame starts after head padding. */
 561                 pcffrm = pfrm + *pfrm + 1;
 562
 563                 /* Read length of CAIF frame (little endian). */
 564                 len = *pcffrm;
 565                 len |= ((*(pcffrm + 1)) << 8) & 0xFF00;
 566                 len += 2;       /* Add FCS fields. */
 567
 568                 /* Sanity check length of CAIF frames. */
 569                 if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) {
 570                         netdev_err(cfhsi->ndev, "%s: Invalid length.\n",
 571                                 __func__);
 572                         return -EPROTO;
 573                 }
 574
 575                 /* Allocate SKB (OK even in IRQ context). */
 576                 skb = alloc_skb(len + 1, GFP_ATOMIC);
 577                 if (!skb) {
 578                         netdev_err(cfhsi->ndev, "%s: Out of memory !\n",
 579                                 __func__);
 580                         cfhsi->rx_state.nfrms = nfrms;
 581                         return -ENOMEM;
 582                 }
 583                 caif_assert(skb != NULL);
 584
 585                 skb_put_data(skb, pcffrm, len);
 586
 587                 skb->protocol = htons(ETH_P_CAIF);
 588                 skb_reset_mac_header(skb);
 589                 skb->dev = cfhsi->ndev;
 590
 591                 /*
 592                  * We're called in callback from HSI
 593                  * and don't know the context we're running in.
 594                  */
 595                 if (in_interrupt())
 596                         netif_rx(skb);
 597                 else
 598                         netif_rx_ni(skb);
 599
 600                 /* Update network statistics. */
 601                 cfhsi->ndev->stats.rx_packets++;
 602                 cfhsi->ndev->stats.rx_bytes += len;
 603
 604                 pfrm += *plen;
 605                 rx_sz += *plen;
 606                 plen++;
 607                 nfrms++;
 608         }
 609
 610         return rx_sz;
 611 }
 612
 613 static void cfhsi_rx_done(struct cfhsi *cfhsi)
 614 {
 615         int res;
 616         int desc_pld_len = 0, rx_len, rx_state;
 617         struct cfhsi_desc *desc = NULL;
 618         u8 *rx_ptr, *rx_buf;
 619         struct cfhsi_desc *piggy_desc = NULL;
 620
 621         desc = (struct cfhsi_desc *)cfhsi->rx_buf;
 622
 623         netdev_dbg(cfhsi->ndev, "%s\n", __func__);
 624
 625         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 626                 return;
 627
 628         /* Update inactivity timer if pending. */
 629         spin_lock_bh(&cfhsi->lock);
 630         mod_timer_pending(&cfhsi->inactivity_timer,
 631                         jiffies + cfhsi->cfg.inactivity_timeout);
 632         spin_unlock_bh(&cfhsi->lock);
 633
 634         if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) {
 635                 desc_pld_len = cfhsi_rx_desc_len(desc);
 636
 637                 if (desc_pld_len < 0)
 638                         goto out_of_sync;
 639
 640                 rx_buf = cfhsi->rx_buf;
 641                 rx_len = desc_pld_len;
 642                 if (desc_pld_len > 0 && (desc->header & CFHSI_PIGGY_DESC))
 643                         rx_len += CFHSI_DESC_SZ;
 644                 if (desc_pld_len == 0)
 645                         rx_buf = cfhsi->rx_flip_buf;
 646         } else {
 647                 rx_buf = cfhsi->rx_flip_buf;
 648
 649                 rx_len = CFHSI_DESC_SZ;
 650                 if (cfhsi->rx_state.pld_len > 0 &&
 651                                 (desc->header & CFHSI_PIGGY_DESC)) {
 652
 653                         piggy_desc = (struct cfhsi_desc *)
 654                                 (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ +
 655                                                 cfhsi->rx_state.pld_len);
 656
 657                         cfhsi->rx_state.piggy_desc = true;
 658
 659                         /* Extract payload len from piggy-backed descriptor. */
 660                         desc_pld_len = cfhsi_rx_desc_len(piggy_desc);
 661                         if (desc_pld_len < 0)
 662                                 goto out_of_sync;
 663
 664                         if (desc_pld_len > 0) {
 665                                 rx_len = desc_pld_len;
 666                                 if (piggy_desc->header & CFHSI_PIGGY_DESC)
 667                                         rx_len += CFHSI_DESC_SZ;
 668                         }
 669
 670                         /*
 671                          * Copy needed information from the piggy-backed
 672                          * descriptor to the descriptor in the start.
 673                          */
 674                         memcpy(rx_buf, (u8 *)piggy_desc,
 675                                         CFHSI_DESC_SHORT_SZ);
 676                 }
 677         }
 678
 679         if (desc_pld_len) {
 680                 rx_state = CFHSI_RX_STATE_PAYLOAD;
 681                 rx_ptr = rx_buf + CFHSI_DESC_SZ;
 682         } else {
 683                 rx_state = CFHSI_RX_STATE_DESC;
 684                 rx_ptr = rx_buf;
 685                 rx_len = CFHSI_DESC_SZ;
 686         }
 687
 688         /* Initiate next read */
 689         if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) {
 690                 /* Set up new transfer. */
 691                 netdev_dbg(cfhsi->ndev, "%s: Start RX.\n",
 692                                 __func__);
 693
 694                 res = cfhsi->ops->cfhsi_rx(rx_ptr, rx_len,
 695                                 cfhsi->ops);
 696                 if (WARN_ON(res < 0)) {
 697                         netdev_err(cfhsi->ndev, "%s: RX error %d.\n",
 698                                 __func__, res);
 699                         cfhsi->ndev->stats.rx_errors++;
 700                         cfhsi->ndev->stats.rx_dropped++;
 701                 }
 702         }
 703
 704         if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) {
 705                 /* Extract payload from descriptor */
 706                 if (cfhsi_rx_desc(desc, cfhsi) < 0)
 707                         goto out_of_sync;
 708         } else {
 709                 /* Extract payload */
 710                 if (cfhsi_rx_pld(desc, cfhsi) < 0)
 711                         goto out_of_sync;
 712                 if (piggy_desc) {
 713                         /* Extract any payload in piggyback descriptor. */
 714                         if (cfhsi_rx_desc(piggy_desc, cfhsi) < 0)
 715                                 goto out_of_sync;
 716                         /* Mark no embedded frame after extracting it */
 717                         piggy_desc->offset = 0;
 718                 }
 719         }
 720
 721         /* Update state info */
 722         memset(&cfhsi->rx_state, 0, sizeof(cfhsi->rx_state));
 723         cfhsi->rx_state.state = rx_state;
 724         cfhsi->rx_ptr = rx_ptr;
 725         cfhsi->rx_len = rx_len;
 726         cfhsi->rx_state.pld_len = desc_pld_len;
 727         cfhsi->rx_state.piggy_desc = desc->header & CFHSI_PIGGY_DESC;
 728
 729         if (rx_buf != cfhsi->rx_buf)
 730                 swap(cfhsi->rx_buf, cfhsi->rx_flip_buf);
 731         return;
 732
 733 out_of_sync:
 734         netdev_err(cfhsi->ndev, "%s: Out of sync.\n", __func__);
 735         print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE,
 736                         cfhsi->rx_buf, CFHSI_DESC_SZ);
 737         schedule_work(&cfhsi->out_of_sync_work);
 738 }
 739
 740 static void cfhsi_rx_slowpath(unsigned long arg)
 741 {
 742         struct cfhsi *cfhsi = (struct cfhsi *)arg;
 743
 744         netdev_dbg(cfhsi->ndev, "%s.\n",
 745                 __func__);
 746
 747         cfhsi_rx_done(cfhsi);
 748 }
 749
 750 static void cfhsi_rx_done_cb(struct cfhsi_cb_ops *cb_ops)
 751 {
 752         struct cfhsi *cfhsi;
 753
 754         cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
 755         netdev_dbg(cfhsi->ndev, "%s.\n",
 756                 __func__);
 757
 758         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 759                 return;
 760
 761         if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits))
 762                 wake_up_interruptible(&cfhsi->flush_fifo_wait);
 763         else
 764                 cfhsi_rx_done(cfhsi);
 765 }
 766
 767 static void cfhsi_wake_up(struct work_struct *work)
 768 {
 769         struct cfhsi *cfhsi = NULL;
 770         int res;
 771         int len;
 772         long ret;
 773
 774         cfhsi = container_of(work, struct cfhsi, wake_up_work);
 775
 776         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 777                 return;
 778
 779         if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) {
 780                 /* It happenes when wakeup is requested by
 781                  * both ends at the same time. */
 782                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 783                 clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 784                 return;
 785         }
 786
 787         /* Activate wake line. */
 788         cfhsi->ops->cfhsi_wake_up(cfhsi->ops);
 789
 790         netdev_dbg(cfhsi->ndev, "%s: Start waiting.\n",
 791                 __func__);
 792
 793         /* Wait for acknowledge. */
 794         ret = CFHSI_WAKE_TOUT;
 795         ret = wait_event_interruptible_timeout(cfhsi->wake_up_wait,
 796                                         test_and_clear_bit(CFHSI_WAKE_UP_ACK,
 797                                                         &cfhsi->bits), ret);
 798         if (unlikely(ret < 0)) {
 799                 /* Interrupted by signal. */
 800                 netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n",
 801                         __func__, ret);
 802
 803                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 804                 cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 805                 return;
 806         } else if (!ret) {
 807                 bool ca_wake = false;
 808                 size_t fifo_occupancy = 0;
 809
 810                 /* Wakeup timeout */
 811                 netdev_dbg(cfhsi->ndev, "%s: Timeout.\n",
 812                         __func__);
 813
 814                 /* Check FIFO to check if modem has sent something. */
 815                 WARN_ON(cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops,
 816                                         &fifo_occupancy));
 817
 818                 netdev_dbg(cfhsi->ndev, "%s: Bytes in FIFO: %u.\n",
 819                                 __func__, (unsigned) fifo_occupancy);
 820
 821                 /* Check if we misssed the interrupt. */
 822                 WARN_ON(cfhsi->ops->cfhsi_get_peer_wake(cfhsi->ops,
 823                                                         &ca_wake));
 824
 825                 if (ca_wake) {
 826                         netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n",
 827                                 __func__);
 828
 829                         /* Clear the CFHSI_WAKE_UP_ACK bit to prevent race. */
 830                         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 831
 832                         /* Continue execution. */
 833                         goto wake_ack;
 834                 }
 835
 836                 clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 837                 cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 838                 return;
 839         }
 840 wake_ack:
 841         netdev_dbg(cfhsi->ndev, "%s: Woken.\n",
 842                 __func__);
 843
 844         /* Clear power up bit. */
 845         set_bit(CFHSI_AWAKE, &cfhsi->bits);
 846         clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 847
 848         /* Resume read operation. */
 849         netdev_dbg(cfhsi->ndev, "%s: Start RX.\n", __func__);
 850         res = cfhsi->ops->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->ops);
 851
 852         if (WARN_ON(res < 0))
 853                 netdev_err(cfhsi->ndev, "%s: RX err %d.\n", __func__, res);
 854
 855         /* Clear power up acknowledment. */
 856         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 857
 858         spin_lock_bh(&cfhsi->lock);
 859
 860         /* Resume transmit if queues are not empty. */
 861         if (!cfhsi_tx_queue_len(cfhsi)) {
 862                 netdev_dbg(cfhsi->ndev, "%s: Peer wake, start timer.\n",
 863                         __func__);
 864                 /* Start inactivity timer. */
 865                 mod_timer(&cfhsi->inactivity_timer,
 866                                 jiffies + cfhsi->cfg.inactivity_timeout);
 867                 spin_unlock_bh(&cfhsi->lock);
 868                 return;
 869         }
 870
 871         netdev_dbg(cfhsi->ndev, "%s: Host wake.\n",
 872                 __func__);
 873
 874         spin_unlock_bh(&cfhsi->lock);
 875
 876         /* Create HSI frame. */
 877         len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi);
 878
 879         if (likely(len > 0)) {
 880                 /* Set up new transfer. */
 881                 res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops);
 882                 if (WARN_ON(res < 0)) {
 883                         netdev_err(cfhsi->ndev, "%s: TX error %d.\n",
 884                                 __func__, res);
 885                         cfhsi_abort_tx(cfhsi);
 886                 }
 887         } else {
 888                 netdev_err(cfhsi->ndev,
 889                                 "%s: Failed to create HSI frame: %d.\n",
 890                                 __func__, len);
 891         }
 892 }
 893
 894 static void cfhsi_wake_down(struct work_struct *work)
 895 {
 896         long ret;
 897         struct cfhsi *cfhsi = NULL;
 898         size_t fifo_occupancy = 0;
 899         int retry = CFHSI_WAKE_TOUT;
 900
 901         cfhsi = container_of(work, struct cfhsi, wake_down_work);
 902         netdev_dbg(cfhsi->ndev, "%s.\n", __func__);
 903
 904         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 905                 return;
 906
 907         /* Deactivate wake line. */
 908         cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 909
 910         /* Wait for acknowledge. */
 911         ret = CFHSI_WAKE_TOUT;
 912         ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait,
 913                                         test_and_clear_bit(CFHSI_WAKE_DOWN_ACK,
 914                                                         &cfhsi->bits), ret);
 915         if (ret < 0) {
 916                 /* Interrupted by signal. */
 917                 netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n",
 918                         __func__, ret);
 919                 return;
 920         } else if (!ret) {
 921                 bool ca_wake = true;
 922
 923                 /* Timeout */
 924                 netdev_err(cfhsi->ndev, "%s: Timeout.\n", __func__);
 925
 926                 /* Check if we misssed the interrupt. */
 927                 WARN_ON(cfhsi->ops->cfhsi_get_peer_wake(cfhsi->ops,
 928                                                         &ca_wake));
 929                 if (!ca_wake)
 930                         netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n",
 931                                 __func__);
 932         }
 933
 934         /* Check FIFO occupancy. */
 935         while (retry) {
 936                 WARN_ON(cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops,
 937                                                         &fifo_occupancy));
 938
 939                 if (!fifo_occupancy)
 940                         break;
 941
 942                 set_current_state(TASK_INTERRUPTIBLE);
 943                 schedule_timeout(1);
 944                 retry--;
 945         }
 946
 947         if (!retry)
 948                 netdev_err(cfhsi->ndev, "%s: FIFO Timeout.\n", __func__);
 949
 950         /* Clear AWAKE condition. */
 951         clear_bit(CFHSI_AWAKE, &cfhsi->bits);
 952
 953         /* Cancel pending RX requests. */
 954         cfhsi->ops->cfhsi_rx_cancel(cfhsi->ops);
 955 }
 956
 957 static void cfhsi_out_of_sync(struct work_struct *work)
 958 {
 959         struct cfhsi *cfhsi = NULL;
 960
 961         cfhsi = container_of(work, struct cfhsi, out_of_sync_work);
 962
 963         rtnl_lock();
 964         dev_close(cfhsi->ndev);
 965         rtnl_unlock();
 966 }
 967
 968 static void cfhsi_wake_up_cb(struct cfhsi_cb_ops *cb_ops)
 969 {
 970         struct cfhsi *cfhsi = NULL;
 971
 972         cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
 973         netdev_dbg(cfhsi->ndev, "%s.\n",
 974                 __func__);
 975
 976         set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
 977         wake_up_interruptible(&cfhsi->wake_up_wait);
 978
 979         if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 980                 return;
 981
 982         /* Schedule wake up work queue if the peer initiates. */
 983         if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
 984                 queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 985 }
 986
 987 static void cfhsi_wake_down_cb(struct cfhsi_cb_ops *cb_ops)
 988 {
 989         struct cfhsi *cfhsi = NULL;
 990
 991         cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
 992         netdev_dbg(cfhsi->ndev, "%s.\n",
 993                 __func__);
 994
 995         /* Initiating low power is only permitted by the host (us). */
 996         set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
 997         wake_up_interruptible(&cfhsi->wake_down_wait);
 998 }
 999
1000 static void cfhsi_aggregation_tout(unsigned long arg)
1001 {
1002         struct cfhsi *cfhsi = (struct cfhsi *)arg;
1003
1004         netdev_dbg(cfhsi->ndev, "%s.\n",
1005                 __func__);
1006
1007         cfhsi_start_tx(cfhsi);
1008 }
1009
1010 static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
1011 {
1012         struct cfhsi *cfhsi = NULL;
1013         int start_xfer = 0;
1014         int timer_active;
1015         int prio;
1016
1017         if (!dev)
1018                 return -EINVAL;
1019
1020         cfhsi = netdev_priv(dev);
1021
1022         switch (skb->priority) {
1023         case TC_PRIO_BESTEFFORT:
1024         case TC_PRIO_FILLER:
1025         case TC_PRIO_BULK:
1026                 prio = CFHSI_PRIO_BEBK;
1027                 break;
1028         case TC_PRIO_INTERACTIVE_BULK:
1029                 prio = CFHSI_PRIO_VI;
1030                 break;
1031         case TC_PRIO_INTERACTIVE:
1032                 prio = CFHSI_PRIO_VO;
1033                 break;
1034         case TC_PRIO_CONTROL:
1035         default:
1036                 prio = CFHSI_PRIO_CTL;
1037                 break;
1038         }
1039
1040         spin_lock_bh(&cfhsi->lock);
1041
1042         /* Update aggregation statistics  */
1043         cfhsi_update_aggregation_stats(cfhsi, skb, 1);
1044
1045         /* Queue the SKB */
1046         skb_queue_tail(&cfhsi->qhead[prio], skb);
1047
1048         /* Sanity check; xmit should not be called after unregister_netdev */
1049         if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) {
1050                 spin_unlock_bh(&cfhsi->lock);
1051                 cfhsi_abort_tx(cfhsi);
1052                 return -EINVAL;
1053         }
1054
1055         /* Send flow off if number of packets is above high water mark. */
1056         if (!cfhsi->flow_off_sent &&
1057                 cfhsi_tx_queue_len(cfhsi) > cfhsi->cfg.q_high_mark &&
1058                 cfhsi->cfdev.flowctrl) {
1059                 cfhsi->flow_off_sent = 1;
1060                 cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF);
1061         }
1062
1063         if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) {
1064                 cfhsi->tx_state = CFHSI_TX_STATE_XFER;
1065                 start_xfer = 1;
1066         }
1067
1068         if (!start_xfer) {
1069                 /* Send aggregate if it is possible */
1070                 bool aggregate_ready =
1071                         cfhsi_can_send_aggregate(cfhsi) &&
1072                         del_timer(&cfhsi->aggregation_timer) > 0;
1073                 spin_unlock_bh(&cfhsi->lock);
1074                 if (aggregate_ready)
1075                         cfhsi_start_tx(cfhsi);
1076                 return 0;
1077         }
1078
1079         /* Delete inactivity timer if started. */
1080         timer_active = del_timer_sync(&cfhsi->inactivity_timer);
1081
1082         spin_unlock_bh(&cfhsi->lock);
1083
1084         if (timer_active) {
1085                 struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
1086                 int len;
1087                 int res;
1088
1089                 /* Create HSI frame. */
1090                 len = cfhsi_tx_frm(desc, cfhsi);
1091                 WARN_ON(!len);
1092
1093                 /* Set up new transfer. */
1094                 res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops);
1095                 if (WARN_ON(res < 0)) {
1096                         netdev_err(cfhsi->ndev, "%s: TX error %d.\n",
1097                                 __func__, res);
1098                         cfhsi_abort_tx(cfhsi);
1099                 }
1100         } else {
1101                 /* Schedule wake up work queue if the we initiate. */
1102                 if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
1103                         queue_work(cfhsi->wq, &cfhsi->wake_up_work);
1104         }
1105
1106         return 0;
1107 }
1108
1109 static const struct net_device_ops cfhsi_netdevops;
1110
1111 static void cfhsi_setup(struct net_device *dev)
1112 {
1113         int i;
1114         struct cfhsi *cfhsi = netdev_priv(dev);
1115         dev->features = 0;
1116         dev->type = ARPHRD_CAIF;
1117         dev->flags = IFF_POINTOPOINT | IFF_NOARP;
1118         dev->mtu = CFHSI_MAX_CAIF_FRAME_SZ;
1119         dev->priv_flags |= IFF_NO_QUEUE;
1120         dev->needs_free_netdev = true;
1121         dev->netdev_ops = &cfhsi_netdevops;
1122         for (i = 0; i < CFHSI_PRIO_LAST; ++i)
1123                 skb_queue_head_init(&cfhsi->qhead[i]);
1124         cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
1125         cfhsi->cfdev.use_frag = false;
1126         cfhsi->cfdev.use_stx = false;
1127         cfhsi->cfdev.use_fcs = false;
1128         cfhsi->ndev = dev;
1129         cfhsi->cfg = hsi_default_config;
1130 }
1131
1132 static int cfhsi_open(struct net_device *ndev)
1133 {
1134         struct cfhsi *cfhsi = netdev_priv(ndev);
1135         int res;
1136
1137         clear_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
1138
1139         /* Initialize state vaiables. */
1140         cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
1141         cfhsi->rx_state.state = CFHSI_RX_STATE_DESC;
1142
1143         /* Set flow info */
1144         cfhsi->flow_off_sent = 0;
1145
1146         /*
1147          * Allocate a TX buffer with the size of a HSI packet descriptors
1148          * and the necessary room for CAIF payload frames.
1149          */
1150         cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL);
1151         if (!cfhsi->tx_buf) {
1152                 res = -ENODEV;
1153                 goto err_alloc_tx;
1154         }
1155
1156         /*
1157          * Allocate a RX buffer with the size of two HSI packet descriptors and
1158          * the necessary room for CAIF payload frames.
1159          */
1160         cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
1161         if (!cfhsi->rx_buf) {
1162                 res = -ENODEV;
1163                 goto err_alloc_rx;
1164         }
1165
1166         cfhsi->rx_flip_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
1167         if (!cfhsi->rx_flip_buf) {
1168                 res = -ENODEV;
1169                 goto err_alloc_rx_flip;
1170         }
1171
1172         /* Initialize aggregation timeout */
1173         cfhsi->cfg.aggregation_timeout = hsi_default_config.aggregation_timeout;
1174
1175         /* Initialize recieve vaiables. */
1176         cfhsi->rx_ptr = cfhsi->rx_buf;
1177         cfhsi->rx_len = CFHSI_DESC_SZ;
1178
1179         /* Initialize spin locks. */
1180         spin_lock_init(&cfhsi->lock);
1181
1182         /* Set up the driver. */
1183         cfhsi->cb_ops.tx_done_cb = cfhsi_tx_done_cb;
1184         cfhsi->cb_ops.rx_done_cb = cfhsi_rx_done_cb;
1185         cfhsi->cb_ops.wake_up_cb = cfhsi_wake_up_cb;
1186         cfhsi->cb_ops.wake_down_cb = cfhsi_wake_down_cb;
1187
1188         /* Initialize the work queues. */
1189         INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up);
1190         INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down);
1191         INIT_WORK(&cfhsi->out_of_sync_work, cfhsi_out_of_sync);
1192
1193         /* Clear all bit fields. */
1194         clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
1195         clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
1196         clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
1197         clear_bit(CFHSI_AWAKE, &cfhsi->bits);
1198
1199         /* Create work thread. */
1200         cfhsi->wq = alloc_ordered_workqueue(cfhsi->ndev->name, WQ_MEM_RECLAIM);
1201         if (!cfhsi->wq) {
1202                 netdev_err(cfhsi->ndev, "%s: Failed to create work queue.\n",
1203                         __func__);
1204                 res = -ENODEV;
1205                 goto err_create_wq;
1206         }
1207
1208         /* Initialize wait queues. */
1209         init_waitqueue_head(&cfhsi->wake_up_wait);
1210         init_waitqueue_head(&cfhsi->wake_down_wait);
1211         init_waitqueue_head(&cfhsi->flush_fifo_wait);
1212
1213         /* Setup the inactivity timer. */
1214         init_timer(&cfhsi->inactivity_timer);
1215         cfhsi->inactivity_timer.data = (unsigned long)cfhsi;
1216         cfhsi->inactivity_timer.function = cfhsi_inactivity_tout;
1217         /* Setup the slowpath RX timer. */
1218         init_timer(&cfhsi->rx_slowpath_timer);
1219         cfhsi->rx_slowpath_timer.data = (unsigned long)cfhsi;
1220         cfhsi->rx_slowpath_timer.function = cfhsi_rx_slowpath;
1221         /* Setup the aggregation timer. */
1222         init_timer(&cfhsi->aggregation_timer);
1223         cfhsi->aggregation_timer.data = (unsigned long)cfhsi;
1224         cfhsi->aggregation_timer.function = cfhsi_aggregation_tout;
1225
1226         /* Activate HSI interface. */
1227         res = cfhsi->ops->cfhsi_up(cfhsi->ops);
1228         if (res) {
1229                 netdev_err(cfhsi->ndev,
1230                         "%s: can't activate HSI interface: %d.\n",
1231                         __func__, res);
1232                 goto err_activate;
1233         }
1234
1235         /* Flush FIFO */
1236         res = cfhsi_flush_fifo(cfhsi);
1237         if (res) {
1238                 netdev_err(cfhsi->ndev, "%s: Can't flush FIFO: %d.\n",
1239                         __func__, res);
1240                 goto err_net_reg;
1241         }
1242         return res;
1243
1244  err_net_reg:
1245         cfhsi->ops->cfhsi_down(cfhsi->ops);
1246  err_activate:
1247         destroy_workqueue(cfhsi->wq);
1248  err_create_wq:
1249         kfree(cfhsi->rx_flip_buf);
1250  err_alloc_rx_flip:
1251         kfree(cfhsi->rx_buf);
1252  err_alloc_rx:
1253         kfree(cfhsi->tx_buf);
1254  err_alloc_tx:
1255         return res;
1256 }
1257
1258 static int cfhsi_close(struct net_device *ndev)
1259 {
1260         struct cfhsi *cfhsi = netdev_priv(ndev);
1261         u8 *tx_buf, *rx_buf, *flip_buf;
1262
1263         /* going to shutdown driver */
1264         set_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
1265
1266         /* Delete timers if pending */
1267         del_timer_sync(&cfhsi->inactivity_timer);
1268         del_timer_sync(&cfhsi->rx_slowpath_timer);
1269         del_timer_sync(&cfhsi->aggregation_timer);
1270
1271         /* Cancel pending RX request (if any) */
1272         cfhsi->ops->cfhsi_rx_cancel(cfhsi->ops);
1273
1274         /* Destroy workqueue */
1275         destroy_workqueue(cfhsi->wq);
1276
1277         /* Store bufferes: will be freed later. */
1278         tx_buf = cfhsi->tx_buf;
1279         rx_buf = cfhsi->rx_buf;
1280         flip_buf = cfhsi->rx_flip_buf;
1281         /* Flush transmit queues. */
1282         cfhsi_abort_tx(cfhsi);
1283
1284         /* Deactivate interface */
1285         cfhsi->ops->cfhsi_down(cfhsi->ops);
1286
1287         /* Free buffers. */
1288         kfree(tx_buf);
1289         kfree(rx_buf);
1290         kfree(flip_buf);
1291         return 0;
1292 }
1293
1294 static void cfhsi_uninit(struct net_device *dev)
1295 {
1296         struct cfhsi *cfhsi = netdev_priv(dev);
1297         ASSERT_RTNL();
1298         symbol_put(cfhsi_get_device);
1299         list_del(&cfhsi->list);
1300 }
1301
1302 static const struct net_device_ops cfhsi_netdevops = {
1303         .ndo_uninit = cfhsi_uninit,
1304         .ndo_open = cfhsi_open,
1305         .ndo_stop = cfhsi_close,
1306         .ndo_start_xmit = cfhsi_xmit
1307 };
1308
1309 static void cfhsi_netlink_parms(struct nlattr *data[], struct cfhsi *cfhsi)
1310 {
1311         int i;
1312
1313         if (!data) {
1314                 pr_debug("no params data found\n");
1315                 return;
1316         }
1317
1318         i = __IFLA_CAIF_HSI_INACTIVITY_TOUT;
1319         /*
1320          * Inactivity timeout in millisecs. Lowest possible value is 1,
1321          * and highest possible is NEXT_TIMER_MAX_DELTA.
1322          */
1323         if (data[i]) {
1324                 u32 inactivity_timeout = nla_get_u32(data[i]);
1325                 /* Pre-calculate inactivity timeout. */
1326                 cfhsi->cfg.inactivity_timeout = inactivity_timeout * HZ / 1000;
1327                 if (cfhsi->cfg.inactivity_timeout == 0)
1328                         cfhsi->cfg.inactivity_timeout = 1;
1329                 else if (cfhsi->cfg.inactivity_timeout > NEXT_TIMER_MAX_DELTA)
1330                         cfhsi->cfg.inactivity_timeout = NEXT_TIMER_MAX_DELTA;
1331         }
1332
1333         i = __IFLA_CAIF_HSI_AGGREGATION_TOUT;
1334         if (data[i])
1335                 cfhsi->cfg.aggregation_timeout = nla_get_u32(data[i]);
1336
1337         i = __IFLA_CAIF_HSI_HEAD_ALIGN;
1338         if (data[i])
1339                 cfhsi->cfg.head_align = nla_get_u32(data[i]);
1340
1341         i = __IFLA_CAIF_HSI_TAIL_ALIGN;
1342         if (data[i])
1343                 cfhsi->cfg.tail_align = nla_get_u32(data[i]);
1344
1345         i = __IFLA_CAIF_HSI_QHIGH_WATERMARK;
1346         if (data[i])
1347                 cfhsi->cfg.q_high_mark = nla_get_u32(data[i]);
1348
1349         i = __IFLA_CAIF_HSI_QLOW_WATERMARK;
1350         if (data[i])
1351                 cfhsi->cfg.q_low_mark = nla_get_u32(data[i]);
1352 }
1353
1354 static int caif_hsi_changelink(struct net_device *dev, struct nlattr *tb[],
1355                                struct nlattr *data[],
1356                                struct netlink_ext_ack *extack)
1357 {
1358         cfhsi_netlink_parms(data, netdev_priv(dev));
1359         netdev_state_change(dev);
1360         return 0;
1361 }
1362
1363 static const struct nla_policy caif_hsi_policy[__IFLA_CAIF_HSI_MAX + 1] = {
1364         [__IFLA_CAIF_HSI_INACTIVITY_TOUT] = { .type = NLA_U32, .len = 4 },
1365         [__IFLA_CAIF_HSI_AGGREGATION_TOUT] = { .type = NLA_U32, .len = 4 },
1366         [__IFLA_CAIF_HSI_HEAD_ALIGN] = { .type = NLA_U32, .len = 4 },
1367         [__IFLA_CAIF_HSI_TAIL_ALIGN] = { .type = NLA_U32, .len = 4 },
1368         [__IFLA_CAIF_HSI_QHIGH_WATERMARK] = { .type = NLA_U32, .len = 4 },
1369         [__IFLA_CAIF_HSI_QLOW_WATERMARK] = { .type = NLA_U32, .len = 4 },
1370 };
1371
1372 static size_t caif_hsi_get_size(const struct net_device *dev)
1373 {
1374         int i;
1375         size_t s = 0;
1376         for (i = __IFLA_CAIF_HSI_UNSPEC + 1; i < __IFLA_CAIF_HSI_MAX; i++)
1377                 s += nla_total_size(caif_hsi_policy[i].len);
1378         return s;
1379 }
1380
1381 static int caif_hsi_fill_info(struct sk_buff *skb, const struct net_device *dev)
1382 {
1383         struct cfhsi *cfhsi = netdev_priv(dev);
1384
1385         if (nla_put_u32(skb, __IFLA_CAIF_HSI_INACTIVITY_TOUT,
1386                         cfhsi->cfg.inactivity_timeout) ||
1387             nla_put_u32(skb, __IFLA_CAIF_HSI_AGGREGATION_TOUT,
1388                         cfhsi->cfg.aggregation_timeout) ||
1389             nla_put_u32(skb, __IFLA_CAIF_HSI_HEAD_ALIGN,
1390                         cfhsi->cfg.head_align) ||
1391             nla_put_u32(skb, __IFLA_CAIF_HSI_TAIL_ALIGN,
1392                         cfhsi->cfg.tail_align) ||
1393             nla_put_u32(skb, __IFLA_CAIF_HSI_QHIGH_WATERMARK,
1394                         cfhsi->cfg.q_high_mark) ||
1395             nla_put_u32(skb, __IFLA_CAIF_HSI_QLOW_WATERMARK,
1396                         cfhsi->cfg.q_low_mark))
1397                 return -EMSGSIZE;
1398
1399         return 0;
1400 }
1401
1402 static int caif_hsi_newlink(struct net *src_net, struct net_device *dev,
1403                             struct nlattr *tb[], struct nlattr *data[],
1404                             struct netlink_ext_ack *extack)
1405 {
1406         struct cfhsi *cfhsi = NULL;
1407         struct cfhsi_ops *(*get_ops)(void);
1408
1409         ASSERT_RTNL();
1410
1411         cfhsi = netdev_priv(dev);
1412         cfhsi_netlink_parms(data, cfhsi);
1413
1414         get_ops = symbol_get(cfhsi_get_ops);
1415         if (!get_ops) {
1416                 pr_err("%s: failed to get the cfhsi_ops\n", __func__);
1417                 return -ENODEV;
1418         }
1419
1420         /* Assign the HSI device. */
1421         cfhsi->ops = (*get_ops)();
1422         if (!cfhsi->ops) {
1423                 pr_err("%s: failed to get the cfhsi_ops\n", __func__);
1424                 goto err;
1425         }
1426
1427         /* Assign the driver to this HSI device. */
1428         cfhsi->ops->cb_ops = &cfhsi->cb_ops;
1429         if (register_netdevice(dev)) {
1430                 pr_warn("%s: caif_hsi device registration failed\n", __func__);
1431                 goto err;
1432         }
1433         /* Add CAIF HSI device to list. */
1434         list_add_tail(&cfhsi->list, &cfhsi_list);
1435
1436         return 0;
1437 err:
1438         symbol_put(cfhsi_get_ops);
1439         return -ENODEV;
1440 }
1441
1442 static struct rtnl_link_ops caif_hsi_link_ops __read_mostly = {
1443         .kind           = "cfhsi",
1444         .priv_size      = sizeof(struct cfhsi),
1445         .setup          = cfhsi_setup,
1446         .maxtype        = __IFLA_CAIF_HSI_MAX,
1447         .policy = caif_hsi_policy,
1448         .newlink        = caif_hsi_newlink,
1449         .changelink     = caif_hsi_changelink,
1450         .get_size       = caif_hsi_get_size,
1451         .fill_info      = caif_hsi_fill_info,
1452 };
1453
1454 static void __exit cfhsi_exit_module(void)
1455 {
1456         struct list_head *list_node;
1457         struct list_head *n;
1458         struct cfhsi *cfhsi;
1459
1460         rtnl_link_unregister(&caif_hsi_link_ops);
1461
1462         rtnl_lock();
1463         list_for_each_safe(list_node, n, &cfhsi_list) {
1464                 cfhsi = list_entry(list_node, struct cfhsi, list);
1465                 unregister_netdevice(cfhsi->ndev);
1466         }
1467         rtnl_unlock();
1468 }
1469
1470 static int __init cfhsi_init_module(void)
1471 {
1472         return rtnl_link_register(&caif_hsi_link_ops);
1473 }
1474
1475 module_init(cfhsi_init_module);
1476 module_exit(cfhsi_exit_module);