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