ipv6: implemented DHCPv6 relay support

3 files changed, 108 insertions, 17 deletions

diff --git a/accel-pppd/ipv6/dhcpv6.c b/accel-pppd/ipv6/dhcpv6.c
index fc6487b8..6daed667 100644
--- a/accel-pppd/ipv6/dhcpv6.c
+++ b/accel-pppd/ipv6/dhcpv6.c
@@ -442,6 +442,8 @@ static void dhcpv6_send_reply(struct dhcpv6_packet *req, struct dhcpv6_pd *pd, i
 		dhcpv6_packet_print(reply, log_ppp_info2);
 	}
 
+	dhcpv6_fill_relay_info(reply);
+
 	net->sendto(pd->hnd.fd, reply->hdr, reply->endptr - (void *)reply->hdr, 0, (struct sockaddr *)&req->addr, sizeof(req->addr));
 
 	dhcpv6_packet_free(reply);
@@ -594,6 +596,8 @@ static void dhcpv6_send_reply2(struct dhcpv6_packet *req, struct dhcpv6_pd *pd,
 		dhcpv6_packet_print(reply, log_ppp_info2);
 	}
 
+	dhcpv6_fill_relay_info(reply);
+
 	net->sendto(pd->hnd.fd, reply->hdr, reply->endptr - (void *)reply->hdr, 0, (struct sockaddr *)&req->addr, sizeof(req->addr));
 
 out:
@@ -739,6 +743,7 @@ static void dhcpv6_recv_decline(struct dhcpv6_packet *pkt)
 	// don't answer
 }
 
+
 static void dhcpv6_recv_packet(struct dhcpv6_packet *pkt)
 {
 	if (conf_verbose) {
diff --git a/accel-pppd/ipv6/dhcpv6.h b/accel-pppd/ipv6/dhcpv6.h
index 1efbf722..28ea3cf0 100644
--- a/accel-pppd/ipv6/dhcpv6.h
+++ b/accel-pppd/ipv6/dhcpv6.h
@@ -34,6 +34,7 @@
 #define D6_OPTION_DOMAIN_LIST     24
 #define D6_OPTION_IA_PD           25
 #define D6_OPTION_IAPREFIX        26
+#define D6_OPTION_IAPREFIX        26
 
 #define D6_SOLICIT                 1
 #define D6_ADVERTISE               2
@@ -73,6 +74,14 @@ struct dhcpv6_msg_hdr {
 	uint8_t data[0];
 } __packed;
 
+struct dhcpv6_relay_hdr {
+	uint8_t type;
+	uint8_t hop_cnt;
+	struct in6_addr link_addr;
+	struct in6_addr peer_addr;
+	uint8_t data[0];
+} __packed;
+
 struct dhcpv6_duid {
 	uint16_t type;
 	union {
@@ -154,6 +163,14 @@ struct dhcpv6_option {
 
 struct dhcpv6_pd;
 
+struct dhcpv6_relay {
+	struct list_head entry;
+	int hop_cnt;
+	struct in6_addr link_addr;
+	struct in6_addr peer_addr;
+	void *hdr;
+};
+
 struct dhcpv6_packet {
 	struct ap_session *ses;
 	struct dhcpv6_pd *pd;
@@ -162,6 +179,9 @@ struct dhcpv6_packet {
 	struct dhcpv6_msg_hdr *hdr;
 	struct dhcpv6_opt_clientid *clientid;
 	struct dhcpv6_opt_serverid *serverid;
+
+	struct list_head relay_list;
+
 	int rapid_commit:1;
 
 	struct list_head opt_list;
@@ -174,5 +194,6 @@ void dhcpv6_packet_print(struct dhcpv6_packet *pkt, void (*print)(const char *fm
 struct dhcpv6_packet *dhcpv6_packet_alloc_reply(struct dhcpv6_packet *req, int type);
 struct dhcpv6_option *dhcpv6_option_alloc(struct dhcpv6_packet *pkt, int code, int len);
 struct dhcpv6_option *dhcpv6_nested_option_alloc(struct dhcpv6_packet *pkt, struct dhcpv6_option *opt, int code, int len);
+void dhcpv6_fill_relay_info(struct dhcpv6_packet *pkt);
 
 #endif
diff --git a/accel-pppd/ipv6/dhcpv6_packet.c b/accel-pppd/ipv6/dhcpv6_packet.c
index 664a1168..aae260e9 100644
--- a/accel-pppd/ipv6/dhcpv6_packet.c
+++ b/accel-pppd/ipv6/dhcpv6_packet.c
@@ -104,9 +104,11 @@ struct dhcpv6_packet *dhcpv6_packet_parse(const void *buf, size_t size)
 {
 	struct dhcpv6_packet *pkt;
 	struct dhcpv6_opt_hdr *opth;
+	struct dhcpv6_relay *rel;
+	struct dhcpv6_relay_hdr *rhdr;
 	void *ptr, *endptr;
 
-	pkt = _malloc(sizeof(*pkt));
+	pkt = _malloc(sizeof(*pkt) + size);
 	if (!pkt) {
 		log_emerg("out of memory\n");
 		return NULL;
@@ -114,18 +116,42 @@ struct dhcpv6_packet *dhcpv6_packet_parse(const void *buf, size_t size)
 
 	memset(pkt, 0, sizeof(*pkt));
 	INIT_LIST_HEAD(&pkt->opt_list);
+	INIT_LIST_HEAD(&pkt->relay_list);
 
-	pkt->hdr = _malloc(size);
-	if (!pkt->hdr) {
-		log_emerg("out of memory\n");
-		_free(pkt);
-		return NULL;
-	}
+	pkt->hdr = (void *)(pkt + 1);
 
 	memcpy(pkt->hdr, buf, size);
+	endptr =  ((void *)pkt->hdr) + size;
+
+	while (pkt->hdr->type == D6_RELAY_FORW) {
+		rhdr = (struct dhcpv6_relay_hdr *)pkt->hdr;
+		rel = _malloc(sizeof(*rel));
+		rel->hop_cnt = rhdr->hop_cnt;
+		memcpy(&rel->link_addr, &rhdr->link_addr, sizeof(rel->link_addr));
+		memcpy(&rel->peer_addr, &rhdr->peer_addr, sizeof(rel->peer_addr));
+
+		list_add_tail(&rel->entry, &pkt->relay_list);
+
+		ptr = rhdr->data;
+		while (ptr < endptr) {
+			opth = ptr;
+
+			if (ptr + sizeof(*opth) + ntohs(opth->len) > endptr) {
+				log_warn("dhcpv6: invalid packet received\n");
+				dhcpv6_packet_free(pkt);
+				return NULL;
+			}
+
+			if (opth->code == htons(D6_OPTION_RELAY_MSG)) {
+				pkt->hdr = (struct dhcpv6_msg_hdr *)opth->data;
+				endptr = opth->data + sizeof(*opth) + ntohs(opth->len);
+			}
+
+			ptr += sizeof(*opth) + ntohs(opth->len);
+		}
+	}
 
 	ptr = pkt->hdr->data;
-	endptr =  ((void *)pkt->hdr) + size;
 
 	while (ptr < endptr) {
 		opth = ptr;
@@ -199,11 +225,36 @@ struct dhcpv6_option *dhcpv6_nested_option_alloc(struct dhcpv6_packet *pkt, stru
 	return opt;
 }
 
+void dhcpv6_fill_relay_info(struct dhcpv6_packet *pkt)
+{
+	struct dhcpv6_relay *rel;
+	struct dhcpv6_opt_hdr *opt;
+	struct dhcpv6_relay_hdr *rhdr;
+
+	if (list_empty(&pkt->relay_list))
+		return;
+
+	list_for_each_entry(rel, &pkt->relay_list, entry) {
+		rhdr = (struct dhcpv6_relay_hdr *)rel->hdr;
+		rhdr->type = D6_RELAY_REPL;
+		rhdr->hop_cnt = rel->hop_cnt;
+		memcpy(&rhdr->link_addr, &rel->link_addr, sizeof(rhdr->link_addr));
+		memcpy(&rhdr->peer_addr, &rel->peer_addr, sizeof(rhdr->peer_addr));
+		opt = (struct dhcpv6_opt_hdr *)rhdr->data;
+		opt->code = htons(D6_OPTION_RELAY_MSG);
+		opt->len = (uint8_t *)pkt->endptr - rhdr->data;
+	}
+
+	rel = list_entry(pkt->relay_list.next, typeof(*rel), entry);
+
+	pkt->hdr = (struct dhcpv6_msg_hdr *)rel->hdr;
+}
 
 struct dhcpv6_packet *dhcpv6_packet_alloc_reply(struct dhcpv6_packet *req, int type)
 {
-	struct dhcpv6_packet *pkt = _malloc(sizeof(*pkt));
+	struct dhcpv6_packet *pkt = _malloc(sizeof(*pkt) + BUF_SIZE);
 	struct dhcpv6_option *opt;
+	struct dhcpv6_relay *rel;
 
 	if (!pkt) {
 		log_emerg("out of memory\n");
@@ -212,20 +263,23 @@ struct dhcpv6_packet *dhcpv6_packet_alloc_reply(struct dhcpv6_packet *req, int t
 
 	memset(pkt, 0, sizeof(*pkt));
 	INIT_LIST_HEAD(&pkt->opt_list);
+	INIT_LIST_HEAD(&pkt->relay_list);
 	pkt->ses = req->ses;
 
-	pkt->hdr = _malloc(BUF_SIZE);
-	if (!pkt->hdr) {
-		log_emerg("out of memory\n");
-		_free(pkt);
-		return NULL;
+	pkt->hdr = (void *)(pkt + 1);
+
+	while (!list_empty(&req->relay_list)) {
+		rel = list_entry(req->relay_list.next, typeof(*rel), entry);
+		rel->hdr = (void *)pkt->hdr;
+		pkt->hdr = (void *)rel->hdr + sizeof(struct dhcpv6_relay_hdr) + sizeof(struct dhcpv6_opt_hdr);
+		list_move_tail(&rel->entry, &pkt->relay_list);
 	}
 
+	pkt->endptr = pkt->hdr->data;
+
 	pkt->hdr->type = type;
 	pkt->hdr->trans_id = req->hdr->trans_id;
 
-	pkt->endptr = pkt->hdr->data;
-
 	opt = dhcpv6_option_alloc(pkt, D6_OPTION_SERVERID, ntohs(req->serverid->hdr.len));
 	memcpy(opt->hdr, req->serverid, sizeof(struct dhcpv6_opt_hdr) + ntohs(req->serverid->hdr.len));
 
@@ -247,10 +301,21 @@ static void free_options(struct list_head *opt_list)
 	}
 }
 
+static void free_relays(struct list_head *list)
+{
+	struct dhcpv6_relay *rel;
+
+	while (!list_empty(list)) {
+		rel = list_entry(list->next, typeof(*rel), entry);
+		list_del(&rel->entry);
+		_free(rel);
+	}
+}
+
 void dhcpv6_packet_free(struct dhcpv6_packet *pkt)
 {
 	free_options(&pkt->opt_list);
-	_free(pkt->hdr);
+	free_relays(&pkt->relay_list);
 	_free(pkt);
 }