Pileus Git - ~andy/linux/blob - 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 == 0) {
 259                 wm_event.sock = netlink_init(NETLINK_WIMAX,
 260                                                 gdm_wimax_event_rcv);
 261                 INIT_LIST_HEAD(&wm_event.evtq);
 262                 INIT_LIST_HEAD(&wm_event.freeq);
 263                 INIT_WORK(&wm_event.ws, __gdm_wimax_event_send);
 264                 spin_lock_init(&wm_event.evt_lock);
 265         }
 266
 267         if (wm_event.sock) {
 268                 wm_event.ref_cnt++;
 269                 return 0;
 270         }
 271
 272         printk(KERN_ERR "Creating WiMax Event netlink is failed\n");
 273         return -1;
 274 }
 275
 276 static void gdm_wimax_event_exit(void)
 277 {
 278         if (wm_event.sock && --wm_event.ref_cnt == 0) {
 279                 struct evt_entry *e, *temp;
 280                 unsigned long flags;
 281
 282                 spin_lock_irqsave(&wm_event.evt_lock, flags);
 283
 284                 list_for_each_entry_safe(e, temp, &wm_event.evtq, list) {
 285                         list_del(&e->list);
 286                         free_event_entry(e);
 287                 }
 288                 list_for_each_entry_safe(e, temp, &wm_event.freeq, list) {
 289                         list_del(&e->list);
 290                         free_event_entry(e);
 291                 }
 292
 293                 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 294                 netlink_exit(wm_event.sock);
 295                 wm_event.sock = NULL;
 296         }
 297 }
 298
 299 static inline struct evt_entry *alloc_event_entry(void)
 300 {
 301         return kmalloc(sizeof(struct evt_entry), GFP_ATOMIC);
 302 }
 303
 304 static inline void free_event_entry(struct evt_entry *e)
 305 {
 306         kfree(e);
 307 }
 308
 309 static struct evt_entry *get_event_entry(void)
 310 {
 311         struct evt_entry *e;
 312
 313         if (list_empty(&wm_event.freeq))
 314                 e = alloc_event_entry();
 315         else {
 316                 e = list_entry(wm_event.freeq.next, struct evt_entry, list);
 317                 list_del(&e->list);
 318         }
 319
 320         return e;
 321 }
 322
 323 static void put_event_entry(struct evt_entry *e)
 324 {
 325         BUG_ON(!e);
 326
 327         list_add_tail(&e->list, &wm_event.freeq);
 328 }
 329
 330 static void __gdm_wimax_event_send(struct work_struct *work)
 331 {
 332         int idx;
 333         unsigned long flags;
 334         struct evt_entry *e;
 335
 336         spin_lock_irqsave(&wm_event.evt_lock, flags);
 337
 338         while (!list_empty(&wm_event.evtq)) {
 339                 e = list_entry(wm_event.evtq.next, struct evt_entry, list);
 340                 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 341
 342                 sscanf(e->dev->name, "wm%d", &idx);
 343                 netlink_send(wm_event.sock, idx, 0, e->evt_data, e->size);
 344
 345                 spin_lock_irqsave(&wm_event.evt_lock, flags);
 346                 list_del(&e->list);
 347                 put_event_entry(e);
 348         }
 349
 350         spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 351 }
 352
 353 static int gdm_wimax_event_send(struct net_device *dev, char *buf, int size)
 354 {
 355         struct evt_entry *e;
 356         unsigned long flags;
 357
 358         #if defined(DEBUG_HCI)
 359         u16 hci_cmd =  ((u8)buf[0]<<8) | (u8)buf[1];
 360         u16 hci_len = ((u8)buf[2]<<8) | (u8)buf[3];
 361         printk(KERN_DEBUG "D=>H: 0x%04x(%d)\n", hci_cmd, hci_len);
 362         #endif
 363
 364         spin_lock_irqsave(&wm_event.evt_lock, flags);
 365
 366         e = get_event_entry();
 367         if (!e) {
 368                 printk(KERN_ERR "%s: No memory for event\n", __func__);
 369                 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 370                 return -ENOMEM;
 371         }
 372
 373         e->dev = dev;
 374         e->size = size;
 375         memcpy(e->evt_data, buf, size);
 376
 377         list_add_tail(&e->list, &wm_event.evtq);
 378         spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 379
 380         schedule_work(&wm_event.ws);
 381
 382         return 0;
 383 }
 384
 385 static void tx_complete(void *arg)
 386 {
 387         struct nic *nic = arg;
 388
 389         if (netif_queue_stopped(nic->netdev))
 390                 netif_wake_queue(nic->netdev);
 391 }
 392
 393 int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev)
 394 {
 395         int ret = 0;
 396         struct nic *nic = netdev_priv(dev);
 397
 398         ret = gdm_wimax_send_with_cb(nic, skb->data, skb->len, tx_complete,
 399                                         nic);
 400         if (ret == -ENOSPC) {
 401                 netif_stop_queue(dev);
 402                 ret = 0;
 403         }
 404
 405         if (ret) {
 406                 skb_pull(skb, HCI_HEADER_SIZE);
 407                 return ret;
 408         }
 409
 410         nic->stats.tx_packets++;
 411         nic->stats.tx_bytes += skb->len - HCI_HEADER_SIZE;
 412         kfree_skb(skb);
 413         return ret;
 414 }
 415
 416 static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev)
 417 {
 418         int ret = 0;
 419         struct nic *nic = netdev_priv(dev);
 420         struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
 421
 422         #if defined(DEBUG_SDU)
 423         dump_eth_packet("TX", skb->data, skb->len);
 424         #endif
 425
 426         ret = gdm_wimax_header(&skb);
 427         if (ret < 0) {
 428                 skb_pull(skb, HCI_HEADER_SIZE);
 429                 return ret;
 430         }
 431
 432         #if !defined(LOOPBACK_TEST)
 433         if (!fsm)
 434                 printk(KERN_ERR "ASSERTION ERROR: fsm is NULL!!\n");
 435         else if (fsm->m_status != M_CONNECTED) {
 436                 printk(KERN_EMERG "ASSERTION ERROR: Device is NOT ready. status=%d\n",
 437                         fsm->m_status);
 438                 kfree_skb(skb);
 439                 return 0;
 440         }
 441         #endif
 442
 443 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 444         ret = gdm_qos_send_hci_pkt(skb, dev);
 445 #else
 446         ret = gdm_wimax_send_tx(skb, dev);
 447 #endif
 448         return ret;
 449 }
 450
 451 static int gdm_wimax_set_config(struct net_device *dev, struct ifmap *map)
 452 {
 453         if (dev->flags & IFF_UP)
 454                 return -EBUSY;
 455
 456         return 0;
 457 }
 458
 459 static void __gdm_wimax_set_mac_addr(struct net_device *dev, char *mac_addr)
 460 {
 461         u16 hci_pkt_buf[32 / sizeof(u16)];
 462         u8 *pkt = (u8 *) &hci_pkt_buf[0];
 463         struct nic *nic = netdev_priv(dev);
 464
 465         /* Since dev is registered as a ethernet device,
 466          * ether_setup has made dev->addr_len to be ETH_ALEN
 467          */
 468         memcpy(dev->dev_addr, mac_addr, dev->addr_len);
 469
 470         /* Let lower layer know of this change by sending
 471          * SetInformation(MAC Address)
 472          */
 473         hci_pkt_buf[0] = H2B(WIMAX_SET_INFO);   /* cmd_evt */
 474         hci_pkt_buf[1] = H2B(8);                        /* size */
 475         pkt[4] = 0; /* T */
 476         pkt[5] = 6; /* L */
 477         memcpy(pkt + 6, mac_addr, dev->addr_len); /* V */
 478
 479         gdm_wimax_send(nic, pkt, HCI_HEADER_SIZE + 8);
 480 }
 481
 482 /* A driver function */
 483 static int gdm_wimax_set_mac_addr(struct net_device *dev, void *p)
 484 {
 485         struct sockaddr *addr = p;
 486
 487         if (netif_running(dev))
 488                 return -EBUSY;
 489
 490         if (!is_valid_ether_addr(addr->sa_data))
 491                 return -EADDRNOTAVAIL;
 492
 493         __gdm_wimax_set_mac_addr(dev, addr->sa_data);
 494
 495         return 0;
 496 }
 497
 498 static struct net_device_stats *gdm_wimax_stats(struct net_device *dev)
 499 {
 500         struct nic *nic = netdev_priv(dev);
 501
 502         return &nic->stats;
 503 }
 504
 505 static int gdm_wimax_open(struct net_device *dev)
 506 {
 507         struct nic *nic = netdev_priv(dev);
 508         struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
 509
 510         netif_start_queue(dev);
 511
 512         if (fsm && fsm->m_status != M_INIT)
 513                 gdm_wimax_ind_if_updown(dev, 1);
 514         return 0;
 515 }
 516
 517 static int gdm_wimax_close(struct net_device *dev)
 518 {
 519         struct nic *nic = netdev_priv(dev);
 520         struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
 521
 522         netif_stop_queue(dev);
 523
 524         if (fsm && fsm->m_status != M_INIT)
 525                 gdm_wimax_ind_if_updown(dev, 0);
 526         return 0;
 527 }
 528
 529 static void kdelete(void **buf)
 530 {
 531         if (buf && *buf) {
 532                 kfree(*buf);
 533                 *buf = NULL;
 534         }
 535 }
 536
 537 static int gdm_wimax_ioctl_get_data(struct data_s *dst, struct data_s *src)
 538 {
 539         int size;
 540
 541         size = dst->size < src->size ? dst->size : src->size;
 542
 543         dst->size = size;
 544         if (src->size) {
 545                 if (!dst->buf)
 546                         return -EINVAL;
 547                 if (copy_to_user(dst->buf, src->buf, size))
 548                         return -EFAULT;
 549         }
 550         return 0;
 551 }
 552
 553 static int gdm_wimax_ioctl_set_data(struct data_s *dst, struct data_s *src)
 554 {
 555         if (!src->size) {
 556                 dst->size = 0;
 557                 return 0;
 558         }
 559
 560         if (!src->buf)
 561                 return -EINVAL;
 562
 563         if (!(dst->buf && dst->size == src->size)) {
 564                 kdelete(&dst->buf);
 565                 dst->buf = kmalloc(src->size, GFP_KERNEL);
 566                 if (dst->buf == NULL)
 567                         return -ENOMEM;
 568         }
 569
 570         if (copy_from_user(dst->buf, src->buf, src->size)) {
 571                 kdelete(&dst->buf);
 572                 return -EFAULT;
 573         }
 574         dst->size = src->size;
 575         return 0;
 576 }
 577
 578 static void gdm_wimax_cleanup_ioctl(struct net_device *dev)
 579 {
 580         struct nic *nic = netdev_priv(dev);
 581         int i;
 582
 583         for (i = 0; i < SIOC_DATA_MAX; i++)
 584                 kdelete(&nic->sdk_data[i].buf);
 585 }
 586
 587 static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm)
 588 {
 589         struct nic *nic = netdev_priv(dev);
 590         struct fsm_s *cur_fsm =
 591                 (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
 592
 593         if (!cur_fsm)
 594                 return;
 595
 596         if (cur_fsm->m_status != new_fsm->m_status ||
 597                 cur_fsm->c_status != new_fsm->c_status) {
 598                 if (new_fsm->m_status == M_CONNECTED)
 599                         netif_carrier_on(dev);
 600                 else if (cur_fsm->m_status == M_CONNECTED) {
 601                         netif_carrier_off(dev);
 602                         #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 603                         gdm_qos_release_list(nic);
 604                         #endif
 605                 }
 606                 gdm_wimax_ind_fsm_update(dev, new_fsm);
 607         }
 608 }
 609
 610 static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 611 {
 612         struct wm_req_s *req = (struct wm_req_s *) ifr;
 613         struct nic *nic = netdev_priv(dev);
 614         int ret;
 615
 616         if (cmd != SIOCWMIOCTL)
 617                 return -EOPNOTSUPP;
 618
 619         switch (req->cmd) {
 620         case SIOCG_DATA:
 621         case SIOCS_DATA:
 622                 if (req->data_id >= SIOC_DATA_MAX) {
 623                         printk(KERN_ERR
 624                                 "%s error: data-index(%d) is invalid!!\n",
 625                                 __func__, req->data_id);
 626                         return -EOPNOTSUPP;
 627                 }
 628                 if (req->cmd == SIOCG_DATA) {
 629                         ret = gdm_wimax_ioctl_get_data(&req->data,
 630                                                 &nic->sdk_data[req->data_id]);
 631                         if (ret < 0)
 632                                 return ret;
 633                 } else if (req->cmd == SIOCS_DATA) {
 634                         if (req->data_id == SIOC_DATA_FSM) {
 635                                 /*NOTE: gdm_update_fsm should be called
 636                                 before gdm_wimax_ioctl_set_data is called*/
 637                                 gdm_update_fsm(dev,
 638                                                 (struct fsm_s *) req->data.buf);
 639                         }
 640                         ret = gdm_wimax_ioctl_set_data(
 641                                 &nic->sdk_data[req->data_id], &req->data);
 642                         if (ret < 0)
 643                                 return ret;
 644                 }
 645                 break;
 646         default:
 647                 printk(KERN_ERR "%s: %x unknown ioctl\n", __func__, cmd);
 648                 return -EOPNOTSUPP;
 649         }
 650
 651         return 0;
 652 }
 653
 654 static void gdm_wimax_prepare_device(struct net_device *dev)
 655 {
 656         struct nic *nic = netdev_priv(dev);
 657         u16 buf[32 / sizeof(u16)];
 658         struct hci_s *hci = (struct hci_s *) buf;
 659         u16 len = 0;
 660         u32 val = 0;
 661
 662         #define BIT_MULTI_CS    0
 663         #define BIT_WIMAX               1
 664         #define BIT_QOS                 2
 665         #define BIT_AGGREGATION 3
 666
 667         /* GetInformation mac address */
 668         len = 0;
 669         hci->cmd_evt = H2B(WIMAX_GET_INFO);
 670         hci->data[len++] = TLV_T(T_MAC_ADDRESS);
 671         hci->length = H2B(len);
 672         gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len);
 673
 674         val = (1<<BIT_WIMAX) | (1<<BIT_MULTI_CS);
 675         #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 676         val |= (1<<BIT_QOS);
 677         #endif
 678         #if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
 679         val |= (1<<BIT_AGGREGATION);
 680         #endif
 681
 682         /* Set capability */
 683         len = 0;
 684         hci->cmd_evt = H2B(WIMAX_SET_INFO);
 685         hci->data[len++] = TLV_T(T_CAPABILITY);
 686         hci->data[len++] = TLV_L(T_CAPABILITY);
 687         val = DH2B(val);
 688         memcpy(&hci->data[len], &val, TLV_L(T_CAPABILITY));
 689         len += TLV_L(T_CAPABILITY);
 690         hci->length = H2B(len);
 691         gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len);
 692
 693         printk(KERN_INFO "GDM WiMax Set CAPABILITY: 0x%08X\n", DB2H(val));
 694 }
 695
 696 static int gdm_wimax_hci_get_tlv(u8 *buf, u8 *T, u16 *L, u8 **V)
 697 {
 698         #define __U82U16(b) ((u16)((u8 *)(b))[0] | ((u16)((u8 *)(b))[1] << 8))
 699         int next_pos;
 700
 701         *T = buf[0];
 702         if (buf[1] == 0x82) {
 703                 *L = B2H(__U82U16(&buf[2]));
 704                 next_pos = 1/*type*/+3/*len*/;
 705         } else {
 706                 *L = buf[1];
 707                 next_pos = 1/*type*/+1/*len*/;
 708         }
 709         *V = &buf[next_pos];
 710
 711         next_pos += *L/*length of val*/;
 712         return next_pos;
 713 }
 714
 715 static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf,
 716                                         int len)
 717 {
 718         u8 T, *V;
 719         u16 L;
 720         u16 cmd_evt, cmd_len;
 721         int pos = HCI_HEADER_SIZE;
 722
 723         cmd_evt = B2H(*(u16 *)&buf[0]);
 724         cmd_len = B2H(*(u16 *)&buf[2]);
 725
 726         if (len < cmd_len + HCI_HEADER_SIZE) {
 727                 printk(KERN_ERR "%s: invalid length [%d/%d]\n", __func__,
 728                         cmd_len + HCI_HEADER_SIZE, len);
 729                 return -1;
 730         }
 731
 732         if (cmd_evt == WIMAX_GET_INFO_RESULT) {
 733                 if (cmd_len < 2) {
 734                         printk(KERN_ERR "%s: len is too short [%x/%d]\n",
 735                                 __func__, cmd_evt, len);
 736                         return -1;
 737                 }
 738
 739                 pos += gdm_wimax_hci_get_tlv(&buf[pos], &T, &L, &V);
 740                 if (T == TLV_T(T_MAC_ADDRESS)) {
 741                         if (L != dev->addr_len) {
 742                                 printk(KERN_ERR
 743                                         "%s Invalid inofrmation result T/L "
 744                                         "[%x/%d]\n", __func__, T, L);
 745                                 return -1;
 746                         }
 747                         printk(KERN_INFO
 748                                 "MAC change [%02x:%02x:%02x:%02x:%02x:%02x]"
 749                                 "->[%02x:%02x:%02x:%02x:%02x:%02x]\n",
 750                                 dev->dev_addr[0], dev->dev_addr[1],
 751                                 dev->dev_addr[2], dev->dev_addr[3],
 752                                 dev->dev_addr[4], dev->dev_addr[5],
 753                                 V[0], V[1], V[2], V[3], V[4], V[5]);
 754                         memcpy(dev->dev_addr, V, dev->addr_len);
 755                         return 1;
 756                 }
 757         }
 758
 759         gdm_wimax_event_send(dev, buf, len);
 760         return 0;
 761 }
 762
 763 static void gdm_wimax_netif_rx(struct net_device *dev, char *buf, int len)
 764 {
 765         struct nic *nic = netdev_priv(dev);
 766         struct sk_buff *skb;
 767         int ret;
 768
 769         #if defined(DEBUG_SDU)
 770         dump_eth_packet("RX", buf, len);
 771         #endif
 772
 773         skb = dev_alloc_skb(len + 2);
 774         if (!skb) {
 775                 printk(KERN_ERR "%s: dev_alloc_skb failed!\n", __func__);
 776                 return;
 777         }
 778         skb_reserve(skb, 2);
 779
 780         nic->stats.rx_packets++;
 781         nic->stats.rx_bytes += len;
 782
 783         memcpy(skb_put(skb, len), buf, len);
 784
 785         skb->dev = dev;
 786         skb->protocol = eth_type_trans(skb, dev); /* what will happen? */
 787
 788         ret = in_interrupt() ? netif_rx(skb) : netif_rx_ni(skb);
 789         if (ret == NET_RX_DROP)
 790                 printk(KERN_ERR "%s skb dropped\n", __func__);
 791 }
 792
 793 static void gdm_wimax_transmit_aggr_pkt(struct net_device *dev, char *buf,
 794                                         int len)
 795 {
 796         #define HCI_PADDING_BYTE        4
 797         #define HCI_RESERVED_BYTE       4
 798         struct hci_s *hci;
 799         int length;
 800
 801         while (len > 0) {
 802                 hci = (struct hci_s *) buf;
 803
 804                 if (B2H(hci->cmd_evt) != WIMAX_RX_SDU) {
 805                         printk(KERN_ERR "Wrong cmd_evt(0x%04X)\n",
 806                                 B2H(hci->cmd_evt));
 807                         break;
 808                 }
 809
 810                 length = B2H(hci->length);
 811                 gdm_wimax_netif_rx(dev, hci->data, length);
 812
 813                 if (length & 0x3) {
 814                         /* Add padding size */
 815                         length += HCI_PADDING_BYTE - (length & 0x3);
 816                 }
 817
 818                 length += HCI_HEADER_SIZE + HCI_RESERVED_BYTE;
 819                 len -= length;
 820                 buf += length;
 821         }
 822 }
 823
 824 static void gdm_wimax_transmit_pkt(struct net_device *dev, char *buf, int len)
 825 {
 826         #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 827         struct nic *nic = netdev_priv(dev);
 828         #endif
 829         u16 cmd_evt, cmd_len;
 830
 831         /* This code is added for certain rx packet to be ignored. */
 832         if (len == 0)
 833                 return;
 834
 835         cmd_evt = B2H(*(u16 *)&buf[0]);
 836         cmd_len = B2H(*(u16 *)&buf[2]);
 837
 838         if (len < cmd_len + HCI_HEADER_SIZE) {
 839                 if (len)
 840                         printk(KERN_ERR "%s: invalid length [%d/%d]\n",
 841                                 __func__, cmd_len + HCI_HEADER_SIZE, len);
 842                 return;
 843         }
 844
 845         switch (cmd_evt) {
 846         case WIMAX_RX_SDU_AGGR:
 847                 gdm_wimax_transmit_aggr_pkt(dev, &buf[HCI_HEADER_SIZE],
 848                                                 cmd_len);
 849                 break;
 850         case WIMAX_RX_SDU:
 851                 gdm_wimax_netif_rx(dev, &buf[HCI_HEADER_SIZE], cmd_len);
 852                 break;
 853         #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 854         case WIMAX_EVT_MODEM_REPORT:
 855                 gdm_recv_qos_hci_packet(nic, buf, len);
 856                 break;
 857         #endif
 858         case WIMAX_SDU_TX_FLOW:
 859                 if (buf[4] == 0) {
 860                         if (!netif_queue_stopped(dev))
 861                                 netif_stop_queue(dev);
 862                 } else if (buf[4] == 1) {
 863                         if (netif_queue_stopped(dev))
 864                                 netif_wake_queue(dev);
 865                 }
 866                 break;
 867         default:
 868                 gdm_wimax_event_send(dev, buf, len);
 869                 break;
 870         }
 871 }
 872
 873 static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm)
 874 {
 875         u16 buf[32 / sizeof(u16)];
 876         u8 *hci_pkt_buf = (u8 *)&buf[0];
 877
 878         /* Indicate updating fsm */
 879         buf[0] = H2B(WIMAX_FSM_UPDATE);
 880         buf[1] = H2B(sizeof(struct fsm_s));
 881         memcpy(&hci_pkt_buf[HCI_HEADER_SIZE], fsm, sizeof(struct fsm_s));
 882
 883         gdm_wimax_event_send(dev, hci_pkt_buf,
 884                                 HCI_HEADER_SIZE + sizeof(struct fsm_s));
 885 }
 886
 887 static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up)
 888 {
 889         u16 buf[32 / sizeof(u16)];
 890         struct hci_s *hci = (struct hci_s *) buf;
 891         unsigned char up_down;
 892
 893         up_down = if_up ? WIMAX_IF_UP : WIMAX_IF_DOWN;
 894
 895         /* Indicate updating fsm */
 896         hci->cmd_evt = H2B(WIMAX_IF_UPDOWN);
 897         hci->length = H2B(sizeof(up_down));
 898         hci->data[0] = up_down;
 899
 900         gdm_wimax_event_send(dev, (char *)hci, HCI_HEADER_SIZE+sizeof(up_down));
 901 }
 902
 903 static void rx_complete(void *arg, void *data, int len)
 904 {
 905         struct nic *nic = arg;
 906
 907         gdm_wimax_transmit_pkt(nic->netdev, data, len);
 908         gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
 909 }
 910
 911 static void prepare_rx_complete(void *arg, void *data, int len)
 912 {
 913         struct nic *nic = arg;
 914         int ret;
 915
 916         ret = gdm_wimax_get_prepared_info(nic->netdev, data, len);
 917         if (ret == 1)
 918                 gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
 919         else {
 920                 if (ret < 0)
 921                         printk(KERN_ERR "get_prepared_info failed(%d)\n", ret);
 922                 gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
 923                 #if 0
 924                 /* Re-prepare WiMax device */
 925                 gdm_wimax_prepare_device(nic->netdev);
 926                 #endif
 927         }
 928 }
 929
 930 static void start_rx_proc(struct nic *nic)
 931 {
 932         gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
 933 }
 934
 935 static struct net_device_ops gdm_netdev_ops = {
 936         .ndo_open                               = gdm_wimax_open,
 937         .ndo_stop                               = gdm_wimax_close,
 938         .ndo_set_config                 = gdm_wimax_set_config,
 939         .ndo_start_xmit                 = gdm_wimax_tx,
 940         .ndo_get_stats                  = gdm_wimax_stats,
 941         .ndo_set_mac_address    = gdm_wimax_set_mac_addr,
 942         .ndo_do_ioctl                   = gdm_wimax_ioctl,
 943 };
 944
 945 int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
 946 {
 947         struct nic *nic = NULL;
 948         struct net_device *dev;
 949         int ret;
 950
 951         dev = (struct net_device *)alloc_netdev(sizeof(*nic),
 952                                                 "wm%d", ether_setup);
 953
 954         if (dev == NULL) {
 955                 printk(KERN_ERR "alloc_etherdev failed\n");
 956                 return -ENOMEM;
 957         }
 958
 959         SET_NETDEV_DEV(dev, pdev);
 960         dev->mtu = 1400;
 961         dev->netdev_ops = &gdm_netdev_ops;
 962         dev->flags &= ~IFF_MULTICAST;
 963         memcpy(dev->dev_addr, gdm_wimax_macaddr, sizeof(gdm_wimax_macaddr));
 964
 965         nic = netdev_priv(dev);
 966         memset(nic, 0, sizeof(*nic));
 967
 968         nic->netdev = dev;
 969         nic->phy_dev = phy_dev;
 970         phy_dev->netdev = dev;
 971
 972         /* event socket init */
 973         ret = gdm_wimax_event_init();
 974         if (ret < 0) {
 975                 printk(KERN_ERR "Cannot create event.\n");
 976                 goto cleanup;
 977         }
 978
 979         ret = register_netdev(dev);
 980         if (ret)
 981                 goto cleanup;
 982
 983         #if defined(LOOPBACK_TEST)
 984         netif_start_queue(dev);
 985         netif_carrier_on(dev);
 986         #else
 987         netif_carrier_off(dev);
 988         #endif
 989
 990 #ifdef CONFIG_WIMAX_GDM72XX_QOS
 991         gdm_qos_init(nic);
 992 #endif
 993
 994         start_rx_proc(nic);
 995
 996         /* Prepare WiMax device */
 997         gdm_wimax_prepare_device(dev);
 998
 999         return 0;
1000
1001 cleanup:
1002         printk(KERN_ERR "register_netdev failed\n");
1003         free_netdev(dev);
1004         return ret;
1005 }
1006
1007 void unregister_wimax_device(struct phy_dev *phy_dev)
1008 {
1009         struct nic *nic = netdev_priv(phy_dev->netdev);
1010         struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
1011
1012         if (fsm)
1013                 fsm->m_status = M_INIT;
1014         unregister_netdev(nic->netdev);
1015
1016         gdm_wimax_event_exit();
1017
1018 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
1019         gdm_qos_release_list(nic);
1020 #endif
1021
1022         gdm_wimax_cleanup_ioctl(phy_dev->netdev);
1023
1024         free_netdev(nic->netdev);
1025 }