drivers/staging/gdm72xx/gdm_wimax.c

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