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
cfhsi_inactivity_tout(unsigned long arg)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
cfhsi_update_aggregation_stats(struct cfhsi * cfhsi,const struct sk_buff * skb,int direction)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
cfhsi_can_send_aggregate(struct cfhsi * cfhsi)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
cfhsi_dequeue(struct cfhsi * cfhsi)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
cfhsi_tx_queue_len(struct cfhsi * cfhsi)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
cfhsi_abort_tx(struct cfhsi * cfhsi)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
cfhsi_flush_fifo(struct cfhsi * cfhsi)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
cfhsi_tx_frm(struct cfhsi_desc * desc,struct cfhsi * cfhsi)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
cfhsi_start_tx(struct cfhsi * cfhsi)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
cfhsi_tx_done(struct cfhsi * cfhsi)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
cfhsi_tx_done_cb(struct cfhsi_cb_ops * cb_ops)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
cfhsi_rx_desc(struct cfhsi_desc * desc,struct cfhsi * cfhsi)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
cfhsi_rx_desc_len(struct cfhsi_desc * desc)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
cfhsi_rx_pld(struct cfhsi_desc * desc,struct cfhsi * cfhsi)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
cfhsi_rx_done(struct cfhsi * cfhsi)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
cfhsi_rx_slowpath(unsigned long arg)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
cfhsi_rx_done_cb(struct cfhsi_cb_ops * cb_ops)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
cfhsi_wake_up(struct work_struct * work)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
cfhsi_wake_down(struct work_struct * work)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
cfhsi_out_of_sync(struct work_struct * work)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
cfhsi_wake_up_cb(struct cfhsi_cb_ops * cb_ops)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
cfhsi_wake_down_cb(struct cfhsi_cb_ops * cb_ops)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
cfhsi_aggregation_tout(unsigned long arg)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
cfhsi_xmit(struct sk_buff * skb,struct net_device * dev)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
cfhsi_setup(struct net_device * dev)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
cfhsi_open(struct net_device * ndev)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
cfhsi_close(struct net_device * ndev)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
cfhsi_uninit(struct net_device * dev)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
cfhsi_netlink_parms(struct nlattr * data[],struct cfhsi * cfhsi)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
caif_hsi_changelink(struct net_device * dev,struct nlattr * tb[],struct nlattr * data[],struct netlink_ext_ack * extack)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
caif_hsi_get_size(const struct net_device * dev)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
caif_hsi_fill_info(struct sk_buff * skb,const struct net_device * dev)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
caif_hsi_newlink(struct net * src_net,struct net_device * dev,struct nlattr * tb[],struct nlattr * data[],struct netlink_ext_ack * extack)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
cfhsi_exit_module(void)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
cfhsi_init_module(void)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);
1477