1 /*
2 * Copyright (C) ST-Ericsson AB 2010
3 * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
4 * Authors: Amarnath Revanna / amarnath.bangalore.revanna@stericsson.com,
5 * Daniel Martensson / daniel.martensson@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/spinlock.h>
12 #include <linux/sched.h>
13 #include <linux/list.h>
14 #include <linux/netdevice.h>
15 #include <linux/if_arp.h>
16
17 #include <net/caif/caif_device.h>
18 #include <net/caif/caif_shm.h>
19
20 #define NR_TX_BUF 6
21 #define NR_RX_BUF 6
22 #define TX_BUF_SZ 0x2000
23 #define RX_BUF_SZ 0x2000
24
25 #define CAIF_NEEDED_HEADROOM 32
26
27 #define CAIF_FLOW_ON 1
28 #define CAIF_FLOW_OFF 0
29
30 #define LOW_WATERMARK 3
31 #define HIGH_WATERMARK 4
32
33 /* Maximum number of CAIF buffers per shared memory buffer. */
34 #define SHM_MAX_FRMS_PER_BUF 10
35
36 /*
37 * Size in bytes of the descriptor area
38 * (With end of descriptor signalling)
39 */
40 #define SHM_CAIF_DESC_SIZE ((SHM_MAX_FRMS_PER_BUF + 1) * \
41 sizeof(struct shm_pck_desc))
42
43 /*
44 * Offset to the first CAIF frame within a shared memory buffer.
45 * Aligned on 32 bytes.
46 */
47 #define SHM_CAIF_FRM_OFS (SHM_CAIF_DESC_SIZE + (SHM_CAIF_DESC_SIZE % 32))
48
49 /* Number of bytes for CAIF shared memory header. */
50 #define SHM_HDR_LEN 1
51
52 /* Number of padding bytes for the complete CAIF frame. */
53 #define SHM_FRM_PAD_LEN 4
54
55 #define CAIF_MAX_MTU 4096
56
57 #define SHM_SET_FULL(x) (((x+1) & 0x0F) << 0)
58 #define SHM_GET_FULL(x) (((x >> 0) & 0x0F) - 1)
59
60 #define SHM_SET_EMPTY(x) (((x+1) & 0x0F) << 4)
61 #define SHM_GET_EMPTY(x) (((x >> 4) & 0x0F) - 1)
62
63 #define SHM_FULL_MASK (0x0F << 0)
64 #define SHM_EMPTY_MASK (0x0F << 4)
65
66 struct shm_pck_desc {
67 /*
68 * Offset from start of shared memory area to start of
69 * shared memory CAIF frame.
70 */
71 u32 frm_ofs;
72 u32 frm_len;
73 };
74
75 struct buf_list {
76 unsigned char *desc_vptr;
77 u32 phy_addr;
78 u32 index;
79 u32 len;
80 u32 frames;
81 u32 frm_ofs;
82 struct list_head list;
83 };
84
85 struct shm_caif_frm {
86 /* Number of bytes of padding before the CAIF frame. */
87 u8 hdr_ofs;
88 };
89
90 struct shmdrv_layer {
91 /* caif_dev_common must always be first in the structure*/
92 struct caif_dev_common cfdev;
93
94 u32 shm_tx_addr;
95 u32 shm_rx_addr;
96 u32 shm_base_addr;
97 u32 tx_empty_available;
98 spinlock_t lock;
99
100 struct list_head tx_empty_list;
101 struct list_head tx_pend_list;
102 struct list_head tx_full_list;
103 struct list_head rx_empty_list;
104 struct list_head rx_pend_list;
105 struct list_head rx_full_list;
106
107 struct workqueue_struct *pshm_tx_workqueue;
108 struct workqueue_struct *pshm_rx_workqueue;
109
110 struct work_struct shm_tx_work;
111 struct work_struct shm_rx_work;
112
113 struct sk_buff_head sk_qhead;
114 struct shmdev_layer *pshm_dev;
115 };
116
shm_netdev_open(struct net_device * shm_netdev)117 static int shm_netdev_open(struct net_device *shm_netdev)
118 {
119 netif_wake_queue(shm_netdev);
120 return 0;
121 }
122
shm_netdev_close(struct net_device * shm_netdev)123 static int shm_netdev_close(struct net_device *shm_netdev)
124 {
125 netif_stop_queue(shm_netdev);
126 return 0;
127 }
128
caif_shmdrv_rx_cb(u32 mbx_msg,void * priv)129 int caif_shmdrv_rx_cb(u32 mbx_msg, void *priv)
130 {
131 struct buf_list *pbuf;
132 struct shmdrv_layer *pshm_drv;
133 struct list_head *pos;
134 u32 avail_emptybuff = 0;
135 unsigned long flags = 0;
136
137 pshm_drv = priv;
138
139 /* Check for received buffers. */
140 if (mbx_msg & SHM_FULL_MASK) {
141 int idx;
142
143 spin_lock_irqsave(&pshm_drv->lock, flags);
144
145 /* Check whether we have any outstanding buffers. */
146 if (list_empty(&pshm_drv->rx_empty_list)) {
147
148 /* Release spin lock. */
149 spin_unlock_irqrestore(&pshm_drv->lock, flags);
150
151 /* We print even in IRQ context... */
152 pr_warn("No empty Rx buffers to fill: "
153 "mbx_msg:%x\n", mbx_msg);
154
155 /* Bail out. */
156 goto err_sync;
157 }
158
159 pbuf =
160 list_entry(pshm_drv->rx_empty_list.next,
161 struct buf_list, list);
162 idx = pbuf->index;
163
164 /* Check buffer synchronization. */
165 if (idx != SHM_GET_FULL(mbx_msg)) {
166
167 /* We print even in IRQ context... */
168 pr_warn(
169 "phyif_shm_mbx_msg_cb: RX full out of sync:"
170 " idx:%d, msg:%x SHM_GET_FULL(mbx_msg):%x\n",
171 idx, mbx_msg, SHM_GET_FULL(mbx_msg));
172
173 spin_unlock_irqrestore(&pshm_drv->lock, flags);
174
175 /* Bail out. */
176 goto err_sync;
177 }
178
179 list_del_init(&pbuf->list);
180 list_add_tail(&pbuf->list, &pshm_drv->rx_full_list);
181
182 spin_unlock_irqrestore(&pshm_drv->lock, flags);
183
184 /* Schedule RX work queue. */
185 if (!work_pending(&pshm_drv->shm_rx_work))
186 queue_work(pshm_drv->pshm_rx_workqueue,
187 &pshm_drv->shm_rx_work);
188 }
189
190 /* Check for emptied buffers. */
191 if (mbx_msg & SHM_EMPTY_MASK) {
192 int idx;
193
194 spin_lock_irqsave(&pshm_drv->lock, flags);
195
196 /* Check whether we have any outstanding buffers. */
197 if (list_empty(&pshm_drv->tx_full_list)) {
198
199 /* We print even in IRQ context... */
200 pr_warn("No TX to empty: msg:%x\n", mbx_msg);
201
202 spin_unlock_irqrestore(&pshm_drv->lock, flags);
203
204 /* Bail out. */
205 goto err_sync;
206 }
207
208 pbuf =
209 list_entry(pshm_drv->tx_full_list.next,
210 struct buf_list, list);
211 idx = pbuf->index;
212
213 /* Check buffer synchronization. */
214 if (idx != SHM_GET_EMPTY(mbx_msg)) {
215
216 spin_unlock_irqrestore(&pshm_drv->lock, flags);
217
218 /* We print even in IRQ context... */
219 pr_warn("TX empty "
220 "out of sync:idx:%d, msg:%x\n", idx, mbx_msg);
221
222 /* Bail out. */
223 goto err_sync;
224 }
225 list_del_init(&pbuf->list);
226
227 /* Reset buffer parameters. */
228 pbuf->frames = 0;
229 pbuf->frm_ofs = SHM_CAIF_FRM_OFS;
230
231 list_add_tail(&pbuf->list, &pshm_drv->tx_empty_list);
232
233 /* Check the available no. of buffers in the empty list */
234 list_for_each(pos, &pshm_drv->tx_empty_list)
235 avail_emptybuff++;
236
237 /* Check whether we have to wake up the transmitter. */
238 if ((avail_emptybuff > HIGH_WATERMARK) &&
239 (!pshm_drv->tx_empty_available)) {
240 pshm_drv->tx_empty_available = 1;
241 spin_unlock_irqrestore(&pshm_drv->lock, flags);
242 pshm_drv->cfdev.flowctrl
243 (pshm_drv->pshm_dev->pshm_netdev,
244 CAIF_FLOW_ON);
245
246
247 /* Schedule the work queue. if required */
248 if (!work_pending(&pshm_drv->shm_tx_work))
249 queue_work(pshm_drv->pshm_tx_workqueue,
250 &pshm_drv->shm_tx_work);
251 } else
252 spin_unlock_irqrestore(&pshm_drv->lock, flags);
253 }
254
255 return 0;
256
257 err_sync:
258 return -EIO;
259 }
260
shm_rx_work_func(struct work_struct * rx_work)261 static void shm_rx_work_func(struct work_struct *rx_work)
262 {
263 struct shmdrv_layer *pshm_drv;
264 struct buf_list *pbuf;
265 unsigned long flags = 0;
266 struct sk_buff *skb;
267 char *p;
268 int ret;
269
270 pshm_drv = container_of(rx_work, struct shmdrv_layer, shm_rx_work);
271
272 while (1) {
273
274 struct shm_pck_desc *pck_desc;
275
276 spin_lock_irqsave(&pshm_drv->lock, flags);
277
278 /* Check for received buffers. */
279 if (list_empty(&pshm_drv->rx_full_list)) {
280 spin_unlock_irqrestore(&pshm_drv->lock, flags);
281 break;
282 }
283
284 pbuf =
285 list_entry(pshm_drv->rx_full_list.next, struct buf_list,
286 list);
287 list_del_init(&pbuf->list);
288 spin_unlock_irqrestore(&pshm_drv->lock, flags);
289
290 /* Retrieve pointer to start of the packet descriptor area. */
291 pck_desc = (struct shm_pck_desc *) pbuf->desc_vptr;
292
293 /*
294 * Check whether descriptor contains a CAIF shared memory
295 * frame.
296 */
297 while (pck_desc->frm_ofs) {
298 unsigned int frm_buf_ofs;
299 unsigned int frm_pck_ofs;
300 unsigned int frm_pck_len;
301 /*
302 * Check whether offset is within buffer limits
303 * (lower).
304 */
305 if (pck_desc->frm_ofs <
306 (pbuf->phy_addr - pshm_drv->shm_base_addr))
307 break;
308 /*
309 * Check whether offset is within buffer limits
310 * (higher).
311 */
312 if (pck_desc->frm_ofs >
313 ((pbuf->phy_addr - pshm_drv->shm_base_addr) +
314 pbuf->len))
315 break;
316
317 /* Calculate offset from start of buffer. */
318 frm_buf_ofs =
319 pck_desc->frm_ofs - (pbuf->phy_addr -
320 pshm_drv->shm_base_addr);
321
322 /*
323 * Calculate offset and length of CAIF packet while
324 * taking care of the shared memory header.
325 */
326 frm_pck_ofs =
327 frm_buf_ofs + SHM_HDR_LEN +
328 (*(pbuf->desc_vptr + frm_buf_ofs));
329 frm_pck_len =
330 (pck_desc->frm_len - SHM_HDR_LEN -
331 (*(pbuf->desc_vptr + frm_buf_ofs)));
332
333 /* Check whether CAIF packet is within buffer limits */
334 if ((frm_pck_ofs + pck_desc->frm_len) > pbuf->len)
335 break;
336
337 /* Get a suitable CAIF packet and copy in data. */
338 skb = netdev_alloc_skb(pshm_drv->pshm_dev->pshm_netdev,
339 frm_pck_len + 1);
340
341 if (skb == NULL) {
342 pr_info("OOM: Try next frame in descriptor\n");
343 break;
344 }
345
346 p = skb_put(skb, frm_pck_len);
347 memcpy(p, pbuf->desc_vptr + frm_pck_ofs, frm_pck_len);
348
349 skb->protocol = htons(ETH_P_CAIF);
350 skb_reset_mac_header(skb);
351 skb->dev = pshm_drv->pshm_dev->pshm_netdev;
352
353 /* Push received packet up the stack. */
354 ret = netif_rx_ni(skb);
355
356 if (!ret) {
357 pshm_drv->pshm_dev->pshm_netdev->stats.
358 rx_packets++;
359 pshm_drv->pshm_dev->pshm_netdev->stats.
360 rx_bytes += pck_desc->frm_len;
361 } else
362 ++pshm_drv->pshm_dev->pshm_netdev->stats.
363 rx_dropped;
364 /* Move to next packet descriptor. */
365 pck_desc++;
366 }
367
368 spin_lock_irqsave(&pshm_drv->lock, flags);
369 list_add_tail(&pbuf->list, &pshm_drv->rx_pend_list);
370
371 spin_unlock_irqrestore(&pshm_drv->lock, flags);
372
373 }
374
375 /* Schedule the work queue. if required */
376 if (!work_pending(&pshm_drv->shm_tx_work))
377 queue_work(pshm_drv->pshm_tx_workqueue, &pshm_drv->shm_tx_work);
378
379 }
380
shm_tx_work_func(struct work_struct * tx_work)381 static void shm_tx_work_func(struct work_struct *tx_work)
382 {
383 u32 mbox_msg;
384 unsigned int frmlen, avail_emptybuff, append = 0;
385 unsigned long flags = 0;
386 struct buf_list *pbuf = NULL;
387 struct shmdrv_layer *pshm_drv;
388 struct shm_caif_frm *frm;
389 struct sk_buff *skb;
390 struct shm_pck_desc *pck_desc;
391 struct list_head *pos;
392
393 pshm_drv = container_of(tx_work, struct shmdrv_layer, shm_tx_work);
394
395 do {
396 /* Initialize mailbox message. */
397 mbox_msg = 0x00;
398 avail_emptybuff = 0;
399
400 spin_lock_irqsave(&pshm_drv->lock, flags);
401
402 /* Check for pending receive buffers. */
403 if (!list_empty(&pshm_drv->rx_pend_list)) {
404
405 pbuf = list_entry(pshm_drv->rx_pend_list.next,
406 struct buf_list, list);
407
408 list_del_init(&pbuf->list);
409 list_add_tail(&pbuf->list, &pshm_drv->rx_empty_list);
410 /*
411 * Value index is never changed,
412 * so read access should be safe.
413 */
414 mbox_msg |= SHM_SET_EMPTY(pbuf->index);
415 }
416
417 skb = skb_peek(&pshm_drv->sk_qhead);
418
419 if (skb == NULL)
420 goto send_msg;
421 /* Check the available no. of buffers in the empty list */
422 list_for_each(pos, &pshm_drv->tx_empty_list)
423 avail_emptybuff++;
424
425 if ((avail_emptybuff < LOW_WATERMARK) &&
426 pshm_drv->tx_empty_available) {
427 /* Update blocking condition. */
428 pshm_drv->tx_empty_available = 0;
429 spin_unlock_irqrestore(&pshm_drv->lock, flags);
430 pshm_drv->cfdev.flowctrl
431 (pshm_drv->pshm_dev->pshm_netdev,
432 CAIF_FLOW_OFF);
433 spin_lock_irqsave(&pshm_drv->lock, flags);
434 }
435 /*
436 * We simply return back to the caller if we do not have space
437 * either in Tx pending list or Tx empty list. In this case,
438 * we hold the received skb in the skb list, waiting to
439 * be transmitted once Tx buffers become available
440 */
441 if (list_empty(&pshm_drv->tx_empty_list))
442 goto send_msg;
443
444 /* Get the first free Tx buffer. */
445 pbuf = list_entry(pshm_drv->tx_empty_list.next,
446 struct buf_list, list);
447 do {
448 if (append) {
449 skb = skb_peek(&pshm_drv->sk_qhead);
450 if (skb == NULL)
451 break;
452 }
453
454 frm = (struct shm_caif_frm *)
455 (pbuf->desc_vptr + pbuf->frm_ofs);
456
457 frm->hdr_ofs = 0;
458 frmlen = 0;
459 frmlen += SHM_HDR_LEN + frm->hdr_ofs + skb->len;
460
461 /* Add tail padding if needed. */
462 if (frmlen % SHM_FRM_PAD_LEN)
463 frmlen += SHM_FRM_PAD_LEN -
464 (frmlen % SHM_FRM_PAD_LEN);
465
466 /*
467 * Verify that packet, header and additional padding
468 * can fit within the buffer frame area.
469 */
470 if (frmlen >= (pbuf->len - pbuf->frm_ofs))
471 break;
472
473 if (!append) {
474 list_del_init(&pbuf->list);
475 append = 1;
476 }
477
478 skb = skb_dequeue(&pshm_drv->sk_qhead);
479 if (skb == NULL)
480 break;
481 /* Copy in CAIF frame. */
482 skb_copy_bits(skb, 0, pbuf->desc_vptr +
483 pbuf->frm_ofs + SHM_HDR_LEN +
484 frm->hdr_ofs, skb->len);
485
486 pshm_drv->pshm_dev->pshm_netdev->stats.tx_packets++;
487 pshm_drv->pshm_dev->pshm_netdev->stats.tx_bytes +=
488 frmlen;
489 dev_kfree_skb_irq(skb);
490
491 /* Fill in the shared memory packet descriptor area. */
492 pck_desc = (struct shm_pck_desc *) (pbuf->desc_vptr);
493 /* Forward to current frame. */
494 pck_desc += pbuf->frames;
495 pck_desc->frm_ofs = (pbuf->phy_addr -
496 pshm_drv->shm_base_addr) +
497 pbuf->frm_ofs;
498 pck_desc->frm_len = frmlen;
499 /* Terminate packet descriptor area. */
500 pck_desc++;
501 pck_desc->frm_ofs = 0;
502 /* Update buffer parameters. */
503 pbuf->frames++;
504 pbuf->frm_ofs += frmlen + (frmlen % 32);
505
506 } while (pbuf->frames < SHM_MAX_FRMS_PER_BUF);
507
508 /* Assign buffer as full. */
509 list_add_tail(&pbuf->list, &pshm_drv->tx_full_list);
510 append = 0;
511 mbox_msg |= SHM_SET_FULL(pbuf->index);
512 send_msg:
513 spin_unlock_irqrestore(&pshm_drv->lock, flags);
514
515 if (mbox_msg)
516 pshm_drv->pshm_dev->pshmdev_mbxsend
517 (pshm_drv->pshm_dev->shm_id, mbox_msg);
518 } while (mbox_msg);
519 }
520
shm_netdev_tx(struct sk_buff * skb,struct net_device * shm_netdev)521 static int shm_netdev_tx(struct sk_buff *skb, struct net_device *shm_netdev)
522 {
523 struct shmdrv_layer *pshm_drv;
524
525 pshm_drv = netdev_priv(shm_netdev);
526
527 skb_queue_tail(&pshm_drv->sk_qhead, skb);
528
529 /* Schedule Tx work queue. for deferred processing of skbs*/
530 if (!work_pending(&pshm_drv->shm_tx_work))
531 queue_work(pshm_drv->pshm_tx_workqueue, &pshm_drv->shm_tx_work);
532
533 return 0;
534 }
535
536 static const struct net_device_ops netdev_ops = {
537 .ndo_open = shm_netdev_open,
538 .ndo_stop = shm_netdev_close,
539 .ndo_start_xmit = shm_netdev_tx,
540 };
541
shm_netdev_setup(struct net_device * pshm_netdev)542 static void shm_netdev_setup(struct net_device *pshm_netdev)
543 {
544 struct shmdrv_layer *pshm_drv;
545 pshm_netdev->netdev_ops = &netdev_ops;
546
547 pshm_netdev->mtu = CAIF_MAX_MTU;
548 pshm_netdev->type = ARPHRD_CAIF;
549 pshm_netdev->hard_header_len = CAIF_NEEDED_HEADROOM;
550 pshm_netdev->tx_queue_len = 0;
551 pshm_netdev->destructor = free_netdev;
552
553 pshm_drv = netdev_priv(pshm_netdev);
554
555 /* Initialize structures in a clean state. */
556 memset(pshm_drv, 0, sizeof(struct shmdrv_layer));
557
558 pshm_drv->cfdev.link_select = CAIF_LINK_LOW_LATENCY;
559 }
560
caif_shmcore_probe(struct shmdev_layer * pshm_dev)561 int caif_shmcore_probe(struct shmdev_layer *pshm_dev)
562 {
563 int result, j;
564 struct shmdrv_layer *pshm_drv = NULL;
565
566 pshm_dev->pshm_netdev = alloc_netdev(sizeof(struct shmdrv_layer),
567 "cfshm%d", shm_netdev_setup);
568 if (!pshm_dev->pshm_netdev)
569 return -ENOMEM;
570
571 pshm_drv = netdev_priv(pshm_dev->pshm_netdev);
572 pshm_drv->pshm_dev = pshm_dev;
573
574 /*
575 * Initialization starts with the verification of the
576 * availability of MBX driver by calling its setup function.
577 * MBX driver must be available by this time for proper
578 * functioning of SHM driver.
579 */
580 if ((pshm_dev->pshmdev_mbxsetup
581 (caif_shmdrv_rx_cb, pshm_dev, pshm_drv)) != 0) {
582 pr_warn("Could not config. SHM Mailbox,"
583 " Bailing out.....\n");
584 free_netdev(pshm_dev->pshm_netdev);
585 return -ENODEV;
586 }
587
588 skb_queue_head_init(&pshm_drv->sk_qhead);
589
590 pr_info("SHM DEVICE[%d] PROBED BY DRIVER, NEW SHM DRIVER"
591 " INSTANCE AT pshm_drv =0x%p\n",
592 pshm_drv->pshm_dev->shm_id, pshm_drv);
593
594 if (pshm_dev->shm_total_sz <
595 (NR_TX_BUF * TX_BUF_SZ + NR_RX_BUF * RX_BUF_SZ)) {
596
597 pr_warn("ERROR, Amount of available"
598 " Phys. SHM cannot accommodate current SHM "
599 "driver configuration, Bailing out ...\n");
600 free_netdev(pshm_dev->pshm_netdev);
601 return -ENOMEM;
602 }
603
604 pshm_drv->shm_base_addr = pshm_dev->shm_base_addr;
605 pshm_drv->shm_tx_addr = pshm_drv->shm_base_addr;
606
607 if (pshm_dev->shm_loopback)
608 pshm_drv->shm_rx_addr = pshm_drv->shm_tx_addr;
609 else
610 pshm_drv->shm_rx_addr = pshm_dev->shm_base_addr +
611 (NR_TX_BUF * TX_BUF_SZ);
612
613 spin_lock_init(&pshm_drv->lock);
614 INIT_LIST_HEAD(&pshm_drv->tx_empty_list);
615 INIT_LIST_HEAD(&pshm_drv->tx_pend_list);
616 INIT_LIST_HEAD(&pshm_drv->tx_full_list);
617
618 INIT_LIST_HEAD(&pshm_drv->rx_empty_list);
619 INIT_LIST_HEAD(&pshm_drv->rx_pend_list);
620 INIT_LIST_HEAD(&pshm_drv->rx_full_list);
621
622 INIT_WORK(&pshm_drv->shm_tx_work, shm_tx_work_func);
623 INIT_WORK(&pshm_drv->shm_rx_work, shm_rx_work_func);
624
625 pshm_drv->pshm_tx_workqueue =
626 create_singlethread_workqueue("shm_tx_work");
627 pshm_drv->pshm_rx_workqueue =
628 create_singlethread_workqueue("shm_rx_work");
629
630 for (j = 0; j < NR_TX_BUF; j++) {
631 struct buf_list *tx_buf =
632 kmalloc(sizeof(struct buf_list), GFP_KERNEL);
633
634 if (tx_buf == NULL) {
635 pr_warn("ERROR, Could not"
636 " allocate dynamic mem. for tx_buf,"
637 " Bailing out ...\n");
638 free_netdev(pshm_dev->pshm_netdev);
639 return -ENOMEM;
640 }
641 tx_buf->index = j;
642 tx_buf->phy_addr = pshm_drv->shm_tx_addr + (TX_BUF_SZ * j);
643 tx_buf->len = TX_BUF_SZ;
644 tx_buf->frames = 0;
645 tx_buf->frm_ofs = SHM_CAIF_FRM_OFS;
646
647 if (pshm_dev->shm_loopback)
648 tx_buf->desc_vptr = (unsigned char *)tx_buf->phy_addr;
649 else
650 tx_buf->desc_vptr =
651 ioremap(tx_buf->phy_addr, TX_BUF_SZ);
652
653 list_add_tail(&tx_buf->list, &pshm_drv->tx_empty_list);
654 }
655
656 for (j = 0; j < NR_RX_BUF; j++) {
657 struct buf_list *rx_buf =
658 kmalloc(sizeof(struct buf_list), GFP_KERNEL);
659
660 if (rx_buf == NULL) {
661 pr_warn("ERROR, Could not"
662 " allocate dynamic mem.for rx_buf,"
663 " Bailing out ...\n");
664 free_netdev(pshm_dev->pshm_netdev);
665 return -ENOMEM;
666 }
667 rx_buf->index = j;
668 rx_buf->phy_addr = pshm_drv->shm_rx_addr + (RX_BUF_SZ * j);
669 rx_buf->len = RX_BUF_SZ;
670
671 if (pshm_dev->shm_loopback)
672 rx_buf->desc_vptr = (unsigned char *)rx_buf->phy_addr;
673 else
674 rx_buf->desc_vptr =
675 ioremap(rx_buf->phy_addr, RX_BUF_SZ);
676 list_add_tail(&rx_buf->list, &pshm_drv->rx_empty_list);
677 }
678
679 pshm_drv->tx_empty_available = 1;
680 result = register_netdev(pshm_dev->pshm_netdev);
681 if (result)
682 pr_warn("ERROR[%d], SHM could not, "
683 "register with NW FRMWK Bailing out ...\n", result);
684
685 return result;
686 }
687
caif_shmcore_remove(struct net_device * pshm_netdev)688 void caif_shmcore_remove(struct net_device *pshm_netdev)
689 {
690 struct buf_list *pbuf;
691 struct shmdrv_layer *pshm_drv = NULL;
692
693 pshm_drv = netdev_priv(pshm_netdev);
694
695 while (!(list_empty(&pshm_drv->tx_pend_list))) {
696 pbuf =
697 list_entry(pshm_drv->tx_pend_list.next,
698 struct buf_list, list);
699
700 list_del(&pbuf->list);
701 kfree(pbuf);
702 }
703
704 while (!(list_empty(&pshm_drv->tx_full_list))) {
705 pbuf =
706 list_entry(pshm_drv->tx_full_list.next,
707 struct buf_list, list);
708 list_del(&pbuf->list);
709 kfree(pbuf);
710 }
711
712 while (!(list_empty(&pshm_drv->tx_empty_list))) {
713 pbuf =
714 list_entry(pshm_drv->tx_empty_list.next,
715 struct buf_list, list);
716 list_del(&pbuf->list);
717 kfree(pbuf);
718 }
719
720 while (!(list_empty(&pshm_drv->rx_full_list))) {
721 pbuf =
722 list_entry(pshm_drv->tx_full_list.next,
723 struct buf_list, list);
724 list_del(&pbuf->list);
725 kfree(pbuf);
726 }
727
728 while (!(list_empty(&pshm_drv->rx_pend_list))) {
729 pbuf =
730 list_entry(pshm_drv->tx_pend_list.next,
731 struct buf_list, list);
732 list_del(&pbuf->list);
733 kfree(pbuf);
734 }
735
736 while (!(list_empty(&pshm_drv->rx_empty_list))) {
737 pbuf =
738 list_entry(pshm_drv->rx_empty_list.next,
739 struct buf_list, list);
740 list_del(&pbuf->list);
741 kfree(pbuf);
742 }
743
744 /* Destroy work queues. */
745 destroy_workqueue(pshm_drv->pshm_tx_workqueue);
746 destroy_workqueue(pshm_drv->pshm_rx_workqueue);
747
748 unregister_netdev(pshm_netdev);
749 }
750