├── CMakeLists.txt
├── qosify-bpf.h
├── main.c
├── loader.c
├── qosify.h
├── README
├── bpf_skb_utils.h
├── dns.c
├── qosify-bpf.c
├── ubus.c
├── interface.c
└── map.c


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.10)
 2 | 
 3 | PROJECT(qosify C)
 4 | 
 5 | ADD_DEFINITIONS(-Os -Wall -Wno-unknown-warning-option -Wno-array-bounds -Wno-format-truncation -Werror --std=gnu99)
 6 | 
 7 | SET(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "")
 8 | 
 9 | IF (NOT DEFINED LIBNL_LIBS)
10 | 	include(FindPkgConfig)
11 | 	pkg_search_module(LIBNL libnl-3.0 libnl-3 libnl nl-3 nl)
12 | 	IF (LIBNL_FOUND)
13 | 		include_directories(${LIBNL_INCLUDE_DIRS})
14 | 		SET(LIBNL_LIBS ${LIBNL_LIBRARIES})
15 | 	ENDIF()
16 | ENDIF()
17 | 
18 | find_library(bpf NAMES bpf)
19 | ADD_EXECUTABLE(qosify main.c loader.c map.c ubus.c interface.c dns.c)
20 | TARGET_LINK_LIBRARIES(qosify ${bpf} ubox ubus ${LIBNL_LIBS})
21 | 
22 | INSTALL(TARGETS qosify
23 | 	RUNTIME DESTINATION ${CMAKE_INSTALL_SBINDIR}
24 | )
25 | 


--------------------------------------------------------------------------------
/qosify-bpf.h:
--------------------------------------------------------------------------------
 1 | // SPDX-License-Identifier: GPL-2.0+
 2 | /*
 3 |  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
 4 |  */
 5 | #ifndef __BPF_QOSIFY_H
 6 | #define __BPF_QOSIFY_H
 7 | 
 8 | #define QOSIFY_MAX_CLASS_ENTRIES	16
 9 | #define QOSIFY_DEFAULT_CLASS_ENTRIES	2
10 | 
11 | #ifndef QOSIFY_FLOW_BUCKET_SHIFT
12 | #define QOSIFY_FLOW_BUCKET_SHIFT	13
13 | #endif
14 | 
15 | #define QOSIFY_FLOW_BUCKETS		(1 << QOSIFY_FLOW_BUCKET_SHIFT)
16 | 
17 | /* rodata per-instance flags */
18 | #define QOSIFY_INGRESS			(1 << 0)
19 | #define QOSIFY_IP_ONLY			(1 << 1)
20 | 
21 | #define QOSIFY_DSCP_VALUE_MASK		((1 << 6) - 1)
22 | #define QOSIFY_DSCP_FALLBACK_FLAG	(1 << 6)
23 | #define QOSIFY_DSCP_CLASS_FLAG		(1 << 7)
24 | 
25 | #define QOSIFY_CLASS_FLAG_PRESENT	(1 << 0)
26 | 
27 | struct qosify_dscp_val {
28 | 	uint8_t ingress;
29 | 	uint8_t egress;
30 | };
31 | 
32 | /* global config data */
33 | 
34 | struct qosify_flow_config {
35 | 	uint8_t dscp_prio;
36 | 	uint8_t dscp_bulk;
37 | 
38 | 	uint8_t bulk_trigger_timeout;
39 | 	uint16_t bulk_trigger_pps;
40 | 
41 | 	uint16_t prio_max_avg_pkt_len;
42 | };
43 | 
44 | struct qosify_config {
45 | 	uint8_t dscp_icmp;
46 | };
47 | 
48 | struct qosify_ip_map_val {
49 | 	uint8_t dscp; /* must be first */
50 | 	uint8_t seen;
51 | };
52 | 
53 | struct qosify_class {
54 | 	struct qosify_flow_config config;
55 | 
56 | 	struct qosify_dscp_val val;
57 | 
58 | 	uint8_t flags;
59 | 
60 | 	uint64_t packets;
61 | };
62 | 
63 | #endif
64 | 


--------------------------------------------------------------------------------
/main.c:
--------------------------------------------------------------------------------
  1 | // SPDX-License-Identifier: GPL-2.0+
  2 | /*
  3 |  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
  4 |  */
  5 | #include <sys/wait.h>
  6 | #include <stdio.h>
  7 | #include <unistd.h>
  8 | #include <stdint.h>
  9 | 
 10 | #include <libubox/uloop.h>
 11 | 
 12 | #include "qosify.h"
 13 | 
 14 | static int usage(const char *progname)
 15 | {
 16 | 	fprintf(stderr, "Usage: %s [options]\n"
 17 | 		"Options:\n"
 18 | 		"	-l <file>	Load defaults from <file>\n"
 19 | 		"	-o		only load program/maps without running as daemon\n"
 20 | 		"\n", progname);
 21 | 
 22 | 	return 1;
 23 | }
 24 | 
 25 | int qosify_run_cmd(char *cmd, bool ignore_error)
 26 | {
 27 | 	char *argv[] = { "sh", "-c", cmd, NULL };
 28 | 	bool first = true;
 29 | 	int status = -1;
 30 | 	char buf[512];
 31 | 	int fds[2];
 32 | 	FILE *f;
 33 | 	int pid;
 34 | 
 35 | 	if (pipe(fds))
 36 | 		return -1;
 37 | 
 38 | 	pid = fork();
 39 | 	if (!pid) {
 40 | 		close(fds[0]);
 41 | 		if (fds[1] != STDOUT_FILENO)
 42 | 			dup2(fds[1], STDOUT_FILENO);
 43 | 		if (fds[1] != STDERR_FILENO)
 44 | 			dup2(fds[1], STDERR_FILENO);
 45 | 		if (fds[1] > STDERR_FILENO)
 46 | 			close(fds[1]);
 47 | 		execv("/bin/sh", argv);
 48 | 		exit(1);
 49 | 	}
 50 | 
 51 | 	if (pid < 0)
 52 | 		return -1;
 53 | 
 54 | 	close(fds[1]);
 55 | 	f = fdopen(fds[0], "r");
 56 | 	if (!f) {
 57 | 		close(fds[0]);
 58 | 		goto out;
 59 | 	}
 60 | 
 61 | 	while (fgets(buf, sizeof(buf), f) != NULL) {
 62 | 		if (!strlen(buf))
 63 | 			break;
 64 | 		if (ignore_error)
 65 | 			continue;
 66 | 		if (first) {
 67 | 			ULOG_WARN("Command: %s\n", cmd);
 68 | 			first = false;
 69 | 		}
 70 | 		ULOG_WARN("%s%s", buf, strchr(buf, '\n') ? "" : "\n");
 71 | 	}
 72 | 
 73 | 	fclose(f);
 74 | 
 75 | out:
 76 | 	while (waitpid(pid, &status, 0) < 0)
 77 | 		if (errno != EINTR)
 78 | 			break;
 79 | 
 80 | 	return status;
 81 | }
 82 | 
 83 | 
 84 | int main(int argc, char **argv)
 85 | {
 86 | 	const char *load_file = NULL;
 87 | 	bool oneshot = false;
 88 | 	int ch;
 89 | 
 90 | 	while ((ch = getopt(argc, argv, "fl:o")) != -1) {
 91 | 		switch (ch) {
 92 | 		case 'f':
 93 | 			break;
 94 | 		case 'l':
 95 | 			load_file = optarg;
 96 | 			break;
 97 | 		case 'o':
 98 | 			oneshot = true;
 99 | 			break;
100 | 		default:
101 | 			return usage(argv[0]);
102 | 		}
103 | 	}
104 | 
105 | 	if (qosify_loader_init())
106 | 		return 2;
107 | 
108 | 	if (qosify_map_init())
109 | 		return 2;
110 | 
111 | 	if (qosify_map_load_file(load_file))
112 | 		return 2;
113 | 
114 | 	if (oneshot)
115 | 		return 0;
116 | 
117 | 	ulog_open(ULOG_SYSLOG, LOG_DAEMON, "qosify");
118 | 	uloop_init();
119 | 
120 | 	if (qosify_ubus_init() ||
121 | 	    qosify_iface_init())
122 | 		return 2;
123 | 
124 | 	qosify_dns_init();
125 | 
126 | 	uloop_run();
127 | 
128 | 	qosify_ubus_stop();
129 | 	qosify_iface_stop();
130 | 
131 | 	uloop_done();
132 | 
133 | 	return 0;
134 | }
135 | 


--------------------------------------------------------------------------------
/loader.c:
--------------------------------------------------------------------------------
  1 | // SPDX-License-Identifier: GPL-2.0+
  2 | /*
  3 |  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
  4 |  */
  5 | #include <sys/resource.h>
  6 | #include <sys/stat.h>
  7 | #include <arpa/inet.h>
  8 | #include <glob.h>
  9 | #include <unistd.h>
 10 | 
 11 | #include "qosify.h"
 12 | 
 13 | static struct {
 14 | 	const char *suffix;
 15 | 	uint32_t flags;
 16 | 	int fd;
 17 | } bpf_progs[] = {
 18 | 	{ "egress_eth",  0 },
 19 | 	{ "egress_ip",   QOSIFY_IP_ONLY },
 20 | 	{ "ingress_eth", QOSIFY_INGRESS },
 21 | 	{ "ingress_ip",  QOSIFY_INGRESS | QOSIFY_IP_ONLY },
 22 | };
 23 | 
 24 | static int qosify_bpf_pr(enum libbpf_print_level level, const char *format,
 25 | 		     va_list args)
 26 | {
 27 | 	return vfprintf(stderr, format, args);
 28 | }
 29 | 
 30 | static void qosify_init_env(void)
 31 | {
 32 | 	struct rlimit limit = {
 33 | 		.rlim_cur = RLIM_INFINITY,
 34 | 		.rlim_max = RLIM_INFINITY,
 35 | 	};
 36 | 
 37 | 	setrlimit(RLIMIT_MEMLOCK, &limit);
 38 | }
 39 | 
 40 | static void qosify_fill_rodata(struct bpf_object *obj, uint32_t flags)
 41 | {
 42 | 	struct bpf_map *map = NULL;
 43 | 
 44 | 	while ((map = bpf_object__next_map(obj, map)) != NULL) {
 45 | 		if (!strstr(bpf_map__name(map), ".rodata"))
 46 | 			continue;
 47 | 
 48 | 		bpf_map__set_initial_value(map, &flags, sizeof(flags));
 49 | 	}
 50 | }
 51 | 
 52 | const char *qosify_get_program(uint32_t flags, int *fd)
 53 | {
 54 | 	int i;
 55 | 
 56 | 	for (i = 0; i < ARRAY_SIZE(bpf_progs); i++) {
 57 | 		if (bpf_progs[i].flags != flags)
 58 | 			continue;
 59 | 
 60 | 		*fd = bpf_progs[i].fd;
 61 | 		return bpf_progs[i].suffix;
 62 | 	}
 63 | 
 64 | 	return NULL;
 65 | }
 66 | 
 67 | 
 68 | static int
 69 | qosify_create_program(int idx)
 70 | {
 71 | 	DECLARE_LIBBPF_OPTS(bpf_object_open_opts, opts,
 72 | 		.pin_root_path = CLASSIFY_DATA_PATH,
 73 | 	);
 74 | 	struct bpf_program *prog;
 75 | 	struct bpf_object *obj;
 76 | 	char path[256];
 77 | 	int err;
 78 | 
 79 | 	snprintf(path, sizeof(path), CLASSIFY_PIN_PATH "_" "%s", bpf_progs[idx].suffix);
 80 | 
 81 | 	obj = bpf_object__open_file(CLASSIFY_PROG_PATH, &opts);
 82 | 	err = libbpf_get_error(obj);
 83 | 	if (err) {
 84 | 		perror("bpf_object__open_file");
 85 | 		return -1;
 86 | 	}
 87 | 
 88 | 	prog = bpf_object__find_program_by_name(obj, "classify");
 89 | 	if (!prog) {
 90 | 		fprintf(stderr, "Can't find classifier prog\n");
 91 | 		return -1;
 92 | 	}
 93 | 
 94 | 	bpf_program__set_type(prog, BPF_PROG_TYPE_SCHED_CLS);
 95 | 
 96 | 	qosify_fill_rodata(obj, bpf_progs[idx].flags);
 97 | 
 98 | 	err = bpf_object__load(obj);
 99 | 	if (err) {
100 | 		perror("bpf_object__load");
101 | 		return -1;
102 | 	}
103 | 
104 | 	libbpf_set_print(NULL);
105 | 
106 | 	unlink(path);
107 | 	err = bpf_program__pin(prog, path);
108 | 	if (err) {
109 | 		fprintf(stderr, "Failed to pin program to %s: %s\n",
110 | 			path, strerror(-err));
111 | 		return -1;
112 | 	}
113 | 
114 | 	bpf_object__close(obj);
115 | 
116 | 	err = bpf_obj_get(path);
117 | 	if (err < 0) {
118 | 		fprintf(stderr, "Failed to load pinned program %s: %s\n",
119 | 			path, strerror(errno));
120 | 	}
121 | 	bpf_progs[idx].fd = err;
122 | 
123 | 	return 0;
124 | }
125 | 
126 | int qosify_loader_init(void)
127 | {
128 | 	glob_t g;
129 | 	int i;
130 | 
131 | 	if (glob(CLASSIFY_DATA_PATH "/*", 0, NULL, &g) == 0) {
132 | 		for (i = 0; i < g.gl_pathc; i++)
133 | 			unlink(g.gl_pathv[i]);
134 | 	}
135 | 
136 | 	libbpf_set_print(qosify_bpf_pr);
137 | 
138 | 	qosify_init_env();
139 | 
140 | 	for (i = 0; i < ARRAY_SIZE(bpf_progs); i++) {
141 | 		if (qosify_create_program(i))
142 | 			return -1;
143 | 	}
144 | 
145 | 	return 0;
146 | }
147 | 


--------------------------------------------------------------------------------
/qosify.h:
--------------------------------------------------------------------------------
  1 | // SPDX-License-Identifier: GPL-2.0+
  2 | /*
  3 |  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
  4 |  */
  5 | #ifndef __QOS_CLASSIFY_H
  6 | #define __QOS_CLASSIFY_H
  7 | 
  8 | #include <stdbool.h>
  9 | #include <regex.h>
 10 | 
 11 | #include <bpf/bpf.h>
 12 | #include <bpf/libbpf.h>
 13 | 
 14 | #include "qosify-bpf.h"
 15 | 
 16 | #include <libubox/utils.h>
 17 | #include <libubox/avl.h>
 18 | #include <libubox/blobmsg.h>
 19 | #include <libubox/ulog.h>
 20 | 
 21 | #include <netinet/in.h>
 22 | 
 23 | #define CLASSIFY_PROG_PATH	"/lib/bpf/qosify-bpf.o"
 24 | #define CLASSIFY_PIN_PATH	"/sys/fs/bpf/qosify"
 25 | #define CLASSIFY_DATA_PATH	"/sys/fs/bpf/qosify_data"
 26 | 
 27 | #define QOSIFY_DNS_IFNAME "ifb-dns"
 28 | 
 29 | #define QOSIFY_PRIO_BASE	0x110
 30 | 
 31 | enum qosify_map_id {
 32 | 	CL_MAP_TCP_PORTS,
 33 | 	CL_MAP_UDP_PORTS,
 34 | 	CL_MAP_IPV4_ADDR,
 35 | 	CL_MAP_IPV6_ADDR,
 36 | 	CL_MAP_CLASS,
 37 | 	CL_MAP_CONFIG,
 38 | 	CL_MAP_DNS,
 39 | 	__CL_MAP_MAX,
 40 | };
 41 | 
 42 | struct qosify_map_data {
 43 | 	enum qosify_map_id id;
 44 | 
 45 | 	bool file : 1;
 46 | 	bool user : 1;
 47 | 
 48 | 	uint8_t dscp;
 49 | 	uint8_t file_dscp;
 50 | 
 51 | 	union {
 52 | 		uint32_t port;
 53 | 		struct in_addr ip;
 54 | 		struct in6_addr ip6;
 55 | 		struct {
 56 | 			uint32_t seq : 30;
 57 | 			uint32_t only_cname : 1;
 58 | 			const char *pattern;
 59 | 			regex_t regex;
 60 | 		} dns;
 61 | 	} addr;
 62 | };
 63 | 
 64 | struct qosify_map_entry {
 65 | 	struct avl_node avl;
 66 | 
 67 | 	uint32_t timeout;
 68 | 
 69 | 	struct qosify_map_data data;
 70 | };
 71 | 
 72 | 
 73 | extern int qosify_map_timeout;
 74 | extern int qosify_active_timeout;
 75 | extern struct qosify_config config;
 76 | extern struct qosify_flow_config flow_config;
 77 | 
 78 | int qosify_run_cmd(char *cmd, bool ignore_error);
 79 | 
 80 | int qosify_loader_init(void);
 81 | const char *qosify_get_program(uint32_t flags, int *fd);
 82 | 
 83 | int qosify_map_init(void);
 84 | int qosify_map_dscp_value(const char *val, uint8_t *dscp);
 85 | int qosify_map_load_file(const char *file);
 86 | void __qosify_map_set_entry(struct qosify_map_data *data);
 87 | int qosify_map_set_entry(enum qosify_map_id id, bool file, const char *str,
 88 | 			 uint8_t dscp);
 89 | void qosify_map_reload(void);
 90 | void qosify_map_clear_files(void);
 91 | void qosify_map_gc(void);
 92 | void qosify_map_dump(struct blob_buf *b);
 93 | void qosify_map_stats(struct blob_buf *b, bool reset);
 94 | void qosify_map_set_dscp_default(enum qosify_map_id id, uint8_t val);
 95 | void qosify_map_reset_config(void);
 96 | void qosify_map_update_config(void);
 97 | void qosify_map_set_classes(struct blob_attr *val);
 98 | int qosify_map_lookup_dns_entry(char *host, bool cname, uint8_t *dscp, uint32_t *seq);
 99 | int qosify_map_add_dns_host(char *host, const char *addr, const char *type, int ttl);
100 | int map_parse_flow_config(struct qosify_flow_config *cfg, struct blob_attr *attr,
101 | 			  bool reset);
102 | int map_fill_dscp_value(uint8_t *dest, struct blob_attr *attr, bool reset);
103 | 
104 | int qosify_iface_init(void);
105 | void qosify_iface_config_update(struct blob_attr *ifaces, struct blob_attr *devs);
106 | void qosify_iface_check(void);
107 | void qosify_iface_status(struct blob_buf *b);
108 | void qosify_iface_get_devices(struct blob_buf *b);
109 | void qosify_iface_stop(void);
110 | 
111 | int qosify_dns_init(void);
112 | void qosify_dns_stop(void);
113 | 
114 | int qosify_ubus_init(void);
115 | void qosify_ubus_stop(void);
116 | int qosify_ubus_check_interface(const char *name, char *ifname, int ifname_len);
117 | void qosify_ubus_update_bridger(bool shutdown);
118 | 
119 | #endif
120 | 


--------------------------------------------------------------------------------
/README:
--------------------------------------------------------------------------------
  1 | QoSify is simple daemon for setting up and managing CAKE along with a custom
  2 | eBPF based classifier that sets DSCP fields of packets.
  3 | 
  4 | It supports the following features:
  5 | - simple TCP/UDP port based mapping
  6 | - IP address based mapping
  7 | - priority boosting based on average packet size
  8 | - bulk flow detection based on number of packets per second
  9 | - dynamically add IP entries with timeout
 10 | - dns regex entries and ubus api for providing dns lookup results
 11 | 
 12 | It can be configured via ubus call qosify config.
 13 | 
 14 | This call supports the following parameters:
 15 | - "reset": BOOL
 16 | 	Reset the config to defaults instead of only updating supplied values
 17 | 
 18 | - "files": ARRAY of STRING
 19 | 	List of files with port/IP/host mappings
 20 | 
 21 | - "timeout": INT32
 22 | 	Default timeout for dynamically added entries
 23 | 
 24 | - "dscp_default_udp": STRING
 25 | 	Default DSCP value for UDP packets
 26 | 
 27 | - "dscp_default_tcp": STRING
 28 | 	Default DSCP value for TCP packets
 29 | 
 30 | - "dscp_prio": STRING
 31 | 	DSCP value for priority-marked packets
 32 | 
 33 | - "dscp_bulk": STRING
 34 | 	DSCP value for bulk-marked packets
 35 | 
 36 | - "dscp_icmp": STRING
 37 | 	DSCP value for ICMP packets
 38 | 
 39 | - "bulk_trigger_pps": INT32
 40 | 	Number of packets per second to trigger bulk flow detection
 41 | 
 42 | - "bulk_trigger_timeout": INT32
 43 | 	Time below bulk_trigger_pps threshold until a bulk flow mark is removed
 44 | 
 45 | - "prio_max_avg_pkt_len": INT32
 46 | 	Maximum average packet length for marking a flow as priority
 47 | 
 48 | - "interfaces": TABLE of TABLE
 49 | 	netifd interfaces to enable QoS on
 50 | 
 51 | - "devices": TABLE of TABLE
 52 | 	netdevs to enable QoS on
 53 | 
 54 | 
 55 | interface/device properties:
 56 | - "bandwidth_up": STRING
 57 | 	Uplink bandwidth (same format as tc)
 58 | 
 59 | - "bandwidth_down": STRING
 60 | 	Downlink bandwidth (same format as tc)
 61 | 
 62 | - "ingress": BOOL
 63 | 	Enable ingress shaping
 64 | 
 65 | - "egress": BOOL
 66 | 	Enable egress shaping
 67 | 
 68 | - "mode": STRING
 69 | 	CAKE diffserv mode
 70 | 
 71 | - "nat": BOOL
 72 | 	Enable CAKE NAT host detection via conntrack
 73 | 
 74 | - "host_isolate": BOOL
 75 | 	Enable CAKE host isolation
 76 | 
 77 | - "autorate_ingress": BOOL
 78 | 	Enable CAKE automatic rate estimation for ingress
 79 | 
 80 | - "ingress_options": STRING
 81 | 	CAKE ingress options
 82 | 
 83 | - "egress_options": STRING
 84 | 	CAKE egress options
 85 | 
 86 | - "options": STRING
 87 | 	CAKE options for ingress + egress
 88 | 
 89 | 
 90 | Mapping file syntax:
 91 | 
 92 | Each line has two whitespace separated fields, match and dscp
 93 | match is one of:
 94 | - tcp:<port>[-<endport>]
 95 | 	TCP single port, or range from <port> to <endport>
 96 | - udp:<port>[-<endport>]
 97 | 	UDP single port, or range from <port> to <endport>
 98 | - <ipaddr>
 99 | 	IPv4 address, e.g. 1.1.1.1
100 | - <ipv6addr>
101 | 	IPv6 address, e.g. ff01::1
102 | - dns:<pattern>
103 | 	fnmatch() pattern supporting * and ? as wildcard characters
104 | - dns:/<regex>
105 | 	POSIX.2 extended regular expression for matching hostnames
106 | 	Only works, if dns lookups are passed to qosify via the add_dns_host ubus call.
107 | - dns_c:...
108 | 	Like dns:... but only matches cname entries
109 | 
110 | dscp can be a raw value, or a codepoint like CS0
111 | Adding a + in front of the value tells qosify to only override the DSCP value if it is zero
112 | DNS entries are compared in the order in which they are specified in the config, using the
113 | first matching entry.
114 | 
115 | 
116 | Planned features:
117 | - Support for LAN host based priority
118 | 


--------------------------------------------------------------------------------
/bpf_skb_utils.h:
--------------------------------------------------------------------------------
  1 | // SPDX-License-Identifier: GPL-2.0-or-later
  2 | /*
  3 |  * Copyright (C) 2022 Felix Fietkau <nbd@nbd.name>
  4 |  * Version: 2022-09-21
  5 |  */
  6 | #ifndef __BPF_SKB_UTILS_H
  7 | #define __BPF_SKB_UTILS_H
  8 | 
  9 | #include <uapi/linux/bpf.h>
 10 | #include <uapi/linux/if_ether.h>
 11 | #include <uapi/linux/ip.h>
 12 | #include <uapi/linux/ipv6.h>
 13 | #include <linux/if_vlan.h>
 14 | #include <linux/ip.h>
 15 | #include <net/ipv6.h>
 16 | #include <bpf/bpf_helpers.h>
 17 | #include <bpf/bpf_endian.h>
 18 | 
 19 | struct skb_parser_info {
 20 | 	struct __sk_buff *skb;
 21 | 	__u32 offset;
 22 | 	int proto;
 23 | };
 24 | 
 25 | static __always_inline void *__skb_data(struct __sk_buff *skb)
 26 | {
 27 | 	return (void *)(long)READ_ONCE(skb->data);
 28 | }
 29 | 
 30 | static __always_inline void *
 31 | skb_ptr(struct __sk_buff *skb, __u32 offset, __u32 len)
 32 | {
 33 | 	void *ptr = __skb_data(skb) + offset;
 34 | 	void *end = (void *)(long)(skb->data_end);
 35 | 
 36 | 	if (ptr + len >= end)
 37 | 		return NULL;
 38 | 
 39 | 	return ptr;
 40 | }
 41 | 
 42 | static __always_inline void *
 43 | skb_info_ptr(struct skb_parser_info *info, __u32 len)
 44 | {
 45 | 	__u32 offset = info->offset;
 46 | 	return skb_ptr(info->skb, offset, len);
 47 | }
 48 | 
 49 | static __always_inline void
 50 | skb_parse_init(struct skb_parser_info *info, struct __sk_buff *skb)
 51 | {
 52 | 	*info = (struct skb_parser_info){
 53 | 		.skb = skb
 54 | 	};
 55 | }
 56 | 
 57 | static __always_inline struct ethhdr *
 58 | skb_parse_ethernet(struct skb_parser_info *info)
 59 | {
 60 | 	struct ethhdr *eth;
 61 | 	int len;
 62 | 
 63 | 	len = sizeof(*eth) + 2 * sizeof(struct vlan_hdr) + sizeof(struct ipv6hdr);
 64 | 	if (len > info->skb->len)
 65 | 		len = info->skb->len;
 66 | 	bpf_skb_pull_data(info->skb, len);
 67 | 
 68 | 	eth = skb_info_ptr(info, sizeof(*eth));
 69 | 	if (!eth)
 70 | 		return NULL;
 71 | 
 72 | 	info->proto = eth->h_proto;
 73 | 	info->offset += sizeof(*eth);
 74 | 
 75 | 	return eth;
 76 | }
 77 | 
 78 | static __always_inline struct vlan_hdr *
 79 | skb_parse_vlan(struct skb_parser_info *info)
 80 | {
 81 | 	struct vlan_hdr *vlh;
 82 | 
 83 | 	if (info->proto != bpf_htons(ETH_P_8021Q) &&
 84 | 	    info->proto != bpf_htons(ETH_P_8021AD))
 85 | 		return NULL;
 86 | 
 87 | 	vlh = skb_info_ptr(info, sizeof(*vlh));
 88 | 	if (!vlh)
 89 | 		return NULL;
 90 | 
 91 | 	info->proto = vlh->h_vlan_encapsulated_proto;
 92 | 	info->offset += sizeof(*vlh);
 93 | 
 94 | 	return vlh;
 95 | }
 96 | 
 97 | static __always_inline struct iphdr *
 98 | skb_parse_ipv4(struct skb_parser_info *info, int min_l4_bytes)
 99 | {
100 | 	struct iphdr *iph;
101 | 	int proto, hdr_len;
102 | 	__u32 pull_len;
103 | 
104 | 	if (info->proto != bpf_htons(ETH_P_IP))
105 | 		return NULL;
106 | 
107 | 	iph = skb_info_ptr(info, sizeof(*iph));
108 | 	if (!iph)
109 | 		return NULL;
110 | 
111 | 	hdr_len = iph->ihl * 4;
112 | 	hdr_len = READ_ONCE(hdr_len) & 0xff;
113 | 	if (hdr_len < sizeof(*iph))
114 | 		return NULL;
115 | 
116 | 	pull_len = info->offset + hdr_len + min_l4_bytes;
117 | 	if (pull_len > info->skb->len)
118 | 		pull_len = info->skb->len;
119 | 
120 | 	if (bpf_skb_pull_data(info->skb, pull_len))
121 | 		return NULL;
122 | 
123 | 	iph = skb_info_ptr(info, sizeof(*iph));
124 | 	if (!iph)
125 | 		return NULL;
126 | 
127 | 	info->proto = iph->protocol;
128 | 	info->offset += hdr_len;
129 | 
130 | 	return iph;
131 | }
132 | 
133 | static __always_inline struct ipv6hdr *
134 | skb_parse_ipv6(struct skb_parser_info *info, int max_l4_bytes)
135 | {
136 | 	struct ipv6hdr *ip6h;
137 | 	__u32 pull_len;
138 | 
139 | 	if (info->proto != bpf_htons(ETH_P_IPV6))
140 | 		return NULL;
141 | 
142 | 	pull_len = info->offset + sizeof(*ip6h) + max_l4_bytes;
143 | 	if (pull_len > info->skb->len)
144 | 		pull_len = info->skb->len;
145 | 
146 | 	if (bpf_skb_pull_data(info->skb, pull_len))
147 | 		return NULL;
148 | 
149 | 	ip6h = skb_info_ptr(info, sizeof(*ip6h));
150 | 	if (!ip6h)
151 | 		return NULL;
152 | 
153 | 	info->proto = READ_ONCE(ip6h->nexthdr);
154 | 	info->offset += sizeof(*ip6h);
155 | 
156 | 	return ip6h;
157 | }
158 | 
159 | static __always_inline struct tcphdr *
160 | skb_parse_tcp(struct skb_parser_info *info)
161 | {
162 | 	struct tcphdr *tcph;
163 | 
164 | 	if (info->proto != IPPROTO_TCP)
165 | 		return NULL;
166 | 
167 | 	tcph = skb_info_ptr(info, sizeof(*tcph));
168 | 	if (!tcph)
169 | 		return NULL;
170 | 
171 | 	info->offset += tcph->doff * 4;
172 | 
173 | 	return tcph;
174 | }
175 | 
176 | #endif
177 | 


--------------------------------------------------------------------------------
/dns.c:
--------------------------------------------------------------------------------
  1 | #include <netinet/if_ether.h>
  2 | #include <netinet/in.h>
  3 | #include <netinet/ip.h>
  4 | #include <netinet/ip6.h>
  5 | #include <netinet/udp.h>
  6 | #include <netpacket/packet.h>
  7 | #include <net/if.h>
  8 | #include <sys/socket.h>
  9 | #include <sys/types.h>
 10 | #include <errno.h>
 11 | #include <resolv.h>
 12 | 
 13 | #include <libubox/uloop.h>
 14 | #include <libubox/avl-cmp.h>
 15 | 
 16 | #define FLAG_RESPONSE		0x8000
 17 | #define FLAG_OPCODE		0x7800
 18 | #define FLAG_AUTHORATIVE	0x0400
 19 | #define FLAG_RCODE		0x000f
 20 | 
 21 | #define TYPE_A			0x0001
 22 | #define TYPE_CNAME		0x0005
 23 | #define TYPE_PTR		0x000c
 24 | #define TYPE_TXT		0x0010
 25 | #define TYPE_AAAA		0x001c
 26 | #define TYPE_SRV		0x0021
 27 | #define TYPE_ANY		0x00ff
 28 | 
 29 | #define IS_COMPRESSED(x)	((x & 0xc0) == 0xc0)
 30 | 
 31 | #define CLASS_FLUSH		0x8000
 32 | #define CLASS_UNICAST		0x8000
 33 | #define CLASS_IN		0x0001
 34 | 
 35 | #define MAX_NAME_LEN            256
 36 | #define MAX_DATA_LEN            8096
 37 | 
 38 | #include "qosify.h"
 39 | 
 40 | static struct uloop_fd ufd;
 41 | static struct uloop_timeout cname_gc_timer;
 42 | static AVL_TREE(cname_cache, avl_strcmp, false, NULL);
 43 | 
 44 | struct vlan_hdr {
 45 | 	uint16_t tci;
 46 | 	uint16_t proto;
 47 | };
 48 | 
 49 | struct packet {
 50 | 	void *buffer;
 51 | 	unsigned int len;
 52 | };
 53 | 
 54 | struct dns_header {
 55 | 	uint16_t id;
 56 | 	uint16_t flags;
 57 | 	uint16_t questions;
 58 | 	uint16_t answers;
 59 | 	uint16_t authority;
 60 | 	uint16_t additional;
 61 | } __packed;
 62 | 
 63 | struct dns_question {
 64 | 	uint16_t type;
 65 | 	uint16_t class;
 66 | } __packed;
 67 | 
 68 | struct dns_answer {
 69 | 	uint16_t type;
 70 | 	uint16_t class;
 71 | 	uint32_t ttl;
 72 | 	uint16_t rdlength;
 73 | } __packed;
 74 | 
 75 | struct cname_entry {
 76 | 	struct avl_node node;
 77 | 	uint32_t seq;
 78 | 	uint8_t dscp;
 79 | 	uint8_t age;
 80 | };
 81 | 
 82 | static void *pkt_peek(struct packet *pkt, unsigned int len)
 83 | {
 84 | 	if (len > pkt->len)
 85 | 		return NULL;
 86 | 
 87 | 	return pkt->buffer;
 88 | }
 89 | 
 90 | 
 91 | static void *pkt_pull(struct packet *pkt, unsigned int len)
 92 | {
 93 | 	void *ret = pkt_peek(pkt, len);
 94 | 
 95 | 	if (!ret)
 96 | 		return NULL;
 97 | 
 98 | 	pkt->buffer += len;
 99 | 	pkt->len -= len;
100 | 
101 | 	return ret;
102 | }
103 | 
104 | static int pkt_pull_name(struct packet *pkt, const void *hdr, char *dest)
105 | {
106 | 	int len;
107 | 
108 | 	if (dest)
109 | 		len = dn_expand(hdr, pkt->buffer + pkt->len, pkt->buffer,
110 | 				(void *)dest, MAX_NAME_LEN);
111 | 	else
112 | 		len = dn_skipname(pkt->buffer, pkt->buffer + pkt->len - 1);
113 | 
114 | 	if (len < 0 || !pkt_pull(pkt, len))
115 | 		return -1;
116 | 
117 | 	return 0;
118 | }
119 | 
120 | static bool
121 | proto_is_vlan(uint16_t proto)
122 | {
123 | 	return proto == ETH_P_8021Q || proto == ETH_P_8021AD;
124 | }
125 | 
126 | static void
127 | cname_cache_set(const char *name, uint8_t dscp, uint32_t seq)
128 | {
129 | 	struct cname_entry *e;
130 | 
131 | 	e = avl_find_element(&cname_cache, name, e, node);
132 | 	if (!e) {
133 | 		char *name_buf;
134 | 
135 | 		e = calloc_a(sizeof(*e), &name_buf, strlen(name) + 1);
136 | 		e->node.key = strcpy(name_buf, name);
137 | 		avl_insert(&cname_cache, &e->node);
138 | 	}
139 | 
140 | 	e->age = 0;
141 | 	e->dscp = dscp;
142 | 	e->seq = seq;
143 | }
144 | 
145 | static int
146 | cname_cache_get(const char *name, uint8_t *dscp, uint32_t *seq)
147 | {
148 | 	struct cname_entry *e;
149 | 
150 | 	e = avl_find_element(&cname_cache, name, e, node);
151 | 	if (!e)
152 | 		return -1;
153 | 
154 | 	if (*dscp == 0xff || e->seq < *seq) {
155 | 		*dscp = e->dscp;
156 | 		*seq = e->seq;
157 | 	}
158 | 
159 | 	return 0;
160 | }
161 | 
162 | static int
163 | dns_parse_question(struct packet *pkt, const void *hdr, uint8_t *dscp, uint32_t *seq)
164 | {
165 | 	char qname[MAX_NAME_LEN];
166 | 
167 | 	if (pkt_pull_name(pkt, hdr, qname) ||
168 | 	    !pkt_pull(pkt, sizeof(struct dns_question)))
169 | 		return -1;
170 | 
171 | 	cname_cache_get(qname, dscp, seq);
172 | 	qosify_map_lookup_dns_entry(qname, false, dscp, seq);
173 | 
174 | 	return 0;
175 | }
176 | 
177 | static int
178 | dns_parse_answer(struct packet *pkt, void *hdr, uint8_t *dscp, uint32_t *seq)
179 | {
180 | 	struct qosify_map_data data = {};
181 | 	char cname[MAX_NAME_LEN];
182 | 	struct dns_answer *a;
183 | 	int prev_timeout;
184 | 	void *rdata;
185 | 	int len;
186 | 
187 | 	if (pkt_pull_name(pkt, hdr, NULL))
188 | 		return -1;
189 | 
190 | 	a = pkt_pull(pkt, sizeof(*a));
191 | 	if (!a)
192 | 		return -1;
193 | 
194 | 	len = be16_to_cpu(a->rdlength);
195 | 	rdata = pkt_pull(pkt, len);
196 | 	if (!rdata)
197 | 		return -1;
198 | 
199 | 	switch (be16_to_cpu(a->type)) {
200 | 	case TYPE_CNAME:
201 | 		if (dn_expand(hdr, pkt->buffer + pkt->len, rdata,
202 | 			      cname, sizeof(cname)) < 0)
203 | 			return -1;
204 | 
205 | 		qosify_map_lookup_dns_entry(cname, true, dscp, seq);
206 | 		cname_cache_set(cname, *dscp, *seq);
207 | 
208 | 		return 0;
209 | 	case TYPE_A:
210 | 		data.id = CL_MAP_IPV4_ADDR;
211 | 		memcpy(&data.addr, rdata, 4);
212 | 		break;
213 | 	case TYPE_AAAA:
214 | 		data.id = CL_MAP_IPV6_ADDR;
215 | 		memcpy(&data.addr, rdata, 16);
216 | 		break;
217 | 	default:
218 | 		return 0;
219 | 	}
220 | 
221 | 	data.user = true;
222 | 	data.dscp = *dscp;
223 | 
224 | 	prev_timeout = qosify_map_timeout;
225 | 	qosify_map_timeout = be32_to_cpu(a->ttl);
226 | 	__qosify_map_set_entry(&data);
227 | 	qosify_map_timeout = prev_timeout;
228 | 
229 | 	return 0;
230 | }
231 | 
232 | static void
233 | qosify_dns_data_cb(struct packet *pkt)
234 | {
235 | 	struct dns_header *h;
236 | 	uint32_t lookup_seq = 0;
237 | 	uint8_t dscp = 0xff;
238 | 	int i;
239 | 
240 | 	h = pkt_pull(pkt, sizeof(*h));
241 | 	if (!h)
242 | 		return;
243 | 
244 | 	if ((h->flags & cpu_to_be16(FLAG_RESPONSE | FLAG_OPCODE | FLAG_RCODE)) !=
245 | 	    cpu_to_be16(FLAG_RESPONSE))
246 | 		return;
247 | 
248 | 	if (h->questions != cpu_to_be16(1))
249 | 		return;
250 | 
251 | 	if (dns_parse_question(pkt, h, &dscp, &lookup_seq))
252 | 		return;
253 | 
254 | 	for (i = 0; i < be16_to_cpu(h->answers); i++)
255 | 		if (dns_parse_answer(pkt, h, &dscp, &lookup_seq))
256 | 			return;
257 | }
258 | 
259 | static void
260 | qosify_dns_packet_cb(struct packet *pkt)
261 | {
262 | 	struct ethhdr *eth;
263 | 	struct ip6_hdr *ip6;
264 | 	struct ip *ip;
265 | 	uint16_t proto;
266 | 
267 | 	eth = pkt_pull(pkt, sizeof(*eth));
268 | 	if (!eth)
269 | 		return;
270 | 
271 | 	proto = be16_to_cpu(eth->h_proto);
272 | 	if (proto_is_vlan(proto)) {
273 | 		struct vlan_hdr *vlan;
274 | 
275 | 		vlan = pkt_pull(pkt, sizeof(*vlan));
276 | 		if (!vlan)
277 | 			return;
278 | 
279 | 		proto = be16_to_cpu(vlan->proto);
280 | 	}
281 | 
282 | 	switch (proto) {
283 | 	case ETH_P_IP:
284 | 		ip = pkt_peek(pkt, sizeof(struct ip));
285 | 		if (!ip)
286 | 			return;
287 | 
288 | 		if (!pkt_pull(pkt, ip->ip_hl * 4))
289 | 			return;
290 | 
291 | 		proto = ip->ip_p;
292 | 		break;
293 | 	case ETH_P_IPV6:
294 | 		ip6 = pkt_pull(pkt, sizeof(*ip6));
295 | 		if (!ip6)
296 | 			return;
297 | 
298 | 		proto = ip6->ip6_nxt;
299 | 		break;
300 | 	default:
301 | 		return;
302 | 	}
303 | 
304 | 	if (proto != IPPROTO_UDP)
305 | 		return;
306 | 
307 | 	if (!pkt_pull(pkt, sizeof(struct udphdr)))
308 | 		return;
309 | 
310 | 	qosify_dns_data_cb(pkt);
311 | }
312 | 
313 | static void
314 | qosify_dns_socket_cb(struct uloop_fd *fd, unsigned int events)
315 | {
316 | 	static uint8_t buf[8192];
317 | 	struct packet pkt = {
318 | 		.buffer = buf,
319 | 	};
320 | 	int len;
321 | 
322 | retry:
323 | 	len = recvfrom(fd->fd, buf, sizeof(buf), MSG_DONTWAIT, NULL, NULL);
324 | 	if (len < 0) {
325 | 		if (errno == EINTR)
326 | 			goto retry;
327 | 		return;
328 | 	}
329 | 
330 | 	if (!len)
331 | 		return;
332 | 
333 | 	pkt.len = len;
334 | 	qosify_dns_packet_cb(&pkt);
335 | }
336 | 
337 | static void
338 | qosify_cname_cache_gc(struct uloop_timeout *timeout)
339 | {
340 | 	struct cname_entry *e, *tmp;
341 | 
342 | 	avl_for_each_element_safe(&cname_cache, e, node, tmp) {
343 | 		if (e->age++ < 5)
344 | 			continue;
345 | 
346 | 		avl_delete(&cname_cache, &e->node);
347 | 		free(e);
348 | 	}
349 | 
350 | 	uloop_timeout_set(timeout, 1000);
351 | }
352 | 
353 | static int
354 | qosify_open_dns_socket(void)
355 | {
356 | 	struct sockaddr_ll sll = {
357 | 		.sll_family = AF_PACKET,
358 | 		.sll_protocol = htons(ETH_P_ALL),
359 | 	};
360 | 	int sock;
361 | 
362 | 	sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
363 | 	if (sock == -1) {
364 | 		ULOG_ERR("failed to create raw socket: %s\n", strerror(errno));
365 | 		return -1;
366 | 	}
367 | 
368 | 	sll.sll_ifindex = if_nametoindex(QOSIFY_DNS_IFNAME);
369 | 	if (bind(sock, (struct sockaddr *)&sll, sizeof(sll))) {
370 | 		ULOG_ERR("failed to bind socket to "QOSIFY_DNS_IFNAME": %s\n",
371 | 			 strerror(errno));
372 | 		goto error;
373 | 	}
374 | 
375 | 	ufd.fd = sock;
376 | 	ufd.cb = qosify_dns_socket_cb;
377 | 	uloop_fd_add(&ufd, ULOOP_READ);
378 | 
379 | 	return 0;
380 | 
381 | error:
382 | 	close(sock);
383 | 	return -1;
384 | }
385 | 
386 | static void
387 | qosify_dns_del_ifb(void)
388 | {
389 | 	qosify_run_cmd("ip link del ifb-dns type ifb", true);
390 | }
391 | 
392 | int qosify_dns_init(void)
393 | {
394 | 	cname_gc_timer.cb = qosify_cname_cache_gc;
395 | 	qosify_cname_cache_gc(&cname_gc_timer);
396 | 
397 | 	qosify_dns_del_ifb();
398 | 
399 | 	if (qosify_run_cmd("ip link add ifb-dns type ifb", false) ||
400 | 	    qosify_run_cmd("ip link set dev ifb-dns up", false) ||
401 | 	    qosify_open_dns_socket())
402 | 		return -1;
403 | 
404 | 	return 0;
405 | }
406 | 
407 | void qosify_dns_stop(void)
408 | {
409 | 	struct cname_entry *e, *tmp;
410 | 
411 | 	if (ufd.registered) {
412 | 		uloop_fd_delete(&ufd);
413 | 		close(ufd.fd);
414 | 	}
415 | 
416 | 	qosify_dns_del_ifb();
417 | 
418 | 	avl_remove_all_elements(&cname_cache, e, node, tmp)
419 | 		free(e);
420 | }
421 | 
422 | 


--------------------------------------------------------------------------------
/qosify-bpf.c:
--------------------------------------------------------------------------------
  1 | // SPDX-License-Identifier: GPL-2.0+
  2 | /*
  3 |  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
  4 |  */
  5 | #define KBUILD_MODNAME "foo"
  6 | #include <uapi/linux/bpf.h>
  7 | #include <uapi/linux/if_ether.h>
  8 | #include <uapi/linux/if_packet.h>
  9 | #include <uapi/linux/ip.h>
 10 | #include <uapi/linux/ipv6.h>
 11 | #include <uapi/linux/in.h>
 12 | #include <uapi/linux/tcp.h>
 13 | #include <uapi/linux/udp.h>
 14 | #include <uapi/linux/filter.h>
 15 | #include <uapi/linux/pkt_cls.h>
 16 | #include <linux/ip.h>
 17 | #include <net/ipv6.h>
 18 | #include <bpf/bpf_helpers.h>
 19 | #include <bpf/bpf_endian.h>
 20 | #include "bpf_skb_utils.h"
 21 | #include "qosify-bpf.h"
 22 | 
 23 | #define INET_ECN_MASK 3
 24 | 
 25 | #define FLOW_CHECK_INTERVAL	((u32)((1000000000ULL) >> 24))
 26 | #define FLOW_TIMEOUT		((u32)((30ULL * 1000000000ULL) >> 24))
 27 | #define FLOW_BULK_TIMEOUT	5
 28 | 
 29 | #define EWMA_SHIFT		12
 30 | 
 31 | const volatile static uint32_t module_flags = 0;
 32 | 
 33 | struct flow_bucket {
 34 | 	__u32 last_update;
 35 | 	__u32 pkt_len_avg;
 36 | 	__u32 pkt_count;
 37 | 	__u32 bulk_timeout;
 38 | };
 39 | 
 40 | struct {
 41 | 	__uint(type, BPF_MAP_TYPE_ARRAY);
 42 | 	__uint(pinning, 1);
 43 | 	__type(key, __u32);
 44 | 	__type(value, struct qosify_config);
 45 | 	__uint(max_entries, 1);
 46 | } config SEC(".maps");
 47 | 
 48 | struct {
 49 | 	__uint(type, BPF_MAP_TYPE_ARRAY);
 50 | 	__uint(pinning, 1);
 51 | 	__type(key, __u32);
 52 | 	__type(value, __u8);
 53 | 	__uint(max_entries, 1 << 16);
 54 | } tcp_ports SEC(".maps");
 55 | 
 56 | struct {
 57 | 	__uint(type, BPF_MAP_TYPE_ARRAY);
 58 | 	__uint(pinning, 1);
 59 | 	__type(key, __u32);
 60 | 	__type(value, __u8);
 61 | 	__uint(max_entries, 1 << 16);
 62 | } udp_ports SEC(".maps");
 63 | 
 64 | struct {
 65 | 	__uint(type, BPF_MAP_TYPE_LRU_HASH);
 66 | 	__uint(pinning, 1);
 67 | 	__type(key, __u32);
 68 | 	__type(value, struct flow_bucket);
 69 | 	__uint(max_entries, QOSIFY_FLOW_BUCKETS);
 70 | } flow_map SEC(".maps");
 71 | 
 72 | struct {
 73 | 	__uint(type, BPF_MAP_TYPE_HASH);
 74 | 	__uint(pinning, 1);
 75 | 	__uint(key_size, sizeof(struct in_addr));
 76 | 	__type(value, struct qosify_ip_map_val);
 77 | 	__uint(max_entries, 100000);
 78 | 	__uint(map_flags, BPF_F_NO_PREALLOC);
 79 | } ipv4_map SEC(".maps");
 80 | 
 81 | struct {
 82 | 	__uint(type, BPF_MAP_TYPE_HASH);
 83 | 	__uint(pinning, 1);
 84 | 	__uint(key_size, sizeof(struct in6_addr));
 85 | 	__type(value, struct qosify_ip_map_val);
 86 | 	__uint(max_entries, 100000);
 87 | 	__uint(map_flags, BPF_F_NO_PREALLOC);
 88 | } ipv6_map SEC(".maps");
 89 | 
 90 | struct {
 91 | 	__uint(type, BPF_MAP_TYPE_ARRAY);
 92 | 	__uint(pinning, 1);
 93 | 	__type(key, __u32);
 94 | 	__type(value, struct qosify_class);
 95 | 	__uint(max_entries, QOSIFY_MAX_CLASS_ENTRIES +
 96 | 			    QOSIFY_DEFAULT_CLASS_ENTRIES);
 97 | } class_map SEC(".maps");
 98 | 
 99 | static struct qosify_config *get_config(void)
100 | {
101 | 	__u32 key = 0;
102 | 
103 | 	return bpf_map_lookup_elem(&config, &key);
104 | }
105 | 
106 | static __always_inline __u32 cur_time(void)
107 | {
108 | 	__u32 val = bpf_ktime_get_ns() >> 24;
109 | 
110 | 	if (!val)
111 | 		val = 1;
112 | 
113 | 	return val;
114 | }
115 | 
116 | static __always_inline __u32 ewma(__u32 *avg, __u32 val)
117 | {
118 | 	if (*avg)
119 | 		*avg = (*avg * 3) / 4 + (val << EWMA_SHIFT) / 4;
120 | 	else
121 | 		*avg = val << EWMA_SHIFT;
122 | 
123 | 	return *avg >> EWMA_SHIFT;
124 | }
125 | 
126 | static __always_inline __u8 dscp_val(struct qosify_dscp_val *val, bool ingress)
127 | {
128 | 	__u8 ival = val->ingress;
129 | 	__u8 eval = val->egress;
130 | 
131 | 	return ingress ? ival : eval;
132 | }
133 | 
134 | static __always_inline void
135 | ipv4_change_dsfield(struct __sk_buff *skb, __u32 offset,
136 | 		    __u8 mask, __u8 value, bool force)
137 | {
138 | 	struct iphdr *iph;
139 | 	__u32 check;
140 | 	__u8 dsfield;
141 | 
142 | 	iph = skb_ptr(skb, offset, sizeof(*iph));
143 | 	if (!iph)
144 | 		return;
145 | 
146 | 	check = bpf_ntohs(iph->check);
147 | 	if ((iph->tos & mask) && !force)
148 | 		return;
149 | 
150 | 	dsfield = (iph->tos & mask) | value;
151 | 	if (iph->tos == dsfield)
152 | 		return;
153 | 
154 | 	check += iph->tos;
155 | 	if ((check + 1) >> 16)
156 | 		check = (check + 1) & 0xffff;
157 | 	check -= dsfield;
158 | 	check += check >> 16;
159 | 	iph->check = bpf_htons(check);
160 | 	iph->tos = dsfield;
161 | }
162 | 
163 | static __always_inline void
164 | ipv6_change_dsfield(struct __sk_buff *skb, __u32 offset,
165 | 		    __u8 mask, __u8 value, bool force)
166 | {
167 | 	struct ipv6hdr *ipv6h;
168 | 	__u16 *p;
169 | 	__u16 val;
170 | 
171 | 	ipv6h = skb_ptr(skb, offset, sizeof(*ipv6h));
172 | 	if (!ipv6h)
173 | 		return;
174 | 
175 | 	p = (__u16 *)ipv6h;
176 | 	if (((*p >> 4) & mask) && !force)
177 | 		return;
178 | 
179 | 	val = (*p & bpf_htons((((__u16)mask << 4) | 0xf00f))) | bpf_htons((__u16)value << 4);
180 | 	if (val == *p)
181 | 		return;
182 | 
183 | 	*p = val;
184 | }
185 | 
186 | static void
187 | parse_l4proto(struct qosify_config *config, struct skb_parser_info *info,
188 | 	      bool ingress, __u8 *out_val)
189 | {
190 | 	struct udphdr *udp;
191 | 	__u32 src, dest, key;
192 | 	__u8 *value;
193 | 	__u8 proto = info->proto;
194 | 
195 | 	udp = skb_info_ptr(info, sizeof(*udp));
196 | 	if (!udp)
197 | 		return;
198 | 
199 | 	if (config && (proto == IPPROTO_ICMP || proto == IPPROTO_ICMPV6)) {
200 | 		*out_val = config->dscp_icmp;
201 | 		return;
202 | 	}
203 | 
204 | 	src = READ_ONCE(udp->source);
205 | 	dest = READ_ONCE(udp->dest);
206 | 	if (ingress)
207 | 		key = src;
208 | 	else
209 | 		key = dest;
210 | 
211 | 	if (proto == IPPROTO_TCP) {
212 | 		value = bpf_map_lookup_elem(&tcp_ports, &key);
213 | 	} else {
214 | 		if (proto != IPPROTO_UDP)
215 | 			key = 0;
216 | 
217 | 		value = bpf_map_lookup_elem(&udp_ports, &key);
218 | 	}
219 | 
220 | 	if (value)
221 | 		*out_val = *value;
222 | }
223 | 
224 | static __always_inline bool
225 | check_flow_bulk(struct qosify_flow_config *config, struct __sk_buff *skb,
226 | 		struct flow_bucket *flow, __u8 *out_val)
227 | {
228 | 	bool trigger = false;
229 | 	__s32 delta;
230 | 	__u32 time;
231 | 	int segs = 1;
232 | 	bool ret = false;
233 | 
234 | 	if (!config->bulk_trigger_pps)
235 | 		return false;
236 | 
237 | 	time = cur_time();
238 | 	if (!flow->last_update)
239 | 		goto reset;
240 | 
241 | 	delta = time - flow->last_update;
242 | 	if ((u32)delta > FLOW_TIMEOUT)
243 | 		goto reset;
244 | 
245 | 	if (skb->gso_segs)
246 | 		segs = skb->gso_segs;
247 | 	flow->pkt_count += segs;
248 | 	if (flow->pkt_count > config->bulk_trigger_pps) {
249 | 		flow->bulk_timeout = config->bulk_trigger_timeout + 1;
250 | 		trigger = true;
251 | 	}
252 | 
253 | 	if (delta >= FLOW_CHECK_INTERVAL) {
254 | 		if (flow->bulk_timeout && !trigger)
255 | 			flow->bulk_timeout--;
256 | 
257 | 		goto clear;
258 | 	}
259 | 
260 | 	goto out;
261 | 
262 | reset:
263 | 	flow->pkt_len_avg = 0;
264 | clear:
265 | 	flow->pkt_count = 1;
266 | 	flow->last_update = time;
267 | out:
268 | 	if (flow->bulk_timeout) {
269 | 		*out_val = config->dscp_bulk;
270 | 		return true;
271 | 	}
272 | 
273 | 	return false;
274 | }
275 | 
276 | static __always_inline bool
277 | check_flow_prio(struct qosify_flow_config *config, struct __sk_buff *skb,
278 | 		struct flow_bucket *flow, __u8 *out_val)
279 | {
280 | 	int cur_len = skb->len;
281 | 
282 | 	if (flow->bulk_timeout)
283 | 		return false;
284 | 
285 | 	if (!config->prio_max_avg_pkt_len)
286 | 		return false;
287 | 
288 | 	if (skb->gso_segs > 1)
289 | 		cur_len /= skb->gso_segs;
290 | 
291 | 	if (ewma(&flow->pkt_len_avg, cur_len) <= config->prio_max_avg_pkt_len) {
292 | 		*out_val = config->dscp_prio;
293 | 		return true;
294 | 	}
295 | 
296 | 	return false;
297 | }
298 | 
299 | static __always_inline bool
300 | check_flow(struct qosify_flow_config *config, struct __sk_buff *skb,
301 | 	   __u8 *out_val)
302 | {
303 | 	struct flow_bucket flow_data;
304 | 	struct flow_bucket *flow;
305 | 	__u32 hash;
306 | 	bool ret = false;
307 | 
308 | 	if (!config)
309 | 		return false;
310 | 
311 | 	if (!config->prio_max_avg_pkt_len && !config->bulk_trigger_pps)
312 | 		return false;
313 | 
314 | 	hash = bpf_get_hash_recalc(skb);
315 | 	flow = bpf_map_lookup_elem(&flow_map, &hash);
316 | 	if (!flow) {
317 | 		memset(&flow_data, 0, sizeof(flow_data));
318 | 		bpf_map_update_elem(&flow_map, &hash, &flow_data, BPF_ANY);
319 | 		flow = bpf_map_lookup_elem(&flow_map, &hash);
320 | 		if (!flow)
321 | 			return false;
322 | 	}
323 | 
324 | 	ret |= check_flow_bulk(config, skb, flow, out_val);
325 | 	ret |= check_flow_prio(config, skb, flow, out_val);
326 | 
327 | 	return ret;
328 | }
329 | 
330 | static __always_inline struct qosify_ip_map_val *
331 | parse_ipv4(struct qosify_config *config, struct skb_parser_info *info,
332 | 	   bool ingress, __u8 *out_val)
333 | {
334 | 	struct iphdr *iph;
335 | 	__u8 ipproto;
336 | 	int hdr_len;
337 | 	void *key;
338 | 
339 | 	iph = skb_parse_ipv4(info, sizeof(struct udphdr));
340 | 	if (!iph)
341 | 		return NULL;
342 | 
343 | 	parse_l4proto(config, info, ingress, out_val);
344 | 
345 | 	if (ingress)
346 | 		key = &iph->saddr;
347 | 	else
348 | 		key = &iph->daddr;
349 | 
350 | 	return bpf_map_lookup_elem(&ipv4_map, key);
351 | }
352 | 
353 | static __always_inline struct qosify_ip_map_val *
354 | parse_ipv6(struct qosify_config *config, struct skb_parser_info *info,
355 | 	   bool ingress, __u8 *out_val)
356 | {
357 | 	struct ipv6hdr *iph;
358 | 	__u8 ipproto;
359 | 	void *key;
360 | 
361 | 	iph = skb_parse_ipv6(info, sizeof(struct udphdr));
362 | 	if (!iph)
363 | 		return NULL;
364 | 
365 | 	if (ingress)
366 | 		key = &iph->saddr;
367 | 	else
368 | 		key = &iph->daddr;
369 | 
370 | 	parse_l4proto(config, info, ingress, out_val);
371 | 
372 | 	return bpf_map_lookup_elem(&ipv6_map, key);
373 | }
374 | 
375 | static __always_inline int
376 | dscp_lookup_class(uint8_t *dscp, bool ingress, struct qosify_class **out_class,
377 | 		  bool counter)
378 | {
379 | 	struct qosify_class *class;
380 | 	__u8 fallback_flag;
381 | 	__u32 key;
382 | 
383 | 	if (!(*dscp & QOSIFY_DSCP_CLASS_FLAG))
384 | 		return 0;
385 | 
386 | 	fallback_flag = *dscp & QOSIFY_DSCP_FALLBACK_FLAG;
387 | 	key = *dscp & QOSIFY_DSCP_VALUE_MASK;
388 | 	class = bpf_map_lookup_elem(&class_map, &key);
389 | 	if (!class)
390 | 		return -1;
391 | 
392 | 	if (!(class->flags & QOSIFY_CLASS_FLAG_PRESENT))
393 | 		return -1;
394 | 
395 | 	if (counter)
396 | 	    class->packets++;
397 | 	*dscp = dscp_val(&class->val, ingress);
398 | 	*dscp |= fallback_flag;
399 | 	*out_class = class;
400 | 
401 | 	return 0;
402 | }
403 | 
404 | SEC("tc")
405 | int classify(struct __sk_buff *skb)
406 | {
407 | 	struct skb_parser_info info;
408 | 	bool ingress = module_flags & QOSIFY_INGRESS;
409 | 	struct qosify_config *config;
410 | 	struct qosify_class *class = NULL;
411 | 	struct qosify_ip_map_val *ip_val;
412 | 	__u32 iph_offset;
413 | 	__u8 dscp = 0;
414 | 	void *iph;
415 | 	bool force;
416 | 	int type;
417 | 
418 | 	config = get_config();
419 | 	if (!config)
420 | 		return TC_ACT_UNSPEC;
421 | 
422 | 	skb_parse_init(&info, skb);
423 | 	if (module_flags & QOSIFY_IP_ONLY) {
424 | 		type = info.proto = skb->protocol;
425 | 	} else if (skb_parse_ethernet(&info)) {
426 | 		skb_parse_vlan(&info);
427 | 		skb_parse_vlan(&info);
428 | 		type = info.proto;
429 | 	} else {
430 | 		return TC_ACT_UNSPEC;
431 | 	}
432 | 
433 | 	iph_offset = info.offset;
434 | 	if (type == bpf_htons(ETH_P_IP))
435 | 		ip_val = parse_ipv4(config, &info, ingress, &dscp);
436 | 	else if (type == bpf_htons(ETH_P_IPV6))
437 | 		ip_val = parse_ipv6(config, &info, ingress, &dscp);
438 | 	else
439 | 		return TC_ACT_UNSPEC;
440 | 
441 | 	if (ip_val) {
442 | 		if (!ip_val->seen)
443 | 			ip_val->seen = 1;
444 | 		dscp = ip_val->dscp;
445 | 	}
446 | 
447 | 	if (dscp_lookup_class(&dscp, ingress, &class, true))
448 | 		return TC_ACT_UNSPEC;
449 | 
450 | 	if (class) {
451 | 		if (check_flow(&class->config, skb, &dscp) &&
452 | 		    dscp_lookup_class(&dscp, ingress, &class, false))
453 | 			return TC_ACT_UNSPEC;
454 | 	}
455 | 
456 | 	dscp &= GENMASK(5, 0);
457 | 	dscp <<= 2;
458 | 	force = !(dscp & QOSIFY_DSCP_FALLBACK_FLAG);
459 | 
460 | 	if (type == bpf_htons(ETH_P_IP))
461 | 		ipv4_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force);
462 | 	else if (type == bpf_htons(ETH_P_IPV6))
463 | 		ipv6_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force);
464 | 
465 | 	return TC_ACT_UNSPEC;
466 | }
467 | 
468 | char _license[] SEC("license") = "GPL";
469 | 


--------------------------------------------------------------------------------
/ubus.c:
--------------------------------------------------------------------------------
  1 | // SPDX-License-Identifier: GPL-2.0+
  2 | /*
  3 |  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
  4 |  */
  5 | #include <libubus.h>
  6 | 
  7 | #include "qosify.h"
  8 | 
  9 | static struct blob_buf b;
 10 | 
 11 | static int
 12 | qosify_ubus_add_array(struct blob_attr *attr, uint8_t val, enum qosify_map_id id)
 13 | {
 14 | 	struct blob_attr *cur;
 15 | 	int rem;
 16 | 
 17 | 	if (blobmsg_check_array(attr, BLOBMSG_TYPE_STRING) < 0)
 18 | 		return UBUS_STATUS_INVALID_ARGUMENT;
 19 | 
 20 | 	blobmsg_for_each_attr(cur, attr, rem)
 21 | 		qosify_map_set_entry(id, false, blobmsg_get_string(cur), val);
 22 | 
 23 | 	return 0;
 24 | }
 25 | 
 26 | static int
 27 | qosify_ubus_set_files(struct blob_attr *attr)
 28 | {
 29 | 	struct blob_attr *cur;
 30 | 	int rem;
 31 | 
 32 | 	if (blobmsg_check_array(attr, BLOBMSG_TYPE_STRING) < 0)
 33 | 		return UBUS_STATUS_INVALID_ARGUMENT;
 34 | 
 35 | 	qosify_map_clear_files();
 36 | 
 37 | 	blobmsg_for_each_attr(cur, attr, rem)
 38 | 		qosify_map_load_file(blobmsg_get_string(cur));
 39 | 
 40 | 	qosify_map_gc();
 41 | 
 42 | 	return 0;
 43 | }
 44 | 
 45 | 
 46 | enum {
 47 | 	CL_ADD_DSCP,
 48 | 	CL_ADD_TIMEOUT,
 49 | 	CL_ADD_IPV4,
 50 | 	CL_ADD_IPV6,
 51 | 	CL_ADD_TCP_PORT,
 52 | 	CL_ADD_UDP_PORT,
 53 | 	CL_ADD_DNS,
 54 | 	__CL_ADD_MAX
 55 | };
 56 | 
 57 | static const struct blobmsg_policy qosify_add_policy[__CL_ADD_MAX] = {
 58 | 	[CL_ADD_DSCP] = { "dscp", BLOBMSG_TYPE_STRING },
 59 | 	[CL_ADD_TIMEOUT] = { "timeout", BLOBMSG_TYPE_INT32 },
 60 | 	[CL_ADD_IPV4] = { "ipv4", BLOBMSG_TYPE_ARRAY },
 61 | 	[CL_ADD_IPV6] = { "ipv6", BLOBMSG_TYPE_ARRAY },
 62 | 	[CL_ADD_TCP_PORT] = { "tcp_port", BLOBMSG_TYPE_ARRAY },
 63 | 	[CL_ADD_UDP_PORT] = { "udp_port", BLOBMSG_TYPE_ARRAY },
 64 | 	[CL_ADD_DNS] = { "dns", BLOBMSG_TYPE_ARRAY },
 65 | };
 66 | 
 67 | 
 68 | static int
 69 | qosify_ubus_reload(struct ubus_context *ctx, struct ubus_object *obj,
 70 | 		   struct ubus_request_data *req, const char *method,
 71 | 		   struct blob_attr *msg)
 72 | {
 73 | 	qosify_map_reload();
 74 | 	return 0;
 75 | }
 76 | 
 77 | 
 78 | static int
 79 | qosify_ubus_add(struct ubus_context *ctx, struct ubus_object *obj,
 80 | 		struct ubus_request_data *req, const char *method,
 81 | 		struct blob_attr *msg)
 82 | {
 83 | 	int prev_timemout = qosify_map_timeout;
 84 | 	struct blob_attr *tb[__CL_ADD_MAX];
 85 | 	struct blob_attr *cur;
 86 | 	uint8_t dscp = 0xff;
 87 | 	int ret;
 88 | 
 89 | 	blobmsg_parse(qosify_add_policy, __CL_ADD_MAX, tb,
 90 | 		      blobmsg_data(msg), blobmsg_len(msg));
 91 | 
 92 | 	if (!strcmp(method, "add")) {
 93 | 		if ((cur = tb[CL_ADD_DSCP]) == NULL ||
 94 | 		    qosify_map_dscp_value(blobmsg_get_string(cur), &dscp))
 95 | 			return UBUS_STATUS_INVALID_ARGUMENT;
 96 | 
 97 | 		if ((cur = tb[CL_ADD_TIMEOUT]) != NULL)
 98 | 			qosify_map_timeout = blobmsg_get_u32(cur);
 99 | 	}
100 | 
101 | 	if ((cur = tb[CL_ADD_IPV4]) != NULL &&
102 | 	    (ret = qosify_ubus_add_array(cur, dscp, CL_MAP_IPV4_ADDR) != 0))
103 | 		return ret;
104 | 
105 | 	if ((cur = tb[CL_ADD_IPV6]) != NULL &&
106 | 	    (ret = qosify_ubus_add_array(cur, dscp, CL_MAP_IPV6_ADDR) != 0))
107 | 		return ret;
108 | 
109 | 	if ((cur = tb[CL_ADD_TCP_PORT]) != NULL &&
110 | 	    (ret = qosify_ubus_add_array(cur, dscp, CL_MAP_TCP_PORTS) != 0))
111 | 		return ret;
112 | 
113 | 	if ((cur = tb[CL_ADD_UDP_PORT]) != NULL &&
114 | 	    (ret = qosify_ubus_add_array(cur, dscp, CL_MAP_UDP_PORTS) != 0))
115 | 		return ret;
116 | 
117 | 	if ((cur = tb[CL_ADD_DNS]) != NULL &&
118 | 	    (ret = qosify_ubus_add_array(cur, dscp, CL_MAP_DNS) != 0))
119 | 		return ret;
120 | 
121 | 	qosify_map_timeout = prev_timemout;
122 | 
123 | 	return 0;
124 | }
125 | 
126 | enum {
127 | 	CL_CONFIG_RESET,
128 | 	CL_CONFIG_FILES,
129 | 	CL_CONFIG_TIMEOUT,
130 | 	CL_CONFIG_DSCP_UDP,
131 | 	CL_CONFIG_DSCP_TCP,
132 | 	CL_CONFIG_DSCP_ICMP,
133 | 	CL_CONFIG_INTERFACES,
134 | 	CL_CONFIG_DEVICES,
135 | 	CL_CONFIG_CLASSES,
136 | 	__CL_CONFIG_MAX
137 | };
138 | 
139 | static const struct blobmsg_policy qosify_config_policy[__CL_CONFIG_MAX] = {
140 | 	[CL_CONFIG_RESET] = { "reset", BLOBMSG_TYPE_BOOL },
141 | 	[CL_CONFIG_FILES] = { "files", BLOBMSG_TYPE_ARRAY },
142 | 	[CL_CONFIG_TIMEOUT] = { "timeout", BLOBMSG_TYPE_INT32 },
143 | 	[CL_CONFIG_DSCP_UDP] = { "dscp_default_udp", BLOBMSG_TYPE_STRING },
144 | 	[CL_CONFIG_DSCP_TCP] = { "dscp_default_tcp", BLOBMSG_TYPE_STRING },
145 | 	[CL_CONFIG_DSCP_ICMP] = { "dscp_icmp", BLOBMSG_TYPE_STRING },
146 | 	[CL_CONFIG_INTERFACES] = { "interfaces", BLOBMSG_TYPE_TABLE },
147 | 	[CL_CONFIG_DEVICES] = { "devices", BLOBMSG_TYPE_TABLE },
148 | 	[CL_CONFIG_CLASSES] = { "classes", BLOBMSG_TYPE_TABLE },
149 | };
150 | 
151 | static int
152 | qosify_ubus_config(struct ubus_context *ctx, struct ubus_object *obj,
153 | 		   struct ubus_request_data *req, const char *method,
154 | 		   struct blob_attr *msg)
155 | {
156 | 	struct blob_attr *tb[__CL_CONFIG_MAX];
157 | 	struct blob_attr *cur;
158 | 	uint8_t dscp;
159 | 	bool reset = false;
160 | 	int ret;
161 | 
162 | 	blobmsg_parse(qosify_config_policy, __CL_CONFIG_MAX, tb,
163 | 		      blobmsg_data(msg), blobmsg_len(msg));
164 | 
165 | 	if ((cur = tb[CL_CONFIG_RESET]) != NULL)
166 | 		reset = blobmsg_get_bool(cur);
167 | 
168 | 	if (reset)
169 | 		qosify_map_reset_config();
170 | 
171 | 	if ((cur = tb[CL_CONFIG_CLASSES]) != NULL || reset)
172 | 		qosify_map_set_classes(cur);
173 | 
174 | 	if ((cur = tb[CL_CONFIG_TIMEOUT]) != NULL)
175 | 		qosify_map_timeout = blobmsg_get_u32(cur);
176 | 
177 | 	if ((cur = tb[CL_CONFIG_FILES]) != NULL &&
178 | 	    (ret = qosify_ubus_set_files(cur) != 0))
179 | 		return ret;
180 | 
181 | 	if (map_parse_flow_config(&flow_config, msg, reset) ||
182 | 	    map_fill_dscp_value(&config.dscp_icmp, tb[CL_CONFIG_DSCP_ICMP], reset))
183 | 		return UBUS_STATUS_INVALID_ARGUMENT;
184 | 
185 | 	map_fill_dscp_value(&dscp, tb[CL_CONFIG_DSCP_UDP], true);
186 | 	if (dscp != 0xff)
187 | 		qosify_map_set_dscp_default(CL_MAP_UDP_PORTS, dscp);
188 | 
189 | 	map_fill_dscp_value(&dscp, tb[CL_CONFIG_DSCP_TCP], true);
190 | 	if (dscp != 0xff)
191 | 		qosify_map_set_dscp_default(CL_MAP_TCP_PORTS, dscp);
192 | 
193 | 	qosify_map_update_config();
194 | 
195 | 	qosify_iface_config_update(tb[CL_CONFIG_INTERFACES], tb[CL_CONFIG_DEVICES]);
196 | 
197 | 	qosify_iface_check();
198 | 
199 | 	return 0;
200 | }
201 | 
202 | 
203 | static int
204 | qosify_ubus_dump(struct ubus_context *ctx, struct ubus_object *obj,
205 | 		 struct ubus_request_data *req, const char *method,
206 | 		 struct blob_attr *msg)
207 | {
208 | 	blob_buf_init(&b, 0);
209 | 	qosify_map_dump(&b);
210 | 	ubus_send_reply(ctx, req, b.head);
211 | 	blob_buf_free(&b);
212 | 
213 | 	return 0;
214 | }
215 | 
216 | static int
217 | qosify_ubus_status(struct ubus_context *ctx, struct ubus_object *obj,
218 | 		 struct ubus_request_data *req, const char *method,
219 | 		 struct blob_attr *msg)
220 | {
221 | 	blob_buf_init(&b, 0);
222 | 	qosify_iface_status(&b);
223 | 	ubus_send_reply(ctx, req, b.head);
224 | 	blob_buf_free(&b);
225 | 
226 | 	return 0;
227 | }
228 | 
229 | static int
230 | qosify_ubus_get_stats(struct ubus_context *ctx, struct ubus_object *obj,
231 | 		      struct ubus_request_data *req, const char *method,
232 | 		      struct blob_attr *msg)
233 | {
234 | 	static const struct blobmsg_policy policy =
235 | 		{ "reset", BLOBMSG_TYPE_BOOL };
236 | 	struct blob_attr *tb;
237 | 	bool reset = false;
238 | 
239 | 	blobmsg_parse(&policy, 1, &tb, blobmsg_data(msg), blobmsg_len(msg));
240 | 
241 | 	reset = tb && blobmsg_get_u8(tb);
242 | 
243 | 	blob_buf_init(&b, 0);
244 | 	qosify_map_stats(&b, reset);
245 | 	ubus_send_reply(ctx, req, b.head);
246 | 	blob_buf_free(&b);
247 | 
248 | 	return 0;
249 | }
250 | 
251 | static int
252 | qosify_ubus_check_devices(struct ubus_context *ctx, struct ubus_object *obj,
253 | 			  struct ubus_request_data *req, const char *method,
254 | 			  struct blob_attr *msg)
255 | {
256 | 	qosify_iface_check();
257 | 
258 | 	return 0;
259 | }
260 | 
261 | enum {
262 | 	CL_DNS_HOST_NAME,
263 | 	CL_DNS_HOST_TYPE,
264 | 	CL_DNS_HOST_ADDR,
265 | 	CL_DNS_HOST_TTL,
266 | 	__CL_DNS_HOST_MAX
267 | };
268 | 
269 | static const struct blobmsg_policy qosify_dns_policy[__CL_DNS_HOST_MAX] = {
270 | 	[CL_DNS_HOST_NAME] = { "name", BLOBMSG_TYPE_STRING },
271 | 	[CL_DNS_HOST_TYPE] = { "type", BLOBMSG_TYPE_STRING },
272 | 	[CL_DNS_HOST_ADDR] = { "address", BLOBMSG_TYPE_STRING },
273 | 	[CL_DNS_HOST_TTL] = { "ttl", BLOBMSG_TYPE_INT32 },
274 | };
275 | 
276 | static int
277 | __qosify_ubus_add_dns_host(struct blob_attr *msg)
278 | {
279 | 	struct blob_attr *tb[__CL_DNS_HOST_MAX];
280 | 	struct blob_attr *cur;
281 | 	uint32_t ttl = 0;
282 | 
283 | 	blobmsg_parse(qosify_dns_policy, __CL_DNS_HOST_MAX, tb,
284 | 		      blobmsg_data(msg), blobmsg_len(msg));
285 | 
286 | 	if (!tb[CL_DNS_HOST_NAME] || !tb[CL_DNS_HOST_TYPE] ||
287 | 	    !tb[CL_DNS_HOST_ADDR])
288 | 		return UBUS_STATUS_INVALID_ARGUMENT;
289 | 
290 | 	if ((cur = tb[CL_DNS_HOST_TTL]) != NULL)
291 | 		ttl = blobmsg_get_u32(cur);
292 | 
293 | 	if (qosify_map_add_dns_host(blobmsg_get_string(tb[CL_DNS_HOST_NAME]),
294 | 				    blobmsg_get_string(tb[CL_DNS_HOST_ADDR]),
295 | 				    blobmsg_get_string(tb[CL_DNS_HOST_TYPE]),
296 | 				    ttl))
297 | 		return UBUS_STATUS_INVALID_ARGUMENT;
298 | 
299 | 	return 0;
300 | }
301 | 
302 | static int
303 | qosify_ubus_add_dns_host(struct ubus_context *ctx, struct ubus_object *obj,
304 | 			 struct ubus_request_data *req, const char *method,
305 | 			 struct blob_attr *msg)
306 | {
307 | 	return __qosify_ubus_add_dns_host(msg);
308 | }
309 | 
310 | static const struct ubus_method qosify_methods[] = {
311 | 	UBUS_METHOD_NOARG("reload", qosify_ubus_reload),
312 | 	UBUS_METHOD("add", qosify_ubus_add, qosify_add_policy),
313 | 	UBUS_METHOD_MASK("remove", qosify_ubus_add, qosify_add_policy,
314 | 			 ((1 << __CL_ADD_MAX) - 1) & ~(1 << CL_ADD_DSCP)),
315 | 	UBUS_METHOD("config", qosify_ubus_config, qosify_config_policy),
316 | 	UBUS_METHOD_NOARG("dump", qosify_ubus_dump),
317 | 	UBUS_METHOD_NOARG("status", qosify_ubus_status),
318 | 	UBUS_METHOD_NOARG("get_stats", qosify_ubus_get_stats),
319 | 	UBUS_METHOD("add_dns_host", qosify_ubus_add_dns_host, qosify_dns_policy),
320 | 	UBUS_METHOD_NOARG("check_devices", qosify_ubus_check_devices),
321 | };
322 | 
323 | static struct ubus_object_type qosify_object_type =
324 | 	UBUS_OBJECT_TYPE("qosify", qosify_methods);
325 | 
326 | static struct ubus_object qosify_object = {
327 | 	.name = "qosify",
328 | 	.type = &qosify_object_type,
329 | 	.methods = qosify_methods,
330 | 	.n_methods = ARRAY_SIZE(qosify_methods),
331 | };
332 | 
333 | static void
334 | qosify_subscribe_dnsmasq(struct ubus_context *ctx)
335 | {
336 | 	static struct ubus_subscriber sub = {
337 | 		.cb = qosify_ubus_add_dns_host,
338 | 	};
339 | 	uint32_t id;
340 | 
341 | 	if (!sub.obj.id &&
342 | 	    ubus_register_subscriber(ctx, &sub))
343 | 		return;
344 | 
345 | 	if (ubus_lookup_id(ctx, "dnsmasq.dns", &id))
346 | 		return;
347 | 
348 | 	ubus_subscribe(ctx, &sub, id);
349 | }
350 | 
351 | static void
352 | qosify_ubus_event_cb(struct ubus_context *ctx, struct ubus_event_handler *ev,
353 | 		     const char *type, struct blob_attr *msg)
354 | {
355 | 	static const struct blobmsg_policy policy =
356 | 		{ "path", BLOBMSG_TYPE_STRING };
357 | 	struct blob_attr *attr;
358 | 	const char *path;
359 | 
360 | 	blobmsg_parse(&policy, 1, &attr, blobmsg_data(msg), blobmsg_len(msg));
361 | 
362 | 	if (!attr)
363 | 		return;
364 | 
365 | 	path = blobmsg_get_string(attr);
366 | 	if (!strcmp(path, "dnsmasq.dns"))
367 | 		qosify_subscribe_dnsmasq(ctx);
368 | 	else if (!strcmp(path, "bridger"))
369 | 		qosify_ubus_update_bridger(false);
370 | }
371 | 
372 | 
373 | static void
374 | ubus_connect_handler(struct ubus_context *ctx)
375 | {
376 | 	static struct ubus_event_handler ev = {
377 | 		.cb = qosify_ubus_event_cb
378 | 	};
379 | 
380 | 	ubus_add_object(ctx, &qosify_object);
381 | 	ubus_register_event_handler(ctx, &ev, "ubus.object.add");
382 | 	qosify_subscribe_dnsmasq(ctx);
383 | }
384 | 
385 | static struct ubus_auto_conn conn;
386 | 
387 | void qosify_ubus_update_bridger(bool shutdown)
388 | {
389 | 	struct ubus_request req;
390 | 	uint32_t id;
391 | 	void *c;
392 | 
393 | 	if (ubus_lookup_id(&conn.ctx, "bridger", &id))
394 | 		return;
395 | 
396 | 	blob_buf_init(&b, 0);
397 | 	blobmsg_add_string(&b, "name", "qosify");
398 | 	c = blobmsg_open_array(&b, "devices");
399 | 	if (!shutdown)
400 | 		qosify_iface_get_devices(&b);
401 | 	blobmsg_close_array(&b, c);
402 | 
403 | 	ubus_invoke_async(&conn.ctx, id, "set_blacklist", b.head, &req);
404 | }
405 | 
406 | int qosify_ubus_init(void)
407 | {
408 | 	conn.cb = ubus_connect_handler;
409 | 	ubus_auto_connect(&conn);
410 | 
411 | 	return 0;
412 | }
413 | 
414 | void qosify_ubus_stop(void)
415 | {
416 | 	qosify_ubus_update_bridger(true);
417 | 	ubus_auto_shutdown(&conn);
418 | }
419 | 
420 | struct iface_req {
421 | 	char *name;
422 | 	int len;
423 | };
424 | 
425 | static void
426 | netifd_if_cb(struct ubus_request *req, int type, struct blob_attr *msg)
427 | {
428 | 	struct iface_req *ifr = req->priv;
429 | 	enum {
430 | 		IFS_ATTR_UP,
431 | 		IFS_ATTR_DEV,
432 | 		__IFS_ATTR_MAX
433 | 	};
434 | 	static const struct blobmsg_policy policy[__IFS_ATTR_MAX] = {
435 | 		[IFS_ATTR_UP] = { "up", BLOBMSG_TYPE_BOOL },
436 | 		[IFS_ATTR_DEV] = { "l3_device", BLOBMSG_TYPE_STRING },
437 | 	};
438 | 	struct blob_attr *tb[__IFS_ATTR_MAX];
439 | 
440 | 	blobmsg_parse(policy, __IFS_ATTR_MAX, tb, blobmsg_data(msg), blobmsg_len(msg));
441 | 
442 | 	if (!tb[IFS_ATTR_UP] || !tb[IFS_ATTR_DEV])
443 | 		return;
444 | 
445 | 	if (!blobmsg_get_bool(tb[IFS_ATTR_UP]))
446 | 		return;
447 | 
448 | 	snprintf(ifr->name, ifr->len, "%s", blobmsg_get_string(tb[IFS_ATTR_DEV]));
449 | }
450 | 
451 | int qosify_ubus_check_interface(const char *name, char *ifname, int ifname_len)
452 | {
453 | 	struct iface_req req = { ifname, ifname_len };
454 | 	char *obj_name = "network.interface.";
455 | 	uint32_t id;
456 | 
457 | #define PREFIX "network.interface."
458 | 	obj_name = alloca(sizeof(PREFIX) + strlen(name) + 1);
459 | 	sprintf(obj_name, PREFIX "%s", name);
460 | #undef PREFIX
461 | 
462 | 	ifname[0] = 0;
463 | 
464 | 	if (ubus_lookup_id(&conn.ctx, obj_name, &id))
465 | 		return -1;
466 | 
467 | 	blob_buf_init(&b, 0);
468 | 	ubus_invoke(&conn.ctx, id, "status", b.head, netifd_if_cb, &req, 1000);
469 | 
470 | 	if (!ifname[0])
471 | 		return -1;
472 | 
473 | 	return 0;
474 | }
475 | 


--------------------------------------------------------------------------------
/interface.c:
--------------------------------------------------------------------------------
  1 | // SPDX-License-Identifier: GPL-2.0+
  2 | /*
  3 |  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
  4 |  */
  5 | #define _GNU_SOURCE
  6 | #include <sys/types.h>
  7 | #include <sys/socket.h>
  8 | #include <sys/ioctl.h>
  9 | #include <net/if_arp.h>
 10 | #include <net/if.h>
 11 | #include <netinet/if_ether.h>
 12 | 
 13 | #include <unistd.h>
 14 | #include <errno.h>
 15 | 
 16 | #include <netlink/msg.h>
 17 | #include <netlink/attr.h>
 18 | #include <netlink/socket.h>
 19 | 
 20 | #include <linux/rtnetlink.h>
 21 | #include <linux/pkt_cls.h>
 22 | 
 23 | #include <libubox/vlist.h>
 24 | #include <libubox/avl-cmp.h>
 25 | #include <libubox/uloop.h>
 26 | 
 27 | #include "qosify.h"
 28 | 
 29 | static void interface_update_cb(struct vlist_tree *tree,
 30 | 				struct vlist_node *node_new,
 31 | 				struct vlist_node *node_old);
 32 | 
 33 | static VLIST_TREE(devices, avl_strcmp, interface_update_cb, true, false);
 34 | static VLIST_TREE(interfaces, avl_strcmp, interface_update_cb, true, false);
 35 | static int socket_fd;
 36 | static struct nl_sock *rtnl_sock;
 37 | 
 38 | #define APPEND(_buf, _ofs, _format, ...) _ofs += snprintf(_buf + _ofs, sizeof(_buf) - _ofs, _format, ##__VA_ARGS__)
 39 | 
 40 | struct qosify_iface_config {
 41 | 	struct blob_attr *data;
 42 | 
 43 | 	bool ingress;
 44 | 	bool egress;
 45 | 	bool nat;
 46 | 	bool host_isolate;
 47 | 	bool autorate_ingress;
 48 | 
 49 | 	const char *bandwidth_up;
 50 | 	const char *bandwidth_down;
 51 | 	const char *mode;
 52 | 	const char *common_opts;
 53 | 	const char *ingress_opts;
 54 | 	const char *egress_opts;
 55 | };
 56 | 
 57 | 
 58 | struct qosify_iface {
 59 | 	struct vlist_node node;
 60 | 
 61 | 	char ifname[IFNAMSIZ];
 62 | 	bool active;
 63 | 
 64 | 	bool device;
 65 | 	struct blob_attr *config_data;
 66 | 	struct qosify_iface_config config;
 67 | };
 68 | 
 69 | enum {
 70 | 	IFACE_ATTR_BW_UP,
 71 | 	IFACE_ATTR_BW_DOWN,
 72 | 	IFACE_ATTR_INGRESS,
 73 | 	IFACE_ATTR_EGRESS,
 74 | 	IFACE_ATTR_MODE,
 75 | 	IFACE_ATTR_NAT,
 76 | 	IFACE_ATTR_HOST_ISOLATE,
 77 | 	IFACE_ATTR_AUTORATE_IN,
 78 | 	IFACE_ATTR_INGRESS_OPTS,
 79 | 	IFACE_ATTR_EGRESS_OPTS,
 80 | 	IFACE_ATTR_OPTS,
 81 | 	__IFACE_ATTR_MAX
 82 | };
 83 | 
 84 | static inline const char *qosify_iface_name(struct qosify_iface *iface)
 85 | {
 86 | 	return iface->node.avl.key;
 87 | }
 88 | 
 89 | static void
 90 | iface_config_parse(struct blob_attr *attr, struct blob_attr **tb)
 91 | {
 92 | 	static const struct blobmsg_policy policy[__IFACE_ATTR_MAX] = {
 93 | 		[IFACE_ATTR_BW_UP] = { "bandwidth_up", BLOBMSG_TYPE_STRING },
 94 | 		[IFACE_ATTR_BW_DOWN] = { "bandwidth_down", BLOBMSG_TYPE_STRING },
 95 | 		[IFACE_ATTR_INGRESS] = { "ingress", BLOBMSG_TYPE_BOOL },
 96 | 		[IFACE_ATTR_EGRESS] = { "egress", BLOBMSG_TYPE_BOOL },
 97 | 		[IFACE_ATTR_MODE] = { "mode", BLOBMSG_TYPE_STRING },
 98 | 		[IFACE_ATTR_NAT] = { "nat", BLOBMSG_TYPE_BOOL },
 99 | 		[IFACE_ATTR_HOST_ISOLATE] = { "host_isolate", BLOBMSG_TYPE_BOOL },
100 | 		[IFACE_ATTR_AUTORATE_IN] = { "autorate_ingress", BLOBMSG_TYPE_BOOL },
101 | 		[IFACE_ATTR_INGRESS_OPTS] = { "ingress_options", BLOBMSG_TYPE_STRING },
102 | 		[IFACE_ATTR_EGRESS_OPTS] = { "egress_options", BLOBMSG_TYPE_STRING },
103 | 		[IFACE_ATTR_OPTS] = { "options", BLOBMSG_TYPE_STRING },
104 | 	};
105 | 
106 | 	blobmsg_parse(policy, __IFACE_ATTR_MAX, tb, blobmsg_data(attr), blobmsg_len(attr));
107 | }
108 | 
109 | static bool
110 | iface_config_equal(struct qosify_iface *if1, struct qosify_iface *if2)
111 | {
112 | 	struct blob_attr *tb1[__IFACE_ATTR_MAX], *tb2[__IFACE_ATTR_MAX];
113 | 	int i;
114 | 
115 | 	iface_config_parse(if1->config_data, tb1);
116 | 	iface_config_parse(if2->config_data, tb2);
117 | 
118 | 	for (i = 0; i < __IFACE_ATTR_MAX; i++) {
119 | 		if (!!tb1[i] != !!tb2[i])
120 | 			return false;
121 | 
122 | 		if (!tb1[i])
123 | 			continue;
124 | 
125 | 		if (blob_raw_len(tb1[i]) != blob_raw_len(tb2[i]))
126 | 			return false;
127 | 
128 | 		if (memcmp(tb1[i], tb2[i], blob_raw_len(tb1[i])) != 0)
129 | 			return false;
130 | 	}
131 | 
132 | 	return true;
133 | }
134 | 
135 | static const char *check_str(struct blob_attr *attr)
136 | {
137 | 	const char *str = blobmsg_get_string(attr);
138 | 
139 | 	if (strchr(str, '\''))
140 | 		return NULL;
141 | 
142 | 	return str;
143 | }
144 | 
145 | static void
146 | iface_config_set(struct qosify_iface *iface, struct blob_attr *attr)
147 | {
148 | 	struct qosify_iface_config *cfg = &iface->config;
149 | 	struct blob_attr *tb[__IFACE_ATTR_MAX];
150 | 	struct blob_attr *cur;
151 | 
152 | 	iface_config_parse(attr, tb);
153 | 
154 | 	memset(cfg, 0, sizeof(*cfg));
155 | 
156 | 	/* defaults */
157 | 	cfg->mode = "diffserv4";
158 | 	cfg->ingress = true;
159 | 	cfg->egress = true;
160 | 	cfg->host_isolate = true;
161 | 	cfg->autorate_ingress = false;
162 | 	cfg->nat = !iface->device;
163 | 
164 | 	if ((cur = tb[IFACE_ATTR_BW_UP]) != NULL)
165 | 		cfg->bandwidth_up = check_str(cur);
166 | 	if ((cur = tb[IFACE_ATTR_BW_DOWN]) != NULL)
167 | 		cfg->bandwidth_down = check_str(cur);
168 | 	if ((cur = tb[IFACE_ATTR_MODE]) != NULL)
169 | 		cfg->mode = check_str(cur);
170 | 	if ((cur = tb[IFACE_ATTR_OPTS]) != NULL)
171 | 		cfg->common_opts = check_str(cur);
172 | 	if ((cur = tb[IFACE_ATTR_EGRESS_OPTS]) != NULL)
173 | 		cfg->egress_opts = check_str(cur);
174 | 	if ((cur = tb[IFACE_ATTR_INGRESS_OPTS]) != NULL)
175 | 		cfg->ingress_opts = check_str(cur);
176 | 	if ((cur = tb[IFACE_ATTR_INGRESS]) != NULL)
177 | 		cfg->ingress = blobmsg_get_bool(cur);
178 | 	if ((cur = tb[IFACE_ATTR_EGRESS]) != NULL)
179 | 		cfg->egress = blobmsg_get_bool(cur);
180 | 	if ((cur = tb[IFACE_ATTR_NAT]) != NULL)
181 | 		cfg->nat = blobmsg_get_bool(cur);
182 | 	if ((cur = tb[IFACE_ATTR_HOST_ISOLATE]) != NULL)
183 | 		cfg->host_isolate = blobmsg_get_bool(cur);
184 | 	if ((cur = tb[IFACE_ATTR_AUTORATE_IN]) != NULL)
185 | 		cfg->autorate_ingress = blobmsg_get_bool(cur);
186 | }
187 | 
188 | static const char *
189 | interface_ifb_name(struct qosify_iface *iface)
190 | {
191 | 	static char ifname[IFNAMSIZ + 1] = "ifb-";
192 | 	int len = strlen(iface->ifname);
193 | 
194 | 	if (len + 4 < IFNAMSIZ) {
195 | 		snprintf(ifname + 4, IFNAMSIZ - 4, "%s", iface->ifname);
196 | 
197 | 		return ifname;
198 | 	}
199 | 
200 | 	ifname[4] = iface->ifname[0];
201 | 	ifname[5] = iface->ifname[1];
202 | 	snprintf(ifname + 6, IFNAMSIZ - 6, "%s", iface->ifname + len - (IFNAMSIZ + 6) - 1);
203 | 
204 | 	return ifname;
205 | }
206 | 
207 | static int
208 | prepare_qdisc_cmd(char *buf, int len, const char *dev, bool add, const char *type)
209 | {
210 | 	return snprintf(buf, len, "tc qdisc %s dev '%s' %s",
211 | 			add ? "add" : "del", dev, type);
212 | }
213 | 
214 | static int
215 | prepare_filter_cmd(char *buf, int len, const char *dev, int prio, bool add, bool egress)
216 | {
217 | 	return snprintf(buf, len, "tc filter %s dev '%s' %sgress prio %d",
218 | 			add ? "add" : "del", dev, egress ? "e" : "in", prio);
219 | }
220 | 
221 | static int
222 | cmd_add_bpf_filter(const char *ifname, int prio, bool egress, bool eth)
223 | {
224 | 	struct tcmsg tcmsg = {
225 | 		.tcm_family = AF_UNSPEC,
226 | 		.tcm_ifindex = if_nametoindex(ifname),
227 | 	};
228 | 	struct nl_msg *msg;
229 | 	struct nlattr *opts;
230 | 	const char *suffix;
231 | 	int prog_fd = -1;
232 | 	char name[32];
233 | 
234 | 	suffix = qosify_get_program(!egress * QOSIFY_INGRESS + !eth * QOSIFY_IP_ONLY, &prog_fd);
235 | 	if (!suffix)
236 | 		return -1;
237 | 
238 | 	snprintf(name, sizeof(name), "qosify_%s", suffix);
239 | 
240 | 	if (egress)
241 | 		tcmsg.tcm_parent = TC_H_MAKE(TC_H_CLSACT, TC_H_MIN_EGRESS);
242 | 	else
243 | 		tcmsg.tcm_parent = TC_H_MAKE(TC_H_CLSACT, TC_H_MIN_INGRESS);
244 | 
245 | 	tcmsg.tcm_info = TC_H_MAKE(prio << 16, htons(ETH_P_ALL));
246 | 
247 | 	msg = nlmsg_alloc_simple(RTM_NEWTFILTER, NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL);
248 | 	nlmsg_append(msg, &tcmsg, sizeof(tcmsg), NLMSG_ALIGNTO);
249 | 	nla_put_string(msg, TCA_KIND, "bpf");
250 | 
251 | 	opts = nla_nest_start(msg, TCA_OPTIONS);
252 | 	nla_put_u32(msg, TCA_BPF_FD, prog_fd);
253 | 	nla_put_string(msg, TCA_BPF_NAME, name);
254 | 	nla_put_u32(msg, TCA_BPF_FLAGS, TCA_BPF_FLAG_ACT_DIRECT);
255 | 	nla_put_u32(msg, TCA_BPF_FLAGS_GEN, TCA_CLS_FLAGS_SKIP_HW);
256 | 	nla_nest_end(msg, opts);
257 | 
258 | 	nl_send_auto_complete(rtnl_sock, msg);
259 | 	nlmsg_free(msg);
260 | 
261 | 	return nl_wait_for_ack(rtnl_sock);
262 | }
263 | 
264 | static int
265 | cmd_add_qdisc(struct qosify_iface *iface, const char *ifname, bool egress, bool eth)
266 | {
267 | 	struct qosify_iface_config *cfg = &iface->config;
268 | 	const char *bw = egress ? cfg->bandwidth_up : cfg->bandwidth_down;
269 | 	const char *dir_opts = egress ? cfg->egress_opts : cfg->ingress_opts;
270 | 	char buf[512];
271 | 	int ofs;
272 | 
273 | 	ofs = prepare_qdisc_cmd(buf, sizeof(buf), ifname, true, "clsact");
274 | 	qosify_run_cmd(buf, true);
275 | 
276 | 	ofs = prepare_qdisc_cmd(buf, sizeof(buf), ifname, true, "root cake");
277 | 	if (bw)
278 | 		APPEND(buf, ofs, " bandwidth %s", bw);
279 | 
280 | 	APPEND(buf, ofs, " %s %sgress", cfg->mode, egress ? "e" : "in");
281 | 	if (!egress && cfg->autorate_ingress)
282 | 		APPEND(buf, ofs, " autorate-ingress");
283 | 
284 | 	if (cfg->host_isolate)
285 | 		APPEND(buf, ofs, " %snat dual-%shost",
286 | 			cfg->nat ? "" : "no",
287 | 			egress ? "src" : "dst");
288 | 	else
289 | 		APPEND(buf, ofs, " flows");
290 | 
291 | 	APPEND(buf, ofs, " %s %s",
292 | 	       cfg->common_opts ? cfg->common_opts : "",
293 | 	       dir_opts ? dir_opts : "");
294 | 
295 | 	return qosify_run_cmd(buf, false);
296 | }
297 | 
298 | static int
299 | cmd_add_ingress(struct qosify_iface *iface, bool eth)
300 | {
301 | 	const char *ifbdev = interface_ifb_name(iface);
302 | 	char buf[256];
303 | 	int prio = QOSIFY_PRIO_BASE;
304 | 	int ofs;
305 | 
306 | 	cmd_add_bpf_filter(iface->ifname, prio++, false, eth);
307 | 
308 | 	ofs = prepare_filter_cmd(buf, sizeof(buf), iface->ifname, prio++, true, false);
309 | 	APPEND(buf, ofs, " protocol ip u32 match ip sport 53 0xffff "
310 | 			 "flowid 1:1 action mirred egress redirect dev " QOSIFY_DNS_IFNAME);
311 | 	qosify_run_cmd(buf, false);
312 | 
313 | 	ofs = prepare_filter_cmd(buf, sizeof(buf), iface->ifname, prio++, true, false);
314 | 	APPEND(buf, ofs, " protocol 802.1Q u32 offset plus 4 match ip sport 53 0xffff "
315 | 			 "flowid 1:1 action mirred egress redirect dev " QOSIFY_DNS_IFNAME);
316 | 	qosify_run_cmd(buf, false);
317 | 
318 | 	ofs = prepare_filter_cmd(buf, sizeof(buf), iface->ifname, prio++, true, false);
319 | 	APPEND(buf, ofs, " protocol ipv6 u32 match ip6 sport 53 0xffff "
320 | 			 "flowid 1:1 action mirred egress redirect dev " QOSIFY_DNS_IFNAME);
321 | 	qosify_run_cmd(buf, false);
322 | 
323 | 	ofs = prepare_filter_cmd(buf, sizeof(buf), iface->ifname, prio++, true, false);
324 | 	APPEND(buf, ofs, " protocol ipv6 u32 offset plus 4 match ip6 sport 53 0xffff "
325 | 			 "flowid 1:1 action mirred egress redirect dev " QOSIFY_DNS_IFNAME);
326 | 	qosify_run_cmd(buf, false);
327 | 
328 | 
329 | 	if (!iface->config.ingress)
330 | 		return 0;
331 | 
332 | 	snprintf(buf, sizeof(buf), "ip link add '%s' type ifb", ifbdev);
333 | 	qosify_run_cmd(buf, false);
334 | 
335 | 	cmd_add_qdisc(iface, ifbdev, false, eth);
336 | 
337 | 	snprintf(buf, sizeof(buf), "ip link set dev '%s' up", ifbdev);
338 | 	qosify_run_cmd(buf, false);
339 | 
340 | 	ofs = prepare_filter_cmd(buf, sizeof(buf), iface->ifname, prio++, true, false);
341 | 	APPEND(buf, ofs, " protocol all u32 match u32 0 0 flowid 1:1"
342 | 			 " action mirred egress redirect dev '%s'", ifbdev);
343 | 	return qosify_run_cmd(buf, false);
344 | }
345 | 
346 | static int cmd_add_egress(struct qosify_iface *iface, bool eth)
347 | {
348 | 	if (!iface->config.egress)
349 | 		return 0;
350 | 
351 | 	cmd_add_qdisc(iface, iface->ifname, true, eth);
352 | 
353 | 	return cmd_add_bpf_filter(iface->ifname, QOSIFY_PRIO_BASE, true, eth);
354 | }
355 | 
356 | static void
357 | interface_clear_qdisc(struct qosify_iface *iface)
358 | {
359 | 	char buf[64];
360 | 	int i;
361 | 
362 | 	prepare_qdisc_cmd(buf, sizeof(buf), iface->ifname, false, "root");
363 | 	qosify_run_cmd(buf, true);
364 | 
365 | 	for (i = 0; i < 6; i++) {
366 | 		prepare_filter_cmd(buf, sizeof(buf), iface->ifname, QOSIFY_PRIO_BASE + i, false, false);
367 | 		qosify_run_cmd(buf, true);
368 | 	}
369 | 
370 | 	prepare_filter_cmd(buf, sizeof(buf), iface->ifname, QOSIFY_PRIO_BASE, false, true);
371 | 	qosify_run_cmd(buf, true);
372 | 
373 | 	snprintf(buf, sizeof(buf), "ip link del '%s'", interface_ifb_name(iface));
374 | 	qosify_run_cmd(buf, true);
375 | }
376 | 
377 | static void
378 | interface_start(struct qosify_iface *iface)
379 | {
380 | 	struct ifreq ifr = {};
381 | 	bool eth;
382 | 
383 | 	if (!iface->ifname[0] || iface->active)
384 | 		return;
385 | 
386 | 	ULOG_INFO("start interface %s\n", iface->ifname);
387 | 
388 | 	strncpy(ifr.ifr_name, iface->ifname, sizeof(ifr.ifr_name));
389 | 	if (ioctl(socket_fd, SIOCGIFHWADDR, &ifr) < 0) {
390 | 		ULOG_ERR("ioctl(SIOCGIFHWADDR, %s) failed: %s\n", iface->ifname, strerror(errno));
391 | 		return;
392 | 	}
393 | 
394 | 	eth = ifr.ifr_hwaddr.sa_family == ARPHRD_ETHER;
395 | 
396 | 	interface_clear_qdisc(iface);
397 | 	cmd_add_egress(iface, eth);
398 | 	cmd_add_ingress(iface, eth);
399 | 
400 | 	iface->active = true;
401 | }
402 | 
403 | static void
404 | interface_stop(struct qosify_iface *iface)
405 | {
406 | 	if (!iface->ifname[0] || !iface->active)
407 | 		return;
408 | 
409 | 	ULOG_INFO("stop interface %s\n", iface->ifname);
410 | 	iface->active = false;
411 | 
412 | 	interface_clear_qdisc(iface);
413 | }
414 | 
415 | static void
416 | interface_set_config(struct qosify_iface *iface, struct blob_attr *config)
417 | {
418 | 	iface->config_data = blob_memdup(config);
419 | 	iface_config_set(iface, iface->config_data);
420 | 	interface_start(iface);
421 | }
422 | 
423 | static void
424 | interface_update_cb(struct vlist_tree *tree,
425 | 		    struct vlist_node *node_new, struct vlist_node *node_old)
426 | {
427 | 	struct qosify_iface *if_new = NULL, *if_old = NULL;
428 | 
429 | 	if (node_new)
430 | 		if_new = container_of(node_new, struct qosify_iface, node);
431 | 	if (node_old)
432 | 		if_old = container_of(node_old, struct qosify_iface, node);
433 | 
434 | 	if (if_new && if_old) {
435 | 		if (!iface_config_equal(if_old, if_new)) {
436 | 			interface_stop(if_old);
437 | 			free(if_old->config_data);
438 | 			interface_set_config(if_old, if_new->config_data);
439 | 		}
440 | 
441 | 		free(if_new);
442 | 		return;
443 | 	}
444 | 
445 | 	if (if_old) {
446 | 		interface_stop(if_old);
447 | 		free(if_old->config_data);
448 | 		free(if_old);
449 | 	}
450 | 
451 | 	if (if_new)
452 | 		interface_set_config(if_new, if_new->config_data);
453 | }
454 | 
455 | static void
456 | interface_create(struct blob_attr *attr, bool device)
457 | {
458 | 	struct qosify_iface *iface;
459 | 	const char *name = blobmsg_name(attr);
460 | 	int name_len = strlen(name);
461 | 	char *name_buf;
462 | 
463 | 	if (strchr(name, '\''))
464 | 		return;
465 | 
466 | 	if (name_len >= IFNAMSIZ)
467 | 		return;
468 | 
469 | 	if (blobmsg_type(attr) != BLOBMSG_TYPE_TABLE)
470 | 		return;
471 | 
472 | 	iface = calloc_a(sizeof(*iface), &name_buf, name_len + 1);
473 | 	strcpy(name_buf, blobmsg_name(attr));
474 | 	iface->config_data = attr;
475 | 	iface->device = device;
476 | 	vlist_add(device ? &devices : &interfaces, &iface->node, name_buf);
477 | }
478 | 
479 | void qosify_iface_config_update(struct blob_attr *ifaces, struct blob_attr *devs)
480 | {
481 | 	struct blob_attr *cur;
482 | 	int rem;
483 | 
484 | 	vlist_update(&devices);
485 | 	blobmsg_for_each_attr(cur, devs, rem)
486 | 		interface_create(cur, true);
487 | 	vlist_flush(&devices);
488 | 
489 | 	vlist_update(&interfaces);
490 | 	blobmsg_for_each_attr(cur, ifaces, rem)
491 | 		interface_create(cur, false);
492 | 	vlist_flush(&interfaces);
493 | }
494 | 
495 | static void
496 | qosify_iface_check_device(struct qosify_iface *iface)
497 | {
498 | 	const char *name = qosify_iface_name(iface);
499 | 	int ifindex;
500 | 
501 | 	ifindex = if_nametoindex(name);
502 | 	if (!ifindex) {
503 | 		interface_stop(iface);
504 | 		iface->ifname[0] = 0;
505 | 	} else {
506 | 		snprintf(iface->ifname, sizeof(iface->ifname), "%s", name);
507 | 		interface_start(iface);
508 | 	}
509 | }
510 | 
511 | static void
512 | qosify_iface_check_interface(struct qosify_iface *iface)
513 | {
514 | 	const char *name = qosify_iface_name(iface);
515 | 	char ifname[IFNAMSIZ];
516 | 
517 | 	if (qosify_ubus_check_interface(name, ifname, sizeof(ifname)) == 0) {
518 | 		snprintf(iface->ifname, sizeof(iface->ifname), "%s", ifname);
519 | 		interface_start(iface);
520 | 	} else {
521 | 		interface_stop(iface);
522 | 		iface->ifname[0] = 0;
523 | 	}
524 | }
525 | 
526 | static void qos_iface_check_cb(struct uloop_timeout *t)
527 | {
528 | 	struct qosify_iface *iface;
529 | 
530 | 	vlist_for_each_element(&devices, iface, node)
531 | 		qosify_iface_check_device(iface);
532 | 	vlist_for_each_element(&interfaces, iface, node)
533 | 		qosify_iface_check_interface(iface);
534 | 	qosify_ubus_update_bridger(false);
535 | }
536 | 
537 | void qosify_iface_check(void)
538 | {
539 | 	static struct uloop_timeout timer = {
540 | 		.cb = qos_iface_check_cb,
541 | 	};
542 | 
543 | 	uloop_timeout_set(&timer, 10);
544 | }
545 | 
546 | static void
547 | __qosify_iface_status(struct blob_buf *b, struct qosify_iface *iface)
548 | {
549 | 	void *c;
550 | 
551 | 	c = blobmsg_open_table(b, qosify_iface_name(iface));
552 | 	blobmsg_add_u8(b, "active", iface->active);
553 | 	if (iface->ifname[0])
554 | 		blobmsg_add_string(b, "ifname", iface->ifname);
555 | 	blobmsg_add_u8(b, "egress", iface->config.egress);
556 | 	blobmsg_add_u8(b, "ingress", iface->config.ingress);
557 | 	blobmsg_close_table(b, c);
558 | 
559 | }
560 | 
561 | void qosify_iface_status(struct blob_buf *b)
562 | {
563 | 	struct qosify_iface *iface;
564 | 	void *c;
565 | 
566 | 	c = blobmsg_open_table(b, "devices");
567 | 	vlist_for_each_element(&devices, iface, node)
568 | 		__qosify_iface_status(b, iface);
569 | 	blobmsg_close_table(b, c);
570 | 
571 | 	c = blobmsg_open_table(b, "interfaces");
572 | 	vlist_for_each_element(&interfaces, iface, node)
573 | 		__qosify_iface_status(b, iface);
574 | 	blobmsg_close_table(b, c);
575 | }
576 | 
577 | static int
578 | qosify_nl_error_cb(struct sockaddr_nl *nla, struct nlmsgerr *err,
579 | 		   void *arg)
580 | {
581 | 	struct nlmsghdr *nlh = (struct nlmsghdr *) err - 1;
582 | 	struct nlattr *tb[NLMSGERR_ATTR_MAX + 1];
583 | 	struct nlattr *attrs;
584 | 	int ack_len = sizeof(*nlh) + sizeof(int) + sizeof(*nlh);
585 | 	int len = nlh->nlmsg_len;
586 | 	const char *errstr = "(unknown)";
587 | 
588 | 	if (!(nlh->nlmsg_flags & NLM_F_ACK_TLVS))
589 | 		return NL_STOP;
590 | 
591 | 	if (!(nlh->nlmsg_flags & NLM_F_CAPPED))
592 | 		ack_len += err->msg.nlmsg_len - sizeof(*nlh);
593 | 
594 | 	attrs = (void *) ((unsigned char *) nlh + ack_len);
595 | 	len -= ack_len;
596 | 
597 | 	nla_parse(tb, NLMSGERR_ATTR_MAX, attrs, len, NULL);
598 | 	if (tb[NLMSGERR_ATTR_MSG])
599 | 		errstr = nla_data(tb[NLMSGERR_ATTR_MSG]);
600 | 
601 | 	ULOG_ERR("Netlink error(%d): %s\n", err->error, errstr);
602 | 
603 | 	return NL_STOP;
604 | }
605 | 
606 | static void
607 | __qosify_iface_get_device(struct blob_buf *b, struct qosify_iface *iface)
608 | {
609 | 	if (!iface->ifname[0] || !iface->active)
610 | 		return;
611 | 
612 | 	blobmsg_add_string(b, NULL, iface->ifname);
613 | }
614 | 
615 | void qosify_iface_get_devices(struct blob_buf *b)
616 | {
617 | 	struct qosify_iface *iface;
618 | 
619 | 	vlist_for_each_element(&devices, iface, node)
620 | 		__qosify_iface_get_device(b, iface);
621 | 	vlist_for_each_element(&interfaces, iface, node)
622 | 		__qosify_iface_get_device(b, iface);
623 | }
624 | 
625 | int qosify_iface_init(void)
626 | {
627 | 	int fd, opt;
628 | 
629 | 	socket_fd = socket(AF_UNIX, SOCK_DGRAM, 0);
630 | 	if (socket < 0)
631 | 		return -1;
632 | 
633 | 	rtnl_sock = nl_socket_alloc();
634 | 	if (!rtnl_sock)
635 | 		return -1;
636 | 
637 | 	if (nl_connect(rtnl_sock, NETLINK_ROUTE))
638 | 		return -1;
639 | 
640 | 	nl_cb_err(nl_socket_get_cb(rtnl_sock), NL_CB_CUSTOM,
641 | 		  qosify_nl_error_cb, NULL);
642 | 
643 | 	fd = nl_socket_get_fd(rtnl_sock);
644 | 	opt = 1;
645 | 	setsockopt(fd, SOL_NETLINK, NETLINK_EXT_ACK, &opt, sizeof(opt));
646 | 
647 | 	opt = 1;
648 | 	setsockopt(fd, SOL_NETLINK, NETLINK_CAP_ACK, &opt, sizeof(opt));
649 | 
650 | 	return 0;
651 | }
652 | 
653 | void qosify_iface_stop(void)
654 | {
655 | 	struct qosify_iface *iface;
656 | 
657 | 	vlist_for_each_element(&interfaces, iface, node)
658 | 		interface_stop(iface);
659 | 	vlist_for_each_element(&devices, iface, node)
660 | 		interface_stop(iface);
661 | 
662 | 	nl_socket_free(rtnl_sock);
663 | }
664 | 
665 | 


--------------------------------------------------------------------------------
/map.c:
--------------------------------------------------------------------------------
   1 | // SPDX-License-Identifier: GPL-2.0+
   2 | /*
   3 |  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
   4 |  */
   5 | #include <arpa/inet.h>
   6 | 
   7 | #include <errno.h>
   8 | #include <stdio.h>
   9 | #include <ctype.h>
  10 | #include <stdlib.h>
  11 | #include <time.h>
  12 | #include <fnmatch.h>
  13 | #include <glob.h>
  14 | 
  15 | #include <libubox/uloop.h>
  16 | #include <libubox/avl-cmp.h>
  17 | 
  18 | #include "qosify.h"
  19 | 
  20 | struct qosify_map_class;
  21 | 
  22 | static int qosify_map_entry_cmp(const void *k1, const void *k2, void *ptr);
  23 | 
  24 | static int qosify_map_fds[__CL_MAP_MAX];
  25 | static AVL_TREE(map_data, qosify_map_entry_cmp, false, NULL);
  26 | static LIST_HEAD(map_files);
  27 | static struct qosify_map_class *map_class[QOSIFY_MAX_CLASS_ENTRIES];
  28 | static uint32_t next_timeout;
  29 | static uint8_t qosify_dscp_default[2] = { 0xff, 0xff };
  30 | int qosify_map_timeout;
  31 | int qosify_active_timeout;
  32 | struct qosify_config config;
  33 | struct qosify_flow_config flow_config;
  34 | static uint32_t map_dns_seq;
  35 | 
  36 | struct qosify_map_file {
  37 | 	struct list_head list;
  38 | 	char filename[];
  39 | };
  40 | 
  41 | struct qosify_map_class {
  42 | 	const char *name;
  43 | 	struct qosify_class data;
  44 | };
  45 | 
  46 | static const struct {
  47 | 	const char *name;
  48 | 	const char *type_name;
  49 | } qosify_map_info[] = {
  50 | 	[CL_MAP_TCP_PORTS] = { "tcp_ports", "tcp_port" },
  51 | 	[CL_MAP_UDP_PORTS] = { "udp_ports", "udp_port" },
  52 | 	[CL_MAP_IPV4_ADDR] = { "ipv4_map", "ipv4_addr" },
  53 | 	[CL_MAP_IPV6_ADDR] = { "ipv6_map", "ipv6_addr" },
  54 | 	[CL_MAP_CONFIG] = { "config", "config" },
  55 | 	[CL_MAP_CLASS] = { "class_map", "class" },
  56 | 	[CL_MAP_DNS] = { "dns", "dns" },
  57 | };
  58 | 
  59 | static const struct {
  60 | 	const char name[5];
  61 | 	uint8_t val;
  62 | } codepoints[] = {
  63 | 	{ "CS0", 0 },
  64 | 	{ "CS1", 8 },
  65 | 	{ "CS2", 16 },
  66 | 	{ "CS3", 24 },
  67 | 	{ "CS4", 32 },
  68 | 	{ "CS5", 40 },
  69 | 	{ "CS6", 48 },
  70 | 	{ "CS7", 56 },
  71 | 	{ "AF11", 10 },
  72 | 	{ "AF12", 12 },
  73 | 	{ "AF13", 14 },
  74 | 	{ "AF21", 18 },
  75 | 	{ "AF22", 20 },
  76 | 	{ "AF23", 22 },
  77 | 	{ "AF31", 26 },
  78 | 	{ "AF32", 28 },
  79 | 	{ "AF33", 30 },
  80 | 	{ "AF41", 34 },
  81 | 	{ "AF42", 36 },
  82 | 	{ "AF43", 38 },
  83 | 	{ "EF", 46 },
  84 | 	{ "VA", 44 },
  85 | 	{ "LE", 1 },
  86 | 	{ "DF", 0 },
  87 | };
  88 | 
  89 | static void qosify_map_timer_cb(struct uloop_timeout *t)
  90 | {
  91 | 	qosify_map_gc();
  92 | }
  93 | 
  94 | static struct uloop_timeout qosify_map_timer = {
  95 | 	.cb = qosify_map_timer_cb,
  96 | };
  97 | 
  98 | static uint32_t qosify_gettime(void)
  99 | {
 100 | 	struct timespec ts;
 101 | 
 102 | 	clock_gettime(CLOCK_MONOTONIC, &ts);
 103 | 
 104 | 	return ts.tv_sec;
 105 | }
 106 | 
 107 | static const char *
 108 | qosify_map_path(enum qosify_map_id id)
 109 | {
 110 | 	static char path[128];
 111 | 	const char *name;
 112 | 
 113 | 	if (id >= ARRAY_SIZE(qosify_map_info))
 114 | 		return NULL;
 115 | 
 116 | 	name = qosify_map_info[id].name;
 117 | 	if (!name)
 118 | 		return NULL;
 119 | 
 120 | 	snprintf(path, sizeof(path), "%s/%s", CLASSIFY_DATA_PATH, name);
 121 | 
 122 | 	return path;
 123 | }
 124 | 
 125 | static int qosify_map_get_fd(enum qosify_map_id id)
 126 | {
 127 | 	const char *path = qosify_map_path(id);
 128 | 	int fd;
 129 | 
 130 | 	if (!path)
 131 | 		return -1;
 132 | 
 133 | 	fd = bpf_obj_get(path);
 134 | 	if (fd < 0)
 135 | 		fprintf(stderr, "Failed to open map %s: %s\n", path, strerror(errno));
 136 | 
 137 | 	return fd;
 138 | }
 139 | 
 140 | static void qosify_map_clear_list(enum qosify_map_id id)
 141 | {
 142 | 	int fd = qosify_map_fds[id];
 143 | 	__u32 key[4] = {};
 144 | 
 145 | 	while (bpf_map_get_next_key(fd, &key, &key) == 0)
 146 | 		bpf_map_delete_elem(fd, &key);
 147 | }
 148 | 
 149 | static void __qosify_map_set_dscp_default(enum qosify_map_id id, uint8_t val)
 150 | {
 151 | 	struct qosify_map_data data = {
 152 | 		.id = id,
 153 | 	};
 154 | 	struct qosify_class class = {
 155 | 		.val.ingress = val,
 156 | 		.val.egress = val,
 157 | 	};
 158 | 	uint32_t key;
 159 | 	int fd;
 160 | 	int i;
 161 | 
 162 | 	if (!(val & QOSIFY_DSCP_CLASS_FLAG)) {
 163 | 		if (id == CL_MAP_TCP_PORTS)
 164 | 			key = QOSIFY_MAX_CLASS_ENTRIES;
 165 | 		else if (id == CL_MAP_UDP_PORTS)
 166 | 			key = QOSIFY_MAX_CLASS_ENTRIES + 1;
 167 | 		else
 168 | 			return;
 169 | 
 170 | 		fd = qosify_map_fds[CL_MAP_CLASS];
 171 | 
 172 | 		memcpy(&class.config, &flow_config, sizeof(class.config));
 173 | 		bpf_map_update_elem(fd, &key, &class, BPF_ANY);
 174 | 
 175 | 		val = key | QOSIFY_DSCP_CLASS_FLAG;
 176 | 	}
 177 | 
 178 | 	fd = qosify_map_fds[id];
 179 | 	for (i = 0; i < (1 << 16); i++) {
 180 | 		data.addr.port = htons(i);
 181 | 		if (avl_find(&map_data, &data))
 182 | 			continue;
 183 | 
 184 | 		bpf_map_update_elem(fd, &data.addr, &val, BPF_ANY);
 185 | 	}
 186 | }
 187 | 
 188 | void qosify_map_set_dscp_default(enum qosify_map_id id, uint8_t val)
 189 | {
 190 | 	bool udp;
 191 | 
 192 | 	if (id == CL_MAP_TCP_PORTS)
 193 | 		udp = false;
 194 | 	else if (id == CL_MAP_UDP_PORTS)
 195 | 		udp = true;
 196 | 	else
 197 | 		return;
 198 | 
 199 | 	if (val != 0xff) {
 200 | 		if (qosify_dscp_default[udp] == val)
 201 | 			return;
 202 | 
 203 | 		qosify_dscp_default[udp] = val;
 204 | 	}
 205 | 
 206 | 	__qosify_map_set_dscp_default(id, qosify_dscp_default[udp]);
 207 | }
 208 | 
 209 | int qosify_map_init(void)
 210 | {
 211 | 	int i;
 212 | 
 213 | 	for (i = 0; i < CL_MAP_DNS; i++) {
 214 | 		qosify_map_fds[i] = qosify_map_get_fd(i);
 215 | 		if (qosify_map_fds[i] < 0)
 216 | 			return -1;
 217 | 	}
 218 | 
 219 | 	qosify_map_clear_list(CL_MAP_IPV4_ADDR);
 220 | 	qosify_map_clear_list(CL_MAP_IPV6_ADDR);
 221 | 	qosify_map_reset_config();
 222 | 
 223 | 	return 0;
 224 | }
 225 | 
 226 | static char *str_skip(char *str, bool space)
 227 | {
 228 | 	while (*str && isspace(*str) == space)
 229 | 		str++;
 230 | 
 231 | 	return str;
 232 | }
 233 | 
 234 | static int
 235 | qosify_map_codepoint(const char *val)
 236 | {
 237 | 	int i;
 238 | 
 239 | 	for (i = 0; i < ARRAY_SIZE(codepoints); i++)
 240 | 		if (!strcmp(codepoints[i].name, val))
 241 | 			return codepoints[i].val;
 242 | 
 243 | 	return 0xff;
 244 | }
 245 | 
 246 | static int qosify_map_entry_cmp(const void *k1, const void *k2, void *ptr)
 247 | {
 248 | 	const struct qosify_map_data *d1 = k1;
 249 | 	const struct qosify_map_data *d2 = k2;
 250 | 
 251 | 	if (d1->id != d2->id)
 252 | 		return d2->id - d1->id;
 253 | 
 254 | 	if (d1->id == CL_MAP_DNS)
 255 | 		return strcmp(d1->addr.dns.pattern, d2->addr.dns.pattern);
 256 | 
 257 | 	return memcmp(&d1->addr, &d2->addr, sizeof(d1->addr));
 258 | }
 259 | 
 260 | static struct qosify_map_entry *
 261 | __qosify_map_alloc_entry(struct qosify_map_data *data)
 262 | {
 263 | 	struct qosify_map_entry *e;
 264 | 	char *pattern;
 265 | 	char *c;
 266 | 
 267 | 	if (data->id < CL_MAP_DNS) {
 268 | 		e = calloc(1, sizeof(*e));
 269 | 		memcpy(&e->data.addr, &data->addr, sizeof(e->data.addr));
 270 | 
 271 | 		return e;
 272 | 	}
 273 | 
 274 | 	e = calloc_a(sizeof(*e), &pattern, strlen(data->addr.dns.pattern) + 1);
 275 | 	strcpy(pattern, data->addr.dns.pattern);
 276 | 	e->data.addr.dns.pattern = pattern;
 277 | 
 278 | 	for (c = pattern; *c; c++)
 279 | 		*c = tolower(*c);
 280 | 
 281 | 	if (pattern[0] == '/' &&
 282 | 	    regcomp(&e->data.addr.dns.regex, pattern + 1,
 283 | 		    REG_EXTENDED | REG_NOSUB)) {
 284 | 		free(e);
 285 | 		return NULL;
 286 | 	}
 287 | 
 288 | 	return e;
 289 | }
 290 | 
 291 | void __qosify_map_set_entry(struct qosify_map_data *data)
 292 | {
 293 | 	int fd = qosify_map_fds[data->id];
 294 | 	struct qosify_map_entry *e;
 295 | 	bool file = data->file;
 296 | 	uint8_t prev_dscp = 0xff;
 297 | 	int32_t delta = 0;
 298 | 	bool add = data->dscp != 0xff;
 299 | 
 300 | 	e = avl_find_element(&map_data, data, e, avl);
 301 | 	if (!e) {
 302 | 		if (!add)
 303 | 			return;
 304 | 
 305 | 		e = __qosify_map_alloc_entry(data);
 306 | 		if (!e)
 307 | 			return;
 308 | 
 309 | 		e->avl.key = &e->data;
 310 | 		e->data.id = data->id;
 311 | 		avl_insert(&map_data, &e->avl);
 312 | 	} else {
 313 | 		prev_dscp = e->data.dscp;
 314 | 	}
 315 | 
 316 | 	if (file)
 317 | 		e->data.file = add;
 318 | 	else
 319 | 		e->data.user = add;
 320 | 
 321 | 	if (add) {
 322 | 		if (file)
 323 | 			e->data.file_dscp = data->dscp;
 324 | 		if (!e->data.user || !file)
 325 | 			e->data.dscp = data->dscp;
 326 | 	} else if (e->data.file && !file) {
 327 | 		e->data.dscp = e->data.file_dscp;
 328 | 	}
 329 | 
 330 | 	if (e->data.dscp != prev_dscp && data->id < CL_MAP_DNS) {
 331 | 		struct qosify_ip_map_val val = {
 332 | 			.dscp = e->data.dscp,
 333 | 			.seen = 1,
 334 | 		};
 335 | 
 336 | 		bpf_map_update_elem(fd, &data->addr, &val, BPF_ANY);
 337 | 	}
 338 | 
 339 | 	if (data->id == CL_MAP_DNS)
 340 | 		e->data.addr.dns.seq = ++map_dns_seq;
 341 | 
 342 | 	if (add) {
 343 | 		if (qosify_map_timeout == ~0 || file) {
 344 | 			e->timeout = ~0;
 345 | 			return;
 346 | 		}
 347 | 
 348 | 		e->timeout = qosify_gettime() + qosify_map_timeout;
 349 | 		delta = e->timeout - next_timeout;
 350 | 		if (next_timeout && delta >= 0)
 351 | 			return;
 352 | 	}
 353 | 
 354 | 	uloop_timeout_set(&qosify_map_timer, 1);
 355 | }
 356 | 
 357 | static int
 358 | qosify_map_set_port(struct qosify_map_data *data, const char *str)
 359 | {
 360 | 	unsigned long start_port, end_port;
 361 | 	char *err;
 362 | 	int i;
 363 | 
 364 | 	start_port = end_port = strtoul(str, &err, 0);
 365 | 	if (err && *err) {
 366 | 		if (*err == '-')
 367 | 			end_port = strtoul(err + 1, &err, 0);
 368 | 		if (*err)
 369 | 			return -1;
 370 | 	}
 371 | 
 372 | 	if (!start_port || end_port < start_port ||
 373 | 	    end_port >= 65535)
 374 | 		return -1;
 375 | 
 376 | 	for (i = start_port; i <= end_port; i++) {
 377 | 		data->addr.port = htons(i);
 378 | 		__qosify_map_set_entry(data);
 379 | 	}
 380 | 
 381 | 	return 0;
 382 | }
 383 | 
 384 | static int
 385 | qosify_map_fill_ip(struct qosify_map_data *data, const char *str)
 386 | {
 387 | 	int af;
 388 | 
 389 | 	if (data->id == CL_MAP_IPV6_ADDR)
 390 | 		af = AF_INET6;
 391 | 	else
 392 | 		af = AF_INET;
 393 | 
 394 | 	if (inet_pton(af, str, &data->addr) != 1)
 395 | 		return -1;
 396 | 
 397 | 	return 0;
 398 | }
 399 | 
 400 | int qosify_map_set_entry(enum qosify_map_id id, bool file, const char *str,
 401 | 			 uint8_t dscp)
 402 | {
 403 | 	struct qosify_map_data data = {
 404 | 		.id = id,
 405 | 		.file = file,
 406 | 		.dscp = dscp,
 407 | 	};
 408 | 
 409 | 	switch (id) {
 410 | 	case CL_MAP_DNS:
 411 | 		data.addr.dns.pattern = str;
 412 | 		if (str[-2] == 'c')
 413 | 			data.addr.dns.only_cname = 1;
 414 | 		break;
 415 | 	case CL_MAP_TCP_PORTS:
 416 | 	case CL_MAP_UDP_PORTS:
 417 | 		return qosify_map_set_port(&data, str);
 418 | 	case CL_MAP_IPV4_ADDR:
 419 | 	case CL_MAP_IPV6_ADDR:
 420 | 		if (qosify_map_fill_ip(&data, str))
 421 | 			return -1;
 422 | 		break;
 423 | 	default:
 424 | 		return -1;
 425 | 	}
 426 | 
 427 | 	__qosify_map_set_entry(&data);
 428 | 
 429 | 	return 0;
 430 | }
 431 | 
 432 | static int
 433 | __qosify_map_dscp_value(const char *val, uint8_t *dscp_val)
 434 | {
 435 | 	unsigned long dscp;
 436 | 	bool fallback = false;
 437 | 	char *err;
 438 | 
 439 | 	if (*val == '+') {
 440 | 		fallback = true;
 441 | 		val++;
 442 | 	}
 443 | 
 444 | 	dscp = strtoul(val, &err, 0);
 445 | 	if (err && *err)
 446 | 		dscp = qosify_map_codepoint(val);
 447 | 
 448 | 	if (dscp >= 64)
 449 | 		return -1;
 450 | 
 451 | 	*dscp_val = dscp | (fallback << 6);
 452 | 
 453 | 	return 0;
 454 | }
 455 | 
 456 | static int
 457 | qosify_map_check_class(const char *val, uint8_t *dscp_val)
 458 | {
 459 | 	int i;
 460 | 
 461 | 	for (i = 0; i < ARRAY_SIZE(map_class); i++) {
 462 | 		if (map_class[i] && !strcmp(val, map_class[i]->name)) {
 463 | 			*dscp_val = i | QOSIFY_DSCP_CLASS_FLAG;
 464 | 			return 0;
 465 | 		}
 466 | 	}
 467 | 
 468 | 	return -1;
 469 | }
 470 | 
 471 | int qosify_map_dscp_value(const char *val, uint8_t *dscp_val)
 472 | {
 473 | 	uint8_t fallback = 0;
 474 | 
 475 | 	if (*val == '+') {
 476 | 		fallback = QOSIFY_DSCP_FALLBACK_FLAG;
 477 | 		val++;
 478 | 	}
 479 | 
 480 | 	if (qosify_map_check_class(val, dscp_val) &&
 481 | 	    __qosify_map_dscp_value(val, dscp_val))
 482 | 			return -1;
 483 | 
 484 | 	*dscp_val |= fallback;
 485 | 
 486 | 	return 0;
 487 | }
 488 | 
 489 | static void
 490 | qosify_map_dscp_codepoint_str(char *dest, int len, uint8_t dscp)
 491 | {
 492 | 	int i;
 493 | 
 494 | 	if (dscp & QOSIFY_DSCP_FALLBACK_FLAG) {
 495 | 		*(dest++) = '+';
 496 | 		len--;
 497 | 		dscp &= ~QOSIFY_DSCP_FALLBACK_FLAG;
 498 | 	}
 499 | 
 500 | 	for (i = 0; i < ARRAY_SIZE(codepoints); i++) {
 501 | 		if (codepoints[i].val != dscp)
 502 | 			continue;
 503 | 
 504 | 		snprintf(dest, len, "%s", codepoints[i].name);
 505 | 		return;
 506 | 	}
 507 | 
 508 | 	snprintf(dest, len, "0x%x", dscp);
 509 | }
 510 | 
 511 | static void
 512 | qosify_map_parse_line(char *str)
 513 | {
 514 | 	const char *key, *value;
 515 | 	uint8_t dscp;
 516 | 
 517 | 	str = str_skip(str, true);
 518 | 	key = str;
 519 | 
 520 | 	str = str_skip(str, false);
 521 | 	if (!*str)
 522 | 		return;
 523 | 
 524 | 	*(str++) = 0;
 525 | 	str = str_skip(str, true);
 526 | 	value = str;
 527 | 
 528 | 	if (qosify_map_dscp_value(value, &dscp))
 529 | 		return;
 530 | 
 531 | 	if (!strncmp(key, "dns:", 4))
 532 | 		qosify_map_set_entry(CL_MAP_DNS, true, key + 4, dscp);
 533 | 	if (!strncmp(key, "dns_q:", 6) || !strncmp(key, "dns_c:", 6))
 534 | 		qosify_map_set_entry(CL_MAP_DNS, true, key + 6, dscp);
 535 | 	if (!strncmp(key, "tcp:", 4))
 536 | 		qosify_map_set_entry(CL_MAP_TCP_PORTS, true, key + 4, dscp);
 537 | 	else if (!strncmp(key, "udp:", 4))
 538 | 		qosify_map_set_entry(CL_MAP_UDP_PORTS, true, key + 4, dscp);
 539 | 	else if (strchr(key, ':'))
 540 | 		qosify_map_set_entry(CL_MAP_IPV6_ADDR, true, key, dscp);
 541 | 	else if (strchr(key, '.'))
 542 | 		qosify_map_set_entry(CL_MAP_IPV4_ADDR, true, key, dscp);
 543 | }
 544 | 
 545 | static void
 546 | __qosify_map_load_file_data(FILE *f)
 547 | {
 548 | 	char line[1024];
 549 | 	char *cur;
 550 | 
 551 | 	while (fgets(line, sizeof(line), f)) {
 552 | 		cur = strchr(line, '#');
 553 | 		if (cur)
 554 | 			*cur = 0;
 555 | 
 556 | 		cur = line + strlen(line);
 557 | 		if (cur == line)
 558 | 			continue;
 559 | 
 560 | 		while (cur > line && isspace(cur[-1]))
 561 | 			cur--;
 562 | 
 563 | 		*cur = 0;
 564 | 		qosify_map_parse_line(line);
 565 | 	}
 566 | 
 567 | }
 568 | 
 569 | static int
 570 | __qosify_map_load_file(const char *file)
 571 | {
 572 | 	glob_t gl;
 573 | 	FILE *f;
 574 | 	int i;
 575 | 
 576 | 	if (!file)
 577 | 		return 0;
 578 | 
 579 | 	glob(file, 0, NULL, &gl);
 580 | 
 581 | 	for (i = 0; i < gl.gl_pathc; i++) {
 582 | 		f = fopen(file, "r");
 583 | 		if (!f)
 584 | 			continue;
 585 | 
 586 | 		__qosify_map_load_file_data(f);
 587 | 		fclose(f);
 588 | 	}
 589 | 
 590 | 	globfree(&gl);
 591 | 
 592 | 	return 0;
 593 | }
 594 | 
 595 | int qosify_map_load_file(const char *file)
 596 | {
 597 | 	struct qosify_map_file *f;
 598 | 
 599 | 	if (!file)
 600 | 		return 0;
 601 | 
 602 | 	f = calloc(1, sizeof(*f) + strlen(file) + 1);
 603 | 	strcpy(f->filename, file);
 604 | 	list_add_tail(&f->list, &map_files);
 605 | 
 606 | 	return __qosify_map_load_file(file);
 607 | }
 608 | 
 609 | static void qosify_map_reset_file_entries(void)
 610 | {
 611 | 	struct qosify_map_entry *e;
 612 | 
 613 | 	map_dns_seq = 0;
 614 | 	avl_for_each_element(&map_data, e, avl)
 615 | 		e->data.file = false;
 616 | }
 617 | 
 618 | void qosify_map_clear_files(void)
 619 | {
 620 | 	struct qosify_map_file *f, *tmp;
 621 | 
 622 | 	qosify_map_reset_file_entries();
 623 | 
 624 | 	list_for_each_entry_safe(f, tmp, &map_files, list) {
 625 | 		list_del(&f->list);
 626 | 		free(f);
 627 | 	}
 628 | }
 629 | 
 630 | void qosify_map_reset_config(void)
 631 | {
 632 | 	qosify_map_clear_files();
 633 | 	qosify_map_set_dscp_default(CL_MAP_TCP_PORTS, 0);
 634 | 	qosify_map_set_dscp_default(CL_MAP_UDP_PORTS, 0);
 635 | 	qosify_map_timeout = 3600;
 636 | 	qosify_active_timeout = 300;
 637 | 
 638 | 	memset(&config, 0, sizeof(config));
 639 | 	flow_config.dscp_prio = 0xff;
 640 | 	flow_config.dscp_bulk = 0xff;
 641 | 	config.dscp_icmp = 0xff;
 642 | }
 643 | 
 644 | void qosify_map_reload(void)
 645 | {
 646 | 	struct qosify_map_file *f;
 647 | 
 648 | 	qosify_map_reset_file_entries();
 649 | 
 650 | 	list_for_each_entry(f, &map_files, list)
 651 | 		__qosify_map_load_file(f->filename);
 652 | 
 653 | 	qosify_map_gc();
 654 | 
 655 | 	qosify_map_set_dscp_default(CL_MAP_TCP_PORTS, 0xff);
 656 | 	qosify_map_set_dscp_default(CL_MAP_UDP_PORTS, 0xff);
 657 | }
 658 | 
 659 | static void qosify_map_free_entry(struct qosify_map_entry *e)
 660 | {
 661 | 	int fd = qosify_map_fds[e->data.id];
 662 | 
 663 | 	avl_delete(&map_data, &e->avl);
 664 | 	if (e->data.id < CL_MAP_DNS)
 665 | 		bpf_map_delete_elem(fd, &e->data.addr);
 666 | 	free(e);
 667 | }
 668 | 
 669 | static bool
 670 | qosify_map_entry_refresh_timeout(struct qosify_map_entry *e)
 671 | {
 672 | 	struct qosify_ip_map_val val;
 673 | 	int fd = qosify_map_fds[e->data.id];
 674 | 
 675 | 	if (e->data.id != CL_MAP_IPV4_ADDR &&
 676 | 	    e->data.id != CL_MAP_IPV6_ADDR)
 677 | 		return false;
 678 | 
 679 | 	if (bpf_map_lookup_elem(fd, &e->data.addr, &val))
 680 | 		return false;
 681 | 
 682 | 	if (!val.seen)
 683 | 		return false;
 684 | 
 685 | 	e->timeout = qosify_gettime() + qosify_active_timeout;
 686 | 	val.seen = 0;
 687 | 	bpf_map_update_elem(fd, &e->data.addr, &val, BPF_ANY);
 688 | 
 689 | 	return true;
 690 | }
 691 | 
 692 | void qosify_map_gc(void)
 693 | {
 694 | 	struct qosify_map_entry *e, *tmp;
 695 | 	int32_t timeout = 0;
 696 | 	uint32_t cur_time = qosify_gettime();
 697 | 
 698 | 	next_timeout = 0;
 699 | 	avl_for_each_element_safe(&map_data, e, avl, tmp) {
 700 | 		int32_t cur_timeout;
 701 | 
 702 | 		if (e->data.user && e->timeout != ~0) {
 703 | 			cur_timeout = e->timeout - cur_time;
 704 | 			if (cur_timeout <= 0 &&
 705 | 			    qosify_map_entry_refresh_timeout(e))
 706 | 				cur_timeout = e->timeout - cur_time;
 707 | 			if (cur_timeout <= 0) {
 708 | 				e->data.user = false;
 709 | 				e->data.dscp = e->data.file_dscp;
 710 | 			} else if (!timeout || cur_timeout < timeout) {
 711 | 				timeout = cur_timeout;
 712 | 				next_timeout = e->timeout;
 713 | 			}
 714 | 		}
 715 | 
 716 | 		if (e->data.file || e->data.user)
 717 | 			continue;
 718 | 
 719 | 		qosify_map_free_entry(e);
 720 | 	}
 721 | 
 722 | 	if (!timeout)
 723 | 		return;
 724 | 
 725 | 	uloop_timeout_set(&qosify_map_timer, timeout * 1000);
 726 | }
 727 | 
 728 | int qosify_map_lookup_dns_entry(char *host, bool cname, uint8_t *dscp, uint32_t *seq)
 729 | {
 730 | 	struct qosify_map_data data = {
 731 | 		.id = CL_MAP_DNS,
 732 | 		.addr.dns.pattern = "",
 733 | 	};
 734 | 	struct qosify_map_entry *e;
 735 | 	bool ret = -1;
 736 | 	char *c;
 737 | 
 738 | 	e = avl_find_ge_element(&map_data, &data, e, avl);
 739 | 	if (!e)
 740 | 		return -1;
 741 | 
 742 | 	for (c = host; *c; c++)
 743 | 		*c = tolower(*c);
 744 | 
 745 | 	avl_for_element_to_last(&map_data, e, e, avl) {
 746 | 		regex_t *regex = &e->data.addr.dns.regex;
 747 | 
 748 | 		if (e->data.id != CL_MAP_DNS)
 749 | 			break;
 750 | 
 751 | 		if (!cname && e->data.addr.dns.only_cname)
 752 | 			continue;
 753 | 
 754 | 		if (e->data.addr.dns.pattern[0] == '/') {
 755 | 			if (regexec(regex, host, 0, NULL, 0) != 0)
 756 | 				continue;
 757 | 		} else {
 758 | 			if (fnmatch(e->data.addr.dns.pattern, host, 0))
 759 | 				continue;
 760 | 		}
 761 | 
 762 | 		if (*dscp == 0xff || e->data.addr.dns.seq < *seq) {
 763 | 			*dscp = e->data.dscp;
 764 | 			*seq = e->data.addr.dns.seq;
 765 | 		}
 766 | 		ret = 0;
 767 | 	}
 768 | 
 769 | 	return ret;
 770 | }
 771 | 
 772 | 
 773 | int qosify_map_add_dns_host(char *host, const char *addr, const char *type, int ttl)
 774 | {
 775 | 	struct qosify_map_data data = {
 776 | 		.dscp = 0xff
 777 | 	};
 778 | 	int prev_timeout = qosify_map_timeout;
 779 | 	uint32_t lookup_seq = 0;
 780 | 
 781 | 	if (qosify_map_lookup_dns_entry(host, false, &data.dscp, &lookup_seq))
 782 | 		return 0;
 783 | 
 784 | 	data.user = true;
 785 | 	if (!strcmp(type, "A"))
 786 | 		data.id = CL_MAP_IPV4_ADDR;
 787 | 	else if (!strcmp(type, "AAAA"))
 788 | 		data.id = CL_MAP_IPV6_ADDR;
 789 | 	else
 790 | 		return 0;
 791 | 
 792 | 	if (qosify_map_fill_ip(&data, addr))
 793 | 		return -1;
 794 | 
 795 | 	if (ttl)
 796 | 		qosify_map_timeout = ttl;
 797 | 	__qosify_map_set_entry(&data);
 798 | 	qosify_map_timeout = prev_timeout;
 799 | 
 800 | 	return 0;
 801 | }
 802 | 
 803 | static void
 804 | blobmsg_add_dscp(struct blob_buf *b, const char *name, uint8_t dscp)
 805 | {
 806 | 	int buf_len = 8;
 807 | 	char *buf;
 808 | 
 809 | 	if (dscp & QOSIFY_DSCP_CLASS_FLAG) {
 810 | 		const char *val;
 811 | 		int idx;
 812 | 
 813 | 		idx = dscp & QOSIFY_DSCP_VALUE_MASK;
 814 | 		if (map_class[idx])
 815 | 			val = map_class[idx]->name;
 816 | 		else
 817 | 			val = "<invalid>";
 818 | 
 819 | 		blobmsg_printf(b, name, "%s%s",
 820 | 			       (dscp & QOSIFY_DSCP_FALLBACK_FLAG) ? "+" : "", val);
 821 | 		return;
 822 | 	}
 823 | 
 824 | 	buf = blobmsg_alloc_string_buffer(b, name, buf_len);
 825 | 	qosify_map_dscp_codepoint_str(buf, buf_len, dscp);
 826 | 	blobmsg_add_string_buffer(b);
 827 | }
 828 | 
 829 | 
 830 | void qosify_map_dump(struct blob_buf *b)
 831 | {
 832 | 	struct qosify_map_entry *e;
 833 | 	uint32_t cur_time = qosify_gettime();
 834 | 	int buf_len = INET6_ADDRSTRLEN + 1;
 835 | 	char *buf;
 836 | 	void *a;
 837 | 	int af;
 838 | 
 839 | 	a = blobmsg_open_array(b, "entries");
 840 | 	avl_for_each_element(&map_data, e, avl) {
 841 | 		void *c;
 842 | 
 843 | 		if (!e->data.file && !e->data.user)
 844 | 			continue;
 845 | 
 846 | 		c = blobmsg_open_table(b, NULL);
 847 | 		if (e->data.user && e->timeout != ~0) {
 848 | 			int32_t cur_timeout = e->timeout - cur_time;
 849 | 
 850 | 			if (cur_timeout < 0)
 851 | 				cur_timeout = 0;
 852 | 
 853 | 			blobmsg_add_u32(b, "timeout", cur_timeout);
 854 | 		}
 855 | 
 856 | 		blobmsg_add_u8(b, "file", e->data.file);
 857 | 		blobmsg_add_u8(b, "user", e->data.user);
 858 | 
 859 | 		blobmsg_add_dscp(b, "dscp", e->data.dscp);
 860 | 
 861 | 		blobmsg_add_string(b, "type", qosify_map_info[e->data.id].type_name);
 862 | 
 863 | 		switch (e->data.id) {
 864 | 		case CL_MAP_TCP_PORTS:
 865 | 		case CL_MAP_UDP_PORTS:
 866 | 			blobmsg_printf(b, "addr", "%d", ntohs(e->data.addr.port));
 867 | 			break;
 868 | 		case CL_MAP_IPV4_ADDR:
 869 | 		case CL_MAP_IPV6_ADDR:
 870 | 			buf = blobmsg_alloc_string_buffer(b, "addr", buf_len);
 871 | 			af = e->data.id == CL_MAP_IPV6_ADDR ? AF_INET6 : AF_INET;
 872 | 			inet_ntop(af, &e->data.addr, buf, buf_len);
 873 | 			blobmsg_add_string_buffer(b);
 874 | 			break;
 875 | 		case CL_MAP_DNS:
 876 | 			blobmsg_add_string(b, "addr", e->data.addr.dns.pattern);
 877 | 			break;
 878 | 		default:
 879 | 			break;
 880 | 		}
 881 | 		blobmsg_close_table(b, c);
 882 | 	}
 883 | 	blobmsg_close_array(b, a);
 884 | }
 885 | 
 886 | void qosify_map_stats(struct blob_buf *b, bool reset)
 887 | {
 888 | 	struct qosify_class data;
 889 | 	uint32_t i;
 890 | 
 891 | 	for (i = 0; i < ARRAY_SIZE(map_class); i++) {
 892 | 		void *c;
 893 | 
 894 | 		if (!map_class[i])
 895 | 			continue;
 896 | 
 897 | 		if (bpf_map_lookup_elem(qosify_map_fds[CL_MAP_CLASS], &i, &data) < 0)
 898 | 			continue;
 899 | 
 900 | 		c = blobmsg_open_table(b, map_class[i]->name);
 901 | 		blobmsg_add_u64(b, "packets", data.packets);
 902 | 		blobmsg_close_table(b, c);
 903 | 
 904 | 		if (!reset)
 905 | 			continue;
 906 | 
 907 | 		data.packets = 0;
 908 | 		bpf_map_update_elem(qosify_map_fds[CL_MAP_CLASS], &i, &data, BPF_ANY);
 909 | 	}
 910 | }
 911 | 
 912 | static int32_t
 913 | qosify_map_get_class_id(const char *name)
 914 | {
 915 | 	int i;
 916 | 
 917 | 	for (i = 0; i < ARRAY_SIZE(map_class); i++)
 918 | 		if (map_class[i] && !strcmp(map_class[i]->name, name))
 919 | 			return i;
 920 | 
 921 | 	for (i = 0; i < ARRAY_SIZE(map_class); i++)
 922 | 		if (!map_class[i])
 923 | 			return i;
 924 | 
 925 | 	for (i = 0; i < ARRAY_SIZE(map_class); i++) {
 926 | 		if (!(map_class[i]->data.flags & QOSIFY_CLASS_FLAG_PRESENT)) {
 927 | 			free(map_class[i]);
 928 | 			map_class[i] = NULL;
 929 | 			return i;
 930 | 		}
 931 | 	}
 932 | 
 933 | 	return -1;
 934 | }
 935 | 
 936 | int map_fill_dscp_value(uint8_t *dest, struct blob_attr *attr, bool reset)
 937 | {
 938 | 	if (reset)
 939 | 		 *dest = 0xff;
 940 | 
 941 | 	if (!attr)
 942 | 		return 0;
 943 | 
 944 | 	if (qosify_map_dscp_value(blobmsg_get_string(attr), dest))
 945 | 		return -1;
 946 | 
 947 | 	return 0;
 948 | }
 949 | 
 950 | int map_parse_flow_config(struct qosify_flow_config *cfg, struct blob_attr *attr,
 951 | 			  bool reset)
 952 | {
 953 | 	enum {
 954 | 		CL_CONFIG_DSCP_PRIO,
 955 | 		CL_CONFIG_DSCP_BULK,
 956 | 		CL_CONFIG_BULK_TIMEOUT,
 957 | 		CL_CONFIG_BULK_PPS,
 958 | 		CL_CONFIG_PRIO_PKT_LEN,
 959 | 		__CL_CONFIG_MAX
 960 | 	};
 961 | 	static const struct blobmsg_policy policy[__CL_CONFIG_MAX] = {
 962 | 		[CL_CONFIG_DSCP_PRIO] = { "dscp_prio", BLOBMSG_TYPE_STRING },
 963 | 		[CL_CONFIG_DSCP_BULK] = { "dscp_bulk", BLOBMSG_TYPE_STRING },
 964 | 		[CL_CONFIG_BULK_TIMEOUT] = { "bulk_trigger_timeout", BLOBMSG_TYPE_INT32 },
 965 | 		[CL_CONFIG_BULK_PPS] = { "bulk_trigger_pps", BLOBMSG_TYPE_INT32 },
 966 | 		[CL_CONFIG_PRIO_PKT_LEN] = { "prio_max_avg_pkt_len", BLOBMSG_TYPE_INT32 },
 967 | 	};
 968 | 	struct blob_attr *tb[__CL_CONFIG_MAX];
 969 | 	struct blob_attr *cur;
 970 | 
 971 | 	if (reset)
 972 | 	    memset(cfg, 0, sizeof(*cfg));
 973 | 
 974 | 	blobmsg_parse(policy, __CL_CONFIG_MAX, tb, blobmsg_data(attr), blobmsg_len(attr));
 975 | 
 976 | 	if (map_fill_dscp_value(&cfg->dscp_prio, tb[CL_CONFIG_DSCP_PRIO], reset) ||
 977 | 	    map_fill_dscp_value(&cfg->dscp_bulk, tb[CL_CONFIG_DSCP_BULK], reset))
 978 | 		return -1;
 979 | 
 980 | 	if ((cur = tb[CL_CONFIG_BULK_TIMEOUT]) != NULL)
 981 | 		cfg->bulk_trigger_timeout = blobmsg_get_u32(cur);
 982 | 
 983 | 	if ((cur = tb[CL_CONFIG_BULK_PPS]) != NULL)
 984 | 		cfg->bulk_trigger_pps = blobmsg_get_u32(cur);
 985 | 
 986 | 	if ((cur = tb[CL_CONFIG_PRIO_PKT_LEN]) != NULL)
 987 | 		cfg->prio_max_avg_pkt_len = blobmsg_get_u32(cur);
 988 | 
 989 | 	return 0;
 990 | }
 991 | 
 992 | static int
 993 | qosify_map_create_class(struct blob_attr *attr)
 994 | {
 995 | 	struct qosify_map_class *class;
 996 | 	enum {
 997 | 		MAP_CLASS_INGRESS,
 998 | 		MAP_CLASS_EGRESS,
 999 | 		__MAP_CLASS_MAX
1000 | 	};
1001 | 	static const struct blobmsg_policy policy[__MAP_CLASS_MAX] = {
1002 | 		[MAP_CLASS_INGRESS] = { "ingress", BLOBMSG_TYPE_STRING },
1003 | 		[MAP_CLASS_EGRESS] = { "egress", BLOBMSG_TYPE_STRING },
1004 | 	};
1005 | 	struct blob_attr *tb[__MAP_CLASS_MAX];
1006 | 	const char *name;
1007 | 	char *name_buf;
1008 | 	int32_t slot;
1009 | 
1010 | 	blobmsg_parse(policy, __MAP_CLASS_MAX, tb,
1011 | 		      blobmsg_data(attr), blobmsg_len(attr));
1012 | 
1013 | 	if (!tb[MAP_CLASS_INGRESS] || !tb[MAP_CLASS_EGRESS])
1014 | 		return -1;
1015 | 
1016 | 	name = blobmsg_name(attr);
1017 | 	slot = qosify_map_get_class_id(name);
1018 | 	if (slot < 0)
1019 | 		return -1;
1020 | 
1021 | 	class = map_class[slot];
1022 | 	if (!class) {
1023 | 		class = calloc_a(sizeof(*class), &name_buf, strlen(name) + 1);
1024 | 		class->name = strcpy(name_buf, name);
1025 | 		map_class[slot] = class;
1026 | 	}
1027 | 
1028 | 	class->data.flags |= QOSIFY_CLASS_FLAG_PRESENT;
1029 | 	if (__qosify_map_dscp_value(blobmsg_get_string(tb[MAP_CLASS_INGRESS]),
1030 | 				    &class->data.val.ingress) ||
1031 | 	    __qosify_map_dscp_value(blobmsg_get_string(tb[MAP_CLASS_EGRESS]),
1032 | 				    &class->data.val.egress)) {
1033 | 		map_class[slot] = NULL;
1034 | 		free(class);
1035 | 		return -1;
1036 | 	}
1037 | 
1038 | 	return 0;
1039 | }
1040 | 
1041 | void qosify_map_set_classes(struct blob_attr *val)
1042 | {
1043 | 	int fd = qosify_map_fds[CL_MAP_CLASS];
1044 | 	struct qosify_class empty_data = {};
1045 | 	struct blob_attr *cur;
1046 | 	int32_t i;
1047 | 	int rem;
1048 | 
1049 | 	for (i = 0; i < ARRAY_SIZE(map_class); i++)
1050 | 		if (map_class[i])
1051 | 			map_class[i]->data.flags &= ~QOSIFY_CLASS_FLAG_PRESENT;
1052 | 
1053 | 	blobmsg_for_each_attr(cur, val, rem)
1054 | 		qosify_map_create_class(cur);
1055 | 
1056 | 	for (i = 0; i < ARRAY_SIZE(map_class); i++) {
1057 | 		if (map_class[i] &&
1058 | 		    (map_class[i]->data.flags & QOSIFY_CLASS_FLAG_PRESENT))
1059 | 			continue;
1060 | 
1061 | 		free(map_class[i]);
1062 | 		map_class[i] = NULL;
1063 | 	}
1064 | 
1065 | 	blobmsg_for_each_attr(cur, val, rem) {
1066 | 		i = qosify_map_get_class_id(blobmsg_name(cur));
1067 | 		if (i < 0 || !map_class[i])
1068 | 			continue;
1069 | 
1070 | 		map_parse_flow_config(&map_class[i]->data.config, cur, true);
1071 | 	}
1072 | 
1073 | 	for (i = 0; i < ARRAY_SIZE(map_class); i++) {
1074 | 		struct qosify_class *data;
1075 | 
1076 | 		data = map_class[i] ? &map_class[i]->data : &empty_data;
1077 | 		bpf_map_update_elem(fd, &i, data, BPF_ANY);
1078 | 	}
1079 | }
1080 | 
1081 | void qosify_map_update_config(void)
1082 | {
1083 | 	int fd = qosify_map_fds[CL_MAP_CONFIG];
1084 | 	uint32_t key = 0;
1085 | 
1086 | 	bpf_map_update_elem(fd, &key, &config, BPF_ANY);
1087 | }
1088 | 


--------------------------------------------------------------------------------