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