projects / deliverable / linux.git / blob
? search:


f1936b92533b9828f720a9a87b6cfa9187b158e0
[deliverable/linux.git] / drivers / staging / gdm72xx / gdm_wimax.c
1 /*
2 * Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved.
3 *
4 * This software is licensed under the terms of the GNU General Public
5 * License version 2, as published by the Free Software Foundation, and
6 * may be copied, distributed, and modified under those terms.
7 *
8 * This program is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 */
13
14 #include <linux/version.h>
15 #include <linux/etherdevice.h>
16 #include <asm/byteorder.h>
17 #include <linux/ip.h>
18 #include <linux/ipv6.h>
19 #include <linux/udp.h>
20 #include <linux/in.h>
21
22 #include "gdm_wimax.h"
23 #include "hci.h"
24 #include "wm_ioctl.h"
25 #include "netlink_k.h"
26
27 #define gdm_wimax_send(n, d, l) \
28 (n->phy_dev->send_func)(n->phy_dev->priv_dev, d, l, NULL, NULL)
29 #define gdm_wimax_send_with_cb(n, d, l, c, b) \
30 (n->phy_dev->send_func)(n->phy_dev->priv_dev, d, l, c, b)
31 #define gdm_wimax_rcv_with_cb(n, c, b) \
32 (n->phy_dev->rcv_func)(n->phy_dev->priv_dev, c, b)
33
34 #define EVT_MAX_SIZE 2048
35
36 struct evt_entry {
37 struct list_head list;
38 struct net_device *dev;
39 char evt_data[EVT_MAX_SIZE];
40 int size;
41 };
42
43 static void __gdm_wimax_event_send(struct work_struct *work);
44 static inline struct evt_entry *alloc_event_entry(void);
45 static inline void free_event_entry(struct evt_entry *e);
46 static struct evt_entry *get_event_entry(void);
47 static void put_event_entry(struct evt_entry *e);
48
49 static struct {
50 int ref_cnt;
51 struct sock *sock;
52 struct list_head evtq;
53 spinlock_t evt_lock;
54
55 struct list_head freeq;
56 struct work_struct ws;
57 } wm_event;
58
59 static u8 gdm_wimax_macaddr[6] = {0x00, 0x0a, 0x3b, 0xf0, 0x01, 0x30};
60
61 static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm);
62 static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up);
63
64 #if defined(DEBUG_SDU)
65 static void printk_hex(u8 *buf, u32 size)
66 {
67 int i;
68
69 for (i = 0; i < size; i++) {
70 if (i && i % 16 == 0)
71 printk(KERN_DEBUG "\n%02x ", *buf++);
72 else
73 printk(KERN_DEBUG "%02x ", *buf++);
74 }
75
76 printk(KERN_DEBUG "\n");
77 }
78
79 static const char *get_protocol_name(u16 protocol)
80 {
81 static char buf[32];
82 const char *name = "-";
83
84 switch (protocol) {
85 case ETH_P_ARP:
86 name = "ARP";
87 break;
88 case ETH_P_IP:
89 name = "IP";
90 break;
91 case ETH_P_IPV6:
92 name = "IPv6";
93 break;
94 }
95
96 sprintf(buf, "0x%04x(%s)", protocol, name);
97 return buf;
98 }
99
100 static const char *get_ip_protocol_name(u8 ip_protocol)
101 {
102 static char buf[32];
103 const char *name = "-";
104
105 switch (ip_protocol) {
106 case IPPROTO_TCP:
107 name = "TCP";
108 break;
109 case IPPROTO_UDP:
110 name = "UDP";
111 break;
112 case IPPROTO_ICMP:
113 name = "ICMP";
114 break;
115 }
116
117 sprintf(buf, "%u(%s)", ip_protocol, name);
118 return buf;
119 }
120
121 static const char *get_port_name(u16 port)
122 {
123 static char buf[32];
124 const char *name = "-";
125
126 switch (port) {
127 case 67:
128 name = "DHCP-Server";
129 break;
130 case 68:
131 name = "DHCP-Client";
132 break;
133 case 69:
134 name = "TFTP";
135 break;
136 }
137
138 sprintf(buf, "%u(%s)", port, name);
139 return buf;
140 }
141
142 static void dump_eth_packet(const char *title, u8 *data, int len)
143 {
144 struct iphdr *ih = NULL;
145 struct udphdr *uh = NULL;
146 u16 protocol = 0;
147 u8 ip_protocol = 0;
148 u16 port = 0;
149
150 protocol = (data[12]<<8) | data[13];
151 ih = (struct iphdr *) (data+ETH_HLEN);
152
153 if (protocol == ETH_P_IP) {
154 uh = (struct udphdr *) ((char *)ih + sizeof(struct iphdr));
155 ip_protocol = ih->protocol;
156 port = ntohs(uh->dest);
157 } else if (protocol == ETH_P_IPV6) {
158 struct ipv6hdr *i6h = (struct ipv6hdr *) data;
159 uh = (struct udphdr *) ((char *)i6h + sizeof(struct ipv6hdr));
160 ip_protocol = i6h->nexthdr;
161 port = ntohs(uh->dest);
162 }
163
164 printk(KERN_DEBUG "[%s] len=%d, %s, %s, %s\n",
165 title, len,
166 get_protocol_name(protocol),
167 get_ip_protocol_name(ip_protocol),
168 get_port_name(port));
169
170 #if 1
171 if (!(data[0] == 0xff && data[1] == 0xff)) {
172 if (protocol == ETH_P_IP) {
173 printk(KERN_DEBUG " src=%u.%u.%u.%u\n",
174 NIPQUAD(ih->saddr));
175 } else if (protocol == ETH_P_IPV6) {
176 #ifdef NIP6
177 printk(KERN_DEBUG " src=%x:%x:%x:%x:%x:%x:%x:%x\n",
178 NIP6(ih->saddr));
179 #else
180 printk(KERN_DEBUG " src=%pI6\n", &ih->saddr);
181 #endif
182 }
183 }
184 #endif
185
186 #if (DUMP_PACKET & DUMP_SDU_ALL)
187 printk_hex(data, len);
188 #else
189 #if (DUMP_PACKET & DUMP_SDU_ARP)
190 if (protocol == ETH_P_ARP)
191 printk_hex(data, len);
192 #endif
193 #if (DUMP_PACKET & DUMP_SDU_IP)
194 if (protocol == ETH_P_IP || protocol == ETH_P_IPV6)
195 printk_hex(data, len);
196 #else
197 #if (DUMP_PACKET & DUMP_SDU_IP_TCP)
198 if (ip_protocol == IPPROTO_TCP)
199 printk_hex(data, len);
200 #endif
201 #if (DUMP_PACKET & DUMP_SDU_IP_UDP)
202 if (ip_protocol == IPPROTO_UDP)
203 printk_hex(data, len);
204 #endif
205 #if (DUMP_PACKET & DUMP_SDU_IP_ICMP)
206 if (ip_protocol == IPPROTO_ICMP)
207 printk_hex(data, len);
208 #endif
209 #endif
210 #endif
211 }
212 #endif
213
214
215 static inline int gdm_wimax_header(struct sk_buff **pskb)
216 {
217 u16 buf[HCI_HEADER_SIZE / sizeof(u16)];
218 struct sk_buff *skb = *pskb;
219 int ret = 0;
220
221 if (unlikely(skb_headroom(skb) < HCI_HEADER_SIZE)) {
222 struct sk_buff *skb2;
223
224 skb2 = skb_realloc_headroom(skb, HCI_HEADER_SIZE);
225 if (skb2 == NULL)
226 return -ENOMEM;
227 if (skb->sk)
228 skb_set_owner_w(skb2, skb->sk);
229 kfree_skb(skb);
230 skb = skb2;
231 }
232
233 skb_push(skb, HCI_HEADER_SIZE);
234 buf[0] = H2B(WIMAX_TX_SDU);
235 buf[1] = H2B(skb->len - HCI_HEADER_SIZE);
236 memcpy(skb->data, buf, HCI_HEADER_SIZE);
237
238 *pskb = skb;
239 return ret;
240 }
241
242 static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
243 int len)
244 {
245 struct nic *nic = netdev_priv(dev);
246
247 #if defined(DEBUG_HCI)
248 u8 *buf = (u8 *) msg;
249 u16 hci_cmd = (buf[0]<<8) | buf[1];
250 u16 hci_len = (buf[2]<<8) | buf[3];
251 printk(KERN_DEBUG "H=>D: 0x%04x(%d)\n", hci_cmd, hci_len);
252 #endif
253
254 gdm_wimax_send(nic, msg, len);
255 }
256
257 static int gdm_wimax_event_init(void)
258 {
259 if (wm_event.ref_cnt == 0) {
260 wm_event.sock = netlink_init(NETLINK_WIMAX,
261 gdm_wimax_event_rcv);
262 INIT_LIST_HEAD(&wm_event.evtq);
263 INIT_LIST_HEAD(&wm_event.freeq);
264 INIT_WORK(&wm_event.ws, __gdm_wimax_event_send);
265 spin_lock_init(&wm_event.evt_lock);
266 }
267
268 if (wm_event.sock) {
269 wm_event.ref_cnt++;
270 return 0;
271 }
272
273 printk(KERN_ERR "Creating WiMax Event netlink is failed\n");
274 return -1;
275 }
276
277 static void gdm_wimax_event_exit(void)
278 {
279 if (wm_event.sock && --wm_event.ref_cnt == 0) {
280 struct evt_entry *e, *temp;
281 unsigned long flags;
282
283 spin_lock_irqsave(&wm_event.evt_lock, flags);
284
285 list_for_each_entry_safe(e, temp, &wm_event.evtq, list) {
286 list_del(&e->list);
287 free_event_entry(e);
288 }
289 list_for_each_entry_safe(e, temp, &wm_event.freeq, list) {
290 list_del(&e->list);
291 free_event_entry(e);
292 }
293
294 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
295 netlink_exit(wm_event.sock);
296 wm_event.sock = NULL;
297 }
298 }
299
300 static inline struct evt_entry *alloc_event_entry(void)
301 {
302 return kmalloc(sizeof(struct evt_entry), GFP_ATOMIC);
303 }
304
305 static inline void free_event_entry(struct evt_entry *e)
306 {
307 kfree(e);
308 }
309
310 static struct evt_entry *get_event_entry(void)
311 {
312 struct evt_entry *e;
313
314 if (list_empty(&wm_event.freeq))
315 e = alloc_event_entry();
316 else {
317 e = list_entry(wm_event.freeq.next, struct evt_entry, list);
318 list_del(&e->list);
319 }
320
321 return e;
322 }
323
324 static void put_event_entry(struct evt_entry *e)
325 {
326 BUG_ON(!e);
327
328 list_add_tail(&e->list, &wm_event.freeq);
329 }
330
331 static void __gdm_wimax_event_send(struct work_struct *work)
332 {
333 int idx;
334 unsigned long flags;
335 struct evt_entry *e;
336
337 spin_lock_irqsave(&wm_event.evt_lock, flags);
338
339 while (!list_empty(&wm_event.evtq)) {
340 e = list_entry(wm_event.evtq.next, struct evt_entry, list);
341 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
342
343 sscanf(e->dev->name, "wm%d", &idx);
344 netlink_send(wm_event.sock, idx, 0, e->evt_data, e->size);
345
346 spin_lock_irqsave(&wm_event.evt_lock, flags);
347 list_del(&e->list);
348 put_event_entry(e);
349 }
350
351 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
352 }
353
354 static int gdm_wimax_event_send(struct net_device *dev, char *buf, int size)
355 {
356 struct evt_entry *e;
357 unsigned long flags;
358
359 #if defined(DEBUG_HCI)
360 u16 hci_cmd = ((u8)buf[0]<<8) | (u8)buf[1];
361 u16 hci_len = ((u8)buf[2]<<8) | (u8)buf[3];
362 printk(KERN_DEBUG "D=>H: 0x%04x(%d)\n", hci_cmd, hci_len);
363 #endif
364
365 spin_lock_irqsave(&wm_event.evt_lock, flags);
366
367 e = get_event_entry();
368 if (!e) {
369 printk(KERN_ERR "%s: No memory for event\n", __func__);
370 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
371 return -ENOMEM;
372 }
373
374 e->dev = dev;
375 e->size = size;
376 memcpy(e->evt_data, buf, size);
377
378 list_add_tail(&e->list, &wm_event.evtq);
379 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
380
381 schedule_work(&wm_event.ws);
382
383 return 0;
384 }
385
386 static void tx_complete(void *arg)
387 {
388 struct nic *nic = arg;
389
390 if (netif_queue_stopped(nic->netdev))
391 netif_wake_queue(nic->netdev);
392 }
393
394 int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev)
395 {
396 int ret = 0;
397 struct nic *nic = netdev_priv(dev);
398
399 ret = gdm_wimax_send_with_cb(nic, skb->data, skb->len, tx_complete,
400 nic);
401 if (ret == -ENOSPC) {
402 netif_stop_queue(dev);
403 ret = 0;
404 }
405
406 if (ret) {
407 skb_pull(skb, HCI_HEADER_SIZE);
408 return ret;
409 }
410
411 nic->stats.tx_packets++;
412 nic->stats.tx_bytes += skb->len - HCI_HEADER_SIZE;
413 kfree_skb(skb);
414 return ret;
415 }
416
417 static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev)
418 {
419 int ret = 0;
420 struct nic *nic = netdev_priv(dev);
421 struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
422
423 #if defined(DEBUG_SDU)
424 dump_eth_packet("TX", skb->data, skb->len);
425 #endif
426
427 ret = gdm_wimax_header(&skb);
428 if (ret < 0) {
429 skb_pull(skb, HCI_HEADER_SIZE);
430 return ret;
431 }
432
433 #if !defined(LOOPBACK_TEST)
434 if (!fsm)
435 printk(KERN_ERR "ASSERTION ERROR: fsm is NULL!!\n");
436 else if (fsm->m_status != M_CONNECTED) {
437 printk(KERN_EMERG "ASSERTION ERROR: Device is NOT ready. status=%d\n",
438 fsm->m_status);
439 kfree_skb(skb);
440 return 0;
441 }
442 #endif
443
444 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
445 ret = gdm_qos_send_hci_pkt(skb, dev);
446 #else
447 ret = gdm_wimax_send_tx(skb, dev);
448 #endif
449 return ret;
450 }
451
452 static int gdm_wimax_set_config(struct net_device *dev, struct ifmap *map)
453 {
454 if (dev->flags & IFF_UP)
455 return -EBUSY;
456
457 return 0;
458 }
459
460 static void __gdm_wimax_set_mac_addr(struct net_device *dev, char *mac_addr)
461 {
462 u16 hci_pkt_buf[32 / sizeof(u16)];
463 u8 *pkt = (u8 *) &hci_pkt_buf[0];
464 struct nic *nic = netdev_priv(dev);
465
466 /* Since dev is registered as a ethernet device,
467 * ether_setup has made dev->addr_len to be ETH_ALEN
468 */
469 memcpy(dev->dev_addr, mac_addr, dev->addr_len);
470
471 /* Let lower layer know of this change by sending
472 * SetInformation(MAC Address)
473 */
474 hci_pkt_buf[0] = H2B(WIMAX_SET_INFO); /* cmd_evt */
475 hci_pkt_buf[1] = H2B(8); /* size */
476 pkt[4] = 0; /* T */
477 pkt[5] = 6; /* L */
478 memcpy(pkt + 6, mac_addr, dev->addr_len); /* V */
479
480 gdm_wimax_send(nic, pkt, HCI_HEADER_SIZE + 8);
481 }
482
483 /* A driver function */
484 static int gdm_wimax_set_mac_addr(struct net_device *dev, void *p)
485 {
486 struct sockaddr *addr = p;
487
488 if (netif_running(dev))
489 return -EBUSY;
490
491 if (!is_valid_ether_addr(addr->sa_data))
492 return -EADDRNOTAVAIL;
493
494 __gdm_wimax_set_mac_addr(dev, addr->sa_data);
495
496 return 0;
497 }
498
499 static struct net_device_stats *gdm_wimax_stats(struct net_device *dev)
500 {
501 struct nic *nic = netdev_priv(dev);
502
503 return &nic->stats;
504 }
505
506 static int gdm_wimax_open(struct net_device *dev)
507 {
508 struct nic *nic = netdev_priv(dev);
509 struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
510
511 netif_start_queue(dev);
512
513 if (fsm && fsm->m_status != M_INIT)
514 gdm_wimax_ind_if_updown(dev, 1);
515 return 0;
516 }
517
518 static int gdm_wimax_close(struct net_device *dev)
519 {
520 struct nic *nic = netdev_priv(dev);
521 struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
522
523 netif_stop_queue(dev);
524
525 if (fsm && fsm->m_status != M_INIT)
526 gdm_wimax_ind_if_updown(dev, 0);
527 return 0;
528 }
529
530 static void kdelete(void **buf)
531 {
532 if (buf && *buf) {
533 kfree(*buf);
534 *buf = NULL;
535 }
536 }
537
538 static int gdm_wimax_ioctl_get_data(struct data_s *dst, struct data_s *src)
539 {
540 int size;
541
542 size = dst->size < src->size ? dst->size : src->size;
543
544 dst->size = size;
545 if (src->size) {
546 if (!dst->buf)
547 return -EINVAL;
548 if (copy_to_user(dst->buf, src->buf, size))
549 return -EFAULT;
550 }
551 return 0;
552 }
553
554 static int gdm_wimax_ioctl_set_data(struct data_s *dst, struct data_s *src)
555 {
556 if (!src->size) {
557 dst->size = 0;
558 return 0;
559 }
560
561 if (!src->buf)
562 return -EINVAL;
563
564 if (!(dst->buf && dst->size == src->size)) {
565 kdelete(&dst->buf);
566 dst->buf = kmalloc(src->size, GFP_KERNEL);
567 if (dst->buf == NULL)
568 return -ENOMEM;
569 }
570
571 if (copy_from_user(dst->buf, src->buf, src->size)) {
572 kdelete(&dst->buf);
573 return -EFAULT;
574 }
575 dst->size = src->size;
576 return 0;
577 }
578
579 static void gdm_wimax_cleanup_ioctl(struct net_device *dev)
580 {
581 struct nic *nic = netdev_priv(dev);
582 int i;
583
584 for (i = 0; i < SIOC_DATA_MAX; i++)
585 kdelete(&nic->sdk_data[i].buf);
586 }
587
588 static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm)
589 {
590 struct nic *nic = netdev_priv(dev);
591 struct fsm_s *cur_fsm =
592 (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
593
594 if (!cur_fsm)
595 return;
596
597 if (cur_fsm->m_status != new_fsm->m_status ||
598 cur_fsm->c_status != new_fsm->c_status) {
599 if (new_fsm->m_status == M_CONNECTED)
600 netif_carrier_on(dev);
601 else if (cur_fsm->m_status == M_CONNECTED) {
602 netif_carrier_off(dev);
603 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
604 gdm_qos_release_list(nic);
605 #endif
606 }
607 gdm_wimax_ind_fsm_update(dev, new_fsm);
608 }
609 }
610
611 static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
612 {
613 struct wm_req_s *req = (struct wm_req_s *) ifr;
614 struct nic *nic = netdev_priv(dev);
615 int ret;
616
617 if (cmd != SIOCWMIOCTL)
618 return -EOPNOTSUPP;
619
620 switch (req->cmd) {
621 case SIOCG_DATA:
622 case SIOCS_DATA:
623 if (req->data_id >= SIOC_DATA_MAX) {
624 printk(KERN_ERR
625 "%s error: data-index(%d) is invalid!!\n",
626 __func__, req->data_id);
627 return -EOPNOTSUPP;
628 }
629 if (req->cmd == SIOCG_DATA) {
630 ret = gdm_wimax_ioctl_get_data(&req->data,
631 &nic->sdk_data[req->data_id]);
632 if (ret < 0)
633 return ret;
634 } else if (req->cmd == SIOCS_DATA) {
635 if (req->data_id == SIOC_DATA_FSM) {
636 /*NOTE: gdm_update_fsm should be called
637 before gdm_wimax_ioctl_set_data is called*/
638 gdm_update_fsm(dev,
639 (struct fsm_s *) req->data.buf);
640 }
641 ret = gdm_wimax_ioctl_set_data(
642 &nic->sdk_data[req->data_id], &req->data);
643 if (ret < 0)
644 return ret;
645 }
646 break;
647 default:
648 printk(KERN_ERR "%s: %x unknown ioctl\n", __func__, cmd);
649 return -EOPNOTSUPP;
650 }
651
652 return 0;
653 }
654
655 static void gdm_wimax_prepare_device(struct net_device *dev)
656 {
657 struct nic *nic = netdev_priv(dev);
658 u16 buf[32 / sizeof(u16)];
659 struct hci_s *hci = (struct hci_s *) buf;
660 u16 len = 0;
661 u32 val = 0;
662
663 #define BIT_MULTI_CS 0
664 #define BIT_WIMAX 1
665 #define BIT_QOS 2
666 #define BIT_AGGREGATION 3
667
668 /* GetInformation mac address */
669 len = 0;
670 hci->cmd_evt = H2B(WIMAX_GET_INFO);
671 hci->data[len++] = TLV_T(T_MAC_ADDRESS);
672 hci->length = H2B(len);
673 gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len);
674
675 val = (1<<BIT_WIMAX) | (1<<BIT_MULTI_CS);
676 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
677 val |= (1<<BIT_QOS);
678 #endif
679 #if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
680 val |= (1<<BIT_AGGREGATION);
681 #endif
682
683 /* Set capability */
684 len = 0;
685 hci->cmd_evt = H2B(WIMAX_SET_INFO);
686 hci->data[len++] = TLV_T(T_CAPABILITY);
687 hci->data[len++] = TLV_L(T_CAPABILITY);
688 val = DH2B(val);
689 memcpy(&hci->data[len], &val, TLV_L(T_CAPABILITY));
690 len += TLV_L(T_CAPABILITY);
691 hci->length = H2B(len);
692 gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len);
693
694 printk(KERN_INFO "GDM WiMax Set CAPABILITY: 0x%08X\n", DB2H(val));
695 }
696
697 static int gdm_wimax_hci_get_tlv(u8 *buf, u8 *T, u16 *L, u8 **V)
698 {
699 #define __U82U16(b) ((u16)((u8 *)(b))[0] | ((u16)((u8 *)(b))[1] << 8))
700 int next_pos;
701
702 *T = buf[0];
703 if (buf[1] == 0x82) {
704 *L = B2H(__U82U16(&buf[2]));
705 next_pos = 1/*type*/+3/*len*/;
706 } else {
707 *L = buf[1];
708 next_pos = 1/*type*/+1/*len*/;
709 }
710 *V = &buf[next_pos];
711
712 next_pos += *L/*length of val*/;
713 return next_pos;
714 }
715
716 static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf,
717 int len)
718 {
719 u8 T, *V;
720 u16 L;
721 u16 cmd_evt, cmd_len;
722 int pos = HCI_HEADER_SIZE;
723
724 cmd_evt = B2H(*(u16 *)&buf[0]);
725 cmd_len = B2H(*(u16 *)&buf[2]);
726
727 if (len < cmd_len + HCI_HEADER_SIZE) {
728 printk(KERN_ERR "%s: invalid length [%d/%d]\n", __func__,
729 cmd_len + HCI_HEADER_SIZE, len);
730 return -1;
731 }
732
733 if (cmd_evt == WIMAX_GET_INFO_RESULT) {
734 if (cmd_len < 2) {
735 printk(KERN_ERR "%s: len is too short [%x/%d]\n",
736 __func__, cmd_evt, len);
737 return -1;
738 }
739
740 pos += gdm_wimax_hci_get_tlv(&buf[pos], &T, &L, &V);
741 if (T == TLV_T(T_MAC_ADDRESS)) {
742 if (L != dev->addr_len) {
743 printk(KERN_ERR
744 "%s Invalid inofrmation result T/L "
745 "[%x/%d]\n", __func__, T, L);
746 return -1;
747 }
748 printk(KERN_INFO
749 "MAC change [%02x:%02x:%02x:%02x:%02x:%02x]"
750 "->[%02x:%02x:%02x:%02x:%02x:%02x]\n",
751 dev->dev_addr[0], dev->dev_addr[1],
752 dev->dev_addr[2], dev->dev_addr[3],
753 dev->dev_addr[4], dev->dev_addr[5],
754 V[0], V[1], V[2], V[3], V[4], V[5]);
755 memcpy(dev->dev_addr, V, dev->addr_len);
756 return 1;
757 }
758 }
759
760 gdm_wimax_event_send(dev, buf, len);
761 return 0;
762 }
763
764 static void gdm_wimax_netif_rx(struct net_device *dev, char *buf, int len)
765 {
766 struct nic *nic = netdev_priv(dev);
767 struct sk_buff *skb;
768 int ret;
769
770 #if defined(DEBUG_SDU)
771 dump_eth_packet("RX", buf, len);
772 #endif
773
774 skb = dev_alloc_skb(len + 2);
775 if (!skb) {
776 printk(KERN_ERR "%s: dev_alloc_skb failed!\n", __func__);
777 return;
778 }
779 skb_reserve(skb, 2);
780
781 nic->stats.rx_packets++;
782 nic->stats.rx_bytes += len;
783
784 memcpy(skb_put(skb, len), buf, len);
785
786 skb->dev = dev;
787 skb->protocol = eth_type_trans(skb, dev); /* what will happen? */
788
789 ret = in_interrupt() ? netif_rx(skb) : netif_rx_ni(skb);
790 if (ret == NET_RX_DROP)
791 printk(KERN_ERR "%s skb dropped\n", __func__);
792 }
793
794 static void gdm_wimax_transmit_aggr_pkt(struct net_device *dev, char *buf,
795 int len)
796 {
797 #define HCI_PADDING_BYTE 4
798 #define HCI_RESERVED_BYTE 4
799 struct hci_s *hci;
800 int length;
801
802 while (len > 0) {
803 hci = (struct hci_s *) buf;
804
805 if (B2H(hci->cmd_evt) != WIMAX_RX_SDU) {
806 printk(KERN_ERR "Wrong cmd_evt(0x%04X)\n",
807 B2H(hci->cmd_evt));
808 break;
809 }
810
811 length = B2H(hci->length);
812 gdm_wimax_netif_rx(dev, hci->data, length);
813
814 if (length & 0x3) {
815 /* Add padding size */
816 length += HCI_PADDING_BYTE - (length & 0x3);
817 }
818
819 length += HCI_HEADER_SIZE + HCI_RESERVED_BYTE;
820 len -= length;
821 buf += length;
822 }
823 }
824
825 static void gdm_wimax_transmit_pkt(struct net_device *dev, char *buf, int len)
826 {
827 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
828 struct nic *nic = netdev_priv(dev);
829 #endif
830 u16 cmd_evt, cmd_len;
831
832 /* This code is added for certain rx packet to be ignored. */
833 if (len == 0)
834 return;
835
836 cmd_evt = B2H(*(u16 *)&buf[0]);
837 cmd_len = B2H(*(u16 *)&buf[2]);
838
839 if (len < cmd_len + HCI_HEADER_SIZE) {
840 if (len)
841 printk(KERN_ERR "%s: invalid length [%d/%d]\n",
842 __func__, cmd_len + HCI_HEADER_SIZE, len);
843 return;
844 }
845
846 switch (cmd_evt) {
847 case WIMAX_RX_SDU_AGGR:
848 gdm_wimax_transmit_aggr_pkt(dev, &buf[HCI_HEADER_SIZE],
849 cmd_len);
850 break;
851 case WIMAX_RX_SDU:
852 gdm_wimax_netif_rx(dev, &buf[HCI_HEADER_SIZE], cmd_len);
853 break;
854 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
855 case WIMAX_EVT_MODEM_REPORT:
856 gdm_recv_qos_hci_packet(nic, buf, len);
857 break;
858 #endif
859 case WIMAX_SDU_TX_FLOW:
860 if (buf[4] == 0) {
861 if (!netif_queue_stopped(dev))
862 netif_stop_queue(dev);
863 } else if (buf[4] == 1) {
864 if (netif_queue_stopped(dev))
865 netif_wake_queue(dev);
866 }
867 break;
868 default:
869 gdm_wimax_event_send(dev, buf, len);
870 break;
871 }
872 }
873
874 static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm)
875 {
876 u16 buf[32 / sizeof(u16)];
877 u8 *hci_pkt_buf = (u8 *)&buf[0];
878
879 /* Indicate updating fsm */
880 buf[0] = H2B(WIMAX_FSM_UPDATE);
881 buf[1] = H2B(sizeof(struct fsm_s));
882 memcpy(&hci_pkt_buf[HCI_HEADER_SIZE], fsm, sizeof(struct fsm_s));
883
884 gdm_wimax_event_send(dev, hci_pkt_buf,
885 HCI_HEADER_SIZE + sizeof(struct fsm_s));
886 }
887
888 static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up)
889 {
890 u16 buf[32 / sizeof(u16)];
891 struct hci_s *hci = (struct hci_s *) buf;
892 unsigned char up_down;
893
894 up_down = if_up ? WIMAX_IF_UP : WIMAX_IF_DOWN;
895
896 /* Indicate updating fsm */
897 hci->cmd_evt = H2B(WIMAX_IF_UPDOWN);
898 hci->length = H2B(sizeof(up_down));
899 hci->data[0] = up_down;
900
901 gdm_wimax_event_send(dev, (char *)hci, HCI_HEADER_SIZE+sizeof(up_down));
902 }
903
904 static void rx_complete(void *arg, void *data, int len)
905 {
906 struct nic *nic = arg;
907
908 gdm_wimax_transmit_pkt(nic->netdev, data, len);
909 gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
910 }
911
912 static void prepare_rx_complete(void *arg, void *data, int len)
913 {
914 struct nic *nic = arg;
915 int ret;
916
917 ret = gdm_wimax_get_prepared_info(nic->netdev, data, len);
918 if (ret == 1)
919 gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
920 else {
921 if (ret < 0)
922 printk(KERN_ERR "get_prepared_info failed(%d)\n", ret);
923 gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
924 #if 0
925 /* Re-prepare WiMax device */
926 gdm_wimax_prepare_device(nic->netdev);
927 #endif
928 }
929 }
930
931 static void start_rx_proc(struct nic *nic)
932 {
933 gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
934 }
935
936 static struct net_device_ops gdm_netdev_ops = {
937 .ndo_open = gdm_wimax_open,
938 .ndo_stop = gdm_wimax_close,
939 .ndo_set_config = gdm_wimax_set_config,
940 .ndo_start_xmit = gdm_wimax_tx,
941 .ndo_get_stats = gdm_wimax_stats,
942 .ndo_set_mac_address = gdm_wimax_set_mac_addr,
943 .ndo_do_ioctl = gdm_wimax_ioctl,
944 };
945
946 int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
947 {
948 struct nic *nic = NULL;
949 struct net_device *dev;
950 int ret;
951
952 dev = (struct net_device *)alloc_netdev(sizeof(*nic),
953 "wm%d", ether_setup);
954
955 if (dev == NULL) {
956 printk(KERN_ERR "alloc_etherdev failed\n");
957 return -ENOMEM;
958 }
959
960 SET_NETDEV_DEV(dev, pdev);
961 dev->mtu = 1400;
962 dev->netdev_ops = &gdm_netdev_ops;
963 dev->flags &= ~IFF_MULTICAST;
964 memcpy(dev->dev_addr, gdm_wimax_macaddr, sizeof(gdm_wimax_macaddr));
965
966 nic = netdev_priv(dev);
967 memset(nic, 0, sizeof(*nic));
968
969 nic->netdev = dev;
970 nic->phy_dev = phy_dev;
971 phy_dev->netdev = dev;
972
973 /* event socket init */
974 ret = gdm_wimax_event_init();
975 if (ret < 0) {
976 printk(KERN_ERR "Cannot create event.\n");
977 goto cleanup;
978 }
979
980 ret = register_netdev(dev);
981 if (ret)
982 goto cleanup;
983
984 #if defined(LOOPBACK_TEST)
985 netif_start_queue(dev);
986 netif_carrier_on(dev);
987 #else
988 netif_carrier_off(dev);
989 #endif
990
991 #ifdef CONFIG_WIMAX_GDM72XX_QOS
992 gdm_qos_init(nic);
993 #endif
994
995 start_rx_proc(nic);
996
997 /* Prepare WiMax device */
998 gdm_wimax_prepare_device(dev);
999
1000 return 0;
1001
1002 cleanup:
1003 printk(KERN_ERR "register_netdev failed\n");
1004 free_netdev(dev);
1005 return ret;
1006 }
1007
1008 void unregister_wimax_device(struct phy_dev *phy_dev)
1009 {
1010 struct nic *nic = netdev_priv(phy_dev->netdev);
1011 struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
1012
1013 if (fsm)
1014 fsm->m_status = M_INIT;
1015 unregister_netdev(nic->netdev);
1016
1017 gdm_wimax_event_exit();
1018
1019 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
1020 gdm_qos_release_list(nic);
1021 #endif
1022
1023 gdm_wimax_cleanup_ioctl(phy_dev->netdev);
1024
1025 free_netdev(nic->netdev);
1026 }
Linux kernel - Deliverables
This page took 0.068048 seconds and 4 git commands to generate.