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 */
16void 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 */
28void 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 */
41struct 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 */
57void 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 */
66void 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
76int 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
95int 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
130void oz_elt_stream_get(struct oz_elt_stream *st)
131{
132	atomic_inc(&st->ref_count);
133}
134
135void 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 */
148int 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
208int 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
249int oz_are_elts_available(struct oz_elt_buf *buf)
250{
251	return !list_empty(&buf->order_list);
252}
253