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