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