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)
132 struct evt_entry *tmp;
134 spin_lock_irqsave(&wm_event.evt_lock, flags);
136 list_for_each_entry_safe(e, tmp, &wm_event.evtq, 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 = 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 = 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(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);
404 if (copy_from_user(dst->buf, 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 =
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.
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 (TLV_T(T_MAC_ADDRESS) == T) {
585 if (dev->addr_len != L) {
587 "%s Invalid information 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);
612 dev->stats.rx_packets++;
613 dev->stats.rx_bytes += len;
615 memcpy(skb_put(skb, len), buf, len);
618 skb->protocol = eth_type_trans(skb, dev); /* what will happen? */
620 ret = in_interrupt() ? netif_rx(skb) : netif_rx_ni(skb);
621 if (ret == NET_RX_DROP)
622 netdev_err(dev, "%s skb dropped\n", __func__);
625 static void gdm_wimax_transmit_aggr_pkt(struct net_device *dev, char *buf,
628 #define HCI_PADDING_BYTE 4
629 #define HCI_RESERVED_BYTE 4
634 hci = (struct hci_s *)buf;
636 if (hci->cmd_evt != cpu_to_be16(WIMAX_RX_SDU)) {
637 netdev_err(dev, "Wrong cmd_evt(0x%04X)\n",
638 be16_to_cpu(hci->cmd_evt));
642 length = be16_to_cpu(hci->length);
643 gdm_wimax_netif_rx(dev, hci->data, length);
646 /* Add padding size */
647 length += HCI_PADDING_BYTE - (length & 0x3);
650 length += HCI_HEADER_SIZE + HCI_RESERVED_BYTE;
656 static void gdm_wimax_transmit_pkt(struct net_device *dev, char *buf, int len)
658 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
659 struct nic *nic = netdev_priv(dev);
661 u16 cmd_evt, cmd_len;
663 /* This code is added for certain rx packet to be ignored. */
667 cmd_evt = be16_to_cpup((const __be16 *)&buf[0]);
668 cmd_len = be16_to_cpup((const __be16 *)&buf[2]);
670 if (len < cmd_len + HCI_HEADER_SIZE) {
672 netdev_err(dev, "%s: invalid length [%d/%d]\n",
673 __func__, cmd_len + HCI_HEADER_SIZE, len);
678 case WIMAX_RX_SDU_AGGR:
679 gdm_wimax_transmit_aggr_pkt(dev, &buf[HCI_HEADER_SIZE],
683 gdm_wimax_netif_rx(dev, &buf[HCI_HEADER_SIZE], cmd_len);
685 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
686 case WIMAX_EVT_MODEM_REPORT:
687 gdm_recv_qos_hci_packet(nic, buf, len);
690 case WIMAX_SDU_TX_FLOW:
692 if (!netif_queue_stopped(dev))
693 netif_stop_queue(dev);
694 } else if (buf[4] == 1) {
695 if (netif_queue_stopped(dev))
696 netif_wake_queue(dev);
700 gdm_wimax_event_send(dev, buf, len);
705 static void rx_complete(void *arg, void *data, int len)
707 struct nic *nic = arg;
709 gdm_wimax_transmit_pkt(nic->netdev, data, len);
710 gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
713 static void prepare_rx_complete(void *arg, void *data, int len)
715 struct nic *nic = arg;
718 ret = gdm_wimax_get_prepared_info(nic->netdev, data, len);
720 gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
723 netdev_err(nic->netdev,
724 "get_prepared_info failed(%d)\n", ret);
725 gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
729 static void start_rx_proc(struct nic *nic)
731 gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
734 static struct net_device_ops gdm_netdev_ops = {
735 .ndo_open = gdm_wimax_open,
736 .ndo_stop = gdm_wimax_close,
737 .ndo_set_config = gdm_wimax_set_config,
738 .ndo_start_xmit = gdm_wimax_tx,
739 .ndo_set_mac_address = gdm_wimax_set_mac_addr,
740 .ndo_do_ioctl = gdm_wimax_ioctl,
743 int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
745 struct nic *nic = NULL;
746 struct net_device *dev;
749 dev = alloc_netdev(sizeof(*nic), "wm%d", NET_NAME_UNKNOWN,
753 pr_err("alloc_etherdev failed\n");
757 SET_NETDEV_DEV(dev, pdev);
759 dev->netdev_ops = &gdm_netdev_ops;
760 dev->flags &= ~IFF_MULTICAST;
761 memcpy(dev->dev_addr, gdm_wimax_macaddr, sizeof(gdm_wimax_macaddr));
763 nic = netdev_priv(dev);
765 nic->phy_dev = phy_dev;
766 phy_dev->netdev = dev;
768 /* event socket init */
769 ret = gdm_wimax_event_init();
771 pr_err("Cannot create event.\n");
775 ret = register_netdev(dev);
779 netif_carrier_off(dev);
781 #ifdef CONFIG_WIMAX_GDM72XX_QOS
787 /* Prepare WiMax device */
788 gdm_wimax_prepare_device(dev);
793 pr_err("register_netdev failed\n");
798 void unregister_wimax_device(struct phy_dev *phy_dev)
800 struct nic *nic = netdev_priv(phy_dev->netdev);
801 struct fsm_s *fsm = nic->sdk_data[SIOC_DATA_FSM].buf;
804 fsm->m_status = M_INIT;
805 unregister_netdev(nic->netdev);
807 gdm_wimax_event_exit();
809 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
810 gdm_qos_release_list(nic);
813 gdm_wimax_cleanup_ioctl(phy_dev->netdev);
815 free_netdev(nic->netdev);