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