rtnl: Add function to detect virtual clocks.

Add a function using ethtool netlink to check whether a PHC is a virtual
clock of an interface.

Signed-off-by: Miroslav Lichvar <[email protected]>
Acked-by: Hangbin Liu <[email protected]>

incdefs.sh

@@ -86,6 +86,10 @@ kernel_flags()
 	if grep -q HWTSTAMP_TX_ONESTEP_P2P ${prefix}${tstamp}; then
 		printf " -DHAVE_ONESTEP_P2P"
 	fi
+
+	if grep -q SOF_TIMESTAMPING_BIND_PHC ${prefix}${tstamp}; then
+		printf " -DHAVE_VCLOCKS"
+	fi
 }
 
 flags="$(user_flags)$(kernel_flags)"

missing.h

@@ -251,6 +251,107 @@ enum {
 #define NLA_TYPE_MAX (__NLA_TYPE_MAX - 1)
 #endif /*NLA_TYPE_MAX*/
 
+#ifndef ETHTOOL_GENL_NAME
+#define ETHTOOL_GENL_NAME "ethtool"
+#define ETHTOOL_GENL_VERSION 1
+#endif
+
+#ifndef HAVE_VCLOCKS
+enum {
+	ETHTOOL_MSG_USER_NONE,
+	ETHTOOL_MSG_STRSET_GET,
+	ETHTOOL_MSG_LINKINFO_GET,
+	ETHTOOL_MSG_LINKINFO_SET,
+	ETHTOOL_MSG_LINKMODES_GET,
+	ETHTOOL_MSG_LINKMODES_SET,
+	ETHTOOL_MSG_LINKSTATE_GET,
+	ETHTOOL_MSG_DEBUG_GET,
+	ETHTOOL_MSG_DEBUG_SET,
+	ETHTOOL_MSG_WOL_GET,
+	ETHTOOL_MSG_WOL_SET,
+	ETHTOOL_MSG_FEATURES_GET,
+	ETHTOOL_MSG_FEATURES_SET,
+	ETHTOOL_MSG_PRIVFLAGS_GET,
+	ETHTOOL_MSG_PRIVFLAGS_SET,
+	ETHTOOL_MSG_RINGS_GET,
+	ETHTOOL_MSG_RINGS_SET,
+	ETHTOOL_MSG_CHANNELS_GET,
+	ETHTOOL_MSG_CHANNELS_SET,
+	ETHTOOL_MSG_COALESCE_GET,
+	ETHTOOL_MSG_COALESCE_SET,
+	ETHTOOL_MSG_PAUSE_GET,
+	ETHTOOL_MSG_PAUSE_SET,
+	ETHTOOL_MSG_EEE_GET,
+	ETHTOOL_MSG_EEE_SET,
+	ETHTOOL_MSG_TSINFO_GET,
+	ETHTOOL_MSG_CABLE_TEST_ACT,
+	ETHTOOL_MSG_CABLE_TEST_TDR_ACT,
+	ETHTOOL_MSG_TUNNEL_INFO_GET,
+	ETHTOOL_MSG_FEC_GET,
+	ETHTOOL_MSG_FEC_SET,
+	ETHTOOL_MSG_MODULE_EEPROM_GET,
+	ETHTOOL_MSG_STATS_GET,
+	ETHTOOL_MSG_PHC_VCLOCKS_GET,
+};
+
+enum {
+	ETHTOOL_MSG_KERNEL_NONE,
+	ETHTOOL_MSG_STRSET_GET_REPLY,
+	ETHTOOL_MSG_LINKINFO_GET_REPLY,
+	ETHTOOL_MSG_LINKINFO_NTF,
+	ETHTOOL_MSG_LINKMODES_GET_REPLY,
+	ETHTOOL_MSG_LINKMODES_NTF,
+	ETHTOOL_MSG_LINKSTATE_GET_REPLY,
+	ETHTOOL_MSG_DEBUG_GET_REPLY,
+	ETHTOOL_MSG_DEBUG_NTF,
+	ETHTOOL_MSG_WOL_GET_REPLY,
+	ETHTOOL_MSG_WOL_NTF,
+	ETHTOOL_MSG_FEATURES_GET_REPLY,
+	ETHTOOL_MSG_FEATURES_SET_REPLY,
+	ETHTOOL_MSG_FEATURES_NTF,
+	ETHTOOL_MSG_PRIVFLAGS_GET_REPLY,
+	ETHTOOL_MSG_PRIVFLAGS_NTF,
+	ETHTOOL_MSG_RINGS_GET_REPLY,
+	ETHTOOL_MSG_RINGS_NTF,
+	ETHTOOL_MSG_CHANNELS_GET_REPLY,
+	ETHTOOL_MSG_CHANNELS_NTF,
+	ETHTOOL_MSG_COALESCE_GET_REPLY,
+	ETHTOOL_MSG_COALESCE_NTF,
+	ETHTOOL_MSG_PAUSE_GET_REPLY,
+	ETHTOOL_MSG_PAUSE_NTF,
+	ETHTOOL_MSG_EEE_GET_REPLY,
+	ETHTOOL_MSG_EEE_NTF,
+	ETHTOOL_MSG_TSINFO_GET_REPLY,
+	ETHTOOL_MSG_CABLE_TEST_NTF,
+	ETHTOOL_MSG_CABLE_TEST_TDR_NTF,
+	ETHTOOL_MSG_TUNNEL_INFO_GET_REPLY,
+	ETHTOOL_MSG_FEC_GET_REPLY,
+	ETHTOOL_MSG_FEC_NTF,
+	ETHTOOL_MSG_MODULE_EEPROM_GET_REPLY,
+	ETHTOOL_MSG_STATS_GET_REPLY,
+	ETHTOOL_MSG_PHC_VCLOCKS_GET_REPLY,
+};
+
+enum {
+	ETHTOOL_A_HEADER_UNSPEC,
+	ETHTOOL_A_HEADER_DEV_INDEX,		/* u32 */
+	ETHTOOL_A_HEADER_DEV_NAME,		/* string */
+	ETHTOOL_A_HEADER_FLAGS,			/* u32 - ETHTOOL_FLAG_* */
+	__ETHTOOL_A_HEADER_CNT,
+	ETHTOOL_A_HEADER_MAX = __ETHTOOL_A_HEADER_CNT - 1
+};
+
+enum {
+	ETHTOOL_A_PHC_VCLOCKS_UNSPEC,
+	ETHTOOL_A_PHC_VCLOCKS_HEADER,			/* nest - _A_HEADER_* */
+	ETHTOOL_A_PHC_VCLOCKS_NUM,			/* u32 */
+	ETHTOOL_A_PHC_VCLOCKS_INDEX,			/* array, s32 */
+	__ETHTOOL_A_PHC_VCLOCKS_CNT,
+	ETHTOOL_A_PHC_VCLOCKS_MAX = (__ETHTOOL_A_PHC_VCLOCKS_CNT - 1)
+};
+
+#endif /* HAVE_VCLOCKS */
+
 #ifdef __UCLIBC__
 
 #if (_XOPEN_SOURCE >= 600 || _POSIX_C_SOURCE >= 200112L) && \

rtnl.c

@@ -19,6 +19,9 @@
 #include <asm/types.h>
 #include <sys/socket.h> /* Must come before linux/netlink.h on some systems. */
 #include <linux/netlink.h>
+#ifdef HAVE_VCLOCKS
+#include <linux/ethtool_netlink.h>
+#endif
 #include <linux/rtnetlink.h>
 #include <linux/genetlink.h>
 #include <linux/if_team.h>
@@ -465,3 +468,85 @@ static int get_team_active_iface(int master_index)
 	nl_close(fd);
 	return index;
 }
+
+static int rtnl_search_vclocks(struct rtattr *attr, int phc_index)
+{
+	int i, len = RTA_PAYLOAD(attr);
+
+	for (i = 0; i < len / sizeof (__s32); i++) {
+		if (((__s32 *)RTA_DATA(attr))[i] == phc_index)
+			return 1;
+	}
+
+	return 0;
+}
+
+int rtnl_iface_has_vclock(const char *device, int phc_index)
+{
+	struct rtattr *tb[ETHTOOL_A_PHC_VCLOCKS_MAX + 1];
+	int index, fd, gf_id, len, ret = 0;
+	struct genlmsghdr *gnlh;
+	struct nlmsghdr *nlh;
+	char msg[BUF_SIZE];
+	struct {
+		struct nlattr attr;
+		uint32_t index;
+	} req;
+
+	index = if_nametoindex(device);
+
+	fd = nl_open(NETLINK_GENERIC);
+	if (fd < 0)
+		return 0;
+
+	gf_id = genl_get_family_id(fd, ETHTOOL_GENL_NAME);
+	if (gf_id < 0) {
+		pr_debug("ethtool netlink not supported");
+		goto no_info;
+	}
+
+	req.attr.nla_len = sizeof(req);
+	req.attr.nla_type = ETHTOOL_A_HEADER_DEV_INDEX;
+	req.index = index;
+
+	len = genl_send_msg(fd, gf_id, ETHTOOL_MSG_PHC_VCLOCKS_GET,
+			    ETHTOOL_GENL_VERSION,
+			    NLA_F_NESTED | ETHTOOL_A_PHC_VCLOCKS_HEADER, 
+			    &req, sizeof(req));
+
+	if (len < 0) {
+		pr_err("send vclock request failed: %m");
+		goto no_info;
+	}
+
+	len = recv(fd, msg, sizeof(msg), 0);
+	if (len < 0) {
+		pr_err("recv vclock failed: %m");
+		goto no_info;
+	}
+
+	for (nlh = (struct nlmsghdr *) msg; NLMSG_OK(nlh, len);
+	     nlh = NLMSG_NEXT(nlh, len)) {
+		if (nlh->nlmsg_type != gf_id)
+			continue;
+
+		gnlh = (struct genlmsghdr *) NLMSG_DATA(nlh);
+		if (gnlh->cmd != ETHTOOL_MSG_PHC_VCLOCKS_GET_REPLY)
+			continue;
+
+		if (rtnl_rtattr_parse(tb, ETHTOOL_A_PHC_VCLOCKS_MAX,
+				      (struct rtattr *) GENLMSG_DATA(msg),
+				      NLMSG_PAYLOAD(nlh, GENL_HDRLEN)))
+			continue;
+
+		if (tb[ETHTOOL_A_PHC_VCLOCKS_INDEX]) {
+			ret = rtnl_search_vclocks(tb[ETHTOOL_A_PHC_VCLOCKS_INDEX],
+						  phc_index);
+			break;
+		}
+	}
+
+no_info:
+	nl_close(fd);
+	return ret;
+}

rtnl.h

@@ -59,6 +59,15 @@ int rtnl_link_query(int fd, const char *device);
  */
 int rtnl_link_status(int fd, const char *device, rtnl_callback cb, void *ctx);
 
+/**
+ * Check if the PHC is a virtual clock of the interface (i.e. sockets bound to
+ * the interface also need to be bound to the clock).
+ * @param device    Name of the interface.
+ * @param phc_index Index of the clock to check.
+ * @return          1 if true, otherwise 0.
+ */
+int rtnl_iface_has_vclock(const char *device, int phc_index);
+
 /**
  * Open a RT netlink socket for monitoring link state.
  * @return    A valid socket, or -1 on error.