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         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 static const char *get_protocol_name(u16 protocol)
  65 {
  66         static char buf[32];
  67         const char *name = "-";
  68
  69         switch (protocol) {
  70         case ETH_P_ARP:
  71                 name = "ARP";
  72                 break;
  73         case ETH_P_IP:
  74                 name = "IP";
  75                 break;
  76         case ETH_P_IPV6:
  77                 name = "IPv6";
  78                 break;
  79         }
  80
  81         sprintf(buf, "0x%04x(%s)", protocol, name);
  82         return buf;
  83 }
  84
  85 static const char *get_ip_protocol_name(u8 ip_protocol)
  86 {
  87         static char buf[32];
  88         const char *name = "-";
  89
  90         switch (ip_protocol) {
  91         case IPPROTO_TCP:
  92                 name = "TCP";
  93                 break;
  94         case IPPROTO_UDP:
  95                 name = "UDP";
  96                 break;
  97         case IPPROTO_ICMP:
  98                 name = "ICMP";
  99                 break;
 100         }
 101
 102         sprintf(buf, "%u(%s)", ip_protocol, name);
 103         return buf;
 104 }
 105
 106 static const char *get_port_name(u16 port)
 107 {
 108         static char buf[32];
 109         const char *name = "-";
 110
 111         switch (port) {
 112         case 67:
 113                 name = "DHCP-Server";
 114                 break;
 115         case 68:
 116                 name = "DHCP-Client";
 117                 break;
 118         case 69:
 119                 name = "TFTP";
 120                 break;
 121         }
 122
 123         sprintf(buf, "%u(%s)", port, name);
 124         return buf;
 125 }
 126
 127 static void dump_eth_packet(struct net_device *dev, const char *title,
 128                             u8 *data, int len)
 129 {
 130         struct iphdr *ih = NULL;
 131         struct udphdr *uh = NULL;
 132         u16 protocol = 0;
 133         u8 ip_protocol = 0;
 134         u16 port = 0;
 135
 136         protocol = (data[12]<<8) | data[13];
 137         ih = (struct iphdr *)(data+ETH_HLEN);
 138
 139         if (protocol == ETH_P_IP) {
 140                 uh = (struct udphdr *)((char *)ih + sizeof(struct iphdr));
 141                 ip_protocol = ih->protocol;
 142                 port = ntohs(uh->dest);
 143         } else if (protocol == ETH_P_IPV6) {
 144                 struct ipv6hdr *i6h = (struct ipv6hdr *)data;
 145
 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", title, len,
 152                    get_protocol_name(protocol),
 153                    get_ip_protocol_name(ip_protocol),
 154                    get_port_name(port));
 155
 156         if (!(data[0] == 0xff && data[1] == 0xff)) {
 157                 if (protocol == ETH_P_IP)
 158                         netdev_dbg(dev, "     src=%pI4\n", &ih->saddr);
 159                 else if (protocol == ETH_P_IPV6)
 160                         netdev_dbg(dev, "     src=%pI6\n", &ih->saddr);
 161         }
 162
 163         print_hex_dump_debug("", DUMP_PREFIX_NONE, 16, 1, data, len, false);
 164 }
 165
 166 static inline int gdm_wimax_header(struct sk_buff **pskb)
 167 {
 168         u16 buf[HCI_HEADER_SIZE / sizeof(u16)];
 169         struct sk_buff *skb = *pskb;
 170
 171         if (unlikely(skb_headroom(skb) < HCI_HEADER_SIZE)) {
 172                 struct sk_buff *skb2;
 173
 174                 skb2 = skb_realloc_headroom(skb, HCI_HEADER_SIZE);
 175                 if (skb2 == NULL)
 176                         return -ENOMEM;
 177                 if (skb->sk)
 178                         skb_set_owner_w(skb2, skb->sk);
 179                 kfree_skb(skb);
 180                 skb = skb2;
 181         }
 182
 183         skb_push(skb, HCI_HEADER_SIZE);
 184         buf[0] = H2B(WIMAX_TX_SDU);
 185         buf[1] = H2B(skb->len - HCI_HEADER_SIZE);
 186         memcpy(skb->data, buf, HCI_HEADER_SIZE);
 187
 188         *pskb = skb;
 189         return 0;
 190 }
 191
 192 static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
 193                                 int len)
 194 {
 195         struct nic *nic = netdev_priv(dev);
 196
 197         u8 *buf = (u8 *)msg;
 198         u16 hci_cmd =  (buf[0]<<8) | buf[1];
 199         u16 hci_len = (buf[2]<<8) | buf[3];
 200
 201         netdev_dbg(dev, "H=>D: 0x%04x(%d)\n", hci_cmd, hci_len);
 202
 203         gdm_wimax_send(nic, msg, len);
 204 }
 205
 206 static int gdm_wimax_event_init(void)
 207 {
 208         if (!wm_event.ref_cnt) {
 209                 wm_event.sock = netlink_init(NETLINK_WIMAX,
 210                                                 gdm_wimax_event_rcv);
 211                 if (wm_event.sock) {
 212                         INIT_LIST_HEAD(&wm_event.evtq);
 213                         INIT_LIST_HEAD(&wm_event.freeq);
 214                         INIT_WORK(&wm_event.ws, __gdm_wimax_event_send);
 215                         spin_lock_init(&wm_event.evt_lock);
 216                 }
 217         }
 218
 219         if (wm_event.sock) {
 220                 wm_event.ref_cnt++;
 221                 return 0;
 222         }
 223
 224         pr_err("Creating WiMax Event netlink is failed\n");
 225         return -1;
 226 }
 227
 228 static void gdm_wimax_event_exit(void)
 229 {
 230         if (wm_event.sock && --wm_event.ref_cnt == 0) {
 231                 struct evt_entry *e, *temp;
 232                 unsigned long flags;
 233
 234                 spin_lock_irqsave(&wm_event.evt_lock, flags);
 235
 236                 list_for_each_entry_safe(e, temp, &wm_event.evtq, list) {
 237                         list_del(&e->list);
 238                         free_event_entry(e);
 239                 }
 240                 list_for_each_entry_safe(e, temp, &wm_event.freeq, list) {
 241                         list_del(&e->list);
 242                         free_event_entry(e);
 243                 }
 244
 245                 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 246                 netlink_exit(wm_event.sock);
 247                 wm_event.sock = NULL;
 248         }
 249 }
 250
 251 static inline struct evt_entry *alloc_event_entry(void)
 252 {
 253         return kmalloc(sizeof(struct evt_entry), GFP_ATOMIC);
 254 }
 255
 256 static inline void free_event_entry(struct evt_entry *e)
 257 {
 258         kfree(e);
 259 }
 260
 261 static struct evt_entry *get_event_entry(void)
 262 {
 263         struct evt_entry *e;
 264
 265         if (list_empty(&wm_event.freeq)) {
 266                 e = alloc_event_entry();
 267         } else {
 268                 e = list_entry(wm_event.freeq.next, struct evt_entry, list);
 269                 list_del(&e->list);
 270         }
 271
 272         return e;
 273 }
 274
 275 static void put_event_entry(struct evt_entry *e)
 276 {
 277         BUG_ON(!e);
 278
 279         list_add_tail(&e->list, &wm_event.freeq);
 280 }
 281
 282 static void __gdm_wimax_event_send(struct work_struct *work)
 283 {
 284         int idx;
 285         unsigned long flags;
 286         struct evt_entry *e;
 287
 288         spin_lock_irqsave(&wm_event.evt_lock, flags);
 289
 290         while (!list_empty(&wm_event.evtq)) {
 291                 e = list_entry(wm_event.evtq.next, struct evt_entry, list);
 292                 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 293
 294                 sscanf(e->dev->name, "wm%d", &idx);
 295                 netlink_send(wm_event.sock, idx, 0, e->evt_data, e->size);
 296
 297                 spin_lock_irqsave(&wm_event.evt_lock, flags);
 298                 list_del(&e->list);
 299                 put_event_entry(e);
 300         }
 301
 302         spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 303 }
 304
 305 static int gdm_wimax_event_send(struct net_device *dev, char *buf, int size)
 306 {
 307         struct evt_entry *e;
 308         unsigned long flags;
 309
 310         u16 hci_cmd =  ((u8)buf[0]<<8) | (u8)buf[1];
 311         u16 hci_len = ((u8)buf[2]<<8) | (u8)buf[3];
 312
 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 = (struct fsm_s *)
 540                                         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(
 578                                 &req->data, &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         }
 866 }
 867
 868 static void start_rx_proc(struct nic *nic)
 869 {
 870         gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
 871 }
 872
 873 static struct net_device_ops gdm_netdev_ops = {
 874         .ndo_open               = gdm_wimax_open,
 875         .ndo_stop               = gdm_wimax_close,
 876         .ndo_set_config         = gdm_wimax_set_config,
 877         .ndo_start_xmit         = gdm_wimax_tx,
 878         .ndo_get_stats          = gdm_wimax_stats,
 879         .ndo_set_mac_address    = gdm_wimax_set_mac_addr,
 880         .ndo_do_ioctl           = gdm_wimax_ioctl,
 881 };
 882
 883 int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
 884 {
 885         struct nic *nic = NULL;
 886         struct net_device *dev;
 887         int ret;
 888
 889         dev = alloc_netdev(sizeof(*nic), "wm%d", ether_setup);
 890
 891         if (dev == NULL) {
 892                 pr_err("alloc_etherdev failed\n");
 893                 return -ENOMEM;
 894         }
 895
 896         SET_NETDEV_DEV(dev, pdev);
 897         dev->mtu = 1400;
 898         dev->netdev_ops = &gdm_netdev_ops;
 899         dev->flags &= ~IFF_MULTICAST;
 900         memcpy(dev->dev_addr, gdm_wimax_macaddr, sizeof(gdm_wimax_macaddr));
 901
 902         nic = netdev_priv(dev);
 903         memset(nic, 0, sizeof(*nic));
 904
 905         nic->netdev = dev;
 906         nic->phy_dev = phy_dev;
 907         phy_dev->netdev = dev;
 908
 909         /* event socket init */
 910         ret = gdm_wimax_event_init();
 911         if (ret < 0) {
 912                 pr_err("Cannot create event.\n");
 913                 goto cleanup;
 914         }
 915
 916         ret = register_netdev(dev);
 917         if (ret)
 918                 goto cleanup;
 919
 920         #if defined(LOOPBACK_TEST)
 921         netif_start_queue(dev);
 922         netif_carrier_on(dev);
 923         #else
 924         netif_carrier_off(dev);
 925         #endif
 926
 927 #ifdef CONFIG_WIMAX_GDM72XX_QOS
 928         gdm_qos_init(nic);
 929 #endif
 930
 931         start_rx_proc(nic);
 932
 933         /* Prepare WiMax device */
 934         gdm_wimax_prepare_device(dev);
 935
 936         return 0;
 937
 938 cleanup:
 939         pr_err("register_netdev failed\n");
 940         free_netdev(dev);
 941         return ret;
 942 }
 943
 944 void unregister_wimax_device(struct phy_dev *phy_dev)
 945 {
 946         struct nic *nic = netdev_priv(phy_dev->netdev);
 947         struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 948
 949         if (fsm)
 950                 fsm->m_status = M_INIT;
 951         unregister_netdev(nic->netdev);
 952
 953         gdm_wimax_event_exit();
 954
 955 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 956         gdm_qos_release_list(nic);
 957 #endif
 958
 959         gdm_wimax_cleanup_ioctl(phy_dev->netdev);
 960
 961         free_netdev(nic->netdev);
 962 }