net: ethtool: ts: Let the active time stamping layer be selectable

Now that the current timestamp is saved in a variable lets add the
ETHTOOL_MSG_TS_SET ethtool netlink socket to make it selectable.

Signed-off-by: Kory Maincent <kory.maincent@bootlin.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/Documentation/networking/ethtool-netlink.rst b/Documentation/networking/ethtool-netlink.rst
index b8d0067..530c177 100644 (file)
--- a/Documentation/networking/ethtool-netlink.rst
+++ b/Documentation/networking/ethtool-netlink.rst
@@ -227,6 +227,7 @@ Userspace to kernel:
   ``ETHTOOL_MSG_MM_SET``                set MAC merge layer parameters
   ``ETHTOOL_MSG_TS_GET``                get current timestamping
   ``ETHTOOL_MSG_TS_LIST_GET``           list available timestampings
+  ``ETHTOOL_MSG_TS_SET``                set current timestamping
   ===================================== =================================
 
 Kernel to userspace:
@@ -2038,6 +2039,21 @@ Kernel response contents:
 
 This command lists all the possible timestamp layer available.
 
+TS_SET
+======
+
+Modify the selected timestamping.
+
+Request contents:
+
+  =======================  ======  ===================
+  ``ETHTOOL_A_TS_HEADER``  nested  reply header
+  ``ETHTOOL_A_TS_LAYER``   u32     timestamping
+  =======================  ======  ===================
+
+This command set the timestamping with one that should be listed by the
+TSLIST_GET command.
+
 Request translation
 ===================
 
@@ -2146,4 +2162,5 @@ are netlink only.
   n/a                                 ``ETHTOOL_MSG_MM_SET``
   n/a                                 ``ETHTOOL_MSG_TS_GET``
   n/a                                 ``ETHTOOL_MSG_TS_LIST_GET``
+  n/a                                 ``ETHTOOL_MSG_TS_SET``
   =================================== =====================================

diff --git a/include/uapi/linux/ethtool_netlink.h b/include/uapi/linux/ethtool_netlink.h
index 62b885d..df6c4fc 100644 (file)
--- a/include/uapi/linux/ethtool_netlink.h
+++ b/include/uapi/linux/ethtool_netlink.h
@@ -59,6 +59,7 @@ enum {
        ETHTOOL_MSG_MM_SET,
        ETHTOOL_MSG_TS_GET,
        ETHTOOL_MSG_TS_LIST_GET,
+       ETHTOOL_MSG_TS_SET,
 
        /* add new constants above here */
        __ETHTOOL_MSG_USER_CNT,

diff --git a/net/ethtool/netlink.c b/net/ethtool/netlink.c
index 842c9db..8322bf7 100644 (file)
--- a/net/ethtool/netlink.c
+++ b/net/ethtool/netlink.c
@@ -308,6 +308,7 @@ ethnl_default_requests[__ETHTOOL_MSG_USER_CNT] = {
        [ETHTOOL_MSG_MM_SET]            = &ethnl_mm_request_ops,
        [ETHTOOL_MSG_TS_GET]            = &ethnl_ts_request_ops,
        [ETHTOOL_MSG_TS_LIST_GET]       = &ethnl_ts_list_request_ops,
+       [ETHTOOL_MSG_TS_SET]            = &ethnl_ts_request_ops,
 };
 
 static struct ethnl_dump_ctx *ethnl_dump_context(struct netlink_callback *cb)
@@ -1148,6 +1149,13 @@ static const struct genl_ops ethtool_genl_ops[] = {
                .policy = ethnl_ts_get_policy,
                .maxattr = ARRAY_SIZE(ethnl_ts_get_policy) - 1,
        },
+       {
+               .cmd    = ETHTOOL_MSG_TS_SET,
+               .flags  = GENL_UNS_ADMIN_PERM,
+               .doit   = ethnl_default_set_doit,
+               .policy = ethnl_ts_set_policy,
+               .maxattr = ARRAY_SIZE(ethnl_ts_set_policy) - 1,
+       },
 };
 
 static const struct genl_multicast_group ethtool_nl_mcgrps[] = {

diff --git a/net/ethtool/netlink.h b/net/ethtool/netlink.h
index ea8c312..8fedf23 100644 (file)
--- a/net/ethtool/netlink.h
+++ b/net/ethtool/netlink.h
@@ -444,6 +444,7 @@ extern const struct nla_policy ethnl_plca_get_status_policy[ETHTOOL_A_PLCA_HEADE
 extern const struct nla_policy ethnl_mm_get_policy[ETHTOOL_A_MM_HEADER + 1];
 extern const struct nla_policy ethnl_mm_set_policy[ETHTOOL_A_MM_MAX + 1];
 extern const struct nla_policy ethnl_ts_get_policy[ETHTOOL_A_TS_HEADER + 1];
+extern const struct nla_policy ethnl_ts_set_policy[ETHTOOL_A_TS_MAX + 1];
 
 int ethnl_set_features(struct sk_buff *skb, struct genl_info *info);
 int ethnl_act_cable_test(struct sk_buff *skb, struct genl_info *info);

diff --git a/net/ethtool/ts.c b/net/ethtool/ts.c
index bd21951..357265e 100644 (file)
--- a/net/ethtool/ts.c
+++ b/net/ethtool/ts.c
@@ -59,6 +59,102 @@ static int ts_fill_reply(struct sk_buff *skb,
        return nla_put_u32(skb, ETHTOOL_A_TS_LAYER, data->ts_layer);
 }
 
+/* TS_SET */
+const struct nla_policy ethnl_ts_set_policy[] = {
+       [ETHTOOL_A_TS_HEADER]   = NLA_POLICY_NESTED(ethnl_header_policy),
+       [ETHTOOL_A_TS_LAYER]    = NLA_POLICY_RANGE(NLA_U32, 0,
+                                                  __TIMESTAMPING_COUNT - 1)
+};
+
+static int ethnl_set_ts_validate(struct ethnl_req_info *req_info,
+                                struct genl_info *info)
+{
+       struct nlattr **tb = info->attrs;
+       const struct net_device_ops *ops = req_info->dev->netdev_ops;
+
+       if (!ops->ndo_hwtstamp_set)
+               return -EOPNOTSUPP;
+
+       if (!tb[ETHTOOL_A_TS_LAYER])
+               return 0;
+
+       return 1;
+}
+
+static int ethnl_set_ts(struct ethnl_req_info *req_info, struct genl_info *info)
+{
+       struct net_device *dev = req_info->dev;
+       const struct ethtool_ops *ops = dev->ethtool_ops;
+       struct kernel_hwtstamp_config config = {0};
+       struct nlattr **tb = info->attrs;
+       enum timestamping_layer ts_layer;
+       bool mod = false;
+       int ret;
+
+       ts_layer = dev->ts_layer;
+       ethnl_update_u32(&ts_layer, tb[ETHTOOL_A_TS_LAYER], &mod);
+
+       if (!mod)
+               return 0;
+
+       if (ts_layer == SOFTWARE_TIMESTAMPING) {
+               struct ethtool_ts_info ts_info = {0};
+
+               if (!ops->get_ts_info) {
+                       NL_SET_ERR_MSG_ATTR(info->extack,
+                                           tb[ETHTOOL_A_TS_LAYER],
+                                           "this net device cannot support timestamping");
+                       return -EINVAL;
+               }
+
+               ops->get_ts_info(dev, &ts_info);
+               if ((ts_info.so_timestamping &
+                   SOF_TIMESTAMPING_SOFTWARE_MASK) !=
+                   SOF_TIMESTAMPING_SOFTWARE_MASK) {
+                       NL_SET_ERR_MSG_ATTR(info->extack,
+                                           tb[ETHTOOL_A_TS_LAYER],
+                                           "this net device cannot support software timestamping");
+                       return -EINVAL;
+               }
+       } else if (ts_layer == MAC_TIMESTAMPING) {
+               struct ethtool_ts_info ts_info = {0};
+
+               if (!ops->get_ts_info) {
+                       NL_SET_ERR_MSG_ATTR(info->extack,
+                                           tb[ETHTOOL_A_TS_LAYER],
+                                           "this net device cannot support timestamping");
+                       return -EINVAL;
+               }
+
+               ops->get_ts_info(dev, &ts_info);
+               if ((ts_info.so_timestamping &
+                   SOF_TIMESTAMPING_HARDWARE_MASK) !=
+                   SOF_TIMESTAMPING_HARDWARE_MASK) {
+                       NL_SET_ERR_MSG_ATTR(info->extack,
+                                           tb[ETHTOOL_A_TS_LAYER],
+                                           "this net device cannot support hardware timestamping");
+                       return -EINVAL;
+               }
+       } else if (ts_layer == PHY_TIMESTAMPING && !phy_has_tsinfo(dev->phydev)) {
+               NL_SET_ERR_MSG_ATTR(info->extack, tb[ETHTOOL_A_TS_LAYER],
+                                   "this phy device cannot support timestamping");
+               return -EINVAL;
+       }
+
+       /* Disable time stamping in the current layer. */
+       if (netif_device_present(dev) &&
+           (dev->ts_layer == PHY_TIMESTAMPING ||
+           dev->ts_layer == MAC_TIMESTAMPING)) {
+               ret = dev_set_hwtstamp_phylib(dev, &config, info->extack);
+               if (ret < 0)
+                       return ret;
+       }
+
+       dev->ts_layer = ts_layer;
+
+       return 1;
+}
+
 const struct ethnl_request_ops ethnl_ts_request_ops = {
        .request_cmd            = ETHTOOL_MSG_TS_GET,
        .reply_cmd              = ETHTOOL_MSG_TS_GET_REPLY,
@@ -69,6 +165,9 @@ const struct ethnl_request_ops ethnl_ts_request_ops = {
        .prepare_data           = ts_prepare_data,
        .reply_size             = ts_reply_size,
        .fill_reply             = ts_fill_reply,
+
+       .set_validate           = ethnl_set_ts_validate,
+       .set                    = ethnl_set_ts,
 };
 
 /* TS_LIST_GET */