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