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 int 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         return 0;
  28 }
  29
  30 /*
  31  * Context: softirq or process
  32  */
  33 void oz_elt_buf_term(struct oz_elt_buf *buf)
  34 {
  35         struct list_head *e;
  36         int i;
  37
  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 /*
  64  * Context: softirq or process
  65  */
  66 struct oz_elt_info *oz_elt_info_alloc(struct oz_elt_buf *buf)
  67 {
  68         struct oz_elt_info *ei;
  69
  70         spin_lock_bh(&buf->lock);
  71         if (buf->free_elts && buf->elt_pool) {
  72                 ei = container_of(buf->elt_pool, struct oz_elt_info, link);
  73                 buf->elt_pool = ei->link.next;
  74                 buf->free_elts--;
  75                 spin_unlock_bh(&buf->lock);
  76                 if (ei->magic != OZ_ELT_INFO_MAGIC_FREE) {
  77                         oz_dbg(ON, "%s: ei with bad magic: 0x%x\n",
  78                                __func__, ei->magic);
  79                 }
  80         } else {
  81                 spin_unlock_bh(&buf->lock);
  82                 ei = kmalloc(sizeof(struct oz_elt_info), GFP_ATOMIC);
  83         }
  84         if (ei) {
  85                 ei->flags = 0;
  86                 ei->app_id = 0;
  87                 ei->callback = NULL;
  88                 ei->context = 0;
  89                 ei->stream = NULL;
  90                 ei->magic = OZ_ELT_INFO_MAGIC_USED;
  91                 INIT_LIST_HEAD(&ei->link);
  92                 INIT_LIST_HEAD(&ei->link_order);
  93         }
  94         return ei;
  95 }
  96
  97 /*
  98  * Precondition: oz_elt_buf.lock must be held.
  99  * Context: softirq or process
 100  */
 101 void oz_elt_info_free(struct oz_elt_buf *buf, struct oz_elt_info *ei)
 102 {
 103         if (ei) {
 104                 if (ei->magic == OZ_ELT_INFO_MAGIC_USED) {
 105                         buf->free_elts++;
 106                         ei->link.next = buf->elt_pool;
 107                         buf->elt_pool = &ei->link;
 108                         ei->magic = OZ_ELT_INFO_MAGIC_FREE;
 109                 } else {
 110                         oz_dbg(ON, "%s: bad magic ei: %p magic: 0x%x\n",
 111                                __func__, ei, ei->magic);
 112                 }
 113         }
 114 }
 115
 116 /*------------------------------------------------------------------------------
 117  * Context: softirq
 118  */
 119 void oz_elt_info_free_chain(struct oz_elt_buf *buf, struct list_head *list)
 120 {
 121         struct list_head *e;
 122
 123         e = list->next;
 124         spin_lock_bh(&buf->lock);
 125         while (e != list) {
 126                 struct oz_elt_info *ei;
 127                 ei = container_of(e, struct oz_elt_info, link);
 128                 e = e->next;
 129                 oz_elt_info_free(buf, ei);
 130         }
 131         spin_unlock_bh(&buf->lock);
 132 }
 133
 134 int oz_elt_stream_create(struct oz_elt_buf *buf, u8 id, int max_buf_count)
 135 {
 136         struct oz_elt_stream *st;
 137
 138         oz_dbg(ON, "%s: (0x%x)\n", __func__, id);
 139
 140         st = kzalloc(sizeof(struct oz_elt_stream), GFP_ATOMIC);
 141         if (st == NULL)
 142                 return -ENOMEM;
 143         atomic_set(&st->ref_count, 1);
 144         st->id = id;
 145         st->max_buf_count = max_buf_count;
 146         INIT_LIST_HEAD(&st->elt_list);
 147         spin_lock_bh(&buf->lock);
 148         list_add_tail(&st->link, &buf->stream_list);
 149         spin_unlock_bh(&buf->lock);
 150         return 0;
 151 }
 152
 153 int oz_elt_stream_delete(struct oz_elt_buf *buf, u8 id)
 154 {
 155         struct list_head *e;
 156         struct oz_elt_stream *st = NULL;
 157
 158         oz_dbg(ON, "%s: (0x%x)\n", __func__, id);
 159         spin_lock_bh(&buf->lock);
 160         e = buf->stream_list.next;
 161         while (e != &buf->stream_list) {
 162                 st = container_of(e, struct oz_elt_stream, link);
 163                 if (st->id == id) {
 164                         list_del(e);
 165                         break;
 166                 }
 167                 st = NULL;
 168         }
 169         if (!st) {
 170                 spin_unlock_bh(&buf->lock);
 171                 return -1;
 172         }
 173         e = st->elt_list.next;
 174         while (e != &st->elt_list) {
 175                 struct oz_elt_info *ei =
 176                         container_of(e, struct oz_elt_info, link);
 177                 e = e->next;
 178                 list_del_init(&ei->link);
 179                 list_del_init(&ei->link_order);
 180                 st->buf_count -= ei->length;
 181                 oz_dbg(STREAM, "Stream down: %d %d %d\n",
 182                        st->buf_count, ei->length, atomic_read(&st->ref_count));
 183                 oz_elt_stream_put(st);
 184                 oz_elt_info_free(buf, ei);
 185         }
 186         spin_unlock_bh(&buf->lock);
 187         oz_elt_stream_put(st);
 188         return 0;
 189 }
 190
 191 void oz_elt_stream_get(struct oz_elt_stream *st)
 192 {
 193         atomic_inc(&st->ref_count);
 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_dbg(ON, "Stream destroyed\n");
 200                 kfree(st);
 201         }
 202 }
 203
 204 /*
 205  * Precondition: Element buffer lock must be held.
 206  * If this function fails the caller is responsible for deallocating the elt
 207  * info structure.
 208  */
 209 int oz_queue_elt_info(struct oz_elt_buf *buf, u8 isoc, u8 id,
 210         struct oz_elt_info *ei)
 211 {
 212         struct oz_elt_stream *st = NULL;
 213         struct list_head *e;
 214
 215         if (id) {
 216                 list_for_each(e, &buf->stream_list) {
 217                         st = container_of(e, struct oz_elt_stream, link);
 218                         if (st->id == id)
 219                                 break;
 220                 }
 221                 if (e == &buf->stream_list) {
 222                         /* Stream specified but stream not known so fail.
 223                          * Caller deallocates element info. */
 224                         return -1;
 225                 }
 226         }
 227         if (st) {
 228                 /* If this is an ISOC fixed element that needs a frame number
 229                  * then insert that now. Earlier we stored the unit count in
 230                  * this field.
 231                  */
 232                 struct oz_isoc_fixed *body = (struct oz_isoc_fixed *)
 233                         &ei->data[sizeof(struct oz_elt)];
 234                 if ((body->app_id == OZ_APPID_USB) && (body->type
 235                         == OZ_USB_ENDPOINT_DATA) &&
 236                         (body->format == OZ_DATA_F_ISOC_FIXED)) {
 237                         u8 unit_count = body->frame_number;
 238                         body->frame_number = st->frame_number;
 239                         st->frame_number += unit_count;
 240                 }
 241                 /* Claim stream and update accounts */
 242                 oz_elt_stream_get(st);
 243                 ei->stream = st;
 244                 st->buf_count += ei->length;
 245                 /* Add to list in stream. */
 246                 list_add_tail(&ei->link, &st->elt_list);
 247                 oz_dbg(STREAM, "Stream up: %d %d\n", st->buf_count, ei->length);
 248                 /* Check if we have too much buffered for this stream. If so
 249                  * start dropping elements until we are back in bounds.
 250                  */
 251                 while ((st->buf_count > st->max_buf_count) &&
 252                         !list_empty(&st->elt_list)) {
 253                         struct oz_elt_info *ei2 =
 254                                 list_first_entry(&st->elt_list,
 255                                         struct oz_elt_info, link);
 256                         list_del_init(&ei2->link);
 257                         list_del_init(&ei2->link_order);
 258                         st->buf_count -= ei2->length;
 259                         oz_elt_info_free(buf, ei2);
 260                         oz_elt_stream_put(st);
 261                 }
 262         }
 263         list_add_tail(&ei->link_order, isoc ?
 264                 &buf->isoc_list : &buf->order_list);
 265         return 0;
 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
 276         spin_lock_bh(&buf->lock);
 277         if (isoc)
 278                 el = &buf->isoc_list;
 279         else
 280                 el = &buf->order_list;
 281         e = el->next;
 282         while (e != el) {
 283                 struct oz_app_hdr *app_hdr;
 284                 ei = container_of(e, struct oz_elt_info, link_order);
 285                 e = e->next;
 286                 if ((*len + ei->length) <= max_len) {
 287                         app_hdr = (struct oz_app_hdr *)
 288                                 &ei->data[sizeof(struct oz_elt)];
 289                         app_hdr->elt_seq_num = buf->tx_seq_num[ei->app_id]++;
 290                         if (buf->tx_seq_num[ei->app_id] == 0)
 291                                 buf->tx_seq_num[ei->app_id] = 1;
 292                         *len += ei->length;
 293                         list_del(&ei->link);
 294                         list_del(&ei->link_order);
 295                         if (ei->stream) {
 296                                 ei->stream->buf_count -= ei->length;
 297                                 oz_dbg(STREAM, "Stream down: %d %d\n",
 298                                        ei->stream->buf_count, ei->length);
 299                                 oz_elt_stream_put(ei->stream);
 300                                 ei->stream = NULL;
 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 int oz_are_elts_available(struct oz_elt_buf *buf)
 314 {
 315         return buf->order_list.next != &buf->order_list;
 316 }
 317
 318 void oz_trim_elt_pool(struct oz_elt_buf *buf)
 319 {
 320         struct list_head *free = NULL;
 321         struct list_head *e;
 322
 323         spin_lock_bh(&buf->lock);
 324         while (buf->free_elts > buf->max_free_elts) {
 325                 e = buf->elt_pool;
 326                 buf->elt_pool = e->next;
 327                 e->next = free;
 328                 free = e;
 329                 buf->free_elts--;
 330         }
 331         spin_unlock_bh(&buf->lock);
 332         while (free) {
 333                 struct oz_elt_info *ei =
 334                         container_of(free, struct oz_elt_info, link);
 335                 free = free->next;
 336                 kfree(ei);
 337         }
 338 }