drivers/staging/ozwpan/ozeltbuf.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/netdevice.h>
   9 #include "ozconfig.h"
  10 #include "ozprotocol.h"
  11 #include "ozeltbuf.h"
  12 #include "ozpd.h"
  13 #include "oztrace.h"
  14 /*------------------------------------------------------------------------------
  15  */
  16 #define OZ_ELT_INFO_MAGIC_USED  0x35791057
  17 #define OZ_ELT_INFO_MAGIC_FREE  0x78940102
  18 /*------------------------------------------------------------------------------
  19  * Context: softirq-serialized
  20  */
  21 int oz_elt_buf_init(struct oz_elt_buf *buf)
  22 {
  23         memset(buf, 0, sizeof(struct oz_elt_buf));
  24         INIT_LIST_HEAD(&buf->stream_list);
  25         INIT_LIST_HEAD(&buf->order_list);
  26         INIT_LIST_HEAD(&buf->isoc_list);
  27         buf->max_free_elts = 32;
  28         spin_lock_init(&buf->lock);
  29         return 0;
  30 }
  31 /*------------------------------------------------------------------------------
  32  * Context: softirq or process
  33  */
  34 void oz_elt_buf_term(struct oz_elt_buf *buf)
  35 {
  36         struct list_head *e;
  37         int i;
  38         /* Free any elements in the order or isoc lists. */
  39         for (i = 0; i < 2; i++) {
  40                 struct list_head *list;
  41                 if (i)
  42                         list = &buf->order_list;
  43                 else
  44                         list = &buf->isoc_list;
  45                 e = list->next;
  46                 while (e != list) {
  47                         struct oz_elt_info *ei =
  48                                 container_of(e, struct oz_elt_info, link_order);
  49                         e = e->next;
  50                         kfree(ei);
  51                 }
  52         }
  53         /* Free any elelment in the pool. */
  54         while (buf->elt_pool) {
  55                 struct oz_elt_info *ei =
  56                         container_of(buf->elt_pool, struct oz_elt_info, link);
  57                 buf->elt_pool = buf->elt_pool->next;
  58                 kfree(ei);
  59         }
  60         buf->free_elts = 0;
  61 }
  62 /*------------------------------------------------------------------------------
  63  * Context: softirq or process
  64  */
  65 struct oz_elt_info *oz_elt_info_alloc(struct oz_elt_buf *buf)
  66 {
  67         struct oz_elt_info *ei = 0;
  68         spin_lock_bh(&buf->lock);
  69         if (buf->free_elts && buf->elt_pool) {
  70                 ei = container_of(buf->elt_pool, struct oz_elt_info, link);
  71                 buf->elt_pool = ei->link.next;
  72                 buf->free_elts--;
  73                 spin_unlock_bh(&buf->lock);
  74                 if (ei->magic != OZ_ELT_INFO_MAGIC_FREE) {
  75                         oz_trace("oz_elt_info_alloc: ei with bad magic: 0x%x\n",
  76                                 ei->magic);
  77                 }
  78         } else {
  79                 spin_unlock_bh(&buf->lock);
  80                 ei = kmalloc(sizeof(struct oz_elt_info), GFP_ATOMIC);
  81         }
  82         if (ei) {
  83                 ei->flags = 0;
  84                 ei->app_id = 0;
  85                 ei->callback = 0;
  86                 ei->context = 0;
  87                 ei->stream = 0;
  88                 ei->magic = OZ_ELT_INFO_MAGIC_USED;
  89                 INIT_LIST_HEAD(&ei->link);
  90                 INIT_LIST_HEAD(&ei->link_order);
  91         }
  92         return ei;
  93 }
  94 /*------------------------------------------------------------------------------
  95  * Precondition: oz_elt_buf.lock must be held.
  96  * Context: softirq or process
  97  */
  98 void oz_elt_info_free(struct oz_elt_buf *buf, struct oz_elt_info *ei)
  99 {
 100         if (ei) {
 101                 if (ei->magic == OZ_ELT_INFO_MAGIC_USED) {
 102                         buf->free_elts++;
 103                         ei->link.next = buf->elt_pool;
 104                         buf->elt_pool = &ei->link;
 105                         ei->magic = OZ_ELT_INFO_MAGIC_FREE;
 106                 } else {
 107                         oz_trace("oz_elt_info_free: bad magic ei: %p"
 108                                 " magic: 0x%x\n",
 109                                 ei, ei->magic);
 110                 }
 111         }
 112 }
 113 /*------------------------------------------------------------------------------
 114  * Context: softirq
 115  */
 116 void oz_elt_info_free_chain(struct oz_elt_buf *buf, struct list_head *list)
 117 {
 118         struct list_head *e;
 119         e = list->next;
 120         spin_lock_bh(&buf->lock);
 121         while (e != list) {
 122                 struct oz_elt_info *ei;
 123                 ei = container_of(e, struct oz_elt_info, link);
 124                 e = e->next;
 125                 oz_elt_info_free(buf, ei);
 126         }
 127         spin_unlock_bh(&buf->lock);
 128 }
 129 /*------------------------------------------------------------------------------
 130  */
 131 int oz_elt_stream_create(struct oz_elt_buf *buf, u8 id, int max_buf_count)
 132 {
 133         struct oz_elt_stream *st;
 134
 135         oz_trace("oz_elt_stream_create(0x%x)\n", id);
 136
 137         st = kzalloc(sizeof(struct oz_elt_stream), GFP_ATOMIC | __GFP_ZERO);
 138         if (st == 0)
 139                 return -ENOMEM;
 140         atomic_set(&st->ref_count, 1);
 141         st->id = id;
 142         st->max_buf_count = max_buf_count;
 143         INIT_LIST_HEAD(&st->elt_list);
 144         spin_lock_bh(&buf->lock);
 145         list_add_tail(&st->link, &buf->stream_list);
 146         spin_unlock_bh(&buf->lock);
 147         return 0;
 148 }
 149 /*------------------------------------------------------------------------------
 150  */
 151 int oz_elt_stream_delete(struct oz_elt_buf *buf, u8 id)
 152 {
 153         struct list_head *e;
 154         struct oz_elt_stream *st;
 155         oz_trace("oz_elt_stream_delete(0x%x)\n", id);
 156         spin_lock_bh(&buf->lock);
 157         e = buf->stream_list.next;
 158         while (e != &buf->stream_list) {
 159                 st = container_of(e, struct oz_elt_stream, link);
 160                 if (st->id == id) {
 161                         list_del(e);
 162                         break;
 163                 }
 164                 st = 0;
 165         }
 166         if (!st) {
 167                 spin_unlock_bh(&buf->lock);
 168                 return -1;
 169         }
 170         e = st->elt_list.next;
 171         while (e != &st->elt_list) {
 172                 struct oz_elt_info *ei =
 173                         container_of(e, struct oz_elt_info, link);
 174                 e = e->next;
 175                 list_del_init(&ei->link);
 176                 list_del_init(&ei->link_order);
 177                 st->buf_count -= ei->length;
 178                 oz_trace2(OZ_TRACE_STREAM, "Stream down: %d  %d %d\n",
 179                         st->buf_count,
 180                         ei->length, atomic_read(&st->ref_count));
 181                 oz_elt_stream_put(st);
 182                 oz_elt_info_free(buf, ei);
 183         }
 184         spin_unlock_bh(&buf->lock);
 185         oz_elt_stream_put(st);
 186         return 0;
 187 }
 188 /*------------------------------------------------------------------------------
 189  */
 190 void oz_elt_stream_get(struct oz_elt_stream *st)
 191 {
 192         atomic_inc(&st->ref_count);
 193 }
 194 /*------------------------------------------------------------------------------
 195  */
 196 void oz_elt_stream_put(struct oz_elt_stream *st)
 197 {
 198         if (atomic_dec_and_test(&st->ref_count)) {
 199                 oz_trace("Stream destroyed\n");
 200                 kfree(st);
 201         }
 202 }
 203 /*------------------------------------------------------------------------------
 204  * Precondition: Element buffer lock must be held.
 205  * If this function fails the caller is responsible for deallocating the elt
 206  * info structure.
 207  */
 208 int oz_queue_elt_info(struct oz_elt_buf *buf, u8 isoc, u8 id,
 209         struct oz_elt_info *ei)
 210 {
 211         struct oz_elt_stream *st = 0;
 212         struct list_head *e;
 213         if (id) {
 214                 list_for_each(e, &buf->stream_list) {
 215                         st = container_of(e, struct oz_elt_stream, link);
 216                         if (st->id == id)
 217                                 break;
 218                 }
 219                 if (e == &buf->stream_list) {
 220                         /* Stream specified but stream not known so fail.
 221                          * Caller deallocates element info. */
 222                         return -1;
 223                 }
 224         }
 225         if (st) {
 226                 /* If this is an ISOC fixed element that needs a frame number
 227                  * then insert that now. Earlier we stored the unit count in
 228                  * this field.
 229                  */
 230                 struct oz_isoc_fixed *body = (struct oz_isoc_fixed *)
 231                         &ei->data[sizeof(struct oz_elt)];
 232                 if ((body->app_id == OZ_APPID_USB) && (body->type
 233                         == OZ_USB_ENDPOINT_DATA) &&
 234                         (body->format == OZ_DATA_F_ISOC_FIXED)) {
 235                         u8 unit_count = body->frame_number;
 236                         body->frame_number = st->frame_number;
 237                         st->frame_number += unit_count;
 238                 }
 239                 /* Claim stream and update accounts */
 240                 oz_elt_stream_get(st);
 241                 ei->stream = st;
 242                 st->buf_count += ei->length;
 243                 /* Add to list in stream. */
 244                 list_add_tail(&ei->link, &st->elt_list);
 245                 oz_trace2(OZ_TRACE_STREAM, "Stream up: %d  %d\n",
 246                         st->buf_count, ei->length);
 247                 /* Check if we have too much buffered for this stream. If so
 248                  * start dropping elements until we are back in bounds.
 249                  */
 250                 while ((st->buf_count > st->max_buf_count) &&
 251                         !list_empty(&st->elt_list)) {
 252                         struct oz_elt_info *ei2 =
 253                                 list_first_entry(&st->elt_list,
 254                                         struct oz_elt_info, link);
 255                         list_del_init(&ei2->link);
 256                         list_del_init(&ei2->link_order);
 257                         st->buf_count -= ei2->length;
 258                         oz_elt_info_free(buf, ei2);
 259                         oz_elt_stream_put(st);
 260                 }
 261         }
 262         list_add_tail(&ei->link_order, isoc ?
 263                 &buf->isoc_list : &buf->order_list);
 264         return 0;
 265 }
 266 /*------------------------------------------------------------------------------
 267  */
 268 int oz_select_elts_for_tx(struct oz_elt_buf *buf, u8 isoc, unsigned *len,
 269                 unsigned max_len, struct list_head *list)
 270 {
 271         int count = 0;
 272         struct list_head *e;
 273         struct list_head *el;
 274         struct oz_elt_info *ei;
 275         spin_lock_bh(&buf->lock);
 276         if (isoc)
 277                 el = &buf->isoc_list;
 278         else
 279                 el = &buf->order_list;
 280         e = el->next;
 281         while (e != el) {
 282                 struct oz_app_hdr *app_hdr;
 283                 ei = container_of(e, struct oz_elt_info, link_order);
 284                 e = e->next;
 285                 if ((*len + ei->length) <= max_len) {
 286                         app_hdr = (struct oz_app_hdr *)
 287                                 &ei->data[sizeof(struct oz_elt)];
 288                         app_hdr->elt_seq_num = buf->tx_seq_num[ei->app_id]++;
 289                         if (buf->tx_seq_num[ei->app_id] == 0)
 290                                 buf->tx_seq_num[ei->app_id] = 1;
 291                         *len += ei->length;
 292                         list_del(&ei->link);
 293                         list_del(&ei->link_order);
 294                         if (ei->stream) {
 295                                 ei->stream->buf_count -= ei->length;
 296                                 oz_trace2(OZ_TRACE_STREAM,
 297                                         "Stream down: %d  %d\n",
 298                                         ei->stream->buf_count, ei->length);
 299                                 oz_elt_stream_put(ei->stream);
 300                                 ei->stream = 0;
 301                         }
 302                         INIT_LIST_HEAD(&ei->link_order);
 303                         list_add_tail(&ei->link, list);
 304                         count++;
 305                 } else {
 306                         break;
 307                 }
 308         }
 309         spin_unlock_bh(&buf->lock);
 310         return count;
 311 }
 312 /*------------------------------------------------------------------------------
 313  */
 314 int oz_are_elts_available(struct oz_elt_buf *buf)
 315 {
 316         return buf->order_list.next != &buf->order_list;
 317 }
 318 /*------------------------------------------------------------------------------
 319  */
 320 void oz_trim_elt_pool(struct oz_elt_buf *buf)
 321 {
 322         struct list_head *free = 0;
 323         struct list_head *e;
 324         spin_lock_bh(&buf->lock);
 325         while (buf->free_elts > buf->max_free_elts) {
 326                 e = buf->elt_pool;
 327                 buf->elt_pool = e->next;
 328                 e->next = free;
 329                 free = e;
 330                 buf->free_elts--;
 331         }
 332         spin_unlock_bh(&buf->lock);
 333         while (free) {
 334                 struct oz_elt_info *ei =
 335                         container_of(free, struct oz_elt_info, link);
 336                 free = free->next;
 337                 kfree(ei);
 338         }
 339 }