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 "ozdbg.h"
  10 #include "ozprotocol.h"
  11 #include "ozeltbuf.h"
  12 #include "ozpd.h"
  13
  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
  39         /* Free any elements in the order or isoc lists. */
  40         for (i = 0; i < 2; i++) {
  41                 struct list_head *list;
  42                 if (i)
  43                         list = &buf->order_list;
  44                 else
  45                         list = &buf->isoc_list;
  46                 e = list->next;
  47                 while (e != list) {
  48                         struct oz_elt_info *ei =
  49                                 container_of(e, struct oz_elt_info, link_order);
  50                         e = e->next;
  51                         kfree(ei);
  52                 }
  53         }
  54         /* Free any elelment in the pool. */
  55         while (buf->elt_pool) {
  56                 struct oz_elt_info *ei =
  57                         container_of(buf->elt_pool, struct oz_elt_info, link);
  58                 buf->elt_pool = buf->elt_pool->next;
  59                 kfree(ei);
  60         }
  61         buf->free_elts = 0;
  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 = NULL;
  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  * 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  * Context: softirq
 116  */
 117 void oz_elt_info_free_chain(struct oz_elt_buf *buf, struct list_head *list)
 118 {
 119         struct list_head *e;
 120
 121         e = list->next;
 122         spin_lock_bh(&buf->lock);
 123         while (e != list) {
 124                 struct oz_elt_info *ei;
 125                 ei = container_of(e, struct oz_elt_info, link);
 126                 e = e->next;
 127                 oz_elt_info_free(buf, ei);
 128         }
 129         spin_unlock_bh(&buf->lock);
 130 }
 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 | __GFP_ZERO);
 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  */
 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  */
 192 void oz_elt_stream_get(struct oz_elt_stream *st)
 193 {
 194         atomic_inc(&st->ref_count);
 195 }
 196 /*------------------------------------------------------------------------------
 197  */
 198 void oz_elt_stream_put(struct oz_elt_stream *st)
 199 {
 200         if (atomic_dec_and_test(&st->ref_count)) {
 201                 oz_dbg(ON, "Stream destroyed\n");
 202                 kfree(st);
 203         }
 204 }
 205 /*------------------------------------------------------------------------------
 206  * Precondition: Element buffer lock must be held.
 207  * If this function fails the caller is responsible for deallocating the elt
 208  * info structure.
 209  */
 210 int oz_queue_elt_info(struct oz_elt_buf *buf, u8 isoc, u8 id,
 211         struct oz_elt_info *ei)
 212 {
 213         struct oz_elt_stream *st = NULL;
 214         struct list_head *e;
 215
 216         if (id) {
 217                 list_for_each(e, &buf->stream_list) {
 218                         st = container_of(e, struct oz_elt_stream, link);
 219                         if (st->id == id)
 220                                 break;
 221                 }
 222                 if (e == &buf->stream_list) {
 223                         /* Stream specified but stream not known so fail.
 224                          * Caller deallocates element info. */
 225                         return -1;
 226                 }
 227         }
 228         if (st) {
 229                 /* If this is an ISOC fixed element that needs a frame number
 230                  * then insert that now. Earlier we stored the unit count in
 231                  * this field.
 232                  */
 233                 struct oz_isoc_fixed *body = (struct oz_isoc_fixed *)
 234                         &ei->data[sizeof(struct oz_elt)];
 235                 if ((body->app_id == OZ_APPID_USB) && (body->type
 236                         == OZ_USB_ENDPOINT_DATA) &&
 237                         (body->format == OZ_DATA_F_ISOC_FIXED)) {
 238                         u8 unit_count = body->frame_number;
 239                         body->frame_number = st->frame_number;
 240                         st->frame_number += unit_count;
 241                 }
 242                 /* Claim stream and update accounts */
 243                 oz_elt_stream_get(st);
 244                 ei->stream = st;
 245                 st->buf_count += ei->length;
 246                 /* Add to list in stream. */
 247                 list_add_tail(&ei->link, &st->elt_list);
 248                 oz_dbg(STREAM, "Stream up: %d %d\n", st->buf_count, ei->length);
 249                 /* Check if we have too much buffered for this stream. If so
 250                  * start dropping elements until we are back in bounds.
 251                  */
 252                 while ((st->buf_count > st->max_buf_count) &&
 253                         !list_empty(&st->elt_list)) {
 254                         struct oz_elt_info *ei2 =
 255                                 list_first_entry(&st->elt_list,
 256                                         struct oz_elt_info, link);
 257                         list_del_init(&ei2->link);
 258                         list_del_init(&ei2->link_order);
 259                         st->buf_count -= ei2->length;
 260                         oz_elt_info_free(buf, ei2);
 261                         oz_elt_stream_put(st);
 262                 }
 263         }
 264         list_add_tail(&ei->link_order, isoc ?
 265                 &buf->isoc_list : &buf->order_list);
 266         return 0;
 267 }
 268 /*------------------------------------------------------------------------------
 269  */
 270 int oz_select_elts_for_tx(struct oz_elt_buf *buf, u8 isoc, unsigned *len,
 271                 unsigned max_len, struct list_head *list)
 272 {
 273         int count = 0;
 274         struct list_head *e;
 275         struct list_head *el;
 276         struct oz_elt_info *ei;
 277
 278         spin_lock_bh(&buf->lock);
 279         if (isoc)
 280                 el = &buf->isoc_list;
 281         else
 282                 el = &buf->order_list;
 283         e = el->next;
 284         while (e != el) {
 285                 struct oz_app_hdr *app_hdr;
 286                 ei = container_of(e, struct oz_elt_info, link_order);
 287                 e = e->next;
 288                 if ((*len + ei->length) <= max_len) {
 289                         app_hdr = (struct oz_app_hdr *)
 290                                 &ei->data[sizeof(struct oz_elt)];
 291                         app_hdr->elt_seq_num = buf->tx_seq_num[ei->app_id]++;
 292                         if (buf->tx_seq_num[ei->app_id] == 0)
 293                                 buf->tx_seq_num[ei->app_id] = 1;
 294                         *len += ei->length;
 295                         list_del(&ei->link);
 296                         list_del(&ei->link_order);
 297                         if (ei->stream) {
 298                                 ei->stream->buf_count -= ei->length;
 299                                 oz_dbg(STREAM, "Stream down: %d %d\n",
 300                                        ei->stream->buf_count, ei->length);
 301                                 oz_elt_stream_put(ei->stream);
 302                                 ei->stream = NULL;
 303                         }
 304                         INIT_LIST_HEAD(&ei->link_order);
 305                         list_add_tail(&ei->link, list);
 306                         count++;
 307                 } else {
 308                         break;
 309                 }
 310         }
 311         spin_unlock_bh(&buf->lock);
 312         return count;
 313 }
 314 /*------------------------------------------------------------------------------
 315  */
 316 int oz_are_elts_available(struct oz_elt_buf *buf)
 317 {
 318         return buf->order_list.next != &buf->order_list;
 319 }
 320 /*------------------------------------------------------------------------------
 321  */
 322 void oz_trim_elt_pool(struct oz_elt_buf *buf)
 323 {
 324         struct list_head *free = NULL;
 325         struct list_head *e;
 326
 327         spin_lock_bh(&buf->lock);
 328         while (buf->free_elts > buf->max_free_elts) {
 329                 e = buf->elt_pool;
 330                 buf->elt_pool = e->next;
 331                 e->next = free;
 332                 free = e;
 333                 buf->free_elts--;
 334         }
 335         spin_unlock_bh(&buf->lock);
 336         while (free) {
 337                 struct oz_elt_info *ei =
 338                         container_of(free, struct oz_elt_info, link);
 339                 free = free->next;
 340                 kfree(ei);
 341         }
 342 }