| /* |
| * Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. |
| * |
| * This software is licensed under the terms of the GNU General Public |
| * License version 2, as published by the Free Software Foundation, and |
| * may be copied, distributed, and modified under those terms. |
| * |
| * This program is distributed in the hope that it will be useful, |
| * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| * GNU General Public License for more details. |
| */ |
| |
| #include <linux/etherdevice.h> |
| #include <asm/byteorder.h> |
| #include <linux/ip.h> |
| #include <linux/ipv6.h> |
| #include <linux/udp.h> |
| #include <linux/in.h> |
| |
| #include "gdm_wimax.h" |
| #include "hci.h" |
| #include "wm_ioctl.h" |
| #include "netlink_k.h" |
| |
| #define gdm_wimax_send(n, d, l) \ |
| (n->phy_dev->send_func)(n->phy_dev->priv_dev, d, l, NULL, NULL) |
| #define gdm_wimax_send_with_cb(n, d, l, c, b) \ |
| (n->phy_dev->send_func)(n->phy_dev->priv_dev, d, l, c, b) |
| #define gdm_wimax_rcv_with_cb(n, c, b) \ |
| (n->phy_dev->rcv_func)(n->phy_dev->priv_dev, c, b) |
| |
| #define EVT_MAX_SIZE 2048 |
| |
| struct evt_entry { |
| struct list_head list; |
| struct net_device *dev; |
| char evt_data[EVT_MAX_SIZE]; |
| int size; |
| }; |
| |
| static void __gdm_wimax_event_send(struct work_struct *work); |
| static inline struct evt_entry *alloc_event_entry(void); |
| static inline void free_event_entry(struct evt_entry *e); |
| static struct evt_entry *get_event_entry(void); |
| static void put_event_entry(struct evt_entry *e); |
| |
| static struct { |
| int ref_cnt; |
| struct sock *sock; |
| struct list_head evtq; |
| spinlock_t evt_lock; |
| |
| struct list_head freeq; |
| struct work_struct ws; |
| } wm_event; |
| |
| static u8 gdm_wimax_macaddr[6] = {0x00, 0x0a, 0x3b, 0xf0, 0x01, 0x30}; |
| |
| static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm); |
| static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up); |
| |
| #if defined(DEBUG_SDU) |
| static void printk_hex(u8 *buf, u32 size) |
| { |
| int i; |
| |
| for (i = 0; i < size; i++) { |
| if (i && i % 16 == 0) |
| printk(KERN_DEBUG "\n%02x ", *buf++); |
| else |
| printk(KERN_DEBUG "%02x ", *buf++); |
| } |
| |
| printk(KERN_DEBUG "\n"); |
| } |
| |
| static const char *get_protocol_name(u16 protocol) |
| { |
| static char buf[32]; |
| const char *name = "-"; |
| |
| switch (protocol) { |
| case ETH_P_ARP: |
| name = "ARP"; |
| break; |
| case ETH_P_IP: |
| name = "IP"; |
| break; |
| case ETH_P_IPV6: |
| name = "IPv6"; |
| break; |
| } |
| |
| sprintf(buf, "0x%04x(%s)", protocol, name); |
| return buf; |
| } |
| |
| static const char *get_ip_protocol_name(u8 ip_protocol) |
| { |
| static char buf[32]; |
| const char *name = "-"; |
| |
| switch (ip_protocol) { |
| case IPPROTO_TCP: |
| name = "TCP"; |
| break; |
| case IPPROTO_UDP: |
| name = "UDP"; |
| break; |
| case IPPROTO_ICMP: |
| name = "ICMP"; |
| break; |
| } |
| |
| sprintf(buf, "%u(%s)", ip_protocol, name); |
| return buf; |
| } |
| |
| static const char *get_port_name(u16 port) |
| { |
| static char buf[32]; |
| const char *name = "-"; |
| |
| switch (port) { |
| case 67: |
| name = "DHCP-Server"; |
| break; |
| case 68: |
| name = "DHCP-Client"; |
| break; |
| case 69: |
| name = "TFTP"; |
| break; |
| } |
| |
| sprintf(buf, "%u(%s)", port, name); |
| return buf; |
| } |
| |
| static void dump_eth_packet(const char *title, u8 *data, int len) |
| { |
| struct iphdr *ih = NULL; |
| struct udphdr *uh = NULL; |
| u16 protocol = 0; |
| u8 ip_protocol = 0; |
| u16 port = 0; |
| |
| protocol = (data[12]<<8) | data[13]; |
| ih = (struct iphdr *) (data+ETH_HLEN); |
| |
| if (protocol == ETH_P_IP) { |
| uh = (struct udphdr *) ((char *)ih + sizeof(struct iphdr)); |
| ip_protocol = ih->protocol; |
| port = ntohs(uh->dest); |
| } else if (protocol == ETH_P_IPV6) { |
| struct ipv6hdr *i6h = (struct ipv6hdr *) data; |
| uh = (struct udphdr *) ((char *)i6h + sizeof(struct ipv6hdr)); |
| ip_protocol = i6h->nexthdr; |
| port = ntohs(uh->dest); |
| } |
| |
| printk(KERN_DEBUG "[%s] len=%d, %s, %s, %s\n", |
| title, len, |
| get_protocol_name(protocol), |
| get_ip_protocol_name(ip_protocol), |
| get_port_name(port)); |
| |
| #if 1 |
| if (!(data[0] == 0xff && data[1] == 0xff)) { |
| if (protocol == ETH_P_IP) { |
| printk(KERN_DEBUG " src=%u.%u.%u.%u\n", |
| NIPQUAD(ih->saddr)); |
| } else if (protocol == ETH_P_IPV6) { |
| #ifdef NIP6 |
| printk(KERN_DEBUG " src=%x:%x:%x:%x:%x:%x:%x:%x\n", |
| NIP6(ih->saddr)); |
| #else |
| printk(KERN_DEBUG " src=%pI6\n", &ih->saddr); |
| #endif |
| } |
| } |
| #endif |
| |
| #if (DUMP_PACKET & DUMP_SDU_ALL) |
| printk_hex(data, len); |
| #else |
| #if (DUMP_PACKET & DUMP_SDU_ARP) |
| if (protocol == ETH_P_ARP) |
| printk_hex(data, len); |
| #endif |
| #if (DUMP_PACKET & DUMP_SDU_IP) |
| if (protocol == ETH_P_IP || protocol == ETH_P_IPV6) |
| printk_hex(data, len); |
| #else |
| #if (DUMP_PACKET & DUMP_SDU_IP_TCP) |
| if (ip_protocol == IPPROTO_TCP) |
| printk_hex(data, len); |
| #endif |
| #if (DUMP_PACKET & DUMP_SDU_IP_UDP) |
| if (ip_protocol == IPPROTO_UDP) |
| printk_hex(data, len); |
| #endif |
| #if (DUMP_PACKET & DUMP_SDU_IP_ICMP) |
| if (ip_protocol == IPPROTO_ICMP) |
| printk_hex(data, len); |
| #endif |
| #endif |
| #endif |
| } |
| #endif |
| |
| |
| static inline int gdm_wimax_header(struct sk_buff **pskb) |
| { |
| u16 buf[HCI_HEADER_SIZE / sizeof(u16)]; |
| struct sk_buff *skb = *pskb; |
| int ret = 0; |
| |
| if (unlikely(skb_headroom(skb) < HCI_HEADER_SIZE)) { |
| struct sk_buff *skb2; |
| |
| skb2 = skb_realloc_headroom(skb, HCI_HEADER_SIZE); |
| if (skb2 == NULL) |
| return -ENOMEM; |
| if (skb->sk) |
| skb_set_owner_w(skb2, skb->sk); |
| kfree_skb(skb); |
| skb = skb2; |
| } |
| |
| skb_push(skb, HCI_HEADER_SIZE); |
| buf[0] = H2B(WIMAX_TX_SDU); |
| buf[1] = H2B(skb->len - HCI_HEADER_SIZE); |
| memcpy(skb->data, buf, HCI_HEADER_SIZE); |
| |
| *pskb = skb; |
| return ret; |
| } |
| |
| static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg, |
| int len) |
| { |
| struct nic *nic = netdev_priv(dev); |
| |
| #if defined(DEBUG_HCI) |
| u8 *buf = (u8 *) msg; |
| u16 hci_cmd = (buf[0]<<8) | buf[1]; |
| u16 hci_len = (buf[2]<<8) | buf[3]; |
| printk(KERN_DEBUG "H=>D: 0x%04x(%d)\n", hci_cmd, hci_len); |
| #endif |
| |
| gdm_wimax_send(nic, msg, len); |
| } |
| |
| static int gdm_wimax_event_init(void) |
| { |
| if (!wm_event.ref_cnt) { |
| wm_event.sock = netlink_init(NETLINK_WIMAX, |
| gdm_wimax_event_rcv); |
| if (wm_event.sock) |
| wm_event.ref_cnt++; |
| INIT_LIST_HEAD(&wm_event.evtq); |
| INIT_LIST_HEAD(&wm_event.freeq); |
| INIT_WORK(&wm_event.ws, __gdm_wimax_event_send); |
| spin_lock_init(&wm_event.evt_lock); |
| return 0; |
| } |
| |
| printk(KERN_ERR "Creating WiMax Event netlink is failed\n"); |
| return -1; |
| } |
| |
| static void gdm_wimax_event_exit(void) |
| { |
| if (wm_event.sock && --wm_event.ref_cnt == 0) { |
| struct evt_entry *e, *temp; |
| unsigned long flags; |
| |
| spin_lock_irqsave(&wm_event.evt_lock, flags); |
| |
| list_for_each_entry_safe(e, temp, &wm_event.evtq, list) { |
| list_del(&e->list); |
| free_event_entry(e); |
| } |
| list_for_each_entry_safe(e, temp, &wm_event.freeq, list) { |
| list_del(&e->list); |
| free_event_entry(e); |
| } |
| |
| spin_unlock_irqrestore(&wm_event.evt_lock, flags); |
| netlink_exit(wm_event.sock); |
| wm_event.sock = NULL; |
| } |
| } |
| |
| static inline struct evt_entry *alloc_event_entry(void) |
| { |
| return kmalloc(sizeof(struct evt_entry), GFP_ATOMIC); |
| } |
| |
| static inline void free_event_entry(struct evt_entry *e) |
| { |
| kfree(e); |
| } |
| |
| static struct evt_entry *get_event_entry(void) |
| { |
| struct evt_entry *e; |
| |
| if (list_empty(&wm_event.freeq)) |
| e = alloc_event_entry(); |
| else { |
| e = list_entry(wm_event.freeq.next, struct evt_entry, list); |
| list_del(&e->list); |
| } |
| |
| return e; |
| } |
| |
| static void put_event_entry(struct evt_entry *e) |
| { |
| BUG_ON(!e); |
| |
| list_add_tail(&e->list, &wm_event.freeq); |
| } |
| |
| static void __gdm_wimax_event_send(struct work_struct *work) |
| { |
| int idx; |
| unsigned long flags; |
| struct evt_entry *e; |
| |
| spin_lock_irqsave(&wm_event.evt_lock, flags); |
| |
| while (!list_empty(&wm_event.evtq)) { |
| e = list_entry(wm_event.evtq.next, struct evt_entry, list); |
| spin_unlock_irqrestore(&wm_event.evt_lock, flags); |
| |
| sscanf(e->dev->name, "wm%d", &idx); |
| netlink_send(wm_event.sock, idx, 0, e->evt_data, e->size); |
| |
| spin_lock_irqsave(&wm_event.evt_lock, flags); |
| list_del(&e->list); |
| put_event_entry(e); |
| } |
| |
| spin_unlock_irqrestore(&wm_event.evt_lock, flags); |
| } |
| |
| static int gdm_wimax_event_send(struct net_device *dev, char *buf, int size) |
| { |
| struct evt_entry *e; |
| unsigned long flags; |
| |
| #if defined(DEBUG_HCI) |
| u16 hci_cmd = ((u8)buf[0]<<8) | (u8)buf[1]; |
| u16 hci_len = ((u8)buf[2]<<8) | (u8)buf[3]; |
| printk(KERN_DEBUG "D=>H: 0x%04x(%d)\n", hci_cmd, hci_len); |
| #endif |
| |
| spin_lock_irqsave(&wm_event.evt_lock, flags); |
| |
| e = get_event_entry(); |
| if (!e) { |
| printk(KERN_ERR "%s: No memory for event\n", __func__); |
| spin_unlock_irqrestore(&wm_event.evt_lock, flags); |
| return -ENOMEM; |
| } |
| |
| e->dev = dev; |
| e->size = size; |
| memcpy(e->evt_data, buf, size); |
| |
| list_add_tail(&e->list, &wm_event.evtq); |
| spin_unlock_irqrestore(&wm_event.evt_lock, flags); |
| |
| schedule_work(&wm_event.ws); |
| |
| return 0; |
| } |
| |
| static void tx_complete(void *arg) |
| { |
| struct nic *nic = arg; |
| |
| if (netif_queue_stopped(nic->netdev)) |
| netif_wake_queue(nic->netdev); |
| } |
| |
| int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev) |
| { |
| int ret = 0; |
| struct nic *nic = netdev_priv(dev); |
| |
| ret = gdm_wimax_send_with_cb(nic, skb->data, skb->len, tx_complete, |
| nic); |
| if (ret == -ENOSPC) { |
| netif_stop_queue(dev); |
| ret = 0; |
| } |
| |
| if (ret) { |
| skb_pull(skb, HCI_HEADER_SIZE); |
| return ret; |
| } |
| |
| nic->stats.tx_packets++; |
| nic->stats.tx_bytes += skb->len - HCI_HEADER_SIZE; |
| kfree_skb(skb); |
| return ret; |
| } |
| |
| static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev) |
| { |
| int ret = 0; |
| struct nic *nic = netdev_priv(dev); |
| struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf; |
| |
| #if defined(DEBUG_SDU) |
| dump_eth_packet("TX", skb->data, skb->len); |
| #endif |
| |
| ret = gdm_wimax_header(&skb); |
| if (ret < 0) { |
| skb_pull(skb, HCI_HEADER_SIZE); |
| return ret; |
| } |
| |
| #if !defined(LOOPBACK_TEST) |
| if (!fsm) |
| printk(KERN_ERR "ASSERTION ERROR: fsm is NULL!!\n"); |
| else if (fsm->m_status != M_CONNECTED) { |
| printk(KERN_EMERG "ASSERTION ERROR: Device is NOT ready. status=%d\n", |
| fsm->m_status); |
| kfree_skb(skb); |
| return 0; |
| } |
| #endif |
| |
| #if defined(CONFIG_WIMAX_GDM72XX_QOS) |
| ret = gdm_qos_send_hci_pkt(skb, dev); |
| #else |
| ret = gdm_wimax_send_tx(skb, dev); |
| #endif |
| return ret; |
| } |
| |
| static int gdm_wimax_set_config(struct net_device *dev, struct ifmap *map) |
| { |
| if (dev->flags & IFF_UP) |
| return -EBUSY; |
| |
| return 0; |
| } |
| |
| static void __gdm_wimax_set_mac_addr(struct net_device *dev, char *mac_addr) |
| { |
| u16 hci_pkt_buf[32 / sizeof(u16)]; |
| u8 *pkt = (u8 *) &hci_pkt_buf[0]; |
| struct nic *nic = netdev_priv(dev); |
| |
| /* Since dev is registered as a ethernet device, |
| * ether_setup has made dev->addr_len to be ETH_ALEN |
| */ |
| memcpy(dev->dev_addr, mac_addr, dev->addr_len); |
| |
| /* Let lower layer know of this change by sending |
| * SetInformation(MAC Address) |
| */ |
| hci_pkt_buf[0] = H2B(WIMAX_SET_INFO); /* cmd_evt */ |
| hci_pkt_buf[1] = H2B(8); /* size */ |
| pkt[4] = 0; /* T */ |
| pkt[5] = 6; /* L */ |
| memcpy(pkt + 6, mac_addr, dev->addr_len); /* V */ |
| |
| gdm_wimax_send(nic, pkt, HCI_HEADER_SIZE + 8); |
| } |
| |
| /* A driver function */ |
| static int gdm_wimax_set_mac_addr(struct net_device *dev, void *p) |
| { |
| struct sockaddr *addr = p; |
| |
| if (netif_running(dev)) |
| return -EBUSY; |
| |
| if (!is_valid_ether_addr(addr->sa_data)) |
| return -EADDRNOTAVAIL; |
| |
| __gdm_wimax_set_mac_addr(dev, addr->sa_data); |
| |
| return 0; |
| } |
| |
| static struct net_device_stats *gdm_wimax_stats(struct net_device *dev) |
| { |
| struct nic *nic = netdev_priv(dev); |
| |
| return &nic->stats; |
| } |
| |
| static int gdm_wimax_open(struct net_device *dev) |
| { |
| struct nic *nic = netdev_priv(dev); |
| struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf; |
| |
| netif_start_queue(dev); |
| |
| if (fsm && fsm->m_status != M_INIT) |
| gdm_wimax_ind_if_updown(dev, 1); |
| return 0; |
| } |
| |
| static int gdm_wimax_close(struct net_device *dev) |
| { |
| struct nic *nic = netdev_priv(dev); |
| struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf; |
| |
| netif_stop_queue(dev); |
| |
| if (fsm && fsm->m_status != M_INIT) |
| gdm_wimax_ind_if_updown(dev, 0); |
| return 0; |
| } |
| |
| static void kdelete(void **buf) |
| { |
| if (buf && *buf) { |
| kfree(*buf); |
| *buf = NULL; |
| } |
| } |
| |
| static int gdm_wimax_ioctl_get_data(struct data_s *dst, struct data_s *src) |
| { |
| int size; |
| |
| size = dst->size < src->size ? dst->size : src->size; |
| |
| dst->size = size; |
| if (src->size) { |
| if (!dst->buf) |
| return -EINVAL; |
| if (copy_to_user(dst->buf, src->buf, size)) |
| return -EFAULT; |
| } |
| return 0; |
| } |
| |
| static int gdm_wimax_ioctl_set_data(struct data_s *dst, struct data_s *src) |
| { |
| if (!src->size) { |
| dst->size = 0; |
| return 0; |
| } |
| |
| if (!src->buf) |
| return -EINVAL; |
| |
| if (!(dst->buf && dst->size == src->size)) { |
| kdelete(&dst->buf); |
| dst->buf = kmalloc(src->size, GFP_KERNEL); |
| if (dst->buf == NULL) |
| return -ENOMEM; |
| } |
| |
| if (copy_from_user(dst->buf, src->buf, src->size)) { |
| kdelete(&dst->buf); |
| return -EFAULT; |
| } |
| dst->size = src->size; |
| return 0; |
| } |
| |
| static void gdm_wimax_cleanup_ioctl(struct net_device *dev) |
| { |
| struct nic *nic = netdev_priv(dev); |
| int i; |
| |
| for (i = 0; i < SIOC_DATA_MAX; i++) |
| kdelete(&nic->sdk_data[i].buf); |
| } |
| |
| static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm) |
| { |
| struct nic *nic = netdev_priv(dev); |
| struct fsm_s *cur_fsm = |
| (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf; |
| |
| if (!cur_fsm) |
| return; |
| |
| if (cur_fsm->m_status != new_fsm->m_status || |
| cur_fsm->c_status != new_fsm->c_status) { |
| if (new_fsm->m_status == M_CONNECTED) |
| netif_carrier_on(dev); |
| else if (cur_fsm->m_status == M_CONNECTED) { |
| netif_carrier_off(dev); |
| #if defined(CONFIG_WIMAX_GDM72XX_QOS) |
| gdm_qos_release_list(nic); |
| #endif |
| } |
| gdm_wimax_ind_fsm_update(dev, new_fsm); |
| } |
| } |
| |
| static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd) |
| { |
| struct wm_req_s *req = (struct wm_req_s *) ifr; |
| struct nic *nic = netdev_priv(dev); |
| int ret; |
| |
| if (cmd != SIOCWMIOCTL) |
| return -EOPNOTSUPP; |
| |
| switch (req->cmd) { |
| case SIOCG_DATA: |
| case SIOCS_DATA: |
| if (req->data_id >= SIOC_DATA_MAX) { |
| printk(KERN_ERR |
| "%s error: data-index(%d) is invalid!!\n", |
| __func__, req->data_id); |
| return -EOPNOTSUPP; |
| } |
| if (req->cmd == SIOCG_DATA) { |
| ret = gdm_wimax_ioctl_get_data(&req->data, |
| &nic->sdk_data[req->data_id]); |
| if (ret < 0) |
| return ret; |
| } else if (req->cmd == SIOCS_DATA) { |
| if (req->data_id == SIOC_DATA_FSM) { |
| /*NOTE: gdm_update_fsm should be called |
| before gdm_wimax_ioctl_set_data is called*/ |
| gdm_update_fsm(dev, |
| (struct fsm_s *) req->data.buf); |
| } |
| ret = gdm_wimax_ioctl_set_data( |
| &nic->sdk_data[req->data_id], &req->data); |
| if (ret < 0) |
| return ret; |
| } |
| break; |
| default: |
| printk(KERN_ERR "%s: %x unknown ioctl\n", __func__, cmd); |
| return -EOPNOTSUPP; |
| } |
| |
| return 0; |
| } |
| |
| static void gdm_wimax_prepare_device(struct net_device *dev) |
| { |
| struct nic *nic = netdev_priv(dev); |
| u16 buf[32 / sizeof(u16)]; |
| struct hci_s *hci = (struct hci_s *) buf; |
| u16 len = 0; |
| u32 val = 0; |
| |
| #define BIT_MULTI_CS 0 |
| #define BIT_WIMAX 1 |
| #define BIT_QOS 2 |
| #define BIT_AGGREGATION 3 |
| |
| /* GetInformation mac address */ |
| len = 0; |
| hci->cmd_evt = H2B(WIMAX_GET_INFO); |
| hci->data[len++] = TLV_T(T_MAC_ADDRESS); |
| hci->length = H2B(len); |
| gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len); |
| |
| val = (1<<BIT_WIMAX) | (1<<BIT_MULTI_CS); |
| #if defined(CONFIG_WIMAX_GDM72XX_QOS) |
| val |= (1<<BIT_QOS); |
| #endif |
| #if defined(CONFIG_WIMAX_GDM72XX_WIMAX2) |
| val |= (1<<BIT_AGGREGATION); |
| #endif |
| |
| /* Set capability */ |
| len = 0; |
| hci->cmd_evt = H2B(WIMAX_SET_INFO); |
| hci->data[len++] = TLV_T(T_CAPABILITY); |
| hci->data[len++] = TLV_L(T_CAPABILITY); |
| val = DH2B(val); |
| memcpy(&hci->data[len], &val, TLV_L(T_CAPABILITY)); |
| len += TLV_L(T_CAPABILITY); |
| hci->length = H2B(len); |
| gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len); |
| |
| printk(KERN_INFO "GDM WiMax Set CAPABILITY: 0x%08X\n", DB2H(val)); |
| } |
| |
| static int gdm_wimax_hci_get_tlv(u8 *buf, u8 *T, u16 *L, u8 **V) |
| { |
| #define __U82U16(b) ((u16)((u8 *)(b))[0] | ((u16)((u8 *)(b))[1] << 8)) |
| int next_pos; |
| |
| *T = buf[0]; |
| if (buf[1] == 0x82) { |
| *L = B2H(__U82U16(&buf[2])); |
| next_pos = 1/*type*/+3/*len*/; |
| } else { |
| *L = buf[1]; |
| next_pos = 1/*type*/+1/*len*/; |
| } |
| *V = &buf[next_pos]; |
| |
| next_pos += *L/*length of val*/; |
| return next_pos; |
| } |
| |
| static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf, |
| int len) |
| { |
| u8 T, *V; |
| u16 L; |
| u16 cmd_evt, cmd_len; |
| int pos = HCI_HEADER_SIZE; |
| |
| cmd_evt = B2H(*(u16 *)&buf[0]); |
| cmd_len = B2H(*(u16 *)&buf[2]); |
| |
| if (len < cmd_len + HCI_HEADER_SIZE) { |
| printk(KERN_ERR "%s: invalid length [%d/%d]\n", __func__, |
| cmd_len + HCI_HEADER_SIZE, len); |
| return -1; |
| } |
| |
| if (cmd_evt == WIMAX_GET_INFO_RESULT) { |
| if (cmd_len < 2) { |
| printk(KERN_ERR "%s: len is too short [%x/%d]\n", |
| __func__, cmd_evt, len); |
| return -1; |
| } |
| |
| pos += gdm_wimax_hci_get_tlv(&buf[pos], &T, &L, &V); |
| if (T == TLV_T(T_MAC_ADDRESS)) { |
| if (L != dev->addr_len) { |
| printk(KERN_ERR |
| "%s Invalid inofrmation result T/L " |
| "[%x/%d]\n", __func__, T, L); |
| return -1; |
| } |
| printk(KERN_INFO "MAC change [%pM]->[%pM]\n", |
| dev->dev_addr, V); |
| memcpy(dev->dev_addr, V, dev->addr_len); |
| return 1; |
| } |
| } |
| |
| gdm_wimax_event_send(dev, buf, len); |
| return 0; |
| } |
| |
| static void gdm_wimax_netif_rx(struct net_device *dev, char *buf, int len) |
| { |
| struct nic *nic = netdev_priv(dev); |
| struct sk_buff *skb; |
| int ret; |
| |
| #if defined(DEBUG_SDU) |
| dump_eth_packet("RX", buf, len); |
| #endif |
| |
| skb = dev_alloc_skb(len + 2); |
| if (!skb) { |
| printk(KERN_ERR "%s: dev_alloc_skb failed!\n", __func__); |
| return; |
| } |
| skb_reserve(skb, 2); |
| |
| nic->stats.rx_packets++; |
| nic->stats.rx_bytes += len; |
| |
| memcpy(skb_put(skb, len), buf, len); |
| |
| skb->dev = dev; |
| skb->protocol = eth_type_trans(skb, dev); /* what will happen? */ |
| |
| ret = in_interrupt() ? netif_rx(skb) : netif_rx_ni(skb); |
| if (ret == NET_RX_DROP) |
| printk(KERN_ERR "%s skb dropped\n", __func__); |
| } |
| |
| static void gdm_wimax_transmit_aggr_pkt(struct net_device *dev, char *buf, |
| int len) |
| { |
| #define HCI_PADDING_BYTE 4 |
| #define HCI_RESERVED_BYTE 4 |
| struct hci_s *hci; |
| int length; |
| |
| while (len > 0) { |
| hci = (struct hci_s *) buf; |
| |
| if (B2H(hci->cmd_evt) != WIMAX_RX_SDU) { |
| printk(KERN_ERR "Wrong cmd_evt(0x%04X)\n", |
| B2H(hci->cmd_evt)); |
| break; |
| } |
| |
| length = B2H(hci->length); |
| gdm_wimax_netif_rx(dev, hci->data, length); |
| |
| if (length & 0x3) { |
| /* Add padding size */ |
| length += HCI_PADDING_BYTE - (length & 0x3); |
| } |
| |
| length += HCI_HEADER_SIZE + HCI_RESERVED_BYTE; |
| len -= length; |
| buf += length; |
| } |
| } |
| |
| static void gdm_wimax_transmit_pkt(struct net_device *dev, char *buf, int len) |
| { |
| #if defined(CONFIG_WIMAX_GDM72XX_QOS) |
| struct nic *nic = netdev_priv(dev); |
| #endif |
| u16 cmd_evt, cmd_len; |
| |
| /* This code is added for certain rx packet to be ignored. */ |
| if (len == 0) |
| return; |
| |
| cmd_evt = B2H(*(u16 *)&buf[0]); |
| cmd_len = B2H(*(u16 *)&buf[2]); |
| |
| if (len < cmd_len + HCI_HEADER_SIZE) { |
| if (len) |
| printk(KERN_ERR "%s: invalid length [%d/%d]\n", |
| __func__, cmd_len + HCI_HEADER_SIZE, len); |
| return; |
| } |
| |
| switch (cmd_evt) { |
| case WIMAX_RX_SDU_AGGR: |
| gdm_wimax_transmit_aggr_pkt(dev, &buf[HCI_HEADER_SIZE], |
| cmd_len); |
| break; |
| case WIMAX_RX_SDU: |
| gdm_wimax_netif_rx(dev, &buf[HCI_HEADER_SIZE], cmd_len); |
| break; |
| #if defined(CONFIG_WIMAX_GDM72XX_QOS) |
| case WIMAX_EVT_MODEM_REPORT: |
| gdm_recv_qos_hci_packet(nic, buf, len); |
| break; |
| #endif |
| case WIMAX_SDU_TX_FLOW: |
| if (buf[4] == 0) { |
| if (!netif_queue_stopped(dev)) |
| netif_stop_queue(dev); |
| } else if (buf[4] == 1) { |
| if (netif_queue_stopped(dev)) |
| netif_wake_queue(dev); |
| } |
| break; |
| default: |
| gdm_wimax_event_send(dev, buf, len); |
| break; |
| } |
| } |
| |
| static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm) |
| { |
| u16 buf[32 / sizeof(u16)]; |
| u8 *hci_pkt_buf = (u8 *)&buf[0]; |
| |
| /* Indicate updating fsm */ |
| buf[0] = H2B(WIMAX_FSM_UPDATE); |
| buf[1] = H2B(sizeof(struct fsm_s)); |
| memcpy(&hci_pkt_buf[HCI_HEADER_SIZE], fsm, sizeof(struct fsm_s)); |
| |
| gdm_wimax_event_send(dev, hci_pkt_buf, |
| HCI_HEADER_SIZE + sizeof(struct fsm_s)); |
| } |
| |
| static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up) |
| { |
| u16 buf[32 / sizeof(u16)]; |
| struct hci_s *hci = (struct hci_s *) buf; |
| unsigned char up_down; |
| |
| up_down = if_up ? WIMAX_IF_UP : WIMAX_IF_DOWN; |
| |
| /* Indicate updating fsm */ |
| hci->cmd_evt = H2B(WIMAX_IF_UPDOWN); |
| hci->length = H2B(sizeof(up_down)); |
| hci->data[0] = up_down; |
| |
| gdm_wimax_event_send(dev, (char *)hci, HCI_HEADER_SIZE+sizeof(up_down)); |
| } |
| |
| static void rx_complete(void *arg, void *data, int len) |
| { |
| struct nic *nic = arg; |
| |
| gdm_wimax_transmit_pkt(nic->netdev, data, len); |
| gdm_wimax_rcv_with_cb(nic, rx_complete, nic); |
| } |
| |
| static void prepare_rx_complete(void *arg, void *data, int len) |
| { |
| struct nic *nic = arg; |
| int ret; |
| |
| ret = gdm_wimax_get_prepared_info(nic->netdev, data, len); |
| if (ret == 1) |
| gdm_wimax_rcv_with_cb(nic, rx_complete, nic); |
| else { |
| if (ret < 0) |
| printk(KERN_ERR "get_prepared_info failed(%d)\n", ret); |
| gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic); |
| #if 0 |
| /* Re-prepare WiMax device */ |
| gdm_wimax_prepare_device(nic->netdev); |
| #endif |
| } |
| } |
| |
| static void start_rx_proc(struct nic *nic) |
| { |
| gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic); |
| } |
| |
| static struct net_device_ops gdm_netdev_ops = { |
| .ndo_open = gdm_wimax_open, |
| .ndo_stop = gdm_wimax_close, |
| .ndo_set_config = gdm_wimax_set_config, |
| .ndo_start_xmit = gdm_wimax_tx, |
| .ndo_get_stats = gdm_wimax_stats, |
| .ndo_set_mac_address = gdm_wimax_set_mac_addr, |
| .ndo_do_ioctl = gdm_wimax_ioctl, |
| }; |
| |
| int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev) |
| { |
| struct nic *nic = NULL; |
| struct net_device *dev; |
| int ret; |
| |
| dev = (struct net_device *)alloc_netdev(sizeof(*nic), |
| "wm%d", ether_setup); |
| |
| if (dev == NULL) { |
| printk(KERN_ERR "alloc_etherdev failed\n"); |
| return -ENOMEM; |
| } |
| |
| SET_NETDEV_DEV(dev, pdev); |
| dev->mtu = 1400; |
| dev->netdev_ops = &gdm_netdev_ops; |
| dev->flags &= ~IFF_MULTICAST; |
| memcpy(dev->dev_addr, gdm_wimax_macaddr, sizeof(gdm_wimax_macaddr)); |
| |
| nic = netdev_priv(dev); |
| memset(nic, 0, sizeof(*nic)); |
| |
| nic->netdev = dev; |
| nic->phy_dev = phy_dev; |
| phy_dev->netdev = dev; |
| |
| /* event socket init */ |
| ret = gdm_wimax_event_init(); |
| if (ret < 0) { |
| printk(KERN_ERR "Cannot create event.\n"); |
| goto cleanup; |
| } |
| |
| ret = register_netdev(dev); |
| if (ret) |
| goto cleanup; |
| |
| #if defined(LOOPBACK_TEST) |
| netif_start_queue(dev); |
| netif_carrier_on(dev); |
| #else |
| netif_carrier_off(dev); |
| #endif |
| |
| #ifdef CONFIG_WIMAX_GDM72XX_QOS |
| gdm_qos_init(nic); |
| #endif |
| |
| start_rx_proc(nic); |
| |
| /* Prepare WiMax device */ |
| gdm_wimax_prepare_device(dev); |
| |
| return 0; |
| |
| cleanup: |
| printk(KERN_ERR "register_netdev failed\n"); |
| free_netdev(dev); |
| return ret; |
| } |
| |
| void unregister_wimax_device(struct phy_dev *phy_dev) |
| { |
| struct nic *nic = netdev_priv(phy_dev->netdev); |
| struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf; |
| |
| if (fsm) |
| fsm->m_status = M_INIT; |
| unregister_netdev(nic->netdev); |
| |
| gdm_wimax_event_exit(); |
| |
| #if defined(CONFIG_WIMAX_GDM72XX_QOS) |
| gdm_qos_release_list(nic); |
| #endif |
| |
| gdm_wimax_cleanup_ioctl(phy_dev->netdev); |
| |
| free_netdev(nic->netdev); |
| } |