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 struct {
  45         int ref_cnt;
  46         struct sock *sock;
  47         struct list_head evtq;
  48         spinlock_t evt_lock;
  49         struct list_head freeq;
  50         struct work_struct ws;
  51 } wm_event;
  52
  53 static u8 gdm_wimax_macaddr[6] = {0x00, 0x0a, 0x3b, 0xf0, 0x01, 0x30};
  54
  55 static inline int gdm_wimax_header(struct sk_buff **pskb)
  56 {
  57         u16 buf[HCI_HEADER_SIZE / sizeof(u16)];
  58         struct hci_s *hci = (struct hci_s *)buf;
  59         struct sk_buff *skb = *pskb;
  60
  61         if (unlikely(skb_headroom(skb) < HCI_HEADER_SIZE)) {
  62                 struct sk_buff *skb2;
  63
  64                 skb2 = skb_realloc_headroom(skb, HCI_HEADER_SIZE);
  65                 if (skb2 == NULL)
  66                         return -ENOMEM;
  67                 if (skb->sk)
  68                         skb_set_owner_w(skb2, skb->sk);
  69                 kfree_skb(skb);
  70                 skb = skb2;
  71         }
  72
  73         skb_push(skb, HCI_HEADER_SIZE);
  74         hci->cmd_evt = cpu_to_be16(WIMAX_TX_SDU);
  75         hci->length = cpu_to_be16(skb->len - HCI_HEADER_SIZE);
  76         memcpy(skb->data, buf, HCI_HEADER_SIZE);
  77
  78         *pskb = skb;
  79         return 0;
  80 }
  81
  82 static inline struct evt_entry *alloc_event_entry(void)
  83 {
  84         return kmalloc(sizeof(struct evt_entry), GFP_ATOMIC);
  85 }
  86
  87 static inline void free_event_entry(struct evt_entry *e)
  88 {
  89         kfree(e);
  90 }
  91
  92 static struct evt_entry *get_event_entry(void)
  93 {
  94         struct evt_entry *e;
  95
  96         if (list_empty(&wm_event.freeq)) {
  97                 e = alloc_event_entry();
  98         } else {
  99                 e = list_entry(wm_event.freeq.next, struct evt_entry, list);
 100                 list_del(&e->list);
 101         }
 102
 103         return e;
 104 }
 105
 106 static void put_event_entry(struct evt_entry *e)
 107 {
 108         BUG_ON(!e);
 109
 110         list_add_tail(&e->list, &wm_event.freeq);
 111 }
 112
 113 static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
 114                                 int len)
 115 {
 116         struct nic *nic = netdev_priv(dev);
 117
 118         u8 *buf = msg;
 119         u16 hci_cmd =  (buf[0]<<8) | buf[1];
 120         u16 hci_len = (buf[2]<<8) | buf[3];
 121
 122         netdev_dbg(dev, "H=>D: 0x%04x(%d)\n", hci_cmd, hci_len);
 123
 124         gdm_wimax_send(nic, msg, len);
 125 }
 126
 127 static void __gdm_wimax_event_send(struct work_struct *work)
 128 {
 129         int idx;
 130         unsigned long flags;
 131         struct evt_entry *e;
 132         struct evt_entry *tmp;
 133
 134         spin_lock_irqsave(&wm_event.evt_lock, flags);
 135
 136         list_for_each_entry_safe(e, tmp, &wm_event.evtq, list) {
 137                 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 138
 139                 if (sscanf(e->dev->name, "wm%d", &idx) == 1)
 140                         netlink_send(wm_event.sock, idx, 0, e->evt_data,
 141                                      e->size);
 142
 143                 spin_lock_irqsave(&wm_event.evt_lock, flags);
 144                 list_del(&e->list);
 145                 put_event_entry(e);
 146         }
 147
 148         spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 149 }
 150
 151 static int gdm_wimax_event_init(void)
 152 {
 153         if (!wm_event.ref_cnt) {
 154                 wm_event.sock = netlink_init(NETLINK_WIMAX,
 155                                                 gdm_wimax_event_rcv);
 156                 if (wm_event.sock) {
 157                         INIT_LIST_HEAD(&wm_event.evtq);
 158                         INIT_LIST_HEAD(&wm_event.freeq);
 159                         INIT_WORK(&wm_event.ws, __gdm_wimax_event_send);
 160                         spin_lock_init(&wm_event.evt_lock);
 161                 }
 162         }
 163
 164         if (wm_event.sock) {
 165                 wm_event.ref_cnt++;
 166                 return 0;
 167         }
 168
 169         pr_err("Creating WiMax Event netlink is failed\n");
 170         return -1;
 171 }
 172
 173 static void gdm_wimax_event_exit(void)
 174 {
 175         if (wm_event.sock && --wm_event.ref_cnt == 0) {
 176                 struct evt_entry *e, *temp;
 177                 unsigned long flags;
 178
 179                 spin_lock_irqsave(&wm_event.evt_lock, flags);
 180
 181                 list_for_each_entry_safe(e, temp, &wm_event.evtq, list) {
 182                         list_del(&e->list);
 183                         free_event_entry(e);
 184                 }
 185                 list_for_each_entry_safe(e, temp, &wm_event.freeq, list) {
 186                         list_del(&e->list);
 187                         free_event_entry(e);
 188                 }
 189
 190                 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 191                 netlink_exit(wm_event.sock);
 192                 wm_event.sock = NULL;
 193         }
 194 }
 195
 196 static int gdm_wimax_event_send(struct net_device *dev, char *buf, int size)
 197 {
 198         struct evt_entry *e;
 199         unsigned long flags;
 200
 201         u16 hci_cmd =  ((u8)buf[0]<<8) | (u8)buf[1];
 202         u16 hci_len = ((u8)buf[2]<<8) | (u8)buf[3];
 203
 204         netdev_dbg(dev, "D=>H: 0x%04x(%d)\n", hci_cmd, hci_len);
 205
 206         spin_lock_irqsave(&wm_event.evt_lock, flags);
 207
 208         e = get_event_entry();
 209         if (!e) {
 210                 netdev_err(dev, "%s: No memory for event\n", __func__);
 211                 spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 212                 return -ENOMEM;
 213         }
 214
 215         e->dev = dev;
 216         e->size = size;
 217         memcpy(e->evt_data, buf, size);
 218
 219         list_add_tail(&e->list, &wm_event.evtq);
 220         spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 221
 222         schedule_work(&wm_event.ws);
 223
 224         return 0;
 225 }
 226
 227 static void tx_complete(void *arg)
 228 {
 229         struct nic *nic = arg;
 230
 231         if (netif_queue_stopped(nic->netdev))
 232                 netif_wake_queue(nic->netdev);
 233 }
 234
 235 int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev)
 236 {
 237         int ret = 0;
 238         struct nic *nic = netdev_priv(dev);
 239
 240         ret = gdm_wimax_send_with_cb(nic, skb->data, skb->len, tx_complete,
 241                                      nic);
 242         if (ret == -ENOSPC) {
 243                 netif_stop_queue(dev);
 244                 ret = 0;
 245         }
 246
 247         if (ret) {
 248                 skb_pull(skb, HCI_HEADER_SIZE);
 249                 return ret;
 250         }
 251
 252         dev->stats.tx_packets++;
 253         dev->stats.tx_bytes += skb->len - HCI_HEADER_SIZE;
 254         kfree_skb(skb);
 255         return ret;
 256 }
 257
 258 static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev)
 259 {
 260         int ret = 0;
 261
 262         ret = gdm_wimax_header(&skb);
 263         if (ret < 0) {
 264                 skb_pull(skb, HCI_HEADER_SIZE);
 265                 return ret;
 266         }
 267
 268 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 269         ret = gdm_qos_send_hci_pkt(skb, dev);
 270 #else
 271         ret = gdm_wimax_send_tx(skb, dev);
 272 #endif
 273         return ret;
 274 }
 275
 276 static int gdm_wimax_set_config(struct net_device *dev, struct ifmap *map)
 277 {
 278         if (dev->flags & IFF_UP)
 279                 return -EBUSY;
 280
 281         return 0;
 282 }
 283
 284 static void __gdm_wimax_set_mac_addr(struct net_device *dev, char *mac_addr)
 285 {
 286         u16 hci_pkt_buf[32 / sizeof(u16)];
 287         struct hci_s *hci = (struct hci_s *)hci_pkt_buf;
 288         struct nic *nic = netdev_priv(dev);
 289
 290         /* Since dev is registered as a ethernet device,
 291          * ether_setup has made dev->addr_len to be ETH_ALEN
 292          */
 293         memcpy(dev->dev_addr, mac_addr, dev->addr_len);
 294
 295         /* Let lower layer know of this change by sending
 296          * SetInformation(MAC Address)
 297          */
 298         hci->cmd_evt = cpu_to_be16(WIMAX_SET_INFO);
 299         hci->length = cpu_to_be16(8);
 300         hci->data[0] = 0; /* T */
 301         hci->data[1] = 6; /* L */
 302         memcpy(&hci->data[2], mac_addr, dev->addr_len); /* V */
 303
 304         gdm_wimax_send(nic, hci, HCI_HEADER_SIZE + 8);
 305 }
 306
 307 /* A driver function */
 308 static int gdm_wimax_set_mac_addr(struct net_device *dev, void *p)
 309 {
 310         struct sockaddr *addr = p;
 311
 312         if (netif_running(dev))
 313                 return -EBUSY;
 314
 315         if (!is_valid_ether_addr(addr->sa_data))
 316                 return -EADDRNOTAVAIL;
 317
 318         __gdm_wimax_set_mac_addr(dev, addr->sa_data);
 319
 320         return 0;
 321 }
 322
 323 static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up)
 324 {
 325         u16 buf[32 / sizeof(u16)];
 326         struct hci_s *hci = (struct hci_s *)buf;
 327         unsigned char up_down;
 328
 329         up_down = if_up ? WIMAX_IF_UP : WIMAX_IF_DOWN;
 330
 331         /* Indicate updating fsm */
 332         hci->cmd_evt = cpu_to_be16(WIMAX_IF_UPDOWN);
 333         hci->length = cpu_to_be16(sizeof(up_down));
 334         hci->data[0] = up_down;
 335
 336         gdm_wimax_event_send(dev, (char *)hci, HCI_HEADER_SIZE+sizeof(up_down));
 337 }
 338
 339 static int gdm_wimax_open(struct net_device *dev)
 340 {
 341         struct nic *nic = netdev_priv(dev);
 342         struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 343
 344         netif_start_queue(dev);
 345
 346         if (fsm && fsm->m_status != M_INIT)
 347                 gdm_wimax_ind_if_updown(dev, 1);
 348         return 0;
 349 }
 350
 351 static int gdm_wimax_close(struct net_device *dev)
 352 {
 353         struct nic *nic = netdev_priv(dev);
 354         struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 355
 356         netif_stop_queue(dev);
 357
 358         if (fsm && fsm->m_status != M_INIT)
 359                 gdm_wimax_ind_if_updown(dev, 0);
 360         return 0;
 361 }
 362
 363 static void kdelete(void **buf)
 364 {
 365         if (buf && *buf) {
 366                 kfree(*buf);
 367                 *buf = NULL;
 368         }
 369 }
 370
 371 static int gdm_wimax_ioctl_get_data(struct data_s *dst, struct data_s *src)
 372 {
 373         int size;
 374
 375         size = dst->size < src->size ? dst->size : src->size;
 376
 377         dst->size = size;
 378         if (src->size) {
 379                 if (!dst->buf)
 380                         return -EINVAL;
 381                 if (copy_to_user((void __user *)dst->buf, src->buf, size))
 382                         return -EFAULT;
 383         }
 384         return 0;
 385 }
 386
 387 static int gdm_wimax_ioctl_set_data(struct data_s *dst, struct data_s *src)
 388 {
 389         if (!src->size) {
 390                 dst->size = 0;
 391                 return 0;
 392         }
 393
 394         if (!src->buf)
 395                 return -EINVAL;
 396
 397         if (!(dst->buf && dst->size == src->size)) {
 398                 kdelete(&dst->buf);
 399                 dst->buf = kmalloc(src->size, GFP_KERNEL);
 400                 if (dst->buf == NULL)
 401                         return -ENOMEM;
 402         }
 403
 404         if (copy_from_user(dst->buf, (void __user *)src->buf, src->size)) {
 405                 kdelete(&dst->buf);
 406                 return -EFAULT;
 407         }
 408         dst->size = src->size;
 409         return 0;
 410 }
 411
 412 static void gdm_wimax_cleanup_ioctl(struct net_device *dev)
 413 {
 414         struct nic *nic = netdev_priv(dev);
 415         int i;
 416
 417         for (i = 0; i < SIOC_DATA_MAX; i++)
 418                 kdelete(&nic->sdk_data[i].buf);
 419 }
 420
 421 static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm)
 422 {
 423         u16 buf[32 / sizeof(u16)];
 424         struct hci_s *hci = (struct hci_s *)buf;
 425
 426         /* Indicate updating fsm */
 427         hci->cmd_evt = cpu_to_be16(WIMAX_FSM_UPDATE);
 428         hci->length = cpu_to_be16(sizeof(struct fsm_s));
 429         memcpy(&hci->data[0], fsm, sizeof(struct fsm_s));
 430
 431         gdm_wimax_event_send(dev, (char *)hci,
 432                              HCI_HEADER_SIZE + sizeof(struct fsm_s));
 433 }
 434
 435 static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm)
 436 {
 437         struct nic *nic = netdev_priv(dev);
 438         struct fsm_s *cur_fsm = (struct fsm_s *)
 439                                         nic->sdk_data[SIOC_DATA_FSM].buf;
 440
 441         if (!cur_fsm)
 442                 return;
 443
 444         if (cur_fsm->m_status != new_fsm->m_status ||
 445             cur_fsm->c_status != new_fsm->c_status) {
 446                 if (new_fsm->m_status == M_CONNECTED) {
 447                         netif_carrier_on(dev);
 448                 } else if (cur_fsm->m_status == M_CONNECTED) {
 449                         netif_carrier_off(dev);
 450                         #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 451                         gdm_qos_release_list(nic);
 452                         #endif
 453                 }
 454                 gdm_wimax_ind_fsm_update(dev, new_fsm);
 455         }
 456 }
 457
 458 static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 459 {
 460         struct wm_req_s *req = (struct wm_req_s *)ifr;
 461         struct nic *nic = netdev_priv(dev);
 462         int ret;
 463
 464         if (cmd != SIOCWMIOCTL)
 465                 return -EOPNOTSUPP;
 466
 467         switch (req->cmd) {
 468         case SIOCG_DATA:
 469         case SIOCS_DATA:
 470                 if (req->data_id >= SIOC_DATA_MAX) {
 471                         netdev_err(dev, "%s error: data-index(%d) is invalid!!\n",
 472                                    __func__, req->data_id);
 473                         return -EOPNOTSUPP;
 474                 }
 475                 if (req->cmd == SIOCG_DATA) {
 476                         ret = gdm_wimax_ioctl_get_data(
 477                                 &req->data, &nic->sdk_data[req->data_id]);
 478                         if (ret < 0)
 479                                 return ret;
 480                 } else if (req->cmd == SIOCS_DATA) {
 481                         if (req->data_id == SIOC_DATA_FSM) {
 482                                 /* NOTE: gdm_update_fsm should be called
 483                                  * before gdm_wimax_ioctl_set_data is called.
 484                                  */
 485                                 gdm_update_fsm(dev,
 486                                                (struct fsm_s *)req->data.buf);
 487                         }
 488                         ret = gdm_wimax_ioctl_set_data(
 489                                 &nic->sdk_data[req->data_id], &req->data);
 490                         if (ret < 0)
 491                                 return ret;
 492                 }
 493                 break;
 494         default:
 495                 netdev_err(dev, "%s: %x unknown ioctl\n", __func__, cmd);
 496                 return -EOPNOTSUPP;
 497         }
 498
 499         return 0;
 500 }
 501
 502 static void gdm_wimax_prepare_device(struct net_device *dev)
 503 {
 504         struct nic *nic = netdev_priv(dev);
 505         u16 buf[32 / sizeof(u16)];
 506         struct hci_s *hci = (struct hci_s *)buf;
 507         u16 len = 0;
 508         u32 val = 0;
 509         __be32 val_be32;
 510
 511         /* GetInformation mac address */
 512         len = 0;
 513         hci->cmd_evt = cpu_to_be16(WIMAX_GET_INFO);
 514         hci->data[len++] = TLV_T(T_MAC_ADDRESS);
 515         hci->length = cpu_to_be16(len);
 516         gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len);
 517
 518         val = T_CAPABILITY_WIMAX | T_CAPABILITY_MULTI_CS;
 519         #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 520         val |= T_CAPABILITY_QOS;
 521         #endif
 522         #if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
 523         val |= T_CAPABILITY_AGGREGATION;
 524         #endif
 525
 526         /* Set capability */
 527         len = 0;
 528         hci->cmd_evt = cpu_to_be16(WIMAX_SET_INFO);
 529         hci->data[len++] = TLV_T(T_CAPABILITY);
 530         hci->data[len++] = TLV_L(T_CAPABILITY);
 531         val_be32 = cpu_to_be32(val);
 532         memcpy(&hci->data[len], &val_be32, TLV_L(T_CAPABILITY));
 533         len += TLV_L(T_CAPABILITY);
 534         hci->length = cpu_to_be16(len);
 535         gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len);
 536
 537         netdev_info(dev, "GDM WiMax Set CAPABILITY: 0x%08X\n", val);
 538 }
 539
 540 static int gdm_wimax_hci_get_tlv(u8 *buf, u8 *T, u16 *L, u8 **V)
 541 {
 542         #define __U82U16(b) ((u16)((u8 *)(b))[0] | ((u16)((u8 *)(b))[1] << 8))
 543         int next_pos;
 544
 545         *T = buf[0];
 546         if (buf[1] == 0x82) {
 547                 *L = be16_to_cpu(__U82U16(&buf[2]));
 548                 next_pos = 1/*type*/+3/*len*/;
 549         } else {
 550                 *L = buf[1];
 551                 next_pos = 1/*type*/+1/*len*/;
 552         }
 553         *V = &buf[next_pos];
 554
 555         next_pos += *L/*length of val*/;
 556         return next_pos;
 557 }
 558
 559 static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf,
 560                                        int len)
 561 {
 562         u8 T, *V;
 563         u16 L;
 564         u16 cmd_evt, cmd_len;
 565         int pos = HCI_HEADER_SIZE;
 566
 567         cmd_evt = be16_to_cpup((const __be16 *)&buf[0]);
 568         cmd_len = be16_to_cpup((const __be16 *)&buf[2]);
 569
 570         if (len < cmd_len + HCI_HEADER_SIZE) {
 571                 netdev_err(dev, "%s: invalid length [%d/%d]\n", __func__,
 572                            cmd_len + HCI_HEADER_SIZE, len);
 573                 return -1;
 574         }
 575
 576         if (cmd_evt == WIMAX_GET_INFO_RESULT) {
 577                 if (cmd_len < 2) {
 578                         netdev_err(dev, "%s: len is too short [%x/%d]\n",
 579                                    __func__, cmd_evt, len);
 580                         return -1;
 581                 }
 582
 583                 pos += gdm_wimax_hci_get_tlv(&buf[pos], &T, &L, &V);
 584                 if (T == TLV_T(T_MAC_ADDRESS)) {
 585                         if (L != dev->addr_len) {
 586                                 netdev_err(dev,
 587                                            "%s Invalid information result T/L [%x/%d]\n",
 588                                            __func__, T, L);
 589                                 return -1;
 590                         }
 591                         netdev_info(dev, "MAC change [%pM]->[%pM]\n",
 592                                     dev->dev_addr, V);
 593                         memcpy(dev->dev_addr, V, dev->addr_len);
 594                         return 1;
 595                 }
 596         }
 597
 598         gdm_wimax_event_send(dev, buf, len);
 599         return 0;
 600 }
 601
 602 static void gdm_wimax_netif_rx(struct net_device *dev, char *buf, int len)
 603 {
 604         struct sk_buff *skb;
 605         int ret;
 606
 607         skb = dev_alloc_skb(len + 2);
 608         if (!skb)
 609                 return;
 610         skb_reserve(skb, 2);
 611
 612         dev->stats.rx_packets++;
 613         dev->stats.rx_bytes += len;
 614
 615         memcpy(skb_put(skb, len), buf, len);
 616
 617         skb->dev = dev;
 618         skb->protocol = eth_type_trans(skb, dev); /* what will happen? */
 619
 620         ret = in_interrupt() ? netif_rx(skb) : netif_rx_ni(skb);
 621         if (ret == NET_RX_DROP)
 622                 netdev_err(dev, "%s skb dropped\n", __func__);
 623 }
 624
 625 static void gdm_wimax_transmit_aggr_pkt(struct net_device *dev, char *buf,
 626                                         int len)
 627 {
 628         #define HCI_PADDING_BYTE        4
 629         #define HCI_RESERVED_BYTE       4
 630         struct hci_s *hci;
 631         int length;
 632
 633         while (len > 0) {
 634                 hci = (struct hci_s *)buf;
 635
 636                 if (hci->cmd_evt != cpu_to_be16(WIMAX_RX_SDU)) {
 637                         netdev_err(dev, "Wrong cmd_evt(0x%04X)\n",
 638                                    be16_to_cpu(hci->cmd_evt));
 639                         break;
 640                 }
 641
 642                 length = be16_to_cpu(hci->length);
 643                 gdm_wimax_netif_rx(dev, hci->data, length);
 644
 645                 if (length & 0x3) {
 646                         /* Add padding size */
 647                         length += HCI_PADDING_BYTE - (length & 0x3);
 648                 }
 649
 650                 length += HCI_HEADER_SIZE + HCI_RESERVED_BYTE;
 651                 len -= length;
 652                 buf += length;
 653         }
 654 }
 655
 656 static void gdm_wimax_transmit_pkt(struct net_device *dev, char *buf, int len)
 657 {
 658         #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 659         struct nic *nic = netdev_priv(dev);
 660         #endif
 661         u16 cmd_evt, cmd_len;
 662
 663         /* This code is added for certain rx packet to be ignored. */
 664         if (len == 0)
 665                 return;
 666
 667         cmd_evt = be16_to_cpup((const __be16 *)&buf[0]);
 668         cmd_len = be16_to_cpup((const __be16 *)&buf[2]);
 669
 670         if (len < cmd_len + HCI_HEADER_SIZE) {
 671                 if (len)
 672                         netdev_err(dev, "%s: invalid length [%d/%d]\n",
 673                                    __func__, cmd_len + HCI_HEADER_SIZE, len);
 674                 return;
 675         }
 676
 677         switch (cmd_evt) {
 678         case WIMAX_RX_SDU_AGGR:
 679                 gdm_wimax_transmit_aggr_pkt(dev, &buf[HCI_HEADER_SIZE],
 680                                             cmd_len);
 681                 break;
 682         case WIMAX_RX_SDU:
 683                 gdm_wimax_netif_rx(dev, &buf[HCI_HEADER_SIZE], cmd_len);
 684                 break;
 685         #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 686         case WIMAX_EVT_MODEM_REPORT:
 687                 gdm_recv_qos_hci_packet(nic, buf, len);
 688                 break;
 689         #endif
 690         case WIMAX_SDU_TX_FLOW:
 691                 if (buf[4] == 0) {
 692                         if (!netif_queue_stopped(dev))
 693                                 netif_stop_queue(dev);
 694                 } else if (buf[4] == 1) {
 695                         if (netif_queue_stopped(dev))
 696                                 netif_wake_queue(dev);
 697                 }
 698                 break;
 699         default:
 700                 gdm_wimax_event_send(dev, buf, len);
 701                 break;
 702         }
 703 }
 704
 705 static void rx_complete(void *arg, void *data, int len)
 706 {
 707         struct nic *nic = arg;
 708
 709         gdm_wimax_transmit_pkt(nic->netdev, data, len);
 710         gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
 711 }
 712
 713 static void prepare_rx_complete(void *arg, void *data, int len)
 714 {
 715         struct nic *nic = arg;
 716         int ret;
 717
 718         ret = gdm_wimax_get_prepared_info(nic->netdev, data, len);
 719         if (ret == 1) {
 720                 gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
 721         } else {
 722                 if (ret < 0)
 723                         netdev_err(nic->netdev,
 724                                    "get_prepared_info failed(%d)\n", ret);
 725                 gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
 726         }
 727 }
 728
 729 static void start_rx_proc(struct nic *nic)
 730 {
 731         gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
 732 }
 733
 734 static struct net_device_ops gdm_netdev_ops = {
 735         .ndo_open               = gdm_wimax_open,
 736         .ndo_stop               = gdm_wimax_close,
 737         .ndo_set_config         = gdm_wimax_set_config,
 738         .ndo_start_xmit         = gdm_wimax_tx,
 739         .ndo_set_mac_address    = gdm_wimax_set_mac_addr,
 740         .ndo_do_ioctl           = gdm_wimax_ioctl,
 741 };
 742
 743 int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
 744 {
 745         struct nic *nic = NULL;
 746         struct net_device *dev;
 747         int ret;
 748
 749         dev = alloc_netdev(sizeof(*nic), "wm%d", NET_NAME_UNKNOWN,
 750                            ether_setup);
 751
 752         if (!dev) {
 753                 pr_err("alloc_etherdev failed\n");
 754                 return -ENOMEM;
 755         }
 756
 757         SET_NETDEV_DEV(dev, pdev);
 758         dev->mtu = 1400;
 759         dev->netdev_ops = &gdm_netdev_ops;
 760         dev->flags &= ~IFF_MULTICAST;
 761         memcpy(dev->dev_addr, gdm_wimax_macaddr, sizeof(gdm_wimax_macaddr));
 762
 763         nic = netdev_priv(dev);
 764         nic->netdev = dev;
 765         nic->phy_dev = phy_dev;
 766         phy_dev->netdev = dev;
 767
 768         /* event socket init */
 769         ret = gdm_wimax_event_init();
 770         if (ret < 0) {
 771                 pr_err("Cannot create event.\n");
 772                 goto cleanup;
 773         }
 774
 775         ret = register_netdev(dev);
 776         if (ret)
 777                 goto cleanup;
 778
 779         netif_carrier_off(dev);
 780
 781 #ifdef CONFIG_WIMAX_GDM72XX_QOS
 782         gdm_qos_init(nic);
 783 #endif
 784
 785         start_rx_proc(nic);
 786
 787         /* Prepare WiMax device */
 788         gdm_wimax_prepare_device(dev);
 789
 790         return 0;
 791
 792 cleanup:
 793         pr_err("register_netdev failed\n");
 794         free_netdev(dev);
 795         return ret;
 796 }
 797
 798 void unregister_wimax_device(struct phy_dev *phy_dev)
 799 {
 800         struct nic *nic = netdev_priv(phy_dev->netdev);
 801         struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 802
 803         if (fsm)
 804                 fsm->m_status = M_INIT;
 805         unregister_netdev(nic->netdev);
 806
 807         gdm_wimax_event_exit();
 808
 809 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
 810         gdm_qos_release_list(nic);
 811 #endif
 812
 813         gdm_wimax_cleanup_ioctl(phy_dev->netdev);
 814
 815         free_netdev(nic->netdev);
 816 }