Skip to content
Snippets Groups Projects
qos.c 9.09 KiB
Newer Older
  • Learn to ignore specific revisions
  • /*
     * rules.c - rules wrapper functions
     *
     * Copyright (C) 2019 IOPSYS Software Solutions AB. All rights reserved.
     *
     * Author: anjan.chanda@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;
    		}
    
    	}
    
    	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;
    		}
    
    	}
    
    #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)
    		 */
    
    Maxim Menshikov's avatar
    Maxim Menshikov committed
    		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;
    
    
    Maxim Menshikov's avatar
    Maxim Menshikov committed
    	dbg("%s: finding DSCP rule with the highest precedence\n", __func__);
    
    	i = 0;
    	list_for_each_entry(r, &a->qos_rules, list) {
    
    Maxim Menshikov's avatar
    Maxim Menshikov committed
    		if (r->spr.precedence > biggest_precedence) {
    
    			biggest_precedence = r->spr.precedence;
    			biggest_precedence_idx = i;
    		}
    		++i;
    	}
    
    
    	dbg("%s: applying DSCP rules...\n", __func__);
    
    	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("hostapd_cli -i %s set_qos_map_set %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("hostapd_cli -i %s send_qos_map_conf %s",
    				   fcfg->name, mac);
    		}
    	}
    
    
    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;
    }