/*
 * qos.c - agent-side QoS implementation
 *
 * Copyright (C) 2023 IOPSYS Software Solutions AB. All rights reserved.
 *
 * Author: maxim.menshikov@iopsys.eu
 *
 */

#if (EASYMESH_VERSION > 2)

#include "qos.h"

#include <easymesh.h>
#include <libubox/list.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "agent.h"
#include "config.h"
#include "qos_internal.h"
#include "utils/debug.h"
#include "wifi.h"

void qos_rule_free(void *rule)
{
	free(rule);
}

void qos_rule_free_all(struct list_head *list_head)
{
	struct qos_rule *p, *tmp;

	list_for_each_entry_safe(p, tmp, list_head, list) {
		list_del(&p->list);
		qos_rule_free(p);
	}
}

int qos_add_dscp_rule(void *agent,
                      struct tlv_spr *spr,
                      struct tlv_dscp_pcp *dscp_pcp)
{
	struct agent *a = (struct agent *)agent;
	struct qos_rule *r;

	dbg("%s: adding DSCP rule...\n", __func__);
	list_for_each_entry(r, &a->qos_rules, list) {
		if (r->spr.rule_id == spr->rule_id) {
			err("%s: DSCP rule exists\n", __func__);
			return -1;
		}
	}

	r = calloc(1, sizeof(struct qos_rule));
	if (r == NULL) {
		return -1;
	}

	r->type = QOS_RULE_TYPE_DSCP_PCP;
	memcpy(&r->spr, spr, sizeof(struct tlv_spr));
	memcpy(&r->dscp, dscp_pcp, sizeof(struct tlv_dscp_pcp));

	dbg("%s: DSCP rule added\n", __func__);
	list_add_tail(&r->list, &a->qos_rules);
	return 0;
}

int qos_del_dscp_rule(void *agent,
                      struct tlv_spr *spr)
{
	struct agent *a = (struct agent *)agent;
	struct qos_rule *r;
	struct qos_rule *tmp;

	dbg("%s: removing DSCP rule...\n", __func__);
	list_for_each_entry_safe(r, tmp, &a->qos_rules, list) {
		if (r->spr.rule_id == spr->rule_id) {
			list_del(&r->list);
			qos_rule_free(r);
			dbg("%s: DSCP rule removed\n", __func__);
			return 0;
		}
	}

	err("%s: rule is not found\n", __func__);
	return -1;
}

enum dscp_pcp_conv_result dscp_pcp2qosmap(const uint8_t dscp_pcp[64],
                                          uint8_t qos_map[58],
                                          size_t *qos_map_len)
{
	/* Internal constants */
#define PCP_COUNT       (8)
#define DSCP_PCP_MAX    (64)
#define DSCP_UP_EXC_MAX (21)
#define RANGE_MIN       (0)
#define RANGE_MAX       (1)
#define RANGE_TOTAL     (2)
#define TMP_BUF_LEN     (50)
#define DSCP_NEUTRAL    (255)

	enum dscp_pcp_conv_result rc = DSCP_PCP_CONV_RESULT_OK;

	/* pcp -> dscp usage table */
	uint8_t pcp_dscp[PCP_COUNT][DSCP_PCP_MAX];
	/* Minimal and maximal DSCPs for PCP */
	uint8_t pcp_dscp_min[PCP_COUNT] = { DSCP_NEUTRAL };
	uint8_t pcp_dscp_max[PCP_COUNT] = { DSCP_NEUTRAL };
	/* DSCP exceptions */
	uint8_t dscp_up[DSCP_UP_EXC_MAX][2];
	size_t total_du = 0;
	size_t i, j;
	size_t final_len = 0;

	memset(pcp_dscp, 0, sizeof(pcp_dscp));

	/* Convert DSCP->PCP to PCP->DSCP usage table */
	for (i = 0; i < DSCP_PCP_MAX; ++i) {
		uint8_t pcp = dscp_pcp[i];
		uint8_t dscp = i;

		if (pcp >= PCP_COUNT) {
			pcp = PCP_COUNT - 1;
		}

		pcp_dscp[pcp][dscp] = 1;
	}

	/* Find the biggest ranges of used DSCPs for PCPs */
	for (i = 0; i < PCP_COUNT; ++i) {
		uint8_t ranges[64][3];
		uint8_t total_ranges = 0;
		int inside = 0;

		uint8_t range_biggest_val;
		int range_biggest_index = -1;

		memset(ranges, 0, sizeof(ranges));

		for (j = 0; j < DSCP_PCP_MAX; ++j) {
			if (pcp_dscp[i][j]) {
				if (inside == 0) {
					inside = 1;
					ranges[total_ranges][RANGE_MIN] = j;
					ranges[total_ranges][RANGE_MAX] = j;
					ranges[total_ranges][RANGE_TOTAL] = 1;

					if (range_biggest_index == -1) {
						range_biggest_index = total_ranges;
						range_biggest_val = 1;
					}

					total_ranges++;
				} else {
					ranges[total_ranges - 1][RANGE_MAX] = j;
					ranges[total_ranges - 1][RANGE_TOTAL]++;
					if (range_biggest_val <
					    ranges[total_ranges - 1][RANGE_TOTAL]) {
						range_biggest_index = total_ranges - 1;
						range_biggest_val++;
					}
				}
			} else {
				if (inside == 1) {
					inside = 0;
				}
			}
		}

		if (range_biggest_index != -1) {
			pcp_dscp_min[i] = ranges[range_biggest_index][RANGE_MIN];
			pcp_dscp_max[i] = ranges[range_biggest_index][RANGE_MAX];
		} else {
			pcp_dscp_min[i] = DSCP_NEUTRAL;
			pcp_dscp_max[i] = DSCP_NEUTRAL;
		}
	}

	/* Find exceptions that don't belong to [DSCP_min, DSCP_max] range */
	for (i = 0; i < DSCP_PCP_MAX; ++i) {
		uint8_t pcp = dscp_pcp[i];
		uint8_t dscp = i;

		if (dscp < pcp_dscp_min[pcp] ||
			dscp > pcp_dscp_max[pcp]) {
			dscp_up[total_du][0] = dscp;
			dscp_up[total_du][1] = pcp;
			total_du++;
			if (total_du == DSCP_UP_EXC_MAX) {
				rc = DSCP_PCP_CONV_RESULT_OK_TOO_MANY_EXC;
				break;
			}
		}
	}

	/* Write out pcp_X_dscp_min,pcp_X_dscp_max */
	final_len = 0;
	for (i = 0; i < total_du; ++i) {
		qos_map[i * 2 + 0] = dscp_up[i][0];
		qos_map[i * 2 + 1] = dscp_up[i][1];
		final_len += 2;
	}

	if (rc != DSCP_PCP_CONV_RESULT_OK &&
		rc != DSCP_PCP_CONV_RESULT_OK_TOO_MANY_EXC)
		return rc;

	/* Write out PCP_x_DSCP_min, PCP_x_DSCP_max */
	for (i = 0; i < PCP_COUNT; ++i) {
		/*
		 * No need for bounds check - result length can't exceed the buffer
		 * length
		 */
		qos_map[final_len++] = pcp_dscp_min[i];
		qos_map[final_len++] = pcp_dscp_max[i];
	}

#undef PCP_COUNT
#undef DSCP_PCP_MAX
#undef DSCP_UP_EXC_MAX
#undef RANGE_MIN
#undef RANGE_MAX
#undef RANGE_TOTAL
#undef TMP_BUF_LEN

	if (rc == DSCP_PCP_CONV_RESULT_OK) {
		*qos_map_len = final_len;
	}

	return rc;
}

enum dscp_pcp_conv_result dscp_pcp2qosmap_simple(int pcp,
                                                 uint8_t qos_map[58],
                                                 size_t *qos_map_len)
{
	size_t i;

	for (i = 0; i <= 0x7; ++i) {
		/*
		 * Map DSCP [0, 63] to PCP passed as argument,
		 * all other PCP->DSCP ranges are mapped to [255, 255] (unused)
		 */
		qos_map[i * 2] = (pcp == i) ? 0 : 255;
		qos_map[i * 2 + 1] = (pcp == i) ? 63 : 255;
		*qos_map_len += 2;
	}

	return DSCP_PCP_CONV_RESULT_OK;
}

int qos_apply_dscp_rules(void *agent)
{
	enum dscp_pcp_conv_result rc;
	struct agent *a = (struct agent *)agent;
	struct qos_rule *r;
	struct netif_apcfg *fcfg = NULL;
	int biggest_precedence_idx = -1;
	int biggest_precedence = -1;
	size_t i;
	int qos_map_likely_broken = 0;

	dbg("%s: finding DSCP rule with the highest precedence\n", __func__);
	i = 0;
	list_for_each_entry(r, &a->qos_rules, list) {
		if (r->spr.precedence > biggest_precedence) {
			biggest_precedence = r->spr.precedence;
			biggest_precedence_idx = i;
		}
		++i;
	}

	dbg("%s: applying DSCP rules...\n", __func__);
	i = 0; /* cppcheck-suppress redundantAssignment */
	list_for_each_entry(r, &a->qos_rules, list) {
		uint8_t qos_map[58];
		size_t  qos_map_len;

		/*
		 * TODO: this part is a little bit strange because it was made
		 *       with SCS/MSCS rules in mind as well.
		 *       In future, the rules will be sorted according to
		 *       precedence (above) and then iterated over in this
		 *       loop without the explicit check for the precedence.
		 */
		if ((i++) != biggest_precedence_idx) {
			dbg("%s: ignore rule with smaller precedence\n", __func__);
			continue;
		}

		if (r->spr.always_match == 0) {
			dbg("%s: don't install not always-match rule\n", __func__);
			continue;
		}

		qos_map_len = 0;
		if (r->spr.output < 0x08) {
			rc = dscp_pcp2qosmap_simple(r->spr.output, qos_map, &qos_map_len);
		} else if (r->spr.output == 0x08) {
			rc = dscp_pcp2qosmap(r->dscp.dscp_pcp, qos_map, &qos_map_len);
		} else {
			err("%s: unsupported SPR output value\n", __func__);
			rc = DSCP_PCP_CONV_RESULT_FAIL;
		}

		if (rc != DSCP_PCP_CONV_RESULT_OK &&
			rc != DSCP_PCP_CONV_RESULT_OK_TOO_MANY_EXC) {
			err("%s: wrong QoS map\n", __func__);
			continue;
		}

		list_for_each_entry(fcfg, &a->cfg.aplist, list) {
			int rv;

			rv = wifi_set_qos_map(fcfg->name, qos_map, qos_map_len);
			if (rv != 0) {
				if (qos_map_likely_broken == 0) {
					err("%s: setting qos map on '%s' failed. "
						"Silencing errors on future attempts\n",
						__func__,
						fcfg->name);
					qos_map_likely_broken = 1;
				} else {
					info("Failed to set QoS Map on '%s'\n", fcfg->name);
				}
			}
		}

		dbg("%s: DSCP rule applied\n", __func__);
	}

	qos_sync_rules_to_nodes(agent);

	dbg("%s: done with DSCP rules\n", __func__);

	return 0;
}

int qos_sync_rules_to_nodes(void *agent)
{
	struct agent *a = (struct agent *)agent;
	struct netif_apcfg *fcfg = NULL;
	struct node *n = NULL;

	if (list_empty(&a->qos_rules))
		return 0;

	list_for_each_entry(n, &a->nodelist, list) {
		list_for_each_entry(fcfg, &a->cfg.aplist, list) {
			int rv;

			rv = wifi_send_qos_map_conf(fcfg->name, n->alid);
			if (rv != 0) {
				err("%s: sending QoS map configuration failed with code %d\n",
					__func__, rv);
			}
		}
	}

	return 0;
}

int qos_get_rules(void *agent)
{
	struct agent *a = (struct agent *)agent;
	struct qos_rule *r;
	int nr = 0;

	info("Registered QoS rules:\n");
	list_for_each_entry(r, &a->qos_rules, list) {
		info("[%d] add %d prec %doutput %d always %d\n",
		     r->spr.rule_id,
		     r->spr.add_remove,
		     r->spr.precedence,
		     r->spr.output,
		     r->spr.always_match);
		nr++;
	}

	return nr;
}
#endif
