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