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