// SPDX-License-Identifier: GPL-2.0
/******************************************************************************
 *
 * Copyright (c) 2020 - 2021 MaxLinear, Inc.
 * Copyright (c) 2020 Intel Corporation
 *
 *****************************************************************************/
#include <net/pkt_cls.h>
#include <linux/version.h>
#include "pon_qos_tc_qos.h"
#include "pon_qos_trace.h"
#include <net/datapath_api.h>
#include <net/datapath_api_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, "%s: qdisc (or its parent port) doesn't exist "
				"or was already deleted.\n", __func__);
		return 0;
	}

	qdisc = pon_qos_qdisc_find(port, parent);
	if (!qdisc) {
		netdev_err(dev, "%s: qdisc doesn't exist or "
				"was already deleted.\n", __func__);
		return 0;
	}

	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_root_tbf_destroy(struct net_device *dev, u32 handle, u32 parent)
{
	struct pon_qos_port *port = NULL;
	struct pon_qos_qdisc *qdisc_sch = NULL;
	struct dp_shaper_conf shaper_conf = {0};
	int ret;

	port = pon_qos_port_get(dev);
	if (!port) {
		netdev_err(dev, "%s: qdisc (or port) doesn't exist or "
				"was already deleted.\n", __func__);
		return 0;
	}

	qdisc_sch = &port->root_qdisc;

	// remove port shaper via datapath api
	shaper_conf.cmd = DP_SHAPER_CMD_REMOVE;
	shaper_conf.type = DP_NODE_PORT;
	shaper_conf.id.cqm_deq_port = port->root_qdisc.epn;
	ret = dp_shaper_conf_set(&shaper_conf, 0);
	if (ret) {
		netdev_err(dev, "%s:%d Failed to remove port Shaper: %d\n",
			   __func__, __LINE__, ret);
		return ret;
	}

	ret = pon_qos_qdisc_tree_del(port, qdisc_sch);
	if (ret) {
		netdev_err(dev, "%s:%d Failed to delete root tbf: %d\n",
			   __func__, __LINE__, ret);
	}

	return 0;
}

static int pon_qos_setup_port_tbf(struct net_device *dev,
				  struct pon_qos_port *port,
				  struct tc_tbf_qopt_offload *opt,
				  u32 handle)
{
	struct dp_shaper_conf shaper_conf = {0};
	int ret;

	shaper_conf.pir  = div_u64(opt->set_params.prate * 8, 1000);
	shaper_conf.cir  = div_u64(opt->set_params.rate * 8, 1000);
	shaper_conf.pbs  = opt->set_params.pburst;
	shaper_conf.cbs  = opt->set_params.burst;
	shaper_conf.cmd = DP_SHAPER_CMD_ADD;
	shaper_conf.type = DP_NODE_PORT;
	shaper_conf.id.cqm_deq_port = port->root_qdisc.epn;

	netdev_dbg(dev, "%s: cir: %d pir: %d cbs: %d pbs: %d\n",
		   __func__, shaper_conf.cir, shaper_conf.pir, shaper_conf.cbs,
		   shaper_conf.pbs);

	ret = dp_shaper_conf_set(&shaper_conf, 0);
	if (ret == DP_FAILURE) {
		netdev_err(dev, "%s:%d Failed to set the Shaper: %d\n",
			   __func__, __LINE__, ret);
	}

	return ret;
}

static inline
struct pon_qos_port *pon_qos_replace_port(struct pon_qos_port *port,
					  struct net_device *dev,
					  int port_id, int deq_idx)
{
	int ret;
	struct pon_qos_port *new_port;

	ret = pon_qos_qdisc_tree_del(port, &port->root_qdisc);
	if (ret) {
		netdev_err(dev, "tc-tbf root qdisc reset failed\n");
		return ERR_PTR(ret);
	}

	new_port = pon_qos_port_alloc(dev);
	if (!new_port) {
		netdev_err(dev, "tc-tbf root port alloc failed\n");
		return ERR_PTR(-ENOMEM);
	}

	new_port->root_qdisc.port = port_id;
	new_port->root_qdisc.deq_idx = deq_idx;
	new_port->root_qdisc.dev = dev;
	return new_port;
}

static int pon_qos_sched_update_root_tbf(struct net_device *dev,
				    struct pon_qos_port *port,
				    u32 handle)
{
	int ret;
	struct pon_qos_qdisc *sch = &port->root_qdisc;

	if (sch->use_cnt)
		return 0;

	sch->type = PON_QOS_QDISC_TBF;
	sch->dev = dev;
	sch->handle = handle;
	sch->parent = TC_H_ROOT;
	sch->use_cnt = 1;

	ret = pon_qos_fill_port_data(sch);
	if (ret < 0) {
		netdev_err(dev, "failed getting port hw config %d\n", ret);
		return ret;
	}

	/* add to radix tree here */
	ret = radix_tree_insert(&port->qdiscs, TC_H_MAJ(handle), sch);
	if (ret) {
		netdev_err(dev, "qdisc insertion to radix tree failed: %d\n",
			   ret);
		return ret;
	}

	return pon_qos_add_sched(sch, 0);
}

static int pon_qos_tc_tbf_replace_root(struct net_device *dev,
				       struct tc_tbf_qopt_offload *opt,
				       u32 handle, int port_id, int deq_idx)
{
	int ret = 0;
	bool newp = false;
	struct pon_qos_port *port;

	port = pon_qos_port_get(dev);
	if (!port) {
		port = pon_qos_port_alloc(dev);
		if (!port) {
			netdev_err(dev, "tc-tbf root port alloc failed\n");
			return -ENOMEM;
		}
		newp = true;
		port->root_qdisc.port = (s32)port_id;
		port->root_qdisc.deq_idx = deq_idx;
		port->root_qdisc.dev = dev;
	} else {
		/* If the root qdisc handle isn't the same as the existing one,
		 * this call replaces the old root qdisc and deletes the whole
		 * previous tree. This also re-allocates the port struct.
		 */
		if (TC_H_MAJ(handle) != TC_H_MAJ(port->root_qdisc.handle)) {
			port = pon_qos_replace_port(port, dev, port_id,
						    deq_idx);
			if (IS_ERR_OR_NULL(port)) {
				netdev_err(dev, "%s: port replace failed\n",
					   __func__);
				return PTR_ERR(port);
			}
			newp = true;
		}
	}

	ret = pon_qos_get_port_info(&port->root_qdisc);
	if (ret) {
		netdev_err(dev, "%s: get port info failed\n", __func__);
		goto err_free_port;
	}

	ret = pon_qos_sched_update_root_tbf(dev, port, handle);
	if (ret) {
		netdev_err(dev, "tc-tbf sched config failed\n");
		goto err_free_qdisc;
	}

	ret = pon_qos_setup_port_tbf(dev, port, opt, handle);
	if (ret) {
		netdev_err(dev, "tc-tbf port shaping failed\n");
		goto err_free_qdisc;
	}

	return ret;

err_free_qdisc:
	ret = pon_qos_sched_del(&port->root_qdisc);
	if (ret < 0)
		netdev_err(dev, "%s: sched del failed\n", __func__);
err_free_port:
	if (newp)
		pon_qos_port_delete(port);
	return ret;
}

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

	if (!dev) {
		netdev_err(dev, "%s: no valid device.\n", __func__);
		return -EINVAL;
	}

	// Check if qdisc is a root qdisc
	if (opt->parent == TC_H_ROOT)
		return pon_qos_tc_tbf_replace_root(dev, opt, handle,
							port_id, deq_idx);

	port = pon_qos_port_get(dev);
	if (!port) {
		netdev_err(dev, "%s: port get failed\n", __func__);
		return -ENODEV;
	}

	qdisc = pon_qos_qdisc_find(port, opt->parent);
	if (!qdisc) {
		netdev_err(dev, "%s: qdisc get failed\n", __func__);
		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,
				  u32 handle)
{
	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) {
		/* Linux deletes some qdiscs from root to the leaf node
		 * which may cause that the tbf offload is already removed
		 * by its parent i.e. deleted by pon_qos_qdisc_tree_del */
		netdev_dbg(dev, "tc-tbf port get failed\n");
		return -ENODEV;
	}

	// Check if qdisc is a root qdisc
	if (opt->parent == TC_H_ROOT)
		return pon_root_tbf_destroy(dev, handle, opt->parent);

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

	qid = pon_qos_qdata_qid_get(dev, qdisc, opt->parent);
	if (!qid) {
		netdev_err(dev, "tc-tbf-destroy: qid get failed\n");
		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,
			   void *type_data,
			   int port_id,
			   int deq_idx)
{
	int err = 0;
#if (KERNEL_VERSION(4, 14, 0) > LINUX_VERSION_CODE)
	struct tc_tbf_qopt_offload *opt =
		((struct tc_to_netdev *)type_data)->sch_tbf;
#else
	struct tc_tbf_qopt_offload *opt =
		(struct tc_tbf_qopt_offload *)type_data;
#endif

	trace_pon_qos_tc_tbf_enter(dev, opt);

	switch (opt->command) {
	case TC_TBF_REPLACE:
		err = pon_qos_tc_tbf_replace(dev, opt, handle,
					     port_id, deq_idx);
		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, handle);
		if (err < 0) {
			/* Expected to fail if root was previously removed */
			netdev_dbg(dev, "tc-tbf destroy failed\n");
			return err;
		}
		break;
	case TC_TBF_STATS:
		return -EOPNOTSUPP;
	default:
		break;
	}

	trace_pon_qos_tc_tbf_exit(dev, opt);

	return 0;
}