qos.c

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

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <net/if_arp.h>

#include <linux/if_bridge.h>

#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include <json-c/json.h>
#include <libubox/blobmsg.h>
#include <libubox/blobmsg_json.h>
#include <libubox/uloop.h>
#include <libubox/ustream.h>
#include <libubox/utils.h>
#include <libubus.h>

#if (EASYMESH_VERSION >2)
#include <openssl/err.h>
#include <openssl/bn.h>
#include <openssl/evp.h>
#include <openssl/x509.h>
#include <openssl/ec.h>
#endif

#include <cmdu.h>
#include <1905_tlvs.h>
#include <easymesh.h>
#include <i1905_wsc.h>
#include <map_module.h>

#include <uci.h>

#include "timer.h"
#include "utils/1905_ubus.h"
#include "utils/utils.h"
#include "utils/debug.h"
#include "utils/liblist.h"
#include "steer_rules.h"
#if (EASYMESH_VERSION > 2)
#include "dpp.h"
#endif
#include "config.h"
#include "nl.h"
#include "agent.h"
#include "qos.h"
#include "qos_internal.h"

#if (EASYMESH_VERSION > 2)

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

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

	list_for_each_entry_safe(p, tmp, list_head, list) {
		qos_rule_free(p);
	}
}

int qos_add_dscp_rule(void *agent,
                      struct tlv_spr *spr,
                      struct ieee1905_dscp_pcp_usr *dscp_pcp)
{
	struct agent *a = (struct agent *)agent;
	struct temp_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 temp_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 ieee1905_dscp_pcp_usr));

	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 temp_rule *r;

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

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

dscp_pcp_conv_result dscp_pcp2qosmap(uint8_t dscp_pcp[64],
                                     char   *qos_map,
                                     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)

	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;

	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;

		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) {
		char pair[TMP_BUF_LEN];

		/*
		 * No need for bounds check - result length can't exceed the buffer
		 * length
		 */
		sprintf(pair, "%d,%d,", dscp_up[i][0], dscp_up[i][1]);
		final_len += strlen(pair);
		if (final_len > qos_map_len) {
			rc = DSCP_PCP_CONV_RESULT_TOO_LONG;
			break;
		}

		strcat(qos_map, pair);
	}

	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) {
		char pair[TMP_BUF_LEN];

		/*
		 * No need for bounds check - result length can't exceed the buffer
		 * length
		 */
		sprintf(pair, "%s%d,%d",
		        (i != 0 || total_du != 0) ? "," : "",
		        pcp_dscp_min[i],
		        pcp_dscp_max[i]);
		final_len += strlen(pair);
		if (final_len > qos_map_len) {
			rc = DSCP_PCP_CONV_RESULT_TOO_LONG;
			break;
		}

		strcat(qos_map, pair);
	}

#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

	return rc;
}

dscp_pcp_conv_result dscp_pcp2qosmap_simple(int pcp, char *str, size_t len)
{
	size_t i;

	for (i = 0; i <= 0x7; ++i) {
		char s[50];

		/*
		 * Map DSCP [0, 63] to PCP passed as argument,
		 * all other PCP->DSCP ranges are mapped to [255, 255] (unused)
		 */
		sprintf(s, "%s%d,%d", (i != 0) ? "," : "",
				(pcp == i) ? 0 : 255,
				(pcp == i) ? 63 : 255);
		if ((strlen(str) + strlen(s)) >= len)
			return DSCP_PCP_CONV_RESULT_TOO_LONG;

		strcat(str, s);
	}

	return DSCP_PCP_CONV_RESULT_OK;
}

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

	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;
	list_for_each_entry(r, &a->qos_rules, list) {
		char str[512] = { '\0' };

		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;
		}

		if (r->spr.output < 0x08) {
			rc = dscp_pcp2qosmap_simple(r->spr.output, str, sizeof(str) - 1);
		} else if (r->spr.output == 0x08) {
			rc = dscp_pcp2qosmap(r->dscp.dscp_pcp, str, sizeof(str) - 1);
		} 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) {
			runCmd("/lib/wifi/multiap set_qos_map %s %s", fcfg->name, str);
		}

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

	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;

	list_for_each_entry(n, &a->nodelist, list) {
		char mac[50];

		hwaddr_ntoa(n->alid, mac);

		list_for_each_entry(fcfg, &a->cfg.aplist, list) {
			runCmd("/lib/wifi/multiap set_qos_map %s %s", fcfg->name, mac);
		}
	}

	return 0;
}

int qos_get_rules(void *agent)
{
	struct agent *a = (struct agent *)agent;
	struct temp_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