1
2
3
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
19
20int 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
33
34void oz_elt_buf_term(struct oz_elt_buf *buf)
35{
36 struct list_head *e;
37 int i;
38
39
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
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
66
67struct 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
100
101
102void 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
119
120void 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
135int 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
154int 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
192void oz_elt_stream_get(struct oz_elt_stream *st)
193{
194 atomic_inc(&st->ref_count);
195}
196
197void 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
207
208
209
210int 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
224
225 return -1;
226 }
227 }
228 if (st) {
229
230
231
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
243 oz_elt_stream_get(st);
244 ei->stream = st;
245 st->buf_count += ei->length;
246
247 list_add_tail(&ei->link, &st->elt_list);
248 oz_dbg(STREAM, "Stream up: %d %d\n", st->buf_count, ei->length);
249
250
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
269int 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
314int oz_are_elts_available(struct oz_elt_buf *buf)
315{
316 return buf->order_list.next != &buf->order_list;
317}
318
319void 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}
340
