Commit | Line | Data |
---|---|---|
bc3157dd CK |
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" | |
bc3157dd CK |
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 | * 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 | /* 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; | |
1ec41a31 | 50 | kfree(ei); |
bc3157dd CK |
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; | |
1ec41a31 | 58 | kfree(ei); |
bc3157dd CK |
59 | } |
60 | buf->free_elts = 0; | |
61 | } | |
62 | /*------------------------------------------------------------------------------ | |
63 | * Context: softirq or process | |
64 | */ | |
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); | |
1ec41a31 | 80 | ei = kmalloc(sizeof(struct oz_elt_info), GFP_ATOMIC); |
bc3157dd CK |
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 | */ | |
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 | */ | |
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 | */ | |
131 | int oz_elt_stream_create(struct oz_elt_buf *buf, u8 id, int max_buf_count) | |
132 | { | |
1ec41a31 GKH |
133 | struct oz_elt_stream *st; |
134 | ||
bc3157dd | 135 | oz_trace("oz_elt_stream_create(0x%x)\n", id); |
1ec41a31 GKH |
136 | |
137 | st = kzalloc(sizeof(struct oz_elt_stream), GFP_ATOMIC | __GFP_ZERO); | |
bc3157dd | 138 | if (st == 0) |
1ec41a31 | 139 | return -ENOMEM; |
bc3157dd CK |
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 | */ | |
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 | */ | |
190 | void oz_elt_stream_get(struct oz_elt_stream *st) | |
191 | { | |
192 | atomic_inc(&st->ref_count); | |
193 | } | |
194 | /*------------------------------------------------------------------------------ | |
195 | */ | |
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"); | |
1ec41a31 | 200 | kfree(st); |
bc3157dd CK |
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 | */ | |
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 | */ | |
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 | */ | |
314 | int oz_are_elts_available(struct oz_elt_buf *buf) | |
315 | { | |
316 | return buf->order_list.next != &buf->order_list; | |
317 | } | |
318 | /*------------------------------------------------------------------------------ | |
319 | */ | |
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; | |
1ec41a31 | 337 | kfree(ei); |
bc3157dd CK |
338 | } |
339 | } |