Initial prefix class support

6 files changed, 221 insertions, 18 deletions

diff --git a/src/dhcpv6-ia.c b/src/dhcpv6-ia.c
index ccf7a58..3d9fe83 100644
--- a/src/dhcpv6-ia.c
+++ b/src/dhcpv6-ia.c
@@ -51,6 +51,8 @@ int setup_dhcpv6_ia_interface(struct interface *iface, bool enable)
 		while (!list_empty(&iface->ia_assignments)) {
 			c = list_first_entry(&iface->ia_assignments, struct dhcpv6_assignment, head);
 			list_del(&c->head);
+			free(c->hostname);
+			free(c->classes);
 			free(c);
 		}
 	}
@@ -96,6 +98,8 @@ int setup_dhcpv6_ia_interface(struct interface *iface, bool enable)
 					a->hostname = strdup(lease->hostname);
 				}
 			} else {
+				free(a->classes);
+				free(a->hostname);
 				free(a);
 			}
 		}
@@ -333,7 +337,21 @@ static bool assign_pd(struct interface *iface, struct dhcpv6_assignment *assign)
 
 static bool assign_na(struct interface *iface, struct dhcpv6_assignment *assign)
 {
-	if (iface->ia_addr_len < 1)
+	bool match = false;
+	for (size_t i = 0; i < iface->ia_addr_len; ++i) {
+		if (!iface->ia_addr[i].has_class) {
+			match = true;
+			continue;
+		} else if (assign->classes_cnt) {
+			for (size_t j = 0; j < assign->classes_cnt; ++j)
+				if (assign->classes[j] == iface->ia_addr[i].class)
+					match = true;
+		} else if (assign->all_class) {
+			match = true;
+		}
+	}
+
+	if (!match)
 		return false;
 
 	// Seed RNG with checksum of DUID
@@ -476,6 +494,7 @@ static void reconf_timer(struct uloop_timeout *event)
 				if ((a->length < 128 && a->clid_len > 0) ||
 						(a->length == 128 && a->clid_len == 0)) {
 					list_del(&a->head);
+					free(a->classes);
 					free(a->hostname);
 					free(a);
 				}
@@ -528,6 +547,21 @@ static size_t append_reply(uint8_t *buf, size_t buflen, uint16_t status,
 					have_non_ula = true;
 
 			for (size_t i = 0; i < iface->ia_addr_len; ++i) {
+				bool match = true;
+				if (iface->ia_addr[i].has_class) {
+					match = false;
+					if (a->classes_cnt) {
+						for (size_t j = 0; j < a->classes_cnt; ++j)
+							if (a->classes[j] == iface->ia_addr[i].class)
+								match = true;
+					} else if (a->all_class) {
+						match = true;
+					}
+				}
+
+				if (!match)
+					continue;
+
 				uint32_t prefix_pref = iface->ia_addr[i].preferred - now;
 				uint32_t prefix_valid = iface->ia_addr[i].valid - now;
 
@@ -547,6 +581,15 @@ static size_t append_reply(uint8_t *buf, size_t buflen, uint16_t status,
 				if (prefix_valid > 86400)
 					prefix_valid = 86400;
 
+#ifdef DHCPV6_OPT_PREFIX_CLASS
+				struct {
+					uint16_t code;
+					uint16_t length;
+					uint16_t class;
+				} pclass = {htons(DHCPV6_OPT_PREFIX_CLASS),
+					htons(2), htons(iface->ia_addr[i].class)};
+#endif
+
 				if (a->length < 128) {
 					struct dhcpv6_ia_prefix p = {
 						.type = htons(DHCPV6_OPT_IA_PREFIX),
@@ -557,12 +600,23 @@ static size_t append_reply(uint8_t *buf, size_t buflen, uint16_t status,
 						.addr = iface->ia_addr[i].addr
 					};
 					p.addr.s6_addr32[1] |= htonl(a->assigned);
+					size_t entrlen = sizeof(p);
 
-					if (datalen + sizeof(p) > buflen || a->assigned == 0)
+#ifdef DHCPV6_OPT_PREFIX_CLASS
+					if (iface->ia_addr[i].has_class) {
+						entrlen += sizeof(pclass);
+						p.len = htons(entrlen);
+					}
+#endif
+
+					if (datalen + entrlen > buflen || a->assigned == 0)
 						continue;
 
 					memcpy(buf + datalen, &p, sizeof(p));
-					datalen += sizeof(p);
+#ifdef DHCPV6_OPT_PREFIX_CLASS
+					memcpy(buf + datalen + sizeof(p), &pclass, sizeof(pclass));
+#endif
+					datalen += entrlen;
 				} else {
 					struct dhcpv6_ia_addr n = {
 						.type = htons(DHCPV6_OPT_IA_ADDR),
@@ -572,12 +626,23 @@ static size_t append_reply(uint8_t *buf, size_t buflen, uint16_t status,
 						.valid = htonl(prefix_valid)
 					};
 					n.addr.s6_addr32[3] = htonl(a->assigned);
+					size_t entrlen = sizeof(n);
+
+#ifdef DHCPV6_OPT_PREFIX_CLASS
+					if (iface->ia_addr[i].has_class) {
+						entrlen += sizeof(pclass);
+						n.len = htons(entrlen);
+					}
+#endif
 
-					if (datalen + sizeof(n) > buflen || a->assigned == 0)
+					if (datalen + entrlen > buflen || a->assigned == 0)
 						continue;
 
 					memcpy(buf + datalen, &n, sizeof(n));
-					datalen += sizeof(n);
+#ifdef DHCPV6_OPT_PREFIX_CLASS
+					memcpy(buf + datalen + sizeof(n), &pclass, sizeof(pclass));
+#endif
+					datalen += entrlen;
 				}
 
 				// Calculate T1 / T2 based on non-deprecated addresses
@@ -690,6 +755,7 @@ size_t dhcpv6_handle_ia(uint8_t *buf, size_t buflen, struct interface *iface,
 	uint8_t *clid_data = NULL, clid_len = 0, mac[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	char hostname[256];
 	size_t hostname_len = 0;
+	bool class_oro = false;
 	dhcpv6_for_each_option(start, end, otype, olen, odata) {
 		if (otype == DHCPV6_OPT_CLIENTID) {
 			clid_data = odata;
@@ -706,6 +772,14 @@ size_t dhcpv6_handle_ia(uint8_t *buf, size_t buflen, struct interface *iface,
 
 			if (dn_expand(&fqdn_buf[1], &fqdn_buf[olen], &fqdn_buf[1], hostname, sizeof(hostname)) > 0)
 				hostname_len = strcspn(hostname, ".");
+		} else if (otype == DHCPV6_OPT_ORO) {
+#ifdef DHCPV6_OPT_PREFIX_CLASS
+			for (size_t i = 0; i + 1 < olen; i += 2) {
+				if (odata[i] == (DHCPV6_OPT_PREFIX_CLASS >> 8) &&
+						odata[i + 1] == (DHCPV6_OPT_PREFIX_CLASS & 0xff))
+					class_oro = true;
+			}
+#endif
 		} else if (otype == DHCPV6_OPT_RECONF_ACCEPT) {
 			accept_reconf = true;
 		}
@@ -729,25 +803,65 @@ size_t dhcpv6_handle_ia(uint8_t *buf, size_t buflen, struct interface *iface,
 		uint8_t reqlen = (is_pd) ? 62 : 128;
 		uint32_t reqhint = 0;
 
+		const uint8_t classes_max = 32;
+		uint8_t classes_cnt = 0;
+		uint16_t classes[classes_max];
+
 		// Parse request hint for IA-PD
 		if (is_pd) {
 			uint8_t *sdata;
 			uint16_t stype, slen;
 			dhcpv6_for_each_option(&ia[1], odata + olen, stype, slen, sdata) {
-				if (stype == DHCPV6_OPT_IA_PREFIX && slen >= sizeof(struct dhcpv6_ia_prefix) - 4) {
-					struct dhcpv6_ia_prefix *p = (struct dhcpv6_ia_prefix*)&sdata[-4];
-					if (p->prefix) {
-						reqlen = p->prefix;
-						reqhint = ntohl(p->addr.s6_addr32[1]);
-						if (reqlen > 32 && reqlen <= 64)
-							reqhint &= (1U << (64 - reqlen)) - 1;
-					}
-					break;
+				if (stype != DHCPV6_OPT_IA_PREFIX || slen < sizeof(struct dhcpv6_ia_prefix) - 4)
+					continue;
+
+				struct dhcpv6_ia_prefix *p = (struct dhcpv6_ia_prefix*)&sdata[-4];
+				if (p->prefix) {
+					reqlen = p->prefix;
+					reqhint = ntohl(p->addr.s6_addr32[1]);
+					if (reqlen > 32 && reqlen <= 64)
+						reqhint &= (1U << (64 - reqlen)) - 1;
+				}
+
+#ifdef DHCPV6_OPT_PREFIX_CLASS
+				uint8_t *xdata;
+				uint16_t xtype, xlen;
+				dhcpv6_for_each_option(&p[1], sdata + slen, xtype, xlen, xdata) {
+					if (xtype != DHCPV6_OPT_PREFIX_CLASS || xlen != 2)
+						continue;
+
+					if (classes_cnt >= classes_max)
+						continue;
+
+					classes[classes_cnt++] = (uint16_t)xdata[0] << 8 | (uint16_t)xdata[1];
 				}
+#endif
 			}
 
 			if (reqlen > 64)
 				reqlen = 64;
+		} else if (is_na) {
+			uint8_t *sdata;
+			uint16_t stype, slen;
+			dhcpv6_for_each_option(&ia[1], odata + olen, stype, slen, sdata) {
+				if (stype != DHCPV6_OPT_IA_ADDR || slen < sizeof(struct dhcpv6_ia_addr) - 4)
+					continue;
+
+#ifdef DHCPV6_OPT_PREFIX_CLASS
+				uint8_t *xdata;
+				uint16_t xtype, xlen;
+				struct dhcpv6_ia_addr *p = (struct dhcpv6_ia_addr*)&sdata[-4];
+				dhcpv6_for_each_option(&p[1], sdata + slen, xtype, xlen, xdata) {
+					if (xtype != DHCPV6_OPT_PREFIX_CLASS || xlen != 2)
+						continue;
+
+					if (classes_cnt >= classes_max)
+						continue;
+
+					classes[classes_cnt++] = (uint16_t)xdata[0] << 8 | (uint16_t)xdata[1];
+				}
+#endif
+			}
 		}
 
 		// Find assignment
@@ -784,6 +898,11 @@ size_t dhcpv6_handle_ia(uint8_t *buf, size_t buflen, struct interface *iface,
 				a->length = reqlen;
 				a->peer = *addr;
 				a->assigned = reqhint;
+				a->all_class = class_oro;
+				a->classes_cnt = classes_cnt;
+				a->classes = malloc(classes_cnt * sizeof(uint16_t));
+				memcpy(a->classes, classes, classes_cnt * sizeof(uint16_t));
+
 				if (first)
 					memcpy(a->key, first->key, sizeof(a->key));
 				else
@@ -841,6 +960,7 @@ size_t dhcpv6_handle_ia(uint8_t *buf, size_t buflen, struct interface *iface,
 				apply_lease(iface, a, true);
 				update_state = true;
 			} else if (!assigned && a) { // Cleanup failed assignment
+				free(a->classes);
 				free(a->hostname);
 				free(a);
 			}
diff --git a/src/dhcpv6.h b/src/dhcpv6.h
index 004c3cf..693bc57 100644
--- a/src/dhcpv6.h
+++ b/src/dhcpv6.h
@@ -40,6 +40,7 @@
 #define DHCPV6_OPT_SERVERID 2
 #define DHCPV6_OPT_IA_NA 3
 #define DHCPV6_OPT_IA_ADDR 5
+#define DHCPV6_OPT_ORO 6
 #define DHCPV6_OPT_STATUS 13
 #define DHCPV6_OPT_RELAY_MSG 9
 #define DHCPV6_OPT_AUTH 11
@@ -53,6 +54,11 @@
 #define DHCPV6_OPT_INFO_REFRESH 32
 #define DHCPV6_OPT_FQDN 39
 
+#ifdef EXT_PREFIX_CLASS
+/* draft-bhandari-dhc-class-based-prefix, not yet standardized */
+#define DHCPV6_OPT_PREFIX_CLASS EXT_PREFIX_CLASS
+#endif
+
 #define DHCPV6_DUID_VENDOR 2
 
 #define DHCPV6_STATUS_OK 0
@@ -134,6 +140,9 @@ struct dhcpv6_assignment {
 	struct sockaddr_in6 peer;
 	time_t valid_until;
 	time_t reconf_sent;
+	bool all_class;
+	uint8_t classes_cnt;
+	uint16_t *classes;
 	int reconf_cnt;
 	char *hostname;
 	uint8_t key[16];
diff --git a/src/odhcpd.c b/src/odhcpd.c
index 2859907..5a800b7 100644
--- a/src/odhcpd.c
+++ b/src/odhcpd.c
@@ -240,6 +240,15 @@ ssize_t odhcpd_get_interface_addresses(int ifindex,
 		if (ifa->ifa_flags & IFA_F_DEPRECATED)
 			addrs[ret].preferred = 0;
 
+		addrs[ret].has_class = false;
+		addrs[ret].class = 0;
+#ifdef WITH_UBUS
+		struct interface *iface = odhcpd_get_interface_by_index(ifindex);
+		if (iface) {
+			addrs[ret].has_class = true;
+			addrs[ret].class = ubus_get_class(iface->ifname, &addrs[ret].addr);
+		}
+#endif
 		++ret;
 	}
 
diff --git a/src/odhcpd.h b/src/odhcpd.h
index 9949253..510dd6a 100644
--- a/src/odhcpd.h
+++ b/src/odhcpd.h
@@ -68,6 +68,8 @@ struct odhcpd_event {
 struct odhcpd_ipaddr {
 	struct in6_addr addr;
 	uint8_t prefix;
+	bool has_class;
+	uint16_t class;
 	uint32_t preferred;
 	uint32_t valid;
 };
@@ -193,6 +195,7 @@ int init_ubus(void);
 const char* ubus_get_ifname(const char *name);
 void ubus_apply_network(void);
 bool ubus_has_prefix(const char *name, const char *ifname);
+uint16_t ubus_get_class(const char *ifname, const struct in6_addr *addr);
 #endif
 
 
diff --git a/src/router.c b/src/router.c
index 9258acf..5abf1e6 100644
--- a/src/router.c
+++ b/src/router.c
@@ -169,7 +169,7 @@ static bool parse_routes(struct odhcpd_ipaddr *n, ssize_t len)
 
 	char line[512], ifname[16];
 	bool found_default = false;
-	struct odhcpd_ipaddr p = {IN6ADDR_ANY_INIT, 0, 0, 0};
+	struct odhcpd_ipaddr p = {IN6ADDR_ANY_INIT, 0, false, 0, 0, 0};
 	while (fgets(line, sizeof(line), fp_route)) {
 		uint32_t rflags;
 		if (sscanf(line, "00000000000000000000000000000000 00 "
@@ -254,7 +254,7 @@ static void send_router_advert(struct uloop_timeout *event)
 
 	for (ssize_t i = 0; i < ipcnt; ++i) {
 		struct odhcpd_ipaddr *addr = &addrs[i];
-		if (addr->prefix > 64)
+		if (addr->prefix > 64 || addr->has_class)
 			continue; // Address not suitable
 
 		if (addr->preferred > MaxPreferredTime)
diff --git a/src/ubus.c b/src/ubus.c
index dc2f171..317a9c1 100644
--- a/src/ubus.c
+++ b/src/ubus.c
@@ -161,6 +161,7 @@ enum {
 	IFACE_ATTR_UP,
 	IFACE_ATTR_DATA,
 	IFACE_ATTR_PREFIX,
+	IFACE_ATTR_ADDRESS,
 	IFACE_ATTR_MAX,
 };
 
@@ -170,6 +171,7 @@ static const struct blobmsg_policy iface_attrs[IFACE_ATTR_MAX] = {
 	[IFACE_ATTR_UP] = { .name = "up", .type = BLOBMSG_TYPE_BOOL },
 	[IFACE_ATTR_DATA] = { .name = "data", .type = BLOBMSG_TYPE_TABLE },
 	[IFACE_ATTR_PREFIX] = { .name = "ipv6-prefix", .type = BLOBMSG_TYPE_ARRAY },
+	[IFACE_ATTR_ADDRESS] = { .name = "ipv6-address", .type = BLOBMSG_TYPE_ARRAY },
 };
 
 static void handle_dump(_unused struct ubus_request *req, _unused int type, struct blob_attr *msg)
@@ -331,8 +333,8 @@ bool ubus_has_prefix(const char *name, const char *ifname)
 		if (!tb[IFACE_ATTR_INTERFACE] || !tb[IFACE_ATTR_IFNAME])
 			continue;
 
-		if (!strcmp(name, blobmsg_get_string(tb[IFACE_ATTR_INTERFACE])) ||
-				!strcmp(ifname, blobmsg_get_string(tb[IFACE_ATTR_IFNAME])))
+		if (strcmp(name, blobmsg_get_string(tb[IFACE_ATTR_INTERFACE])) ||
+				strcmp(ifname, blobmsg_get_string(tb[IFACE_ATTR_IFNAME])))
 			continue;
 
 		if ((cur = tb[IFACE_ATTR_PREFIX])) {
@@ -351,6 +353,66 @@ bool ubus_has_prefix(const char *name, const char *ifname)
 }
 
 
+enum {
+	ADDR_ATTR_ADDR,
+	ADDR_ATTR_CLASS,
+	ADDR_ATTR_MAX
+};
+
+static const struct blobmsg_policy addr_attrs[ADDR_ATTR_MAX] = {
+	[ADDR_ATTR_ADDR] = { .name = "address", .type = BLOBMSG_TYPE_STRING },
+	[ADDR_ATTR_CLASS] = { .name = "class", .type = BLOBMSG_TYPE_STRING },
+};
+
+uint16_t ubus_get_class(const char *ifname, const struct in6_addr *addr)
+{
+	struct blob_attr *c, *cur;
+	unsigned rem;
+
+	if (!dump)
+		return 0;
+
+	blobmsg_for_each_attr(c, dump, rem) {
+		struct blob_attr *tb[IFACE_ATTR_MAX];
+		blobmsg_parse(iface_attrs, IFACE_ATTR_MAX, tb, blobmsg_data(c), blobmsg_data_len(c));
+
+		if (!tb[IFACE_ATTR_IFNAME])
+			continue;
+
+		if (strcmp(ifname, blobmsg_get_string(tb[IFACE_ATTR_IFNAME])))
+			continue;
+
+		if ((cur = tb[IFACE_ATTR_ADDRESS])) {
+			if (blobmsg_type(cur) != BLOBMSG_TYPE_ARRAY || !blobmsg_check_attr(cur, NULL))
+				continue;
+
+			struct blob_attr *d;
+			unsigned drem;
+			blobmsg_for_each_attr(d, cur, drem) {
+				struct blob_attr *t[ADDR_ATTR_MAX];
+				blobmsg_parse(addr_attrs, ADDR_ATTR_MAX, t, blobmsg_data(d), blobmsg_data_len(d));
+
+				if (!t[ADDR_ATTR_ADDR] || !t[ADDR_ATTR_CLASS])
+					continue;
+
+				const char *addrs = blobmsg_get_string(t[ADDR_ATTR_ADDR]);
+				const char *class = blobmsg_get_string(t[ADDR_ATTR_CLASS]);
+
+				struct in6_addr ip6addr;
+				inet_pton(AF_INET6, addrs, &ip6addr);
+
+				if (IN6_ARE_ADDR_EQUAL(&ip6addr, addr))
+					return atoi(class);
+			}
+		}
+
+		return 0;
+	}
+
+	return 0;
+}
+
+
 int init_ubus(void)
 {
 	if (!(ubus = ubus_connect(NULL))) {