From acccd780df517b0e5925de4497688b6238bee10c Mon Sep 17 00:00:00 2001
From: Hangbin Liu <haliu@redhat.com>
Date: Mon, 6 Aug 2018 22:02:09 +0800
Subject: [PATCH 7/8] ndptool: add -T target support

Currently ndptool can send a Neighbour Solicitation, but does not target
an IP address, so the NS packet doesn't really make sense.

Extend ndptool to target a destination for Neighbour Solicitation.

v2:
1) remove function ipv6_addr_is_multicast()
2) inline some help functions.
3) update code style.
4) rename parameter -d/--dest to -T/--target

Signed-off-by: Hangbin Liu <haliu@redhat.com>
Signed-off-by: Jiri Pirko <jiri@mellanox.com>
---
 include/ndp.h   |   2 +
 libndp/libndp.c | 121 ++++++++++++++++++++++++++++++++++++++++++++++++
 man/ndptool.8   |   4 ++
 utils/ndptool.c |  21 +++++++--
 4 files changed, 145 insertions(+), 3 deletions(-)

diff --git a/include/ndp.h b/include/ndp.h
index 0dc1468..698bba7 100644
--- a/include/ndp.h
+++ b/include/ndp.h
@@ -79,6 +79,8 @@ enum ndp_msg_type ndp_msg_type(struct ndp_msg *msg);
 struct in6_addr *ndp_msg_addrto(struct ndp_msg *msg);
 uint32_t ndp_msg_ifindex(struct ndp_msg *msg);
 void ndp_msg_ifindex_set(struct ndp_msg *msg, uint32_t ifindex);
+void ndp_msg_target_set(struct ndp_msg *msg, struct in6_addr *target);
+void ndp_msg_opt_set(struct ndp_msg *msg);
 int ndp_msg_send(struct ndp *ndp, struct ndp_msg *msg);
 int ndp_msg_send_with_flags(struct ndp *ndp, struct ndp_msg *msg, uint8_t flags);
 
diff --git a/libndp/libndp.c b/libndp/libndp.c
index baacb76..bec55d4 100644
--- a/libndp/libndp.c
+++ b/libndp/libndp.c
@@ -32,10 +32,14 @@
 #include <net/ethernet.h>
 #include <assert.h>
 #include <ndp.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
 
 #include "ndp_private.h"
 #include "list.h"
 
+#define pr_err(args...) fprintf(stderr, ##args)
+
 /**
  * SECTION: logging
  * @short_description: libndp logging facility
@@ -308,6 +312,23 @@ static void ndp_msg_addrto_adjust_all_routers(struct in6_addr *addr)
 	addr->s6_addr32[3] = htonl(0x2);
 }
 
+/*
+ * compute link-local solicited-node multicast address
+ */
+static void ndp_msg_addrto_adjust_solicit_multi(struct in6_addr *addr,
+						struct in6_addr *target)
+{
+	struct in6_addr any = IN6ADDR_ANY_INIT;
+
+	/* Don't set addr to target if target is default IN6ADDR_ANY_INIT */
+	if (!memcmp(target, &any, sizeof(any)))
+		return;
+	addr->s6_addr32[0] = htonl(0xFF020000);
+	addr->s6_addr32[1] = 0;
+	addr->s6_addr32[2] = htonl(0x1);
+	addr->s6_addr32[3] = htonl(0xFF000000) | target->s6_addr32[3];
+}
+
 static bool ndp_msg_addrto_validate_link_local(struct in6_addr *addr)
 {
 	return IN6_IS_ADDR_LINKLOCAL (addr);
@@ -679,6 +700,106 @@ void ndp_msg_ifindex_set(struct ndp_msg *msg, uint32_t ifindex)
 	msg->ifindex = ifindex;
 }
 
+/**
+ * ndp_msg_target_set:
+ * @msg: message structure
+ * @target: ns,na target
+ *
+ * Set target address for NS and NA.
+ **/
+NDP_EXPORT
+void ndp_msg_target_set(struct ndp_msg *msg, struct in6_addr *target)
+{
+	enum ndp_msg_type msg_type = ndp_msg_type(msg);
+	switch (msg_type) {
+		case NDP_MSG_NS:
+			((struct ndp_msgna*)&msg->nd_msg)->na->nd_na_target = *target;
+			/*
+			 * Neighbor Solicitations are multicast when the node
+			 * needs to resolve an address and unicast when the
+			 * node seeks to verify the reachability of a
+			 * neighbor.
+			 *
+			 * In this case we don't know if we have a cache of
+			 * target, so we use multicast to resolve the target
+			 * address.
+			 * */
+			ndp_msg_addrto_adjust_solicit_multi(&msg->addrto, target);
+			break;
+		case NDP_MSG_NA:
+			((struct ndp_msgns*)&msg->nd_msg)->ns->nd_ns_target = *target;
+			break;
+		default:
+			break;
+	}
+}
+
+static int ndp_get_iface_mac(int ifindex, char *ptr)
+{
+	int sockfd;
+	struct ifreq ifr;
+
+	sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+	if (sockfd == -1) {
+		pr_err("%s: Failed to create socket", __func__);
+		return -errno;
+	}
+
+	if (if_indextoname(ifindex, (char *)&ifr.ifr_name) == NULL) {
+		pr_err("%s: Failed to get iface name with index %d", __func__, ifindex);
+		return -errno;
+	}
+
+	if (ioctl(sockfd, SIOCGIFHWADDR, &ifr) < 0) {
+		pr_err("%s: Failed to get iface mac with index %d\n", __func__, ifindex);
+		return -errno;
+	}
+
+	memcpy(ptr, &ifr.ifr_hwaddr.sa_data, sizeof(ifr.ifr_hwaddr.sa_data));
+
+	return 0;
+}
+
+static void ndp_msg_opt_set_linkaddr(struct ndp_msg *msg, int ndp_opt)
+{
+	char *opts_start = ndp_msg_payload_opts(msg);
+	struct nd_opt_hdr *s_laddr_opt = (struct nd_opt_hdr *) opts_start;
+	char *opt_data = (char *) s_laddr_opt + sizeof(struct nd_opt_hdr);
+	int err;
+
+	err = ndp_get_iface_mac(ndp_msg_ifindex(msg), opt_data);
+	if (err)
+		return;
+
+	opt_data += 6;
+	s_laddr_opt->nd_opt_type = ndp_opt;
+	s_laddr_opt->nd_opt_len = (opt_data - opts_start) >> 3;
+	msg->len += opt_data - opts_start;
+}
+
+/**
+ * ndp_msg_opt_set:
+ * @msg: message structure
+ *
+ * Set neighbor discovery option info.
+ **/
+NDP_EXPORT
+void ndp_msg_opt_set(struct ndp_msg *msg)
+{
+	enum ndp_msg_type msg_type = ndp_msg_type(msg);
+
+	switch (msg_type) {
+		case NDP_MSG_NS:
+			ndp_msg_opt_set_linkaddr(msg, ND_OPT_SOURCE_LINKADDR);
+			break;
+		case NDP_MSG_NA:
+			ndp_msg_opt_set_linkaddr(msg, ND_OPT_TARGET_LINKADDR);
+			break;
+		default:
+			break;
+	}
+}
+
 /**
  * ndp_msg_send:
  * @ndp: libndp library context
diff --git a/man/ndptool.8 b/man/ndptool.8
index ef765dc..dd6ddee 100644
--- a/man/ndptool.8
+++ b/man/ndptool.8
@@ -41,6 +41,10 @@ Neighbor Advertisement.
 .B "\-i ifname, \-\-ifname ifname"
 Specified interface name.
 
+.TP
+.B "\-T target, \-\-target target"
+Specified target address for NS/NA message.
+
 .TP
 .B "\-U, \-\-unsolicited"
 Send Unsolicited NA.
diff --git a/utils/ndptool.c b/utils/ndptool.c
index 96479fa..ba24d75 100644
--- a/utils/ndptool.c
+++ b/utils/ndptool.c
@@ -135,6 +135,7 @@ static void print_help(const char *argv0) {
             "\t-v --verbose             Increase output verbosity\n"
             "\t-t --msg-type=TYPE       Specify message type\n"
 	    "\t                         (\"rs\", \"ra\", \"ns\", \"na\")\n"
+            "\t-T --target=TARGET       Target address for NS or NA\n"
             "\t-i --ifname=IFNAME       Specify interface name\n"
             "\t-U --unsolicited         Send Unsolicited NA\n"
 	    "Available commands:\n"
@@ -333,7 +334,7 @@ static int run_cmd_monitor(struct ndp *ndp, enum ndp_msg_type msg_type,
 }
 
 static int run_cmd_send(struct ndp *ndp, enum ndp_msg_type msg_type,
-			uint32_t ifindex)
+			uint32_t ifindex, struct in6_addr *target)
 {
 	struct ndp_msg *msg;
 	int err;
@@ -344,6 +345,8 @@ static int run_cmd_send(struct ndp *ndp, enum ndp_msg_type msg_type,
 		return err;
 	}
 	ndp_msg_ifindex_set(msg, ifindex);
+	ndp_msg_target_set(msg, target);
+	ndp_msg_opt_set(msg);
 
 	err = ndp_msg_send_with_flags(ndp, msg, flags);
 	if (err) {
@@ -384,6 +387,7 @@ int main(int argc, char **argv)
 		{ "verbose",	no_argument,		NULL, 'v' },
 		{ "msg-type",	required_argument,	NULL, 't' },
 		{ "ifname",	required_argument,	NULL, 'i' },
+		{ "target",	required_argument,	NULL, 'T' },
 		{ "unsolicited",no_argument,		NULL, 'U' },
 		{ NULL, 0, NULL, 0 }
 	};
@@ -392,12 +396,14 @@ int main(int argc, char **argv)
 	char *msgtypestr = NULL;
 	enum ndp_msg_type msg_type;
 	char *ifname = NULL;
+	char *addr = NULL;
+	struct in6_addr target = IN6ADDR_ANY_INIT;
 	uint32_t ifindex;
 	char *cmd_name;
 	int err;
 	int res = EXIT_FAILURE;
 
-	while ((opt = getopt_long(argc, argv, "hvt:i:U",
+	while ((opt = getopt_long(argc, argv, "hvt:T:i:U",
 				  long_options, NULL)) >= 0) {
 
 		switch(opt) {
@@ -415,6 +421,10 @@ int main(int argc, char **argv)
 			free(ifname);
 			ifname = strdup(optarg);
 			break;
+		case 'd':
+			free(addr);
+			addr = strdup(optarg);
+			break;
 		case 'U':
 			flags |= ND_OPT_NA_UNSOL;
 			break;
@@ -448,6 +458,11 @@ int main(int argc, char **argv)
 		}
 	}
 
+	if (addr && inet_pton(AF_INET6, addr, &target) <= 0) {
+		pr_err("Invalid target address \"%s\"\n", addr);
+		goto errout;
+	}
+
 	err = get_msg_type(&msg_type, msgtypestr);
 	if (err) {
 		pr_err("Invalid message type \"%s\" selected\n", msgtypestr);
@@ -478,7 +493,7 @@ int main(int argc, char **argv)
 			print_help(argv0);
 			goto errout;
 		}
-		err = run_cmd_send(ndp, msg_type, ifindex);
+		err = run_cmd_send(ndp, msg_type, ifindex, &target);
 	} else {
 		pr_err("Unknown command \"%s\"\n", cmd_name);
 		goto ndp_close;
-- 
2.19.2