drivers/staging/gdm72xx/gdm_wimax.c

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