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