Pileus Git - ~andy/linux/blob - drivers/staging/gdm72xx/gdm_wimax.c

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