[Linuxptp-devel] [PATCHv3 04/10] rtnl: update function rtnl_link_status to get bond slave info

[Hangbin L. <liu...@gm...>]

Update function rtnl_link_status to get bond slave info. Pass the slave index
to call back functions. i.e. port_link_status.

Also check the interface index of rtnl message in function rtnl_link_status.
Then we don't need to check it in port_link_status.

Signed-off-by: Hangbin Liu <liu...@gm...>
---
 port.c |  6 ++---
 rtnl.c | 88 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-------
 rtnl.h | 13 +++++-----
 3 files changed, 89 insertions(+), 18 deletions(-)

diff --git a/port.c b/port.c
index 5b85d87..05fc321 100644
--- a/port.c
+++ b/port.c
@@ -2221,11 +2221,11 @@ void port_dispatch(struct port *p, enum fsm_event event, int mdiff)
 	}
 }
 
-static void port_link_status(void *ctx, int index, int linkup)
+static void port_link_status(void *ctx, int linkup, int ts_index)
 {
 	struct port *p = ctx;
 
-	if (index != if_nametoindex(p->name) || p->link_status == linkup)
+	if (p->link_status == linkup)
 		return;
 
 	p->link_status = linkup;
@@ -2280,7 +2280,7 @@ enum fsm_event port_event(struct port *p, int fd_index)
 
 	case FD_RTNL:
 		pr_debug("port %hu: received link status notification", portnum(p));
-		rtnl_link_status(fd, port_link_status, p);
+		rtnl_link_status(fd, p->name, port_link_status, p);
 		return port_link_status_get(p) ? EV_FAULT_CLEARED : EV_FAULT_DETECTED;
 	}
 
diff --git a/rtnl.c b/rtnl.c
index 8ecf6fe..b695edd 100644
--- a/rtnl.c
+++ b/rtnl.c
@@ -31,6 +31,9 @@
 
 static int rtnl_len;
 static char *rtnl_buf;
+static int rtnl_rtattr_parse(struct rtattr *tb[], int max, struct rtattr *rta, int len);
+#define rtnl_nested_rtattr_parse(tb, max, rta) \
+	(rtnl_rtattr_parse((tb), (max), RTA_DATA(rta), RTA_PAYLOAD(rta)))
 
 int rtnl_close(int fd)
 {
@@ -84,15 +87,69 @@ int rtnl_link_query(int fd, char *device)
 	return 0;
 }
 
-int rtnl_link_status(int fd, rtnl_callback cb, void *ctx)
+static inline __u32 rta_getattr_u32(const struct rtattr *rta)
 {
-	int index, len;
+	return *(__u32 *)RTA_DATA(rta);
+}
+
+static inline const char *rta_getattr_str(const struct rtattr *rta)
+{
+	return (const char *)RTA_DATA(rta);
+}
+
+static int rtnl_rtattr_parse(struct rtattr *tb[], int max, struct rtattr *rta, int len)
+{
+	unsigned short type;
+
+	memset(tb, 0, sizeof(struct rtattr *) * max);
+	while (RTA_OK(rta, len)) {
+		type = rta->rta_type;
+		if ((type <= max) && (!tb[type]))
+			tb[type] = rta;
+		rta = RTA_NEXT(rta, len);
+	}
+	if (len)
+		pr_err("Length mismatch: len %d, rta_len=%d\n", len, rta->rta_len);
+	return 0;
+}
+
+static int rtnl_linkinfo_parse(struct rtattr *rta)
+{
+	int index = -1;
+	const char *kind;
+	struct rtattr *linkinfo[IFLA_INFO_MAX];
+	struct rtattr *bond[IFLA_BOND_MAX];
+
+	rtnl_nested_rtattr_parse(linkinfo, IFLA_INFO_MAX, rta);
+
+	if (linkinfo[IFLA_INFO_KIND]) {
+		kind = rta_getattr_str(linkinfo[IFLA_INFO_KIND]);
+
+		if (kind && !strncmp(kind, "bond", 4) &&
+		    linkinfo[IFLA_INFO_DATA]) {
+			rtnl_nested_rtattr_parse(bond, IFLA_BOND_MAX,
+						 linkinfo[IFLA_INFO_DATA]);
+
+			if (bond[IFLA_BOND_ACTIVE_SLAVE]) {
+				index = rta_getattr_u32(bond[IFLA_BOND_ACTIVE_SLAVE]);
+			}
+		}
+	}
+	return index;
+}
+
+int rtnl_link_status(int fd, char *device, rtnl_callback cb, void *ctx)
+{
+	int index, len, link_up;
+	int slave_index = -1;
 	struct iovec iov;
 	struct sockaddr_nl sa;
 	struct msghdr msg;
 	struct nlmsghdr *nh;
 	struct ifinfomsg *info = NULL;
+	struct rtattr *tb[IFLA_MAX+1];
 
+	index = if_nametoindex(device);
 	if (!rtnl_buf) {
 		rtnl_len = 4096;
 		rtnl_buf = malloc(rtnl_len);
@@ -135,14 +192,27 @@ int rtnl_link_status(int fd, rtnl_callback cb, void *ctx)
 	nh = (struct nlmsghdr *) rtnl_buf;
 
 	for ( ; NLMSG_OK(nh, len); nh = NLMSG_NEXT(nh, len)) {
-		if (nh->nlmsg_type == RTM_NEWLINK) {
-			info = NLMSG_DATA(nh);
-			index = info->ifi_index;
-			pr_debug("interface index %d is %s", index,
-				 info->ifi_flags & IFF_RUNNING ? "up" : "down");
-			cb(ctx, index, info->ifi_flags & IFF_RUNNING ? 1 : 0);
-		}
+		if (nh->nlmsg_type != RTM_NEWLINK)
+			continue;
+
+		info = NLMSG_DATA(nh);
+		if (index != info->ifi_index)
+			continue;
+
+		link_up = info->ifi_flags & IFF_RUNNING ? 1 : 0;
+		pr_debug("interface index %d is %s", index,
+			 link_up ? "up" : "down");
+
+		rtnl_rtattr_parse(tb, IFLA_MAX, IFLA_RTA(info),
+				  IFLA_PAYLOAD(nh));
+
+		if (tb[IFLA_LINKINFO])
+			slave_index = rtnl_linkinfo_parse(tb[IFLA_LINKINFO]);
+
+		if (cb)
+			cb(ctx, link_up, slave_index);
 	}
+
 	return 0;
 }
 
diff --git a/rtnl.h b/rtnl.h
index 5c93eec..20f1491 100644
--- a/rtnl.h
+++ b/rtnl.h
@@ -20,7 +20,7 @@
 #ifndef HAVE_RTNL_H
 #define HAVE_RTNL_H
 
-typedef void (*rtnl_callback)(void *ctx, int index, int linkup);
+typedef void (*rtnl_callback)(void *ctx, int linkup, int ts_index);
 
 /**
  * Close a RT netlink socket.
@@ -39,12 +39,13 @@ int rtnl_link_query(int fd, char *device);
 
 /**
  * Read kernel messages looking for a link up/down events.
- * @param fd   Readable socket obtained via rtnl_open().
- * @param cb   Callback function to be invoked on each event.
- * @param ctx  Private context passed to the callback.
- * @return     Zero on success, non-zero otherwise.
+ * @param fd     Readable socket obtained via rtnl_open().
+ * @param device The device which we need to get link info.
+ * @param cb     Callback function to be invoked on each event.
+ * @param ctx    Private context passed to the callback.
+ * @return       A slave index, or -1 on error.
  */
-int rtnl_link_status(int fd, rtnl_callback cb, void *ctx);
+int rtnl_link_status(int fd, char *device, rtnl_callback cb, void *ctx);
 
 /**
  * Open a RT netlink socket for monitoring link state.
-- 
2.5.5