drivers/staging/ozwpan/ozpd.c

   1 /* -----------------------------------------------------------------------------
   2  * Copyright (c) 2011 Ozmo Inc
   3  * Released under the GNU General Public License Version 2 (GPLv2).
   4  * -----------------------------------------------------------------------------
   5  */
   6
   7 #include <linux/module.h>
   8 #include <linux/timer.h>
   9 #include <linux/sched.h>
  10 #include <linux/netdevice.h>
  11 #include <linux/etherdevice.h>
  12 #include <linux/errno.h>
  13 #include "ozdbg.h"
  14 #include "ozprotocol.h"
  15 #include "ozeltbuf.h"
  16 #include "ozpd.h"
  17 #include "ozproto.h"
  18 #include "ozcdev.h"
  19 #include "ozusbsvc.h"
  20 #include <asm/unaligned.h>
  21 #include <linux/uaccess.h>
  22 #include <net/psnap.h>
  23
  24 static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd);
  25 static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f);
  26 static void oz_tx_isoc_free(struct oz_pd *pd, struct oz_tx_frame *f);
  27 static struct sk_buff *oz_build_frame(struct oz_pd *pd, struct oz_tx_frame *f);
  28 static int oz_send_isoc_frame(struct oz_pd *pd);
  29 static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f);
  30 static void oz_isoc_stream_free(struct oz_isoc_stream *st);
  31 static int oz_send_next_queued_frame(struct oz_pd *pd, int more_data);
  32 static void oz_isoc_destructor(struct sk_buff *skb);
  33
  34 /*
  35  * Counts the uncompleted isoc frames submitted to netcard.
  36  */
  37 static atomic_t g_submitted_isoc = ATOMIC_INIT(0);
  38
  39 /* Application handler functions.
  40  */
  41 static const struct oz_app_if g_app_if[OZ_NB_APPS] = {
  42         [OZ_APPID_USB] = {
  43                 .init      = oz_usb_init,
  44                 .term      = oz_usb_term,
  45                 .start     = oz_usb_start,
  46                 .stop      = oz_usb_stop,
  47                 .rx        = oz_usb_rx,
  48                 .heartbeat = oz_usb_heartbeat,
  49                 .farewell  = oz_usb_farewell,
  50         },
  51         [OZ_APPID_SERIAL] = {
  52                 .init      = oz_cdev_init,
  53                 .term      = oz_cdev_term,
  54                 .start     = oz_cdev_start,
  55                 .stop      = oz_cdev_stop,
  56                 .rx        = oz_cdev_rx,
  57         },
  58 };
  59
  60
  61 /*
  62  * Context: softirq or process
  63  */
  64 void oz_pd_set_state(struct oz_pd *pd, unsigned state)
  65 {
  66         pd->state = state;
  67         switch (state) {
  68         case OZ_PD_S_IDLE:
  69                 oz_pd_dbg(pd, ON, "PD State: OZ_PD_S_IDLE\n");
  70                 break;
  71         case OZ_PD_S_CONNECTED:
  72                 oz_pd_dbg(pd, ON, "PD State: OZ_PD_S_CONNECTED\n");
  73                 break;
  74         case OZ_PD_S_STOPPED:
  75                 oz_pd_dbg(pd, ON, "PD State: OZ_PD_S_STOPPED\n");
  76                 break;
  77         case OZ_PD_S_SLEEP:
  78                 oz_pd_dbg(pd, ON, "PD State: OZ_PD_S_SLEEP\n");
  79                 break;
  80         }
  81 }
  82
  83 /*
  84  * Context: softirq or process
  85  */
  86 void oz_pd_get(struct oz_pd *pd)
  87 {
  88         atomic_inc(&pd->ref_count);
  89 }
  90
  91 /*
  92  * Context: softirq or process
  93  */
  94 void oz_pd_put(struct oz_pd *pd)
  95 {
  96         if (atomic_dec_and_test(&pd->ref_count))
  97                 oz_pd_destroy(pd);
  98 }
  99
 100 /*
 101  * Context: softirq-serialized
 102  */
 103 struct oz_pd *oz_pd_alloc(const u8 *mac_addr)
 104 {
 105         struct oz_pd *pd;
 106         int i;
 107
 108         pd = kzalloc(sizeof(struct oz_pd), GFP_ATOMIC);
 109         if (!pd)
 110                 return NULL;
 111
 112         atomic_set(&pd->ref_count, 2);
 113         for (i = 0; i < OZ_NB_APPS; i++)
 114                 spin_lock_init(&pd->app_lock[i]);
 115         pd->last_rx_pkt_num = 0xffffffff;
 116         oz_pd_set_state(pd, OZ_PD_S_IDLE);
 117         pd->max_tx_size = OZ_MAX_TX_SIZE;
 118         ether_addr_copy(pd->mac_addr, mac_addr);
 119         oz_elt_buf_init(&pd->elt_buff);
 120         spin_lock_init(&pd->tx_frame_lock);
 121         INIT_LIST_HEAD(&pd->tx_queue);
 122         INIT_LIST_HEAD(&pd->farewell_list);
 123         pd->last_sent_frame = &pd->tx_queue;
 124         spin_lock_init(&pd->stream_lock);
 125         INIT_LIST_HEAD(&pd->stream_list);
 126         tasklet_init(&pd->heartbeat_tasklet, oz_pd_heartbeat_handler,
 127                                                 (unsigned long)pd);
 128         tasklet_init(&pd->timeout_tasklet, oz_pd_timeout_handler,
 129                                                 (unsigned long)pd);
 130         hrtimer_init(&pd->heartbeat, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 131         hrtimer_init(&pd->timeout, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 132         pd->heartbeat.function = oz_pd_heartbeat_event;
 133         pd->timeout.function = oz_pd_timeout_event;
 134
 135         return pd;
 136 }
 137
 138 /*
 139  * Context: softirq or process
 140  */
 141 static void oz_pd_free(struct work_struct *work)
 142 {
 143         struct list_head *e, *n;
 144         struct oz_pd *pd;
 145
 146         oz_pd_dbg(pd, ON, "Destroying PD\n");
 147         pd = container_of(work, struct oz_pd, workitem);
 148         /*Disable timer tasklets*/
 149         tasklet_kill(&pd->heartbeat_tasklet);
 150         tasklet_kill(&pd->timeout_tasklet);
 151
 152         /* Free streams, queued tx frames and farewells. */
 153
 154         list_for_each_safe(e, n, &pd->stream_list)
 155                 oz_isoc_stream_free(list_entry(e, struct oz_isoc_stream, link));
 156
 157         list_for_each_safe(e, n, &pd->tx_queue) {
 158                 struct oz_tx_frame *f = list_entry(e, struct oz_tx_frame, link);
 159
 160                 if (f->skb != NULL)
 161                         kfree_skb(f->skb);
 162                 oz_retire_frame(pd, f);
 163         }
 164
 165         oz_elt_buf_term(&pd->elt_buff);
 166
 167         list_for_each_safe(e, n, &pd->farewell_list)
 168                 kfree(list_entry(e, struct oz_farewell, link));
 169
 170         if (pd->net_dev)
 171                 dev_put(pd->net_dev);
 172         kfree(pd);
 173 }
 174
 175 /*
 176  * Context: softirq or Process
 177  */
 178 void oz_pd_destroy(struct oz_pd *pd)
 179 {
 180         if (hrtimer_active(&pd->timeout))
 181                 hrtimer_cancel(&pd->timeout);
 182         if (hrtimer_active(&pd->heartbeat))
 183                 hrtimer_cancel(&pd->heartbeat);
 184
 185         INIT_WORK(&pd->workitem, oz_pd_free);
 186         if (!schedule_work(&pd->workitem))
 187                 oz_pd_dbg(pd, ON, "failed to schedule workitem\n");
 188 }
 189
 190 /*
 191  * Context: softirq-serialized
 192  */
 193 int oz_services_start(struct oz_pd *pd, u16 apps, int resume)
 194 {
 195         int i, rc = 0;
 196
 197         oz_pd_dbg(pd, ON, "%s: (0x%x) resume(%d)\n", __func__, apps, resume);
 198         for (i = 0; i < OZ_NB_APPS; i++) {
 199                 if (g_app_if[i].start && (apps & (1 << i))) {
 200                         if (g_app_if[i].start(pd, resume)) {
 201                                 rc = -1;
 202                                 oz_pd_dbg(pd, ON,
 203                                           "Unable to start service %d\n", i);
 204                                 break;
 205                         }
 206                         spin_lock_bh(&g_polling_lock);
 207                         pd->total_apps |= (1 << i);
 208                         if (resume)
 209                                 pd->paused_apps &= ~(1 << i);
 210                         spin_unlock_bh(&g_polling_lock);
 211                 }
 212         }
 213         return rc;
 214 }
 215
 216 /*
 217  * Context: softirq or process
 218  */
 219 void oz_services_stop(struct oz_pd *pd, u16 apps, int pause)
 220 {
 221         int i;
 222
 223         oz_pd_dbg(pd, ON, "%s: (0x%x) pause(%d)\n", __func__, apps, pause);
 224         for (i = 0; i < OZ_NB_APPS; i++) {
 225                 if (g_app_if[i].stop && (apps & (1 << i))) {
 226                         spin_lock_bh(&g_polling_lock);
 227                         if (pause) {
 228                                 pd->paused_apps |=  (1 << i);
 229                         } else {
 230                                 pd->total_apps  &= ~(1 << i);
 231                                 pd->paused_apps &= ~(1 << i);
 232                         }
 233                         spin_unlock_bh(&g_polling_lock);
 234                         g_app_if[i].stop(pd, pause);
 235                 }
 236         }
 237 }
 238
 239 /*
 240  * Context: softirq
 241  */
 242 void oz_pd_heartbeat(struct oz_pd *pd, u16 apps)
 243 {
 244         int i, more = 0;
 245
 246         for (i = 0; i < OZ_NB_APPS; i++) {
 247                 if (g_app_if[i].heartbeat && (apps & (1 << i))) {
 248                         if (g_app_if[i].heartbeat(pd))
 249                                 more = 1;
 250                 }
 251         }
 252         if ((!more) && (hrtimer_active(&pd->heartbeat)))
 253                 hrtimer_cancel(&pd->heartbeat);
 254         if (pd->mode & OZ_F_ISOC_ANYTIME) {
 255                 int count = 8;
 256
 257                 while (count-- && (oz_send_isoc_frame(pd) >= 0))
 258                         ;
 259         }
 260 }
 261
 262 /*
 263  * Context: softirq or process
 264  */
 265 void oz_pd_stop(struct oz_pd *pd)
 266 {
 267         u16 stop_apps;
 268
 269         oz_dbg(ON, "oz_pd_stop() State = 0x%x\n", pd->state);
 270         oz_pd_indicate_farewells(pd);
 271         spin_lock_bh(&g_polling_lock);
 272         stop_apps = pd->total_apps;
 273         pd->total_apps = 0;
 274         pd->paused_apps = 0;
 275         spin_unlock_bh(&g_polling_lock);
 276         oz_services_stop(pd, stop_apps, 0);
 277         spin_lock_bh(&g_polling_lock);
 278         oz_pd_set_state(pd, OZ_PD_S_STOPPED);
 279         /* Remove from PD list.*/
 280         list_del(&pd->link);
 281         spin_unlock_bh(&g_polling_lock);
 282         oz_dbg(ON, "pd ref count = %d\n", atomic_read(&pd->ref_count));
 283         oz_pd_put(pd);
 284 }
 285
 286 /*
 287  * Context: softirq
 288  */
 289 int oz_pd_sleep(struct oz_pd *pd)
 290 {
 291         int do_stop = 0;
 292         u16 stop_apps;
 293
 294         spin_lock_bh(&g_polling_lock);
 295         if (pd->state & (OZ_PD_S_SLEEP | OZ_PD_S_STOPPED)) {
 296                 spin_unlock_bh(&g_polling_lock);
 297                 return 0;
 298         }
 299         if (pd->keep_alive && pd->session_id)
 300                 oz_pd_set_state(pd, OZ_PD_S_SLEEP);
 301         else
 302                 do_stop = 1;
 303
 304         stop_apps = pd->total_apps;
 305         spin_unlock_bh(&g_polling_lock);
 306         if (do_stop) {
 307                 oz_pd_stop(pd);
 308         } else {
 309                 oz_services_stop(pd, stop_apps, 1);
 310                 oz_timer_add(pd, OZ_TIMER_STOP, pd->keep_alive);
 311         }
 312         return do_stop;
 313 }
 314
 315 /*
 316  * Context: softirq
 317  */
 318 static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd)
 319 {
 320         struct oz_tx_frame *f;
 321
 322         f = kmem_cache_alloc(oz_tx_frame_cache, GFP_ATOMIC);
 323         if (f) {
 324                 f->total_size = sizeof(struct oz_hdr);
 325                 INIT_LIST_HEAD(&f->link);
 326                 INIT_LIST_HEAD(&f->elt_list);
 327         }
 328         return f;
 329 }
 330
 331 /*
 332  * Context: softirq or process
 333  */
 334 static void oz_tx_isoc_free(struct oz_pd *pd, struct oz_tx_frame *f)
 335 {
 336         pd->nb_queued_isoc_frames--;
 337         list_del_init(&f->link);
 338
 339         kmem_cache_free(oz_tx_frame_cache, f);
 340
 341         oz_dbg(TX_FRAMES, "Releasing ISOC Frame isoc_nb= %d\n",
 342                pd->nb_queued_isoc_frames);
 343 }
 344
 345 /*
 346  * Context: softirq or process
 347  */
 348 static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f)
 349 {
 350         kmem_cache_free(oz_tx_frame_cache, f);
 351 }
 352
 353 /*
 354  * Context: softirq-serialized
 355  */
 356 static void oz_set_more_bit(struct sk_buff *skb)
 357 {
 358         struct oz_hdr *oz_hdr = (struct oz_hdr *)skb_network_header(skb);
 359
 360         oz_hdr->control |= OZ_F_MORE_DATA;
 361 }
 362
 363 /*
 364  * Context: softirq-serialized
 365  */
 366 static void oz_set_last_pkt_nb(struct oz_pd *pd, struct sk_buff *skb)
 367 {
 368         struct oz_hdr *oz_hdr = (struct oz_hdr *)skb_network_header(skb);
 369
 370         oz_hdr->last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
 371 }
 372
 373 /*
 374  * Context: softirq
 375  */
 376 int oz_prepare_frame(struct oz_pd *pd, int empty)
 377 {
 378         struct oz_tx_frame *f;
 379
 380         if ((pd->mode & OZ_MODE_MASK) != OZ_MODE_TRIGGERED)
 381                 return -1;
 382         if (pd->nb_queued_frames >= OZ_MAX_QUEUED_FRAMES)
 383                 return -1;
 384         if (!empty && !oz_are_elts_available(&pd->elt_buff))
 385                 return -1;
 386         f = oz_tx_frame_alloc(pd);
 387         if (f == NULL)
 388                 return -1;
 389         f->skb = NULL;
 390         f->hdr.control =
 391                 (OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT) | OZ_F_ACK_REQUESTED;
 392         ++pd->last_tx_pkt_num;
 393         put_unaligned(cpu_to_le32(pd->last_tx_pkt_num), &f->hdr.pkt_num);
 394         if (empty == 0) {
 395                 oz_select_elts_for_tx(&pd->elt_buff, 0, &f->total_size,
 396                         pd->max_tx_size, &f->elt_list);
 397         }
 398         spin_lock(&pd->tx_frame_lock);
 399         list_add_tail(&f->link, &pd->tx_queue);
 400         pd->nb_queued_frames++;
 401         spin_unlock(&pd->tx_frame_lock);
 402         return 0;
 403 }
 404
 405 /*
 406  * Context: softirq-serialized
 407  */
 408 static struct sk_buff *oz_build_frame(struct oz_pd *pd, struct oz_tx_frame *f)
 409 {
 410         struct sk_buff *skb;
 411         struct net_device *dev = pd->net_dev;
 412         struct oz_hdr *oz_hdr;
 413         struct oz_elt *elt;
 414         struct oz_elt_info *ei;
 415
 416         /* Allocate skb with enough space for the lower layers as well
 417          * as the space we need.
 418          */
 419         skb = alloc_skb(f->total_size + OZ_ALLOCATED_SPACE(dev), GFP_ATOMIC);
 420         if (skb == NULL)
 421                 return NULL;
 422         /* Reserve the head room for lower layers.
 423          */
 424         skb_reserve(skb, LL_RESERVED_SPACE(dev));
 425         skb_reset_network_header(skb);
 426         skb->dev = dev;
 427         skb->protocol = htons(OZ_ETHERTYPE);
 428         if (dev_hard_header(skb, dev, OZ_ETHERTYPE, pd->mac_addr,
 429                 dev->dev_addr, skb->len) < 0)
 430                 goto fail;
 431         /* Push the tail to the end of the area we are going to copy to.
 432          */
 433         oz_hdr = (struct oz_hdr *)skb_put(skb, f->total_size);
 434         f->hdr.last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
 435         memcpy(oz_hdr, &f->hdr, sizeof(struct oz_hdr));
 436         /* Copy the elements into the frame body.
 437          */
 438         elt = (struct oz_elt *)(oz_hdr+1);
 439         list_for_each_entry(ei, &f->elt_list, link) {
 440                 memcpy(elt, ei->data, ei->length);
 441                 elt = oz_next_elt(elt);
 442         }
 443         return skb;
 444 fail:
 445         kfree_skb(skb);
 446         return NULL;
 447 }
 448
 449 /*
 450  * Context: softirq or process
 451  */
 452 static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f)
 453 {
 454         struct oz_elt_info *ei, *n;
 455
 456         list_for_each_entry_safe(ei, n, &f->elt_list, link) {
 457                 list_del_init(&ei->link);
 458                 if (ei->callback)
 459                         ei->callback(pd, ei->context);
 460                 spin_lock_bh(&pd->elt_buff.lock);
 461                 oz_elt_info_free(&pd->elt_buff, ei);
 462                 spin_unlock_bh(&pd->elt_buff.lock);
 463         }
 464         oz_tx_frame_free(pd, f);
 465 }
 466
 467 /*
 468  * Context: softirq-serialized
 469  */
 470 static int oz_send_next_queued_frame(struct oz_pd *pd, int more_data)
 471 {
 472         struct sk_buff *skb;
 473         struct oz_tx_frame *f;
 474         struct list_head *e;
 475
 476         spin_lock(&pd->tx_frame_lock);
 477         e = pd->last_sent_frame->next;
 478         if (e == &pd->tx_queue) {
 479                 spin_unlock(&pd->tx_frame_lock);
 480                 return -1;
 481         }
 482         f = list_entry(e, struct oz_tx_frame, link);
 483
 484         if (f->skb != NULL) {
 485                 skb = f->skb;
 486                 oz_tx_isoc_free(pd, f);
 487                 spin_unlock(&pd->tx_frame_lock);
 488                 if (more_data)
 489                         oz_set_more_bit(skb);
 490                 oz_set_last_pkt_nb(pd, skb);
 491                 if ((int)atomic_read(&g_submitted_isoc) <
 492                                                         OZ_MAX_SUBMITTED_ISOC) {
 493                         if (dev_queue_xmit(skb) < 0) {
 494                                 oz_dbg(TX_FRAMES, "Dropping ISOC Frame\n");
 495                                 return -1;
 496                         }
 497                         atomic_inc(&g_submitted_isoc);
 498                         oz_dbg(TX_FRAMES, "Sending ISOC Frame, nb_isoc= %d\n",
 499                                pd->nb_queued_isoc_frames);
 500                         return 0;
 501                 }
 502                 kfree_skb(skb);
 503                 oz_dbg(TX_FRAMES, "Dropping ISOC Frame>\n");
 504                 return -1;
 505         }
 506
 507         pd->last_sent_frame = e;
 508         skb = oz_build_frame(pd, f);
 509         spin_unlock(&pd->tx_frame_lock);
 510         if (!skb)
 511                 return -1;
 512         if (more_data)
 513                 oz_set_more_bit(skb);
 514         oz_dbg(TX_FRAMES, "TX frame PN=0x%x\n", f->hdr.pkt_num);
 515         if (dev_queue_xmit(skb) < 0)
 516                 return -1;
 517
 518         return 0;
 519 }
 520
 521 /*
 522  * Context: softirq-serialized
 523  */
 524 void oz_send_queued_frames(struct oz_pd *pd, int backlog)
 525 {
 526         while (oz_prepare_frame(pd, 0) >= 0)
 527                 backlog++;
 528
 529         switch (pd->mode & (OZ_F_ISOC_NO_ELTS | OZ_F_ISOC_ANYTIME)) {
 530
 531                 case OZ_F_ISOC_NO_ELTS: {
 532                         backlog += pd->nb_queued_isoc_frames;
 533                         if (backlog <= 0)
 534                                 goto out;
 535                         if (backlog > OZ_MAX_SUBMITTED_ISOC)
 536                                 backlog = OZ_MAX_SUBMITTED_ISOC;
 537                         break;
 538                 }
 539                 case OZ_NO_ELTS_ANYTIME: {
 540                         if ((backlog <= 0) && (pd->isoc_sent == 0))
 541                                 goto out;
 542                         break;
 543                 }
 544                 default: {
 545                         if (backlog <= 0)
 546                                 goto out;
 547                         break;
 548                 }
 549         }
 550         while (backlog--) {
 551                 if (oz_send_next_queued_frame(pd, backlog) < 0)
 552                         break;
 553         }
 554         return;
 555
 556 out:    oz_prepare_frame(pd, 1);
 557         oz_send_next_queued_frame(pd, 0);
 558 }
 559
 560 /*
 561  * Context: softirq
 562  */
 563 static int oz_send_isoc_frame(struct oz_pd *pd)
 564 {
 565         struct sk_buff *skb;
 566         struct net_device *dev = pd->net_dev;
 567         struct oz_hdr *oz_hdr;
 568         struct oz_elt *elt;
 569         struct oz_elt_info *ei;
 570         LIST_HEAD(list);
 571         int total_size = sizeof(struct oz_hdr);
 572
 573         oz_select_elts_for_tx(&pd->elt_buff, 1, &total_size,
 574                 pd->max_tx_size, &list);
 575         if (list_empty(&list))
 576                 return 0;
 577         skb = alloc_skb(total_size + OZ_ALLOCATED_SPACE(dev), GFP_ATOMIC);
 578         if (skb == NULL) {
 579                 oz_dbg(ON, "Cannot alloc skb\n");
 580                 oz_elt_info_free_chain(&pd->elt_buff, &list);
 581                 return -1;
 582         }
 583         skb_reserve(skb, LL_RESERVED_SPACE(dev));
 584         skb_reset_network_header(skb);
 585         skb->dev = dev;
 586         skb->protocol = htons(OZ_ETHERTYPE);
 587         if (dev_hard_header(skb, dev, OZ_ETHERTYPE, pd->mac_addr,
 588                 dev->dev_addr, skb->len) < 0) {
 589                 kfree_skb(skb);
 590                 return -1;
 591         }
 592         oz_hdr = (struct oz_hdr *)skb_put(skb, total_size);
 593         oz_hdr->control = (OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT) | OZ_F_ISOC;
 594         oz_hdr->last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
 595         elt = (struct oz_elt *)(oz_hdr+1);
 596
 597         list_for_each_entry(ei, &list, link) {
 598                 memcpy(elt, ei->data, ei->length);
 599                 elt = oz_next_elt(elt);
 600         }
 601         dev_queue_xmit(skb);
 602         oz_elt_info_free_chain(&pd->elt_buff, &list);
 603         return 0;
 604 }
 605
 606 /*
 607  * Context: softirq-serialized
 608  */
 609 void oz_retire_tx_frames(struct oz_pd *pd, u8 lpn)
 610 {
 611         struct oz_tx_frame *f, *tmp = NULL;
 612         u8 diff;
 613         u32 pkt_num;
 614
 615         LIST_HEAD(list);
 616
 617         spin_lock(&pd->tx_frame_lock);
 618         list_for_each_entry(f, &pd->tx_queue, link) {
 619                 pkt_num = le32_to_cpu(get_unaligned(&f->hdr.pkt_num));
 620                 diff = (lpn - (pkt_num & OZ_LAST_PN_MASK)) & OZ_LAST_PN_MASK;
 621                 if ((diff > OZ_LAST_PN_HALF_CYCLE) || (pkt_num == 0))
 622                         break;
 623                 oz_dbg(TX_FRAMES, "Releasing pkt_num= %u, nb= %d\n",
 624                        pkt_num, pd->nb_queued_frames);
 625                 tmp = f;
 626                 pd->nb_queued_frames--;
 627         }
 628         if (tmp)
 629                 list_cut_position(&list, &pd->tx_queue, &tmp->link);
 630         pd->last_sent_frame = &pd->tx_queue;
 631         spin_unlock(&pd->tx_frame_lock);
 632
 633         list_for_each_entry_safe(f, tmp, &list, link)
 634                 oz_retire_frame(pd, f);
 635 }
 636
 637 /*
 638  * Precondition: stream_lock must be held.
 639  * Context: softirq
 640  */
 641 static struct oz_isoc_stream *pd_stream_find(struct oz_pd *pd, u8 ep_num)
 642 {
 643         struct oz_isoc_stream *st;
 644
 645         list_for_each_entry(st, &pd->stream_list, link) {
 646                 if (st->ep_num == ep_num)
 647                         return st;
 648         }
 649         return NULL;
 650 }
 651
 652 /*
 653  * Context: softirq
 654  */
 655 int oz_isoc_stream_create(struct oz_pd *pd, u8 ep_num)
 656 {
 657         struct oz_isoc_stream *st;
 658
 659         st = kzalloc(sizeof(struct oz_isoc_stream), GFP_ATOMIC);
 660         if (!st)
 661                 return -ENOMEM;
 662         st->ep_num = ep_num;
 663         spin_lock_bh(&pd->stream_lock);
 664         if (!pd_stream_find(pd, ep_num)) {
 665                 list_add(&st->link, &pd->stream_list);
 666                 st = NULL;
 667         }
 668         spin_unlock_bh(&pd->stream_lock);
 669         kfree(st);
 670         return 0;
 671 }
 672
 673 /*
 674  * Context: softirq or process
 675  */
 676 static void oz_isoc_stream_free(struct oz_isoc_stream *st)
 677 {
 678         kfree_skb(st->skb);
 679         kfree(st);
 680 }
 681
 682 /*
 683  * Context: softirq
 684  */
 685 int oz_isoc_stream_delete(struct oz_pd *pd, u8 ep_num)
 686 {
 687         struct oz_isoc_stream *st;
 688
 689         spin_lock_bh(&pd->stream_lock);
 690         st = pd_stream_find(pd, ep_num);
 691         if (st)
 692                 list_del(&st->link);
 693         spin_unlock_bh(&pd->stream_lock);
 694         if (st)
 695                 oz_isoc_stream_free(st);
 696         return 0;
 697 }
 698
 699 /*
 700  * Context: any
 701  */
 702 static void oz_isoc_destructor(struct sk_buff *skb)
 703 {
 704         atomic_dec(&g_submitted_isoc);
 705 }
 706
 707 /*
 708  * Context: softirq
 709  */
 710 int oz_send_isoc_unit(struct oz_pd *pd, u8 ep_num, const u8 *data, int len)
 711 {
 712         struct net_device *dev = pd->net_dev;
 713         struct oz_isoc_stream *st;
 714         u8 nb_units = 0;
 715         struct sk_buff *skb = NULL;
 716         struct oz_hdr *oz_hdr = NULL;
 717         int size = 0;
 718
 719         spin_lock_bh(&pd->stream_lock);
 720         st = pd_stream_find(pd, ep_num);
 721         if (st) {
 722                 skb = st->skb;
 723                 st->skb = NULL;
 724                 nb_units = st->nb_units;
 725                 st->nb_units = 0;
 726                 oz_hdr = st->oz_hdr;
 727                 size = st->size;
 728         }
 729         spin_unlock_bh(&pd->stream_lock);
 730         if (!st)
 731                 return 0;
 732         if (!skb) {
 733                 /* Allocate enough space for max size frame. */
 734                 skb = alloc_skb(pd->max_tx_size + OZ_ALLOCATED_SPACE(dev),
 735                                 GFP_ATOMIC);
 736                 if (skb == NULL)
 737                         return 0;
 738                 /* Reserve the head room for lower layers. */
 739                 skb_reserve(skb, LL_RESERVED_SPACE(dev));
 740                 skb_reset_network_header(skb);
 741                 skb->dev = dev;
 742                 skb->protocol = htons(OZ_ETHERTYPE);
 743                 /* For audio packet set priority to AC_VO */
 744                 skb->priority = 0x7;
 745                 size = sizeof(struct oz_hdr) + sizeof(struct oz_isoc_large);
 746                 oz_hdr = (struct oz_hdr *)skb_put(skb, size);
 747         }
 748         memcpy(skb_put(skb, len), data, len);
 749         size += len;
 750         if (++nb_units < pd->ms_per_isoc) {
 751                 spin_lock_bh(&pd->stream_lock);
 752                 st->skb = skb;
 753                 st->nb_units = nb_units;
 754                 st->oz_hdr = oz_hdr;
 755                 st->size = size;
 756                 spin_unlock_bh(&pd->stream_lock);
 757         } else {
 758                 struct oz_hdr oz;
 759                 struct oz_isoc_large iso;
 760
 761                 spin_lock_bh(&pd->stream_lock);
 762                 iso.frame_number = st->frame_num;
 763                 st->frame_num += nb_units;
 764                 spin_unlock_bh(&pd->stream_lock);
 765                 oz.control =
 766                         (OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT) | OZ_F_ISOC;
 767                 oz.last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
 768                 oz.pkt_num = 0;
 769                 iso.endpoint = ep_num;
 770                 iso.format = OZ_DATA_F_ISOC_LARGE;
 771                 iso.ms_data = nb_units;
 772                 memcpy(oz_hdr, &oz, sizeof(oz));
 773                 memcpy(oz_hdr+1, &iso, sizeof(iso));
 774                 if (dev_hard_header(skb, dev, OZ_ETHERTYPE, pd->mac_addr,
 775                                 dev->dev_addr, skb->len) < 0)
 776                         goto out;
 777
 778                 skb->destructor = oz_isoc_destructor;
 779                 /*Queue for Xmit if mode is not ANYTIME*/
 780                 if (!(pd->mode & OZ_F_ISOC_ANYTIME)) {
 781                         struct oz_tx_frame *isoc_unit = NULL;
 782                         int nb = pd->nb_queued_isoc_frames;
 783
 784                         if (nb >= pd->isoc_latency) {
 785                                 struct oz_tx_frame *f;
 786
 787                                 oz_dbg(TX_FRAMES, "Dropping ISOC Unit nb= %d\n",
 788                                        nb);
 789                                 spin_lock(&pd->tx_frame_lock);
 790                                 list_for_each_entry(f, &pd->tx_queue, link) {
 791                                         if (f->skb != NULL) {
 792                                                 oz_tx_isoc_free(pd, f);
 793                                                 break;
 794                                         }
 795                                 }
 796                                 spin_unlock(&pd->tx_frame_lock);
 797                         }
 798                         isoc_unit = oz_tx_frame_alloc(pd);
 799                         if (isoc_unit == NULL)
 800                                 goto out;
 801                         isoc_unit->hdr = oz;
 802                         isoc_unit->skb = skb;
 803                         spin_lock_bh(&pd->tx_frame_lock);
 804                         list_add_tail(&isoc_unit->link, &pd->tx_queue);
 805                         pd->nb_queued_isoc_frames++;
 806                         spin_unlock_bh(&pd->tx_frame_lock);
 807                         oz_dbg(TX_FRAMES,
 808                                "Added ISOC Frame to Tx Queue isoc_nb= %d, nb= %d\n",
 809                                pd->nb_queued_isoc_frames, pd->nb_queued_frames);
 810                         return 0;
 811                 }
 812
 813                 /*In ANYTIME mode Xmit unit immediately*/
 814                 if (atomic_read(&g_submitted_isoc) < OZ_MAX_SUBMITTED_ISOC) {
 815                         atomic_inc(&g_submitted_isoc);
 816                         if (dev_queue_xmit(skb) < 0)
 817                                 return -1;
 818                         return 0;
 819                 }
 820
 821 out:    kfree_skb(skb);
 822         return -1;
 823
 824         }
 825         return 0;
 826 }
 827
 828 /*
 829  * Context: process
 830  */
 831 void oz_apps_init(void)
 832 {
 833         int i;
 834
 835         for (i = 0; i < OZ_NB_APPS; i++) {
 836                 if (g_app_if[i].init)
 837                         g_app_if[i].init();
 838         }
 839 }
 840
 841 /*
 842  * Context: process
 843  */
 844 void oz_apps_term(void)
 845 {
 846         int i;
 847
 848         /* Terminate all the apps. */
 849         for (i = 0; i < OZ_NB_APPS; i++) {
 850                 if (g_app_if[i].term)
 851                         g_app_if[i].term();
 852         }
 853 }
 854
 855 /*
 856  * Context: softirq-serialized
 857  */
 858 void oz_handle_app_elt(struct oz_pd *pd, u8 app_id, struct oz_elt *elt)
 859 {
 860         if (app_id < OZ_NB_APPS && g_app_if[app_id].rx)
 861                 g_app_if[app_id].rx(pd, elt);
 862 }
 863
 864 /*
 865  * Context: softirq or process
 866  */
 867 void oz_pd_indicate_farewells(struct oz_pd *pd)
 868 {
 869         struct oz_farewell *f;
 870         const struct oz_app_if *ai = &g_app_if[OZ_APPID_USB];
 871
 872         while (1) {
 873                 spin_lock_bh(&g_polling_lock);
 874                 if (list_empty(&pd->farewell_list)) {
 875                         spin_unlock_bh(&g_polling_lock);
 876                         break;
 877                 }
 878                 f = list_first_entry(&pd->farewell_list,
 879                                 struct oz_farewell, link);
 880                 list_del(&f->link);
 881                 spin_unlock_bh(&g_polling_lock);
 882                 if (ai->farewell)
 883                         ai->farewell(pd, f->ep_num, f->report, f->len);
 884                 kfree(f);
 885         }
 886 }