feat: add dscp module

common/checksum.h

@@ -0,0 +1,24 @@
+
+#include <stdint.h>
+
+inline uint16_t
+csum_plus(uint16_t val0, uint16_t val1) {
+	uint16_t sum = val0 + val1;
+
+	if (sum < val0) {
+		++sum;
+	}
+
+	return sum;
+}
+
+inline uint16_t
+csum_minus(uint16_t val0, uint16_t val1) {
+	uint16_t sum = val0 - val1;
+
+	if (sum > val0) {
+		--sum;
+	}
+
+	return sum;
+}

lib/dataplane/packet/dscp.c

@@ -0,0 +1,49 @@
+#include "dscp.h"
+#include "checksum.h"
+
+int
+dscp_mark_v4(struct rte_ipv4_hdr *ip4_hdr, struct dscp_config config) {
+	uint8_t mark = ip4_hdr->type_of_service & DSCP_MARK_MASK;
+	if (config.flag & DSCP_MARK_DEFAULT && mark != 0) {
+		// do not remark
+		return -1;
+	}
+
+	uint16_t checksum = ~rte_be_to_cpu_16(ip4_hdr->hdr_checksum);
+	checksum = csum_minus(checksum, mark);
+	uint8_t new_mark = config.tc & DSCP_MARK_MASK;
+	checksum = csum_plus(checksum, new_mark);
+
+	ip4_hdr->hdr_checksum = ~rte_cpu_to_be_16(checksum);
+
+	uint8_t ecn = ip4_hdr->type_of_service & DSCP_ECN_MASK;
+	ip4_hdr->type_of_service = new_mark | ecn;
+	return 0;
+}
+
+static inline uint8_t
+get_ipv6_tc(rte_be32_t vtc_flow) {
+	uint32_t v = rte_be_to_cpu_32(vtc_flow);
+	return v >> RTE_IPV6_HDR_TC_SHIFT;
+}
+
+static inline rte_be32_t
+set_ipv6_tc(rte_be32_t vtc_flow, uint32_t dscp) {
+	uint32_t v = rte_cpu_to_be_32(dscp << RTE_IPV6_HDR_TC_SHIFT);
+	vtc_flow &= ~rte_cpu_to_be_32(RTE_IPV6_HDR_TC_MASK);
+
+	return (v | vtc_flow);
+}
+
+int
+dscp_mark_v6(struct rte_ipv6_hdr *ip6_hdr, struct dscp_config config) {
+	uint8_t tc = get_ipv6_tc(ip6_hdr->vtc_flow);
+	uint8_t mark = tc & DSCP_MARK_MASK;
+	if (config.flag & DSCP_MARK_DEFAULT && mark != 0) {
+		// do not remark
+		return -1;
+	}
+	uint8_t new_mark = config.tc & DSCP_MARK_MASK;
+	ip6_hdr->vtc_flow = set_ipv6_tc(ip6_hdr->vtc_flow, new_mark);
+	return 0;
+}

lib/dataplane/packet/dscp.h

@@ -0,0 +1,20 @@
+#include <stdint.h>
+
+#include <rte_ip.h>
+
+#define DSCP_MARK_NEVER 0
+#define DSCP_MARK_DEFAULT 1
+#define DSCP_MARK_ALWAYS 2
+#define DSCP_MARK_MASK 0xFC
+#define DSCP_ECN_MASK 0x03
+
+struct dscp_config {
+	uint8_t flag;
+	uint8_t tc;
+};
+
+int
+dscp_mark_v4(struct rte_ipv4_hdr *header, struct dscp_config config);
+
+int
+dscp_mark_v6(struct rte_ipv6_hdr *header, struct dscp_config config);

lib/dataplane/packet/meson.build

@@ -7,6 +7,7 @@ sources = files(
   'packet.c',
   'encap.c',
   'decap.c',
+  'dscp.c',
 )
 
 lib_packet_dp = static_library(

modules/dscp/dataplane.c

@@ -0,0 +1,138 @@
+#include "dataplane.h"
+
+#include <rte_ether.h>
+#include <rte_ip.h>
+
+#include "lpm.h"
+
+#include "dataplane/module/module.h"
+#include "dataplane/packet/dscp.h"
+#include "dataplane/packet/packet.h"
+
+struct dscp_module_config {
+	struct module_config config;
+
+	struct lpm lpm_v4;
+	struct lpm lpm_v6;
+	struct dscp_config dscp;
+};
+
+static int
+dscp_handle_v4(struct dscp_module_config *config, struct packet *packet) {
+	struct rte_mbuf *mbuf = packet_to_mbuf(packet);
+
+	struct rte_ipv4_hdr *header = rte_pktmbuf_mtod_offset(
+		mbuf, struct rte_ipv4_hdr *, packet->network_header.offset
+	);
+
+	if (lpm_lookup(&config->lpm_v4, 4, (uint8_t *)&header->dst_addr) !=
+	    LPM_VALUE_INVALID) {
+		return dscp_mark_v4(header, config->dscp);
+	}
+
+	return -1;
+}
+
+static int
+dscp_handle_v6(struct dscp_module_config *config, struct packet *packet) {
+	struct rte_mbuf *mbuf = packet_to_mbuf(packet);
+
+	struct rte_ipv6_hdr *header = rte_pktmbuf_mtod_offset(
+		mbuf, struct rte_ipv6_hdr *, packet->network_header.offset
+	);
+
+	if (lpm_lookup(&config->lpm_v6, 16, (uint8_t *)&header->dst_addr) !=
+	    LPM_VALUE_INVALID) {
+		return dscp_mark_v6(header, config->dscp);
+	}
+
+	return -1;
+}
+
+static inline int
+dscp_handle(struct dscp_module_config *config, struct packet *packet) {
+	uint16_t type = packet->network_header.type;
+	int result = -1;
+	if (type == rte_cpu_to_be_16(RTE_ETHER_TYPE_IPV4)) {
+		result = dscp_handle_v4(config, packet);
+	} else if (type == rte_cpu_to_be_16(RTE_ETHER_TYPE_IPV6)) {
+		result = dscp_handle_v6(config, packet);
+	}
+	return result;
+}
+
+static void
+dscp_handle_packets(
+	struct module *module,
+	struct module_config *config,
+	struct packet_front *packet_front
+) {
+	(void)module;
+	struct dscp_module_config *dscp_config =
+		container_of(config, struct dscp_module_config, config);
+
+	if (dscp_config->dscp.flag != DSCP_MARK_NEVER) {
+		struct packet *packet;
+		while ((packet = packet_list_pop(&packet_front->input)) != NULL
+		) {
+			dscp_handle(dscp_config, packet);
+		}
+	}
+
+	packet_front_switch(packet_front);
+	return;
+}
+
+static int
+dscp_handle_configure(
+	struct module *module,
+	const void *config_data,
+	size_t config_data_size,
+	struct module_config **new_config
+) {
+
+	(void)module;
+
+	struct dscp_module_config *config = (struct dscp_module_config *)malloc(
+		sizeof(struct dscp_module_config)
+	);
+
+	uint8_t ip6min[16] = {0};
+	uint8_t ip6max[16] = {0};
+	memset(ip6max, 0xff, 16);
+	lpm_init(&config->lpm_v4);
+	lpm_init(&config->lpm_v6);
+	lpm_insert(&config->lpm_v4, 4, ip6min, ip6max, 1);
+	lpm_insert(&config->lpm_v6, 16, ip6min, ip6max, 1);
+
+	if (config_data_size != sizeof(struct dscp_config)) {
+		return -1;
+	}
+	config->dscp = *(struct dscp_config *)config_data;
+
+	*new_config = &config->config;
+
+	return 0;
+};
+
+struct dscp_module {
+	struct module module;
+};
+
+struct module *
+new_module_dscp() {
+	struct dscp_module *module =
+		(struct dscp_module *)malloc(sizeof(struct dscp_module));
+
+	if (module == NULL) {
+		return NULL;
+	}
+
+	snprintf(
+		module->module.name, sizeof(module->module.name), "%s", "dscp"
+	);
+	module->module.handler = dscp_handle_packets;
+	module->module.config_handler = dscp_handle_configure;
+
+	return &module->module;
+}

modules/dscp/dataplane.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "dataplane/module/module.h"
+
+struct module *
+new_module_kernel();

modules/dscp/meson.build

@@ -0,0 +1,29 @@
+# common dependecies
+dependencies += lib_common_dep
+dependencies += lib_filter_dep
+
+# dataplane dependecies
+dp_dependencies = dependencies
+dp_dependencies += lib_packet_dp_dep
+
+dp_dependencies += lib_module_dp_dep
+
+dp_sources = files(
+  'dataplane.c',
+)
+
+lib_dscp_dp = static_library(
+  'dscp_dp',
+  dp_sources,
+  dependencies: dp_dependencies,
+  install: false,
+)
+
+lib_dscp_dp_dep = declare_dependency(
+  link_with: lib_dscp_dp,
+  link_args: [
+    '-Wl,--defsym', '-Wl,new_module_dscp=new_module_dscp',
+     '-Wl,--export-dynamic-symbol=new_module_dscp',
+    ],
+)
+

modules/meson.build

@@ -3,3 +3,4 @@ subdir('balancer')
 subdir('acl')
 subdir('route')
 subdir('forward')
+subdir('dscp')