2 * Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved.
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.
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.
14 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
16 #include <linux/etherdevice.h>
17 #include <asm/byteorder.h>
19 #include <linux/ipv6.h>
20 #include <linux/udp.h>
23 #include "gdm_wimax.h"
26 #include "netlink_k.h"
28 #define gdm_wimax_send(n, d, l) \
29 (n->phy_dev->send_func)(n->phy_dev->priv_dev, d, l, NULL, NULL)
30 #define gdm_wimax_send_with_cb(n, d, l, c, b) \
31 (n->phy_dev->send_func)(n->phy_dev->priv_dev, d, l, c, b)
32 #define gdm_wimax_rcv_with_cb(n, c, b) \
33 (n->phy_dev->rcv_func)(n->phy_dev->priv_dev, c, b)
35 #define EVT_MAX_SIZE 2048
38 struct list_head list
;
39 struct net_device
*dev
;
40 char evt_data
[EVT_MAX_SIZE
];
47 struct list_head evtq
;
49 struct list_head freeq
;
50 struct work_struct ws
;
53 static u8 gdm_wimax_macaddr
[6] = {0x00, 0x0a, 0x3b, 0xf0, 0x01, 0x30};
55 static inline int gdm_wimax_header(struct sk_buff
**pskb
)
57 u16 buf
[HCI_HEADER_SIZE
/ sizeof(u16
)];
58 struct hci_s
*hci
= (struct hci_s
*)buf
;
59 struct sk_buff
*skb
= *pskb
;
61 if (unlikely(skb_headroom(skb
) < HCI_HEADER_SIZE
)) {
64 skb2
= skb_realloc_headroom(skb
, HCI_HEADER_SIZE
);
68 skb_set_owner_w(skb2
, skb
->sk
);
73 skb_push(skb
, HCI_HEADER_SIZE
);
74 hci
->cmd_evt
= cpu_to_be16(WIMAX_TX_SDU
);
75 hci
->length
= cpu_to_be16(skb
->len
- HCI_HEADER_SIZE
);
76 memcpy(skb
->data
, buf
, HCI_HEADER_SIZE
);
82 static inline struct evt_entry
*alloc_event_entry(void)
84 return kmalloc(sizeof(struct evt_entry
), GFP_ATOMIC
);
87 static inline void free_event_entry(struct evt_entry
*e
)
92 static struct evt_entry
*get_event_entry(void)
96 if (list_empty(&wm_event
.freeq
)) {
97 e
= alloc_event_entry();
99 e
= list_entry(wm_event
.freeq
.next
, struct evt_entry
, list
);
106 static void put_event_entry(struct evt_entry
*e
)
110 list_add_tail(&e
->list
, &wm_event
.freeq
);
113 static void gdm_wimax_event_rcv(struct net_device
*dev
, u16 type
, void *msg
,
116 struct nic
*nic
= netdev_priv(dev
);
119 u16 hci_cmd
= (buf
[0]<<8) | buf
[1];
120 u16 hci_len
= (buf
[2]<<8) | buf
[3];
122 netdev_dbg(dev
, "H=>D: 0x%04x(%d)\n", hci_cmd
, hci_len
);
124 gdm_wimax_send(nic
, msg
, len
);
127 static void __gdm_wimax_event_send(struct work_struct
*work
)
133 spin_lock_irqsave(&wm_event
.evt_lock
, flags
);
135 while (!list_empty(&wm_event
.evtq
)) {
136 e
= list_entry(wm_event
.evtq
.next
, struct evt_entry
, list
);
137 spin_unlock_irqrestore(&wm_event
.evt_lock
, flags
);
139 if (sscanf(e
->dev
->name
, "wm%d", &idx
) == 1)
140 netlink_send(wm_event
.sock
, idx
, 0, e
->evt_data
,
143 spin_lock_irqsave(&wm_event
.evt_lock
, flags
);
148 spin_unlock_irqrestore(&wm_event
.evt_lock
, flags
);
151 static int gdm_wimax_event_init(void)
153 if (!wm_event
.ref_cnt
) {
154 wm_event
.sock
= netlink_init(NETLINK_WIMAX
,
155 gdm_wimax_event_rcv
);
157 INIT_LIST_HEAD(&wm_event
.evtq
);
158 INIT_LIST_HEAD(&wm_event
.freeq
);
159 INIT_WORK(&wm_event
.ws
, __gdm_wimax_event_send
);
160 spin_lock_init(&wm_event
.evt_lock
);
169 pr_err("Creating WiMax Event netlink is failed\n");
173 static void gdm_wimax_event_exit(void)
175 if (wm_event
.sock
&& --wm_event
.ref_cnt
== 0) {
176 struct evt_entry
*e
, *temp
;
179 spin_lock_irqsave(&wm_event
.evt_lock
, flags
);
181 list_for_each_entry_safe(e
, temp
, &wm_event
.evtq
, list
) {
185 list_for_each_entry_safe(e
, temp
, &wm_event
.freeq
, list
) {
190 spin_unlock_irqrestore(&wm_event
.evt_lock
, flags
);
191 netlink_exit(wm_event
.sock
);
192 wm_event
.sock
= NULL
;
196 static int gdm_wimax_event_send(struct net_device
*dev
, char *buf
, int size
)
201 u16 hci_cmd
= ((u8
)buf
[0]<<8) | (u8
)buf
[1];
202 u16 hci_len
= ((u8
)buf
[2]<<8) | (u8
)buf
[3];
204 netdev_dbg(dev
, "D=>H: 0x%04x(%d)\n", hci_cmd
, hci_len
);
206 spin_lock_irqsave(&wm_event
.evt_lock
, flags
);
208 e
= get_event_entry();
210 netdev_err(dev
, "%s: No memory for event\n", __func__
);
211 spin_unlock_irqrestore(&wm_event
.evt_lock
, flags
);
217 memcpy(e
->evt_data
, buf
, size
);
219 list_add_tail(&e
->list
, &wm_event
.evtq
);
220 spin_unlock_irqrestore(&wm_event
.evt_lock
, flags
);
222 schedule_work(&wm_event
.ws
);
227 static void tx_complete(void *arg
)
229 struct nic
*nic
= arg
;
231 if (netif_queue_stopped(nic
->netdev
))
232 netif_wake_queue(nic
->netdev
);
235 int gdm_wimax_send_tx(struct sk_buff
*skb
, struct net_device
*dev
)
238 struct nic
*nic
= netdev_priv(dev
);
240 ret
= gdm_wimax_send_with_cb(nic
, skb
->data
, skb
->len
, tx_complete
,
242 if (ret
== -ENOSPC
) {
243 netif_stop_queue(dev
);
248 skb_pull(skb
, HCI_HEADER_SIZE
);
252 dev
->stats
.tx_packets
++;
253 dev
->stats
.tx_bytes
+= skb
->len
- HCI_HEADER_SIZE
;
258 static int gdm_wimax_tx(struct sk_buff
*skb
, struct net_device
*dev
)
262 ret
= gdm_wimax_header(&skb
);
264 skb_pull(skb
, HCI_HEADER_SIZE
);
268 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
269 ret
= gdm_qos_send_hci_pkt(skb
, dev
);
271 ret
= gdm_wimax_send_tx(skb
, dev
);
276 static int gdm_wimax_set_config(struct net_device
*dev
, struct ifmap
*map
)
278 if (dev
->flags
& IFF_UP
)
284 static void __gdm_wimax_set_mac_addr(struct net_device
*dev
, char *mac_addr
)
286 u16 hci_pkt_buf
[32 / sizeof(u16
)];
287 struct hci_s
*hci
= (struct hci_s
*)hci_pkt_buf
;
288 struct nic
*nic
= netdev_priv(dev
);
290 /* Since dev is registered as a ethernet device,
291 * ether_setup has made dev->addr_len to be ETH_ALEN
293 memcpy(dev
->dev_addr
, mac_addr
, dev
->addr_len
);
295 /* Let lower layer know of this change by sending
296 * SetInformation(MAC Address)
298 hci
->cmd_evt
= cpu_to_be16(WIMAX_SET_INFO
);
299 hci
->length
= cpu_to_be16(8);
300 hci
->data
[0] = 0; /* T */
301 hci
->data
[1] = 6; /* L */
302 memcpy(&hci
->data
[2], mac_addr
, dev
->addr_len
); /* V */
304 gdm_wimax_send(nic
, hci
, HCI_HEADER_SIZE
+ 8);
307 /* A driver function */
308 static int gdm_wimax_set_mac_addr(struct net_device
*dev
, void *p
)
310 struct sockaddr
*addr
= p
;
312 if (netif_running(dev
))
315 if (!is_valid_ether_addr(addr
->sa_data
))
316 return -EADDRNOTAVAIL
;
318 __gdm_wimax_set_mac_addr(dev
, addr
->sa_data
);
323 static void gdm_wimax_ind_if_updown(struct net_device
*dev
, int if_up
)
325 u16 buf
[32 / sizeof(u16
)];
326 struct hci_s
*hci
= (struct hci_s
*)buf
;
327 unsigned char up_down
;
329 up_down
= if_up
? WIMAX_IF_UP
: WIMAX_IF_DOWN
;
331 /* Indicate updating fsm */
332 hci
->cmd_evt
= cpu_to_be16(WIMAX_IF_UPDOWN
);
333 hci
->length
= cpu_to_be16(sizeof(up_down
));
334 hci
->data
[0] = up_down
;
336 gdm_wimax_event_send(dev
, (char *)hci
, HCI_HEADER_SIZE
+sizeof(up_down
));
339 static int gdm_wimax_open(struct net_device
*dev
)
341 struct nic
*nic
= netdev_priv(dev
);
342 struct fsm_s
*fsm
= (struct fsm_s
*)nic
->sdk_data
[SIOC_DATA_FSM
].buf
;
344 netif_start_queue(dev
);
346 if (fsm
&& fsm
->m_status
!= M_INIT
)
347 gdm_wimax_ind_if_updown(dev
, 1);
351 static int gdm_wimax_close(struct net_device
*dev
)
353 struct nic
*nic
= netdev_priv(dev
);
354 struct fsm_s
*fsm
= (struct fsm_s
*)nic
->sdk_data
[SIOC_DATA_FSM
].buf
;
356 netif_stop_queue(dev
);
358 if (fsm
&& fsm
->m_status
!= M_INIT
)
359 gdm_wimax_ind_if_updown(dev
, 0);
363 static void kdelete(void **buf
)
371 static int gdm_wimax_ioctl_get_data(struct data_s
*dst
, struct data_s
*src
)
375 size
= dst
->size
< src
->size
? dst
->size
: src
->size
;
381 if (copy_to_user((void __user
*)dst
->buf
, src
->buf
, size
))
387 static int gdm_wimax_ioctl_set_data(struct data_s
*dst
, struct data_s
*src
)
397 if (!(dst
->buf
&& dst
->size
== src
->size
)) {
399 dst
->buf
= kmalloc(src
->size
, GFP_KERNEL
);
400 if (dst
->buf
== NULL
)
404 if (copy_from_user(dst
->buf
, (void __user
*)src
->buf
, src
->size
)) {
408 dst
->size
= src
->size
;
412 static void gdm_wimax_cleanup_ioctl(struct net_device
*dev
)
414 struct nic
*nic
= netdev_priv(dev
);
417 for (i
= 0; i
< SIOC_DATA_MAX
; i
++)
418 kdelete(&nic
->sdk_data
[i
].buf
);
421 static void gdm_wimax_ind_fsm_update(struct net_device
*dev
, struct fsm_s
*fsm
)
423 u16 buf
[32 / sizeof(u16
)];
424 struct hci_s
*hci
= (struct hci_s
*)buf
;
426 /* Indicate updating fsm */
427 hci
->cmd_evt
= cpu_to_be16(WIMAX_FSM_UPDATE
);
428 hci
->length
= cpu_to_be16(sizeof(struct fsm_s
));
429 memcpy(&hci
->data
[0], fsm
, sizeof(struct fsm_s
));
431 gdm_wimax_event_send(dev
, (char *)hci
,
432 HCI_HEADER_SIZE
+ sizeof(struct fsm_s
));
435 static void gdm_update_fsm(struct net_device
*dev
, struct fsm_s
*new_fsm
)
437 struct nic
*nic
= netdev_priv(dev
);
438 struct fsm_s
*cur_fsm
= (struct fsm_s
*)
439 nic
->sdk_data
[SIOC_DATA_FSM
].buf
;
444 if (cur_fsm
->m_status
!= new_fsm
->m_status
||
445 cur_fsm
->c_status
!= new_fsm
->c_status
) {
446 if (new_fsm
->m_status
== M_CONNECTED
) {
447 netif_carrier_on(dev
);
448 } else if (cur_fsm
->m_status
== M_CONNECTED
) {
449 netif_carrier_off(dev
);
450 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
451 gdm_qos_release_list(nic
);
454 gdm_wimax_ind_fsm_update(dev
, new_fsm
);
458 static int gdm_wimax_ioctl(struct net_device
*dev
, struct ifreq
*ifr
, int cmd
)
460 struct wm_req_s
*req
= (struct wm_req_s
*)ifr
;
461 struct nic
*nic
= netdev_priv(dev
);
464 if (cmd
!= SIOCWMIOCTL
)
470 if (req
->data_id
>= SIOC_DATA_MAX
) {
471 netdev_err(dev
, "%s error: data-index(%d) is invalid!!\n",
472 __func__
, req
->data_id
);
475 if (req
->cmd
== SIOCG_DATA
) {
476 ret
= gdm_wimax_ioctl_get_data(
477 &req
->data
, &nic
->sdk_data
[req
->data_id
]);
480 } else if (req
->cmd
== SIOCS_DATA
) {
481 if (req
->data_id
== SIOC_DATA_FSM
) {
482 /* NOTE: gdm_update_fsm should be called
483 * before gdm_wimax_ioctl_set_data is called.
486 (struct fsm_s
*)req
->data
.buf
);
488 ret
= gdm_wimax_ioctl_set_data(
489 &nic
->sdk_data
[req
->data_id
], &req
->data
);
495 netdev_err(dev
, "%s: %x unknown ioctl\n", __func__
, cmd
);
502 static void gdm_wimax_prepare_device(struct net_device
*dev
)
504 struct nic
*nic
= netdev_priv(dev
);
505 u16 buf
[32 / sizeof(u16
)];
506 struct hci_s
*hci
= (struct hci_s
*)buf
;
511 /* GetInformation mac address */
513 hci
->cmd_evt
= cpu_to_be16(WIMAX_GET_INFO
);
514 hci
->data
[len
++] = TLV_T(T_MAC_ADDRESS
);
515 hci
->length
= cpu_to_be16(len
);
516 gdm_wimax_send(nic
, hci
, HCI_HEADER_SIZE
+len
);
518 val
= T_CAPABILITY_WIMAX
| T_CAPABILITY_MULTI_CS
;
519 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
520 val
|= T_CAPABILITY_QOS
;
522 #if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
523 val
|= T_CAPABILITY_AGGREGATION
;
528 hci
->cmd_evt
= cpu_to_be16(WIMAX_SET_INFO
);
529 hci
->data
[len
++] = TLV_T(T_CAPABILITY
);
530 hci
->data
[len
++] = TLV_L(T_CAPABILITY
);
531 val_be32
= cpu_to_be32(val
);
532 memcpy(&hci
->data
[len
], &val_be32
, TLV_L(T_CAPABILITY
));
533 len
+= TLV_L(T_CAPABILITY
);
534 hci
->length
= cpu_to_be16(len
);
535 gdm_wimax_send(nic
, hci
, HCI_HEADER_SIZE
+len
);
537 netdev_info(dev
, "GDM WiMax Set CAPABILITY: 0x%08X\n", val
);
540 static int gdm_wimax_hci_get_tlv(u8
*buf
, u8
*T
, u16
*L
, u8
**V
)
542 #define __U82U16(b) ((u16)((u8 *)(b))[0] | ((u16)((u8 *)(b))[1] << 8))
546 if (buf
[1] == 0x82) {
547 *L
= be16_to_cpu(__U82U16(&buf
[2]));
548 next_pos
= 1/*type*/+3/*len*/;
551 next_pos
= 1/*type*/+1/*len*/;
555 next_pos
+= *L
/*length of val*/;
559 static int gdm_wimax_get_prepared_info(struct net_device
*dev
, char *buf
,
564 u16 cmd_evt
, cmd_len
;
565 int pos
= HCI_HEADER_SIZE
;
567 cmd_evt
= be16_to_cpup((const __be16
*)&buf
[0]);
568 cmd_len
= be16_to_cpup((const __be16
*)&buf
[2]);
570 if (len
< cmd_len
+ HCI_HEADER_SIZE
) {
571 netdev_err(dev
, "%s: invalid length [%d/%d]\n", __func__
,
572 cmd_len
+ HCI_HEADER_SIZE
, len
);
576 if (cmd_evt
== WIMAX_GET_INFO_RESULT
) {
578 netdev_err(dev
, "%s: len is too short [%x/%d]\n",
579 __func__
, cmd_evt
, len
);
583 pos
+= gdm_wimax_hci_get_tlv(&buf
[pos
], &T
, &L
, &V
);
584 if (T
== TLV_T(T_MAC_ADDRESS
)) {
585 if (L
!= dev
->addr_len
) {
587 "%s Invalid inofrmation result T/L [%x/%d]\n",
591 netdev_info(dev
, "MAC change [%pM]->[%pM]\n",
593 memcpy(dev
->dev_addr
, V
, dev
->addr_len
);
598 gdm_wimax_event_send(dev
, buf
, len
);
602 static void gdm_wimax_netif_rx(struct net_device
*dev
, char *buf
, int len
)
607 skb
= dev_alloc_skb(len
+ 2);
609 netdev_err(dev
, "%s: dev_alloc_skb failed!\n", __func__
);
614 dev
->stats
.rx_packets
++;
615 dev
->stats
.rx_bytes
+= len
;
617 memcpy(skb_put(skb
, len
), buf
, len
);
620 skb
->protocol
= eth_type_trans(skb
, dev
); /* what will happen? */
622 ret
= in_interrupt() ? netif_rx(skb
) : netif_rx_ni(skb
);
623 if (ret
== NET_RX_DROP
)
624 netdev_err(dev
, "%s skb dropped\n", __func__
);
627 static void gdm_wimax_transmit_aggr_pkt(struct net_device
*dev
, char *buf
,
630 #define HCI_PADDING_BYTE 4
631 #define HCI_RESERVED_BYTE 4
636 hci
= (struct hci_s
*)buf
;
638 if (hci
->cmd_evt
!= cpu_to_be16(WIMAX_RX_SDU
)) {
639 netdev_err(dev
, "Wrong cmd_evt(0x%04X)\n",
640 be16_to_cpu(hci
->cmd_evt
));
644 length
= be16_to_cpu(hci
->length
);
645 gdm_wimax_netif_rx(dev
, hci
->data
, length
);
648 /* Add padding size */
649 length
+= HCI_PADDING_BYTE
- (length
& 0x3);
652 length
+= HCI_HEADER_SIZE
+ HCI_RESERVED_BYTE
;
658 static void gdm_wimax_transmit_pkt(struct net_device
*dev
, char *buf
, int len
)
660 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
661 struct nic
*nic
= netdev_priv(dev
);
663 u16 cmd_evt
, cmd_len
;
665 /* This code is added for certain rx packet to be ignored. */
669 cmd_evt
= be16_to_cpup((const __be16
*)&buf
[0]);
670 cmd_len
= be16_to_cpup((const __be16
*)&buf
[2]);
672 if (len
< cmd_len
+ HCI_HEADER_SIZE
) {
674 netdev_err(dev
, "%s: invalid length [%d/%d]\n",
675 __func__
, cmd_len
+ HCI_HEADER_SIZE
, len
);
680 case WIMAX_RX_SDU_AGGR
:
681 gdm_wimax_transmit_aggr_pkt(dev
, &buf
[HCI_HEADER_SIZE
],
685 gdm_wimax_netif_rx(dev
, &buf
[HCI_HEADER_SIZE
], cmd_len
);
687 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
688 case WIMAX_EVT_MODEM_REPORT
:
689 gdm_recv_qos_hci_packet(nic
, buf
, len
);
692 case WIMAX_SDU_TX_FLOW
:
694 if (!netif_queue_stopped(dev
))
695 netif_stop_queue(dev
);
696 } else if (buf
[4] == 1) {
697 if (netif_queue_stopped(dev
))
698 netif_wake_queue(dev
);
702 gdm_wimax_event_send(dev
, buf
, len
);
707 static void rx_complete(void *arg
, void *data
, int len
)
709 struct nic
*nic
= arg
;
711 gdm_wimax_transmit_pkt(nic
->netdev
, data
, len
);
712 gdm_wimax_rcv_with_cb(nic
, rx_complete
, nic
);
715 static void prepare_rx_complete(void *arg
, void *data
, int len
)
717 struct nic
*nic
= arg
;
720 ret
= gdm_wimax_get_prepared_info(nic
->netdev
, data
, len
);
722 gdm_wimax_rcv_with_cb(nic
, rx_complete
, nic
);
725 netdev_err(nic
->netdev
,
726 "get_prepared_info failed(%d)\n", ret
);
727 gdm_wimax_rcv_with_cb(nic
, prepare_rx_complete
, nic
);
731 static void start_rx_proc(struct nic
*nic
)
733 gdm_wimax_rcv_with_cb(nic
, prepare_rx_complete
, nic
);
736 static struct net_device_ops gdm_netdev_ops
= {
737 .ndo_open
= gdm_wimax_open
,
738 .ndo_stop
= gdm_wimax_close
,
739 .ndo_set_config
= gdm_wimax_set_config
,
740 .ndo_start_xmit
= gdm_wimax_tx
,
741 .ndo_set_mac_address
= gdm_wimax_set_mac_addr
,
742 .ndo_do_ioctl
= gdm_wimax_ioctl
,
745 int register_wimax_device(struct phy_dev
*phy_dev
, struct device
*pdev
)
747 struct nic
*nic
= NULL
;
748 struct net_device
*dev
;
751 dev
= alloc_netdev(sizeof(*nic
), "wm%d", NET_NAME_UNKNOWN
,
755 pr_err("alloc_etherdev failed\n");
759 SET_NETDEV_DEV(dev
, pdev
);
761 dev
->netdev_ops
= &gdm_netdev_ops
;
762 dev
->flags
&= ~IFF_MULTICAST
;
763 memcpy(dev
->dev_addr
, gdm_wimax_macaddr
, sizeof(gdm_wimax_macaddr
));
765 nic
= netdev_priv(dev
);
767 nic
->phy_dev
= phy_dev
;
768 phy_dev
->netdev
= dev
;
770 /* event socket init */
771 ret
= gdm_wimax_event_init();
773 pr_err("Cannot create event.\n");
777 ret
= register_netdev(dev
);
781 netif_carrier_off(dev
);
783 #ifdef CONFIG_WIMAX_GDM72XX_QOS
789 /* Prepare WiMax device */
790 gdm_wimax_prepare_device(dev
);
795 pr_err("register_netdev failed\n");
800 void unregister_wimax_device(struct phy_dev
*phy_dev
)
802 struct nic
*nic
= netdev_priv(phy_dev
->netdev
);
803 struct fsm_s
*fsm
= (struct fsm_s
*)nic
->sdk_data
[SIOC_DATA_FSM
].buf
;
806 fsm
->m_status
= M_INIT
;
807 unregister_netdev(nic
->netdev
);
809 gdm_wimax_event_exit();
811 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
812 gdm_qos_release_list(nic
);
815 gdm_wimax_cleanup_ioctl(phy_dev
->netdev
);
817 free_netdev(nic
->netdev
);