udhcpc6: fix udhcp_find_option to actually find DHCP6 options

udhcp_insert_new_option treats code for IPv6 as follows:

new->data[D6_OPT_CODE] = code >> 8;
new->data[D6_OPT_CODE + 1] = code & 0xff;

udhcp_find_option tests the code as follows:

while (opt_list && opt_list->data[OPT_CODE] < code)
...
if (opt_list && opt_list->data[OPT_CODE] == code)

So yes, OPT_CODE and D6_OPT_CODE are both 0, but the D6_OPT_CLIENTID =
1 value means that the 1 is in the seconds byte, and udhcp_find_option
is only looking at the first byte,  So the send_d6_release can never
find it the created option.

function                                             old     new   delta
udhcp_find_option                                     28      53     +25
attach_option                                        276     284      +8
udhcpc6_main                                        2602    2607      +5
perform_d6_release                                   262     267      +5
udhcpd_main                                         1518    1520      +2
udhcpc_main                                         2542    2544      +2
add_serverid_and_clientid_options                     46      48      +2
------------------------------------------------------------------------------
(add/remove: 0/0 grow/shrink: 7/0 up/down: 49/0)               Total: 49 bytes

Signed-off-by: Denys Vlasenko <vda.linux@googlemail.com>

5 files changed, 34 insertions, 14 deletions

diff --git a/networking/udhcp/common.c b/networking/udhcp/common.c
index 31e525cb0..8e9b93655 100644
--- a/networking/udhcp/common.c
+++ b/networking/udhcp/common.c
@@ -404,14 +404,29 @@ void FAST_FUNC udhcp_add_simple_option(struct dhcp_packet *packet, uint8_t code,
 #endif
 
 /* Find option 'code' in opt_list */
-struct option_set* FAST_FUNC udhcp_find_option(struct option_set *opt_list, uint8_t code)
+struct option_set* FAST_FUNC udhcp_find_option(struct option_set *opt_list, uint8_t code, bool dhcpv6)
 {
-	while (opt_list && opt_list->data[OPT_CODE] < code)
-		opt_list = opt_list->next;
+	IF_NOT_UDHCPC6(bool dhcpv6 = 0;)
+	uint8_t cur_code;
 
-	if (opt_list && opt_list->data[OPT_CODE] == code)
-		return opt_list;
-	return NULL;
+	for (;;) {
+		if (!opt_list)
+			return opt_list; /* NULL */
+		if (!dhcpv6) {
+			cur_code = opt_list->data[OPT_CODE];
+		} else {
+//FIXME: add support for code > 0xff
+			if (opt_list->data[D6_OPT_CODE] != 0)
+				return NULL;
+			cur_code = opt_list->data[D6_OPT_CODE + 1];
+		}
+		if (cur_code >= code) {
+			if (cur_code == code)
+				return opt_list;
+			return NULL;
+		}
+		opt_list = opt_list->next;
+	}
 }
 
 /* Parse string to IP in network order */
@@ -499,7 +514,7 @@ static NOINLINE void attach_option(
 	}
 #endif
 
-	existing = udhcp_find_option(*opt_list, optflag->code);
+	existing = udhcp_find_option(*opt_list, optflag->code, dhcpv6);
 	if (!existing) {
 		/* make a new option */
 		uint8_t *p = udhcp_insert_new_option(opt_list, optflag->code, length, dhcpv6);
diff --git a/networking/udhcp/common.h b/networking/udhcp/common.h
index e374771cb..5882238e3 100644
--- a/networking/udhcp/common.h
+++ b/networking/udhcp/common.h
@@ -245,7 +245,11 @@ void udhcp_add_simple_option(struct dhcp_packet *packet, uint8_t code, uint32_t
 char *dname_dec(const uint8_t *cstr, int clen, const char *pre) FAST_FUNC;
 uint8_t *dname_enc(/*const uint8_t *cstr, int clen,*/ const char *src, int *retlen) FAST_FUNC;
 #endif
-struct option_set *udhcp_find_option(struct option_set *opt_list, uint8_t code) FAST_FUNC;
+#if !ENABLE_UDHCPC6
+#define udhcp_find_option(opt_list, code, dhcpv6) \
+	udhcp_find_option(opt_list, code)
+#endif
+struct option_set *udhcp_find_option(struct option_set *opt_list, uint8_t code, bool dhcpv6) FAST_FUNC;
 
 // RFC 2131  Table 5: Fields and options used by DHCP clients
 //
diff --git a/networking/udhcp/d6_dhcpc.c b/networking/udhcp/d6_dhcpc.c
index 8d11a7539..9d2a8f5d3 100644
--- a/networking/udhcp/d6_dhcpc.c
+++ b/networking/udhcp/d6_dhcpc.c
@@ -888,7 +888,8 @@ int send_d6_release(struct in6_addr *server_ipv6, struct in6_addr *our_cur_ipv6)
 	if (client6_data.ia_pd)
 		opt_ptr = mempcpy(opt_ptr, client6_data.ia_pd, client6_data.ia_pd->len + 2+2);
 	/* Client-id */
-	ci = udhcp_find_option(client_data.options, D6_OPT_CLIENTID);
+///vda
+	ci = udhcp_find_option(client_data.options, D6_OPT_CLIENTID, /*dhcpv6:*/ 1);
 	if (ci)
 		opt_ptr = mempcpy(opt_ptr, ci->data, D6_OPT_DATA + 2+2 + 6);
 
@@ -1272,7 +1273,7 @@ int udhcpc6_main(int argc UNUSED_PARAM, char **argv)
 	}
 
 	clientid_mac_ptr = NULL;
-	if (!udhcp_find_option(client_data.options, D6_OPT_CLIENTID)) {
+	if (!udhcp_find_option(client_data.options, D6_OPT_CLIENTID, /*dhcpv6:*/ 1)) {
 		/* not set, set the default client ID */
 		clientid_mac_ptr = udhcp_insert_new_option(
 				&client_data.options, D6_OPT_CLIENTID,
diff --git a/networking/udhcp/dhcpc.c b/networking/udhcp/dhcpc.c
index 331f13a8c..c757fb37c 100644
--- a/networking/udhcp/dhcpc.c
+++ b/networking/udhcp/dhcpc.c
@@ -658,7 +658,7 @@ static void add_client_options(struct dhcp_packet *packet)
 
 	// This will be needed if we remove -V VENDOR_STR in favor of
 	// -x vendor:VENDOR_STR
-	//if (!udhcp_find_option(packet.options, DHCP_VENDOR))
+	//if (!udhcp_find_option(packet.options, DHCP_VENDOR, /*dhcpv6:*/ 0))
 	//	/* not set, set the default vendor ID */
 	//	...add (DHCP_VENDOR, "udhcp "BB_VER) opt...
 }
@@ -676,7 +676,7 @@ static void add_serverid_and_clientid_options(struct dhcp_packet *packet, uint32
 	 * If the client used a 'client identifier' when it obtained the lease,
 	 * it MUST use the same 'client identifier' in the DHCPRELEASE message.
 	 */
-	ci = udhcp_find_option(client_data.options, DHCP_CLIENT_ID);
+	ci = udhcp_find_option(client_data.options, DHCP_CLIENT_ID, /*dhcpv6:*/ 0);
 	if (ci)
 		udhcp_add_binary_option(packet, ci->data);
 }
@@ -1328,7 +1328,7 @@ int udhcpc_main(int argc UNUSED_PARAM, char **argv)
 	}
 
 	clientid_mac_ptr = NULL;
-	if (!(opt & OPT_C) && !udhcp_find_option(client_data.options, DHCP_CLIENT_ID)) {
+	if (!(opt & OPT_C) && !udhcp_find_option(client_data.options, DHCP_CLIENT_ID, /*dhcpv6:*/ 0)) {
 		/* not suppressed and not set, create default client ID */
 		clientid_mac_ptr = udhcp_insert_new_option(
 				&client_data.options, DHCP_CLIENT_ID,
diff --git a/networking/udhcp/dhcpd.c b/networking/udhcp/dhcpd.c
index 0f5edb75c..66750e2e6 100644
--- a/networking/udhcp/dhcpd.c
+++ b/networking/udhcp/dhcpd.c
@@ -935,7 +935,7 @@ int udhcpd_main(int argc UNUSED_PARAM, char **argv)
 
 	bb_simple_info_msg("started, v"BB_VER);
 
-	option = udhcp_find_option(server_data.options, DHCP_LEASE_TIME);
+	option = udhcp_find_option(server_data.options, DHCP_LEASE_TIME, /*dhcpv6:*/ 0);
 	server_data.max_lease_sec = DEFAULT_LEASE_TIME;
 	if (option) {
 		move_from_unaligned32(server_data.max_lease_sec, option->data + OPT_DATA);