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