// SPDX-License-Identifier: GPL-2.0
/******************************************************************************
 *
 * Copyright (c) 2020 Intel Corporation
 *
 *****************************************************************************/
#include <net/pkt_cls.h>
#include "pon_qos_tc_qos.h"

static int pon_tbf_remove(struct net_device *dev, u32 handle, u32 parent)
{
	struct pon_qos_qdisc *qdisc = NULL;
	struct pon_qos_q_data *qid = NULL;
	struct pon_qos_port *port = NULL;
	int ret;

	port = pon_qos_port_get(dev);
	if (!port) {
		netdev_err(dev, "tc-tbf port get failed\n");
		return -ENODEV;
	}

	qdisc = pon_qos_qdisc_find(port, parent);
	if (!qdisc) {
		netdev_err(dev, "tc-tbf qdisc get failed\n");
		return -ENODEV;
	}

	qid = pon_qos_qdata_qid_get(dev, qdisc, parent);
	if (!qid) {
		ret = pon_qos_get_queue_by_handle(dev, parent, &qid);
		if (ret < 0 || !qid) {
			netdev_err(dev, "%s: handle not found err %d\n",
				   __func__, ret);
			return -EINVAL;
		}
	}

	ret = pon_qos_shaper_remove(qdisc, qid);
	if (ret < 0)
		return ret;

	radix_tree_delete(&port->qdiscs, TC_H_MAJ(handle));

	return 0;
}

static int pon_qos_tc_tbf_replace(struct net_device *dev,
				  struct tc_tbf_qopt_offload *opt)
{
	struct pon_qos_qdisc *qdisc = NULL;
	struct pon_qos_q_data *qid = NULL;
	struct pon_qos_port *port = NULL;
	int ret;

	port = pon_qos_port_get(dev);
	if (!port) {
		netdev_err(dev, "tc-tbf port get failed\n");
		return -ENODEV;
	}

	qdisc = pon_qos_qdisc_find(port, opt->parent);
	if (!qdisc) {
		netdev_err(dev, "tc-tbf qdisc get failed\n");
		return -ENODEV;
	}

	qid = pon_qos_qdata_qid_get(dev, qdisc, opt->parent);
	if (!qid) {
		ret = pon_qos_get_queue_by_handle(dev, opt->parent, &qid);
		if (ret < 0 || !qid) {
			netdev_err(dev, "%s: handle not found err %d\n",
				   __func__, ret);
			return -EINVAL;
		}
	}

	ret = pon_qos_shaper_add(qdisc, qid, &opt->set_params);
	if (ret < 0)
		return ret;

	ret = pon_qos_qdata_add(dev, qid, opt->handle, opt->parent,
				PON_QOS_QDATA_TBF, pon_tbf_remove);
	if (ret < 0)
		return ret;

	ret = radix_tree_insert(&port->qdiscs, TC_H_MAJ(opt->handle), qdisc);
	if (ret < 0) {
		netdev_err(dev, "%s: qdisc insertion to radix tree failed: %d\n",
			   __func__, ret);
		return ret;
	}

	return 0;
}

static int pon_qos_tc_tbf_destroy(struct net_device *dev,
				  struct tc_tbf_qopt_offload *opt)
{
	struct pon_qos_qdisc *qdisc = NULL;
	struct pon_qos_q_data *qid = NULL;
	struct pon_qos_port *port = NULL;
	int ret;

	port = pon_qos_port_get(dev);
	if (!port) {
		netdev_err(dev, "tc-tbf port get failed\n");
		return -ENODEV;
	}

	qdisc = pon_qos_qdisc_find(port, opt->parent);
	if (!qdisc) {
		netdev_err(dev, "tc-tbf qdisc get failed\n");
		return -ENODEV;
	}

	qid = pon_qos_qdata_qid_get(dev, qdisc, opt->parent);
	if (!qid) {
		ret = pon_qos_get_queue_by_handle(dev, opt->parent, &qid);
		if (ret < 0 || !qid) {
			netdev_err(dev, "%s: handle not found err %d\n",
				   __func__, ret);
			return -EINVAL;
		}
	}

	return pon_qos_qdata_remove(dev, qid, opt->handle, opt->parent);
}

int pon_qos_tc_tbf_offload(struct net_device *dev,
			   u32 handle,
			   struct tc_to_netdev *tc)
{
	int err = 0;
	struct tc_tbf_qopt_offload *opt = tc->sch_tbf;

	switch (opt->command) {
	case TC_TBF_REPLACE:
		err = pon_qos_tc_tbf_replace(dev, opt);
		if (err < 0) {
			netdev_err(dev, "tc-tbf replace failed\n");
			return err;
		}
		break;
	case TC_TBF_DESTROY:
		err = pon_qos_tc_tbf_destroy(dev, opt);
		if (err < 0) {
			netdev_err(dev, "tc-tbf destroy failed\n");
			return err;
		}
		break;
	case TC_TBF_STATS:
		return -EOPNOTSUPP;
	default:
		break;
	}

	return 0;
}