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