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 26MODULE_LICENSE("GPL"); 27MODULE_AUTHOR("Daniel Martensson"); 28MODULE_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 34static 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 67static LIST_HEAD(cfhsi_list); 68 69static 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 81static 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 99static 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 118static 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 132static 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 140static 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 163static 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 217static 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 331static 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 367static 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 399static 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 412static 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 500static 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 529static 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 617static 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 737out_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 744static 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 754static 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 771static 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 } 844wake_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 898static 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 961static 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 972static 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 991static 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 1004static 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 1014static 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 1113static const struct net_device_ops cfhsi_netdevops; 1114 1115static 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 1136static 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 1262static 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 1301static 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 1309static 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 1316static 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 1361static 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 1369static 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 1378static 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 1387static 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 1408static 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 dev_net_set(cfhsi->ndev, src_net); 1419 1420 get_ops = symbol_get(cfhsi_get_ops); 1421 if (!get_ops) { 1422 pr_err("%s: failed to get the cfhsi_ops\n", __func__); 1423 return -ENODEV; 1424 } 1425 1426 /* Assign the HSI device. */ 1427 cfhsi->ops = (*get_ops)(); 1428 if (!cfhsi->ops) { 1429 pr_err("%s: failed to get the cfhsi_ops\n", __func__); 1430 goto err; 1431 } 1432 1433 /* Assign the driver to this HSI device. */ 1434 cfhsi->ops->cb_ops = &cfhsi->cb_ops; 1435 if (register_netdevice(dev)) { 1436 pr_warn("%s: caif_hsi device registration failed\n", __func__); 1437 goto err; 1438 } 1439 /* Add CAIF HSI device to list. */ 1440 list_add_tail(&cfhsi->list, &cfhsi_list); 1441 1442 return 0; 1443err: 1444 symbol_put(cfhsi_get_ops); 1445 return -ENODEV; 1446} 1447 1448static struct rtnl_link_ops caif_hsi_link_ops __read_mostly = { 1449 .kind = "cfhsi", 1450 .priv_size = sizeof(struct cfhsi), 1451 .setup = cfhsi_setup, 1452 .maxtype = __IFLA_CAIF_HSI_MAX, 1453 .policy = caif_hsi_policy, 1454 .newlink = caif_hsi_newlink, 1455 .changelink = caif_hsi_changelink, 1456 .get_size = caif_hsi_get_size, 1457 .fill_info = caif_hsi_fill_info, 1458}; 1459 1460static void __exit cfhsi_exit_module(void) 1461{ 1462 struct list_head *list_node; 1463 struct list_head *n; 1464 struct cfhsi *cfhsi; 1465 1466 rtnl_link_unregister(&caif_hsi_link_ops); 1467 1468 rtnl_lock(); 1469 list_for_each_safe(list_node, n, &cfhsi_list) { 1470 cfhsi = list_entry(list_node, struct cfhsi, list); 1471 unregister_netdev(cfhsi->ndev); 1472 } 1473 rtnl_unlock(); 1474} 1475 1476static int __init cfhsi_init_module(void) 1477{ 1478 return rtnl_link_register(&caif_hsi_link_ops); 1479} 1480 1481module_init(cfhsi_init_module); 1482module_exit(cfhsi_exit_module); 1483