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 #define OZ_ELT_INFO_MAGIC_USED  0x35791057
  15 #define OZ_ELT_INFO_MAGIC_FREE  0x78940102
  16
  17 /*
  18  * Context: softirq-serialized
  19  */
  20 int oz_elt_buf_init(struct oz_elt_buf *buf)
  21 {
  22         memset(buf, 0, sizeof(struct oz_elt_buf));
  23         INIT_LIST_HEAD(&buf->stream_list);
  24         INIT_LIST_HEAD(&buf->order_list);
  25         INIT_LIST_HEAD(&buf->isoc_list);
  26         buf->max_free_elts = 32;
  27         spin_lock_init(&buf->lock);
  28         return 0;
  29 }
  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 /*
  65  * Context: softirq or process
  66  */
  67 struct oz_elt_info *oz_elt_info_alloc(struct oz_elt_buf *buf)
  68 {
  69         struct oz_elt_info *ei;
  70
  71         spin_lock_bh(&buf->lock);
  72         if (buf->free_elts && buf->elt_pool) {
  73                 ei = container_of(buf->elt_pool, struct oz_elt_info, link);
  74                 buf->elt_pool = ei->link.next;
  75                 buf->free_elts--;
  76                 spin_unlock_bh(&buf->lock);
  77                 if (ei->magic != OZ_ELT_INFO_MAGIC_FREE) {
  78                         oz_dbg(ON, "%s: ei with bad magic: 0x%x\n",
  79                                __func__, ei->magic);
  80                 }
  81         } else {
  82                 spin_unlock_bh(&buf->lock);
  83                 ei = kmalloc(sizeof(struct oz_elt_info), GFP_ATOMIC);
  84         }
  85         if (ei) {
  86                 ei->flags = 0;
  87                 ei->app_id = 0;
  88                 ei->callback = NULL;
  89                 ei->context = 0;
  90                 ei->stream = NULL;
  91                 ei->magic = OZ_ELT_INFO_MAGIC_USED;
  92                 INIT_LIST_HEAD(&ei->link);
  93                 INIT_LIST_HEAD(&ei->link_order);
  94         }
  95         return ei;
  96 }
  97
  98 /*
  99  * Precondition: oz_elt_buf.lock must be held.
 100  * Context: softirq or process
 101  */
 102 void oz_elt_info_free(struct oz_elt_buf *buf, struct oz_elt_info *ei)
 103 {
 104         if (ei) {
 105                 if (ei->magic == OZ_ELT_INFO_MAGIC_USED) {
 106                         buf->free_elts++;
 107                         ei->link.next = buf->elt_pool;
 108                         buf->elt_pool = &ei->link;
 109                         ei->magic = OZ_ELT_INFO_MAGIC_FREE;
 110                 } else {
 111                         oz_dbg(ON, "%s: bad magic ei: %p magic: 0x%x\n",
 112                                __func__, ei, ei->magic);
 113                 }
 114         }
 115 }
 116
 117 /*------------------------------------------------------------------------------
 118  * Context: softirq
 119  */
 120 void oz_elt_info_free_chain(struct oz_elt_buf *buf, struct list_head *list)
 121 {
 122         struct list_head *e;
 123
 124         e = list->next;
 125         spin_lock_bh(&buf->lock);
 126         while (e != list) {
 127                 struct oz_elt_info *ei;
 128                 ei = container_of(e, struct oz_elt_info, link);
 129                 e = e->next;
 130                 oz_elt_info_free(buf, ei);
 131         }
 132         spin_unlock_bh(&buf->lock);
 133 }
 134
 135 int oz_elt_stream_create(struct oz_elt_buf *buf, u8 id, int max_buf_count)
 136 {
 137         struct oz_elt_stream *st;
 138
 139         oz_dbg(ON, "%s: (0x%x)\n", __func__, id);
 140
 141         st = kzalloc(sizeof(struct oz_elt_stream), GFP_ATOMIC | __GFP_ZERO);
 142         if (st == NULL)
 143                 return -ENOMEM;
 144         atomic_set(&st->ref_count, 1);
 145         st->id = id;
 146         st->max_buf_count = max_buf_count;
 147         INIT_LIST_HEAD(&st->elt_list);
 148         spin_lock_bh(&buf->lock);
 149         list_add_tail(&st->link, &buf->stream_list);
 150         spin_unlock_bh(&buf->lock);
 151         return 0;
 152 }
 153
 154 int oz_elt_stream_delete(struct oz_elt_buf *buf, u8 id)
 155 {
 156         struct list_head *e;
 157         struct oz_elt_stream *st = NULL;
 158
 159         oz_dbg(ON, "%s: (0x%x)\n", __func__, id);
 160         spin_lock_bh(&buf->lock);
 161         e = buf->stream_list.next;
 162         while (e != &buf->stream_list) {
 163                 st = container_of(e, struct oz_elt_stream, link);
 164                 if (st->id == id) {
 165                         list_del(e);
 166                         break;
 167                 }
 168                 st = NULL;
 169         }
 170         if (!st) {
 171                 spin_unlock_bh(&buf->lock);
 172                 return -1;
 173         }
 174         e = st->elt_list.next;
 175         while (e != &st->elt_list) {
 176                 struct oz_elt_info *ei =
 177                         container_of(e, struct oz_elt_info, link);
 178                 e = e->next;
 179                 list_del_init(&ei->link);
 180                 list_del_init(&ei->link_order);
 181                 st->buf_count -= ei->length;
 182                 oz_dbg(STREAM, "Stream down: %d %d %d\n",
 183                        st->buf_count, ei->length, atomic_read(&st->ref_count));
 184                 oz_elt_stream_put(st);
 185                 oz_elt_info_free(buf, ei);
 186         }
 187         spin_unlock_bh(&buf->lock);
 188         oz_elt_stream_put(st);
 189         return 0;
 190 }
 191
 192 void oz_elt_stream_get(struct oz_elt_stream *st)
 193 {
 194         atomic_inc(&st->ref_count);
 195 }
 196
 197 void oz_elt_stream_put(struct oz_elt_stream *st)
 198 {
 199         if (atomic_dec_and_test(&st->ref_count)) {
 200                 oz_dbg(ON, "Stream destroyed\n");
 201                 kfree(st);
 202         }
 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 int oz_select_elts_for_tx(struct oz_elt_buf *buf, u8 isoc, unsigned *len,
 270                 unsigned max_len, struct list_head *list)
 271 {
 272         int count = 0;
 273         struct list_head *e;
 274         struct list_head *el;
 275         struct oz_elt_info *ei;
 276
 277         spin_lock_bh(&buf->lock);
 278         if (isoc)
 279                 el = &buf->isoc_list;
 280         else
 281                 el = &buf->order_list;
 282         e = el->next;
 283         while (e != el) {
 284                 struct oz_app_hdr *app_hdr;
 285                 ei = container_of(e, struct oz_elt_info, link_order);
 286                 e = e->next;
 287                 if ((*len + ei->length) <= max_len) {
 288                         app_hdr = (struct oz_app_hdr *)
 289                                 &ei->data[sizeof(struct oz_elt)];
 290                         app_hdr->elt_seq_num = buf->tx_seq_num[ei->app_id]++;
 291                         if (buf->tx_seq_num[ei->app_id] == 0)
 292                                 buf->tx_seq_num[ei->app_id] = 1;
 293                         *len += ei->length;
 294                         list_del(&ei->link);
 295                         list_del(&ei->link_order);
 296                         if (ei->stream) {
 297                                 ei->stream->buf_count -= ei->length;
 298                                 oz_dbg(STREAM, "Stream down: %d %d\n",
 299                                        ei->stream->buf_count, ei->length);
 300                                 oz_elt_stream_put(ei->stream);
 301                                 ei->stream = NULL;
 302                         }
 303                         INIT_LIST_HEAD(&ei->link_order);
 304                         list_add_tail(&ei->link, list);
 305                         count++;
 306                 } else {
 307                         break;
 308                 }
 309         }
 310         spin_unlock_bh(&buf->lock);
 311         return count;
 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 void oz_trim_elt_pool(struct oz_elt_buf *buf)
 320 {
 321         struct list_head *free = NULL;
 322         struct list_head *e;
 323
 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 }