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 /*
  14  * Context: softirq-serialized
  15  */
  16 void oz_elt_buf_init(struct oz_elt_buf *buf)
  17 {
  18         memset(buf, 0, sizeof(struct oz_elt_buf));
  19         INIT_LIST_HEAD(&buf->stream_list);
  20         INIT_LIST_HEAD(&buf->order_list);
  21         INIT_LIST_HEAD(&buf->isoc_list);
  22         spin_lock_init(&buf->lock);
  23 }
  24
  25 /*
  26  * Context: softirq or process
  27  */
  28 void oz_elt_buf_term(struct oz_elt_buf *buf)
  29 {
  30         struct oz_elt_info *ei, *n;
  31
  32         list_for_each_entry_safe(ei, n, &buf->isoc_list, link_order)
  33                 kfree(ei);
  34         list_for_each_entry_safe(ei, n, &buf->order_list, link_order)
  35                 kfree(ei);
  36 }
  37
  38 /*
  39  * Context: softirq or process
  40  */
  41 struct oz_elt_info *oz_elt_info_alloc(struct oz_elt_buf *buf)
  42 {
  43         struct oz_elt_info *ei;
  44
  45         ei = kmem_cache_zalloc(oz_elt_info_cache, GFP_ATOMIC);
  46         if (ei) {
  47                 INIT_LIST_HEAD(&ei->link);
  48                 INIT_LIST_HEAD(&ei->link_order);
  49         }
  50         return ei;
  51 }
  52
  53 /*
  54  * Precondition: oz_elt_buf.lock must be held.
  55  * Context: softirq or process
  56  */
  57 void oz_elt_info_free(struct oz_elt_buf *buf, struct oz_elt_info *ei)
  58 {
  59         if (ei)
  60                 kmem_cache_free(oz_elt_info_cache, ei);
  61 }
  62
  63 /*------------------------------------------------------------------------------
  64  * Context: softirq
  65  */
  66 void oz_elt_info_free_chain(struct oz_elt_buf *buf, struct list_head *list)
  67 {
  68         struct oz_elt_info *ei, *n;
  69
  70         spin_lock_bh(&buf->lock);
  71         list_for_each_entry_safe(ei, n, list->next, link)
  72                 oz_elt_info_free(buf, ei);
  73         spin_unlock_bh(&buf->lock);
  74 }
  75
  76 int oz_elt_stream_create(struct oz_elt_buf *buf, u8 id, int max_buf_count)
  77 {
  78         struct oz_elt_stream *st;
  79
  80         oz_dbg(ON, "%s: (0x%x)\n", __func__, id);
  81
  82         st = kzalloc(sizeof(struct oz_elt_stream), GFP_ATOMIC);
  83         if (st == NULL)
  84                 return -ENOMEM;
  85         atomic_set(&st->ref_count, 1);
  86         st->id = id;
  87         st->max_buf_count = max_buf_count;
  88         INIT_LIST_HEAD(&st->elt_list);
  89         spin_lock_bh(&buf->lock);
  90         list_add_tail(&st->link, &buf->stream_list);
  91         spin_unlock_bh(&buf->lock);
  92         return 0;
  93 }
  94
  95 int oz_elt_stream_delete(struct oz_elt_buf *buf, u8 id)
  96 {
  97         struct list_head *e, *n;
  98         struct oz_elt_stream *st = NULL;
  99
 100         oz_dbg(ON, "%s: (0x%x)\n", __func__, id);
 101         spin_lock_bh(&buf->lock);
 102         list_for_each(e, &buf->stream_list) {
 103                 st = list_entry(e, struct oz_elt_stream, link);
 104                 if (st->id == id) {
 105                         list_del(e);
 106                         break;
 107                 }
 108                 st = NULL;
 109         }
 110         if (!st) {
 111                 spin_unlock_bh(&buf->lock);
 112                 return -1;
 113         }
 114         list_for_each_safe(e, n, &st->elt_list) {
 115                 struct oz_elt_info *ei =
 116                         list_entry(e, struct oz_elt_info, link);
 117                 list_del_init(&ei->link);
 118                 list_del_init(&ei->link_order);
 119                 st->buf_count -= ei->length;
 120                 oz_dbg(STREAM, "Stream down: %d %d %d\n",
 121                        st->buf_count, ei->length, atomic_read(&st->ref_count));
 122                 oz_elt_stream_put(st);
 123                 oz_elt_info_free(buf, ei);
 124         }
 125         spin_unlock_bh(&buf->lock);
 126         oz_elt_stream_put(st);
 127         return 0;
 128 }
 129
 130 void oz_elt_stream_get(struct oz_elt_stream *st)
 131 {
 132         atomic_inc(&st->ref_count);
 133 }
 134
 135 void oz_elt_stream_put(struct oz_elt_stream *st)
 136 {
 137         if (atomic_dec_and_test(&st->ref_count)) {
 138                 oz_dbg(ON, "Stream destroyed\n");
 139                 kfree(st);
 140         }
 141 }
 142
 143 /*
 144  * Precondition: Element buffer lock must be held.
 145  * If this function fails the caller is responsible for deallocating the elt
 146  * info structure.
 147  */
 148 int oz_queue_elt_info(struct oz_elt_buf *buf, u8 isoc, u8 id,
 149         struct oz_elt_info *ei)
 150 {
 151         struct oz_elt_stream *st = NULL;
 152         struct list_head *e;
 153
 154         if (id) {
 155                 list_for_each(e, &buf->stream_list) {
 156                         st = list_entry(e, struct oz_elt_stream, link);
 157                         if (st->id == id)
 158                                 break;
 159                 }
 160                 if (e == &buf->stream_list) {
 161                         /* Stream specified but stream not known so fail.
 162                          * Caller deallocates element info. */
 163                         return -1;
 164                 }
 165         }
 166         if (st) {
 167                 /* If this is an ISOC fixed element that needs a frame number
 168                  * then insert that now. Earlier we stored the unit count in
 169                  * this field.
 170                  */
 171                 struct oz_isoc_fixed *body = (struct oz_isoc_fixed *)
 172                         &ei->data[sizeof(struct oz_elt)];
 173                 if ((body->app_id == OZ_APPID_USB) && (body->type
 174                         == OZ_USB_ENDPOINT_DATA) &&
 175                         (body->format == OZ_DATA_F_ISOC_FIXED)) {
 176                         u8 unit_count = body->frame_number;
 177
 178                         body->frame_number = st->frame_number;
 179                         st->frame_number += unit_count;
 180                 }
 181                 /* Claim stream and update accounts */
 182                 oz_elt_stream_get(st);
 183                 ei->stream = st;
 184                 st->buf_count += ei->length;
 185                 /* Add to list in stream. */
 186                 list_add_tail(&ei->link, &st->elt_list);
 187                 oz_dbg(STREAM, "Stream up: %d %d\n", st->buf_count, ei->length);
 188                 /* Check if we have too much buffered for this stream. If so
 189                  * start dropping elements until we are back in bounds.
 190                  */
 191                 while ((st->buf_count > st->max_buf_count) &&
 192                         !list_empty(&st->elt_list)) {
 193                         struct oz_elt_info *ei2 =
 194                                 list_first_entry(&st->elt_list,
 195                                         struct oz_elt_info, link);
 196                         list_del_init(&ei2->link);
 197                         list_del_init(&ei2->link_order);
 198                         st->buf_count -= ei2->length;
 199                         oz_elt_info_free(buf, ei2);
 200                         oz_elt_stream_put(st);
 201                 }
 202         }
 203         list_add_tail(&ei->link_order, isoc ?
 204                 &buf->isoc_list : &buf->order_list);
 205         return 0;
 206 }
 207
 208 int oz_select_elts_for_tx(struct oz_elt_buf *buf, u8 isoc, unsigned *len,
 209                 unsigned max_len, struct list_head *list)
 210 {
 211         int count = 0;
 212         struct list_head *el;
 213         struct oz_elt_info *ei, *n;
 214
 215         spin_lock_bh(&buf->lock);
 216         if (isoc)
 217                 el = &buf->isoc_list;
 218         else
 219                 el = &buf->order_list;
 220
 221         list_for_each_entry_safe(ei, n, el, link_order) {
 222                 if ((*len + ei->length) <= max_len) {
 223                         struct oz_app_hdr *app_hdr = (struct oz_app_hdr *)
 224                                 &ei->data[sizeof(struct oz_elt)];
 225                         app_hdr->elt_seq_num = buf->tx_seq_num[ei->app_id]++;
 226                         if (buf->tx_seq_num[ei->app_id] == 0)
 227                                 buf->tx_seq_num[ei->app_id] = 1;
 228                         *len += ei->length;
 229                         list_del(&ei->link);
 230                         list_del(&ei->link_order);
 231                         if (ei->stream) {
 232                                 ei->stream->buf_count -= ei->length;
 233                                 oz_dbg(STREAM, "Stream down: %d %d\n",
 234                                        ei->stream->buf_count, ei->length);
 235                                 oz_elt_stream_put(ei->stream);
 236                                 ei->stream = NULL;
 237                         }
 238                         INIT_LIST_HEAD(&ei->link_order);
 239                         list_add_tail(&ei->link, list);
 240                         count++;
 241                 } else {
 242                         break;
 243                 }
 244         }
 245         spin_unlock_bh(&buf->lock);
 246         return count;
 247 }
 248
 249 int oz_are_elts_available(struct oz_elt_buf *buf)
 250 {
 251         return !list_empty(&buf->order_list);
 252 }