rtnl: update function rtnl_link_status to get bond slave info

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.

Add ifndef IFLA_BOND_MAX in case we build linuxptp on kernel before v3.13-rc1.

Signed-off-by: Hangbin Liu <liuhangbin@gmail.com>

missing.h

@@ -69,6 +69,42 @@ static inline int clock_adjtime(clockid_t id, struct timex *tx)
 }
 #endif
 
+#ifndef IFLA_BOND_MAX
+enum {
+	IFLA_BOND_UNSPEC,
+	IFLA_BOND_MODE,
+	IFLA_BOND_ACTIVE_SLAVE,
+	IFLA_BOND_MIIMON,
+	IFLA_BOND_UPDELAY,
+	IFLA_BOND_DOWNDELAY,
+	IFLA_BOND_USE_CARRIER,
+	IFLA_BOND_ARP_INTERVAL,
+	IFLA_BOND_ARP_IP_TARGET,
+	IFLA_BOND_ARP_VALIDATE,
+	IFLA_BOND_ARP_ALL_TARGETS,
+	IFLA_BOND_PRIMARY,
+	IFLA_BOND_PRIMARY_RESELECT,
+	IFLA_BOND_FAIL_OVER_MAC,
+	IFLA_BOND_XMIT_HASH_POLICY,
+	IFLA_BOND_RESEND_IGMP,
+	IFLA_BOND_NUM_PEER_NOTIF,
+	IFLA_BOND_ALL_SLAVES_ACTIVE,
+	IFLA_BOND_MIN_LINKS,
+	IFLA_BOND_LP_INTERVAL,
+	IFLA_BOND_PACKETS_PER_SLAVE,
+	IFLA_BOND_AD_LACP_RATE,
+	IFLA_BOND_AD_SELECT,
+	IFLA_BOND_AD_INFO,
+	IFLA_BOND_AD_ACTOR_SYS_PRIO,
+	IFLA_BOND_AD_USER_PORT_KEY,
+	IFLA_BOND_AD_ACTOR_SYSTEM,
+	IFLA_BOND_TLB_DYNAMIC_LB,
+	__IFLA_BOND_MAX,
+};
+
+#define IFLA_BOND_MAX   (__IFLA_BOND_MAX - 1)
+#endif	/*IFLA_BOND_MAX*/
+
 #ifdef __UCLIBC__
 
 #if (_XOPEN_SOURCE >= 600 || _POSIX_C_SOURCE >= 200112L) && \

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;
 	}
 

rtnl.c

@@ -26,6 +26,7 @@
 #include <string.h>
 #include <unistd.h>
 
+#include "missing.h"
 #include "print.h"
 #include "rtnl.h"
 
@@ -84,15 +85,79 @@ 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 -1;
+	}
+
+	return 0;
+}
+
+static inline int rtnl_nested_rtattr_parse(struct rtattr *tb[], int max, struct rtattr *rta)
+{
+	return rtnl_rtattr_parse(tb, max, RTA_DATA(rta), RTA_PAYLOAD(rta));
+}
+
+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];
+
+	if (rtnl_nested_rtattr_parse(linkinfo, IFLA_INFO_MAX, rta) < 0)
+		return -1;
+
+	if (linkinfo[IFLA_INFO_KIND]) {
+		kind = rta_getattr_str(linkinfo[IFLA_INFO_KIND]);
+
+		if (kind && !strncmp(kind, "bond", 4) &&
+		    linkinfo[IFLA_INFO_DATA]) {
+			if (rtnl_nested_rtattr_parse(bond, IFLA_BOND_MAX,
+						 linkinfo[IFLA_INFO_DATA]) < 0)
+				return -1;
+
+			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 +200,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;
 }
 

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       Zero on success, non-zero otherwise.
  */
-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.