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