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