1/*
2 **************************************************************************
3 * Copyright (c) 2014, 2015 The Linux Foundation. All rights reserved.
4 * Permission to use, copy, modify, and/or distribute this software for
5 * any purpose with or without fee is hereby granted, provided that the
6 * above copyright notice and this permission notice appear in all copies.
7 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
8 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
9 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
10 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
11 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
12 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT
13 * OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
14 **************************************************************************
15 */
16
17#include "nss_qdisc.h"
18
19/*
20 * nssprio qdisc instance structure
21 */
22struct nss_prio_sched_data {
23	struct nss_qdisc nq;		/* Common base class for all nss qdiscs */
24	int bands;			/* Number of priority bands to use */
25	struct Qdisc *queues[TCA_NSSPRIO_MAX_BANDS];
26					/* Array of child qdisc holder */
27};
28
29/*
30 * nss_prio_enqueue()
31 *	Enqueues a skb to nssprio qdisc.
32 */
33static int nss_prio_enqueue(struct sk_buff *skb, struct Qdisc *sch)
34{
35	return nss_qdisc_enqueue(skb, sch);
36}
37
38/*
39 * nss_prio_dequeue()
40 *	Dequeues a skb to nssprio qdisc.
41 */
42static struct sk_buff *nss_prio_dequeue(struct Qdisc *sch)
43{
44	return nss_qdisc_dequeue(sch);
45}
46
47/*
48 * nss_prio_drop()
49 *	Drops a single skb from linux queue, if not empty.
50 *
51 * Does not drop packets that are queued in the NSS.
52 */
53static unsigned int nss_prio_drop(struct Qdisc *sch)
54{
55	return nss_qdisc_drop(sch);
56}
57
58/*
59 * nss_prio_peek()
60 *	Peeks the first packet in queue for this qdisc.
61 */
62static struct sk_buff *nss_prio_peek(struct Qdisc *sch)
63{
64	return nss_qdisc_peek(sch);
65}
66
67/*
68 * nss_prio_reset()
69 *	Reset the nssprio qdisc
70 */
71static void nss_prio_reset(struct Qdisc *sch)
72{
73	return nss_qdisc_reset(sch);
74}
75
76/*
77 * nss_prio_destroy()
78 *	Destroy the nssprio qdisc
79 */
80static void nss_prio_destroy(struct Qdisc *sch)
81{
82	struct nss_prio_sched_data *q = qdisc_priv(sch);
83	struct nss_if_msg nim;
84	int i;
85
86	nss_qdisc_info("Destroying prio");
87
88	/*
89	 * Destroy all attached child nodes before destroying prio
90	 */
91	for (i = 0; i < q->bands; i++) {
92
93		/*
94		 * We always detach the shaper in NSS before destroying it.
95		 * It is very important to check for noop qdisc since those dont
96		 * exist in the NSS.
97		 */
98		if (q->queues[i] != &noop_qdisc) {
99			struct nss_qdisc *nq_child = qdisc_priv(q->queues[i]);
100			nim.msg.shaper_configure.config.msg.shaper_node_config.qos_tag = q->nq.qos_tag;
101			nim.msg.shaper_configure.config.msg.shaper_node_config.snc.prio_detach.priority = i;
102			if (nss_qdisc_node_detach(&q->nq, nq_child, &nim,
103					NSS_SHAPER_CONFIG_TYPE_PRIO_DETACH) < 0) {
104				nss_qdisc_error("%s: Failed to detach child in band %d from prio %x\n",
105							__func__, i, q->nq.qos_tag);
106				return;
107			}
108		}
109
110		/*
111		 * We can now destroy it
112		 */
113		qdisc_destroy(q->queues[i]);
114	}
115
116	/*
117	 * Stop the polling of basic stats
118	 */
119	nss_qdisc_stop_basic_stats_polling(&q->nq);
120
121	/*
122	 * Destroy the qdisc in NSS
123	 */
124	nss_qdisc_destroy(&q->nq);
125}
126
127/*
128 * nssprio policy structure
129 */
130static const struct nla_policy nss_prio_policy[TCA_NSSPRIO_MAX + 1] = {
131	[TCA_NSSPRIO_PARMS] = { .len = sizeof(struct tc_nssprio_qopt) },
132};
133
134/*
135 * nss_prio_change()
136 *	Function call to configure the nssprio parameters
137 */
138static int nss_prio_change(struct Qdisc *sch, struct nlattr *opt)
139{
140	struct nss_prio_sched_data *q;
141	struct nlattr *na[TCA_NSSPRIO_MAX + 1];
142	struct tc_nssprio_qopt *qopt;
143	int err;
144
145	q = qdisc_priv(sch);
146
147	/*
148	 * Since nssprio can be created with no arguments, opt might be NULL
149	 * (depending on the kernel version). This is still a valid create
150	 * request.
151	 */
152	if (opt == NULL) {
153
154		/*
155		 * If no parameter is passed, set it to the default value.
156		 */
157		sch_tree_lock(sch);
158		q->bands = TCA_NSSPRIO_MAX_BANDS;
159		sch_tree_unlock(sch);
160		return 0;
161	}
162
163	err = nla_parse_nested(na, TCA_NSSPRIO_MAX, opt, nss_prio_policy);
164	if (err < 0) {
165		return err;
166	}
167
168	if (na[TCA_NSSPRIO_PARMS] == NULL) {
169		return -EINVAL;
170	}
171
172	qopt = nla_data(na[TCA_NSSPRIO_PARMS]);
173
174	if (qopt->bands > TCA_NSSPRIO_MAX_BANDS) {
175		return -EINVAL;
176	}
177
178	sch_tree_lock(sch);
179	q->bands = qopt->bands;
180	sch_tree_unlock(sch);
181	nss_qdisc_info("Bands = %u\n", qopt->bands);
182
183	/*
184	 * We do not pass on this information to NSS since
185	 * it not required for operation of prio. This parameter
186	 * is needed only for the proxy operation.
187	 */
188
189	return 0;
190}
191
192/*
193 * nss_prio_init()
194 *	Initializes the nssprio qdisc
195 */
196static int nss_prio_init(struct Qdisc *sch, struct nlattr *opt)
197{
198	struct nss_prio_sched_data *q = qdisc_priv(sch);
199	int i;
200
201	for (i = 0; i < TCA_NSSPRIO_MAX_BANDS; i++)
202		q->queues[i] = &noop_qdisc;
203
204	if (nss_qdisc_init(sch, &q->nq, NSS_SHAPER_NODE_TYPE_PRIO, 0) < 0)
205		return -EINVAL;
206
207	nss_qdisc_info("Nssprio initialized - handle %x parent %x\n",
208			sch->handle, sch->parent);
209
210	if (nss_prio_change(sch, opt) < 0) {
211		nss_qdisc_destroy(&q->nq);
212		return -EINVAL;
213	}
214
215	/*
216	 * Start the stats polling timer
217	 */
218	nss_qdisc_start_basic_stats_polling(&q->nq);
219	return 0;
220}
221
222/*
223 * nss_prio_dump()
224 *	Dump the parameters of nssprio
225 */
226static int nss_prio_dump(struct Qdisc *sch, struct sk_buff *skb)
227{
228	struct nss_prio_sched_data *q = qdisc_priv(sch);
229	struct nlattr *opts = NULL;
230	struct tc_nssprio_qopt qopt;
231
232	nss_qdisc_info("Nssprio dumping");
233	qopt.bands = q->bands;
234
235	opts = nla_nest_start(skb, TCA_OPTIONS);
236	if (opts == NULL || nla_put(skb, TCA_NSSPRIO_PARMS, sizeof(qopt), &qopt)) {
237		goto nla_put_failure;
238	}
239
240	return nla_nest_end(skb, opts);
241
242nla_put_failure:
243	nla_nest_cancel(skb, opts);
244	return -EMSGSIZE;
245}
246
247/*
248 * nss_prio_graft()
249 *	Replaces existing child qdisc with the new qdisc that is passed.
250 */
251static int nss_prio_graft(struct Qdisc *sch, unsigned long arg,
252				struct Qdisc *new, struct Qdisc **old)
253{
254	struct nss_prio_sched_data *q = qdisc_priv(sch);
255	struct nss_qdisc *nq_new = qdisc_priv(new);
256	uint32_t band = (uint32_t)(arg - 1);
257	struct nss_if_msg nim_attach;
258	struct nss_if_msg nim_detach;
259
260	nss_qdisc_info("Grafting band %u, available bands %u\n", band, q->bands);
261
262	if (new == NULL)
263		new = &noop_qdisc;
264
265	if (band > q->bands)
266		return -EINVAL;
267
268	sch_tree_lock(sch);
269	*old = q->queues[band];
270	q->queues[band] = new;
271	qdisc_reset(*old);
272	sch_tree_unlock(sch);
273
274	nss_qdisc_info("%s:Grafting old: %p with new: %p\n", __func__, *old, new);
275	if (*old != &noop_qdisc) {
276		struct nss_qdisc *nq_old = qdisc_priv(*old);
277		nss_qdisc_info("%s:Detaching old: %p\n", __func__, *old);
278		nim_detach.msg.shaper_configure.config.msg.shaper_node_config.qos_tag = q->nq.qos_tag;
279		nim_detach.msg.shaper_configure.config.msg.shaper_node_config.snc.prio_detach.priority = band;
280		if (nss_qdisc_node_detach(&q->nq, nq_old, &nim_detach,
281				NSS_SHAPER_CONFIG_TYPE_PRIO_DETACH) < 0) {
282			return -EINVAL;
283		}
284	}
285
286	if (new != &noop_qdisc) {
287		nss_qdisc_info("%s:Attaching new child with qos tag: %x, priority: %u to "
288				"qos_tag: %x\n", __func__, nq_new->qos_tag, band, q->nq.qos_tag);
289		nim_attach.msg.shaper_configure.config.msg.shaper_node_config.qos_tag = q->nq.qos_tag;
290		nim_attach.msg.shaper_configure.config.msg.shaper_node_config.snc.prio_attach.child_qos_tag = nq_new->qos_tag;
291		nim_attach.msg.shaper_configure.config.msg.shaper_node_config.snc.prio_attach.priority = band;
292		if (nss_qdisc_node_attach(&q->nq, nq_new, &nim_attach,
293				NSS_SHAPER_CONFIG_TYPE_PRIO_ATTACH) < 0) {
294			return -EINVAL;
295		}
296	}
297	nss_qdisc_info("Nssprio grafted");
298
299	return 0;
300}
301
302/*
303 * nss_prio_leaf_class()
304 *	Returns pointer to qdisc if leaf class.
305 */
306static struct Qdisc *nss_prio_leaf(struct Qdisc *sch, unsigned long arg)
307{
308	struct nss_prio_sched_data *q = qdisc_priv(sch);
309	uint32_t band = (uint32_t)(arg - 1);
310
311	nss_qdisc_info("Nssprio returns leaf\n");
312
313	if (band > q->bands)
314		return NULL;
315
316	return q->queues[band];
317}
318
319/*
320 * nss_prio_get()
321 *	Returns the band if provided the classid.
322 */
323static unsigned long nss_prio_get(struct Qdisc *sch, u32 classid)
324{
325	struct nss_prio_sched_data *q = qdisc_priv(sch);
326	unsigned long band = TC_H_MIN(classid);
327
328	nss_qdisc_info("Inside get. Handle - %x Classid - %x Band %lu Available band %u\n", sch->handle, classid, band, q->bands);
329
330	if (band > q->bands)
331		return 0;
332
333	return band;
334}
335
336/*
337 * nss_prio_put()
338 *	Unused API.
339 */
340static void nss_prio_put(struct Qdisc *sch, unsigned long arg)
341{
342	nss_qdisc_info("Inside prio put\n");
343}
344
345/*
346 * nss_prio_walk()
347 *	Walks the priority band.
348 */
349static void nss_prio_walk(struct Qdisc *sch, struct qdisc_walker *arg)
350{
351	struct nss_prio_sched_data *q = qdisc_priv(sch);
352	int i;
353
354	if (arg->stop)
355		return;
356
357	for (i = 0; i < q->bands; i++) {
358		if (arg->count < arg->skip) {
359			arg->count++;
360			continue;
361		}
362		if (arg->fn(sch, i + 1, arg) < 0) {
363			arg->stop = 1;
364			break;
365		}
366		arg->count++;
367	}
368	nss_qdisc_info("Nssprio walk called\n");
369}
370
371/*
372 * nss_prio_dump_class()
373 * 	Dumps all configurable parameters pertaining to this class.
374 */
375static int nss_prio_dump_class(struct Qdisc *sch, unsigned long cl,
376			     struct sk_buff *skb, struct tcmsg *tcm)
377{
378	struct nss_prio_sched_data *q = qdisc_priv(sch);
379
380	tcm->tcm_handle |= TC_H_MIN(cl);
381	tcm->tcm_info = q->queues[cl - 1]->handle;
382
383	nss_qdisc_info("Nssprio dumping class\n");
384	return 0;
385}
386
387/*
388 * nss_prio_dump_class_stats()
389 *	Dumps class statistics.
390 */
391static int nss_prio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
392			     	    struct gnet_dump *d)
393{
394	struct nss_prio_sched_data *q = qdisc_priv(sch);
395	struct Qdisc *cl_q;
396
397	cl_q = q->queues[cl - 1];
398	cl_q->qstats.qlen = cl_q->q.qlen;
399	if (gnet_stats_copy_basic(d, &cl_q->bstats) < 0 ||
400	    gnet_stats_copy_queue(d, &cl_q->qstats) < 0)
401		return -1;
402
403	nss_qdisc_info("Nssprio dumping class stats\n");
404	return 0;
405}
406
407/*
408 * Registration structure for nssprio class
409 */
410const struct Qdisc_class_ops nss_prio_class_ops = {
411	.graft		=	nss_prio_graft,
412	.leaf		=	nss_prio_leaf,
413	.get		=	nss_prio_get,
414	.put		=	nss_prio_put,
415	.walk		=	nss_prio_walk,
416	.dump		=	nss_prio_dump_class,
417	.dump_stats	=	nss_prio_dump_class_stats,
418};
419
420/*
421 * Registration structure for nssprio qdisc
422 */
423struct Qdisc_ops nss_prio_qdisc_ops __read_mostly = {
424	.next		=	NULL,
425	.id		=	"nssprio",
426	.priv_size	=	sizeof(struct nss_prio_sched_data),
427	.cl_ops		=	&nss_prio_class_ops,
428	.enqueue	=	nss_prio_enqueue,
429	.dequeue	=	nss_prio_dequeue,
430	.peek		=	nss_prio_peek,
431	.drop		=	nss_prio_drop,
432	.init		=	nss_prio_init,
433	.reset		=	nss_prio_reset,
434	.destroy	=	nss_prio_destroy,
435	.change		=	nss_prio_change,
436	.dump		=	nss_prio_dump,
437	.owner		=	THIS_MODULE,
438};
439