diff options
Diffstat (limited to 'drivers/staging/gdm72xx/gdm_wimax.c')
| -rw-r--r-- | drivers/staging/gdm72xx/gdm_wimax.c | 1026 |
1 files changed, 1026 insertions, 0 deletions
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c new file mode 100644 index 000000000000..f1936b92533b --- /dev/null +++ b/drivers/staging/gdm72xx/gdm_wimax.c @@ -0,0 +1,1026 @@ +/* + * 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/version.h> +#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 == 0) { + wm_event.sock = netlink_init(NETLINK_WIMAX, + gdm_wimax_event_rcv); + 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); + } + + if (wm_event.sock) { + wm_event.ref_cnt++; + 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 [%02x:%02x:%02x:%02x:%02x:%02x]" + "->[%02x:%02x:%02x:%02x:%02x:%02x]\n", + dev->dev_addr[0], dev->dev_addr[1], + dev->dev_addr[2], dev->dev_addr[3], + dev->dev_addr[4], dev->dev_addr[5], + V[0], V[1], V[2], V[3], V[4], V[5]); + 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); +} |