/*

 * q_mqprio.c	MQ prio qdisc

 *

 *		This program is free software; you can redistribute it and/or

 *		modify it under the terms of the GNU General Public License

 *		as published by the Free Software Foundation; either version

 *		2 of the License, or (at your option) any later version.

 *

 * Author:	John Fastabend, <john.r.fastabend@intel.com>

 */

#include <stdio.h>

#include <stdlib.h>

#include <unistd.h>

#include <fcntl.h>

#include <sys/socket.h>

#include <netinet/in.h>

#include <arpa/inet.h>

#include <string.h>

#include "utils.h"

#include "tc_util.h"

static void explain(void)

{

	fprintf(stderr,

		"Usage: ... mqprio	[num_tc NUMBER] [map P0 P1 ...]\n"

		"			[queues count1@offset1 count2@offset2 ...] "

		"[hw 1|0]\n"

		"			[mode dcb|channel]\n"

		"			[shaper bw_rlimit SHAPER_PARAMS]\n"

		"Where: SHAPER_PARAMS := { min_rate MIN_RATE1 MIN_RATE2 ...|\n"

		"			  max_rate MAX_RATE1 MAX_RATE2 ... }\n");

}

static int mqprio_parse_opt(struct qdisc_util *qu, int argc,

			    char **argv, struct nlmsghdr *n, const char *dev)

{

	int idx;

	struct tc_mqprio_qopt opt = {

		.num_tc = 8,

		.prio_tc_map = { 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 1, 1, 3, 3, 3, 3 },

		.hw = 1,

		.count = { },

		.offset = { },

	};

	__u64 min_rate64[TC_QOPT_MAX_QUEUE] = {0};

	__u64 max_rate64[TC_QOPT_MAX_QUEUE] = {0};

	__u16 shaper = TC_MQPRIO_SHAPER_DCB;

	__u16 mode = TC_MQPRIO_MODE_DCB;

	int cnt_off_pairs = 0;

	struct rtattr *tail;

	__u32 flags = 0;

	while (argc > 0) {

		idx = 0;

		if (strcmp(*argv, "num_tc") == 0) {

			NEXT_ARG();

			if (get_u8(&opt.num_tc, *argv, 10)) {

				fprintf(stderr, "Illegal \"num_tc\"\n");

				return -1;

			}

		} else if (strcmp(*argv, "map") == 0) {

			while (idx < TC_QOPT_MAX_QUEUE && NEXT_ARG_OK()) {

				NEXT_ARG();

				if (get_u8(&opt.prio_tc_map[idx], *argv, 10)) {

					PREV_ARG();

					break;

				}

				idx++;

			}

			for ( ; idx < TC_QOPT_MAX_QUEUE; idx++)

				opt.prio_tc_map[idx] = 0;

		} else if (strcmp(*argv, "queues") == 0) {

			char *tmp, *tok;

			while (idx < TC_QOPT_MAX_QUEUE && NEXT_ARG_OK()) {

				NEXT_ARG();

				tmp = strdup(*argv);

				if (!tmp)

					break;

				tok = strtok(tmp, "@");

				if (get_u16(&opt.count[idx], tok, 10)) {

					free(tmp);

					PREV_ARG();

					break;

				}

				tok = strtok(NULL, "@");

				if (get_u16(&opt.offset[idx], tok, 10)) {

					free(tmp);

					PREV_ARG();

					break;

				}

				free(tmp);

				idx++;

				cnt_off_pairs++;

			}

		} else if (strcmp(*argv, "hw") == 0) {

			NEXT_ARG();

			if (get_u8(&opt.hw, *argv, 10)) {

				fprintf(stderr, "Illegal \"hw\"\n");

				return -1;

			}

			idx++;

		} else if (opt.hw && strcmp(*argv, "mode") == 0) {

			NEXT_ARG();

			if (matches(*argv, "dcb") == 0) {

				mode = TC_MQPRIO_MODE_DCB;

			} else if (matches(*argv, "channel") == 0) {

				mode = TC_MQPRIO_MODE_CHANNEL;

			}  else {

				fprintf(stderr, "Illegal mode (%s)\n",

					*argv);

				return -1;

			}

			if (mode != TC_MQPRIO_MODE_DCB)

				flags |= TC_MQPRIO_F_MODE;

			idx++;

		} else if (opt.hw && strcmp(*argv, "shaper") == 0) {

			NEXT_ARG();

			if (matches(*argv, "dcb") == 0) {

				shaper = TC_MQPRIO_SHAPER_DCB;

			} else if (matches(*argv, "bw_rlimit") == 0) {

				shaper = TC_MQPRIO_SHAPER_BW_RATE;

				if (!NEXT_ARG_OK()) {

					fprintf(stderr, "Incomplete shaper arguments\n");

					return -1;

				}

			}  else {

				fprintf(stderr, "Illegal shaper (%s)\n",

					*argv);

				return -1;

			}

			if (shaper != TC_MQPRIO_SHAPER_DCB)

				flags |= TC_MQPRIO_F_SHAPER;

			idx++;

		} else if ((shaper == TC_MQPRIO_SHAPER_BW_RATE) &&

			   strcmp(*argv, "min_rate") == 0) {

			while (idx < TC_QOPT_MAX_QUEUE && NEXT_ARG_OK()) {

				NEXT_ARG();

				if (get_rate64(&min_rate64[idx], *argv)) {

					PREV_ARG();

					break;

				}

				idx++;

			}

			if (idx < opt.num_tc && !NEXT_ARG_OK()) {

				fprintf(stderr, "Incomplete arguments, min_rate values expected\n");

				return -1;

			}

			flags |= TC_MQPRIO_F_MIN_RATE;

		} else if ((shaper == TC_MQPRIO_SHAPER_BW_RATE) &&

			   strcmp(*argv, "max_rate") == 0) {

			while (idx < TC_QOPT_MAX_QUEUE && NEXT_ARG_OK()) {

				NEXT_ARG();

				if (get_rate64(&max_rate64[idx], *argv)) {

					PREV_ARG();

					break;

				}

				idx++;

			}

			if (idx < opt.num_tc && !NEXT_ARG_OK()) {

				fprintf(stderr, "Incomplete arguments, max_rate values expected\n");

				return -1;

			}

			flags |= TC_MQPRIO_F_MAX_RATE;

		} else if (strcmp(*argv, "help") == 0) {

			explain();

			return -1;

		} else {

			invarg("unknown argument", *argv);

		}

		argc--; argv++;

	}

	if (cnt_off_pairs > opt.num_tc) {

		fprintf(stderr, "queues count/offset pair count %d can not be higher than given num_tc %d\n",

			cnt_off_pairs, opt.num_tc);

		return -1;

	}

	tail = NLMSG_TAIL(n);

	addattr_l(n, 1024, TCA_OPTIONS, &opt, sizeof(opt));

	if (flags & TC_MQPRIO_F_MODE)

		addattr_l(n, 1024, TCA_MQPRIO_MODE,

			  &mode, sizeof(mode));

	if (flags & TC_MQPRIO_F_SHAPER)

		addattr_l(n, 1024, TCA_MQPRIO_SHAPER,

			  &shaper, sizeof(shaper));

	if (flags & TC_MQPRIO_F_MIN_RATE) {

		struct rtattr *start;

		start = addattr_nest(n, 1024,

				     TCA_MQPRIO_MIN_RATE64 | NLA_F_NESTED);

		for (idx = 0; idx < TC_QOPT_MAX_QUEUE; idx++)

			addattr_l(n, 1024, TCA_MQPRIO_MIN_RATE64,

				  &min_rate64[idx], sizeof(min_rate64[idx]));

		addattr_nest_end(n, start);

	}

	if (flags & TC_MQPRIO_F_MAX_RATE) {

		struct rtattr *start;

		start = addattr_nest(n, 1024,

				     TCA_MQPRIO_MAX_RATE64 | NLA_F_NESTED);

		for (idx = 0; idx < TC_QOPT_MAX_QUEUE; idx++)

			addattr_l(n, 1024, TCA_MQPRIO_MAX_RATE64,

				  &max_rate64[idx], sizeof(max_rate64[idx]));

		addattr_nest_end(n, start);

	}

	tail->rta_len = (void *)NLMSG_TAIL(n) - (void *)tail;

	return 0;

}

static int mqprio_print_opt(struct qdisc_util *qu, FILE *f, struct rtattr *opt)

{

	int i;

	struct tc_mqprio_qopt *qopt;

	__u64 min_rate64[TC_QOPT_MAX_QUEUE] = {0};

	__u64 max_rate64[TC_QOPT_MAX_QUEUE] = {0};

	int len;

	SPRINT_BUF(b1);

	if (opt == NULL)

		return 0;

	len = RTA_PAYLOAD(opt) - RTA_ALIGN(sizeof(*qopt));

	if (len < 0) {

		fprintf(stderr, "options size error\n");

		return -1;

	}

	qopt = RTA_DATA(opt);

	fprintf(f, " tc %u map ", qopt->num_tc);

	for (i = 0; i <= TC_PRIO_MAX; i++)

		fprintf(f, "%u ", qopt->prio_tc_map[i]);

	fprintf(f, "\n             queues:");

	for (i = 0; i < qopt->num_tc; i++)

		fprintf(f, "(%u:%u) ", qopt->offset[i],

			qopt->offset[i] + qopt->count[i] - 1);

	if (len > 0) {

		struct rtattr *tb[TCA_MQPRIO_MAX + 1];

		parse_rtattr(tb, TCA_MQPRIO_MAX,

			     RTA_DATA(opt) + RTA_ALIGN(sizeof(*qopt)),

			     len);

		if (tb[TCA_MQPRIO_MODE]) {

			__u16 *mode = RTA_DATA(tb[TCA_MQPRIO_MODE]);

			if (*mode == TC_MQPRIO_MODE_CHANNEL)

				fprintf(f, "\n             mode:channel");

		} else {

			fprintf(f, "\n             mode:dcb");

		}

		if (tb[TCA_MQPRIO_SHAPER]) {

			__u16 *shaper = RTA_DATA(tb[TCA_MQPRIO_SHAPER]);

			if (*shaper == TC_MQPRIO_SHAPER_BW_RATE)

				fprintf(f, "\n             shaper:bw_rlimit");

		} else {

			fprintf(f, "\n             shaper:dcb");

		}

		if (tb[TCA_MQPRIO_MIN_RATE64]) {

			struct rtattr *r;

			int rem = RTA_PAYLOAD(tb[TCA_MQPRIO_MIN_RATE64]);

			__u64 *min = min_rate64;

			for (r = RTA_DATA(tb[TCA_MQPRIO_MIN_RATE64]);

			     RTA_OK(r, rem); r = RTA_NEXT(r, rem)) {

				if (r->rta_type != TCA_MQPRIO_MIN_RATE64)

					return -1;

				*(min++) = rta_getattr_u64(r);

			}

			fprintf(f, "	min_rate:");

			for (i = 0; i < qopt->num_tc; i++)

				fprintf(f, "%s ", sprint_rate(min_rate64[i], b1));

		}

		if (tb[TCA_MQPRIO_MAX_RATE64]) {

			struct rtattr *r;

			int rem = RTA_PAYLOAD(tb[TCA_MQPRIO_MAX_RATE64]);

			__u64 *max = max_rate64;

			for (r = RTA_DATA(tb[TCA_MQPRIO_MAX_RATE64]);

			     RTA_OK(r, rem); r = RTA_NEXT(r, rem)) {

				if (r->rta_type != TCA_MQPRIO_MAX_RATE64)

					return -1;

				*(max++) = rta_getattr_u64(r);

			}

			fprintf(f, "	max_rate:");

			for (i = 0; i < qopt->num_tc; i++)

				fprintf(f, "%s ", sprint_rate(max_rate64[i], b1));

		}

	}

	return 0;

}

struct qdisc_util mqprio_qdisc_util = {

	.id		= "mqprio",

	.parse_qopt	= mqprio_parse_opt,

	.print_qopt	= mqprio_print_opt,

};