On 30/10/2024 13:54, Kory Maincent wrote:
Introduce support for ETHTOOL_MSG_TSCONFIG_GET/SET ethtool netlink socket
to read and configure hwtstamp configuration of a PHC provider. Note that
simultaneous hwtstamp isn't supported; configuring a new one disables the
previous setting.
Signed-off-by: Kory Maincent <kory.maincent@xxxxxxxxxxx>
[ ... ]
+static int ethnl_set_tsconfig(struct ethnl_req_info *req_base,
+ struct genl_info *info)
+{
+ struct kernel_hwtstamp_config hwtst_config = {0}, _hwtst_config = {0};
+ unsigned long mask = 0, req_rx_filter, req_tx_type;
+ struct hwtstamp_provider *hwtstamp = NULL;
+ struct net_device *dev = req_base->dev;
+ struct nlattr **tb = info->attrs;
+ bool mod = false;
+ int ret;
+
+ BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32);
+ BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32);
+
+ if (!netif_device_present(dev))
+ return -ENODEV;
+
+ if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER]) {
+ struct hwtst_provider __hwtst = {.index = -1};
+ struct hwtstamp_provider *__hwtstamp;
+
+ __hwtstamp = rtnl_dereference(dev->hwtstamp);
+ if (__hwtstamp) {
+ __hwtst.index = ptp_clock_index(__hwtstamp->ptp);
+ __hwtst.qualifier = __hwtstamp->qualifier;
+ }
+
+ ret = ts_parse_hwtst_provider(tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER],
+ &__hwtst, info->extack,
+ &mod);
+ if (ret < 0)
+ return ret;
+
+ if (mod) {
+ hwtstamp = kzalloc(sizeof(*hwtstamp), GFP_KERNEL);
+ if (!hwtstamp)
+ return -ENOMEM;
+
+ hwtstamp->ptp = ptp_clock_get_by_index(&dev->dev,
+ __hwtst.index);
+ if (!hwtstamp->ptp) {
+ NL_SET_ERR_MSG_ATTR(info->extack,
+ tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER],
+ "no phc at such index");
+ ret = -ENODEV;
+ goto err_free_hwtstamp;
+ }
+ hwtstamp->qualifier = __hwtst.qualifier;
+ hwtstamp->dev = &dev->dev;
+
+ /* Does the hwtstamp supported in the netdev topology */
+ if (!netdev_support_hwtstamp(dev, hwtstamp)) {
+ NL_SET_ERR_MSG_ATTR(info->extack,
+ tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER],
+ "phc not in this net device topology");
+ ret = -ENODEV;
+ goto err_clock_put;
+ }
+ }
+ }
+
+ /* Get the hwtstamp config from netlink */
+ if (tb[ETHTOOL_A_TSCONFIG_TX_TYPES]) {
+ ret = ethnl_parse_bitset(&req_tx_type, &mask,
+ __HWTSTAMP_TX_CNT,
+ tb[ETHTOOL_A_TSCONFIG_TX_TYPES],
+ ts_tx_type_names, info->extack);
+ if (ret < 0)
+ goto err_clock_put;
+
+ /* Select only one tx type at a time */
+ if (ffs(req_tx_type) != fls(req_tx_type)) {
+ ret = -EINVAL;
+ goto err_clock_put;
+ }
+
+ hwtst_config.tx_type = ffs(req_tx_type) - 1;
+ }
+ if (tb[ETHTOOL_A_TSCONFIG_RX_FILTERS]) {
+ ret = ethnl_parse_bitset(&req_rx_filter, &mask,
+ __HWTSTAMP_FILTER_CNT,
+ tb[ETHTOOL_A_TSCONFIG_RX_FILTERS],
+ ts_rx_filter_names, info->extack);
+ if (ret < 0)
+ goto err_clock_put;
+
+ /* Select only one rx filter at a time */
+ if (ffs(req_rx_filter) != fls(req_rx_filter)) {
+ ret = -EINVAL;
+ goto err_clock_put;
+ }
+
+ hwtst_config.rx_filter = ffs(req_rx_filter) - 1;
+ }
+ if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS]) {
+ ret = nla_get_u32(tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS]);
+ if (ret < 0)
+ goto err_clock_put;
+ hwtst_config.flags = ret;
+ }
+
+ ret = net_hwtstamp_validate(&hwtst_config);
+ if (ret)
+ goto err_clock_put;
+
+ if (mod) {
+ struct kernel_hwtstamp_config zero_config = {0};
+ struct hwtstamp_provider *__hwtstamp;
+
+ /* Disable current time stamping if we try to enable
+ * another one
+ */
+ ret = dev_set_hwtstamp_phylib(dev, &zero_config, info->extack);
_hwtst_config is still inited to 0 here, maybe it can be used to avoid
another stack allocation?
+ if (ret < 0)
+ goto err_clock_put;
+
+ /* Change the selected hwtstamp source */
+ __hwtstamp = rcu_replace_pointer_rtnl(dev->hwtstamp, hwtstamp);
+ if (__hwtstamp)
+ call_rcu(&__hwtstamp->rcu_head,
+ remove_hwtstamp_provider);
+ } else {
+ /* Get current hwtstamp config if we are not changing the
+ * hwtstamp source
+ */
+ ret = dev_get_hwtstamp_phylib(dev, &_hwtst_config);
This may be tricky whithout ifr set properly. But it should force
drivers to be converted.
+ if (ret < 0 && ret != -EOPNOTSUPP)
+ goto err_clock_put;
+ }
+
+ if (memcmp(&hwtst_config, &_hwtst_config, sizeof(hwtst_config))) {
better to use kernel_hwtstamp_config_changed() helper here
+ ret = dev_set_hwtstamp_phylib(dev, &hwtst_config,
+ info->extack);
+ if (ret < 0)
+ return ret;
+
+ ret = tsconfig_send_reply(dev, info);
+ if (ret && ret != -EOPNOTSUPP) {
+ NL_SET_ERR_MSG(info->extack,
+ "error while reading the new configuration set");
+ return ret;
+ }
+
+ return 1;
+ }
+
+ if (mod)
+ return 1;
+
+ return 0;
+
+err_clock_put:
+ if (hwtstamp)
+ ptp_clock_put(&dev->dev, hwtstamp->ptp);
+err_free_hwtstamp:
+ kfree(hwtstamp);
+
+ return ret;
+}
+
+const struct ethnl_request_ops ethnl_tsconfig_request_ops = {
+ .request_cmd = ETHTOOL_MSG_TSCONFIG_GET,
+ .reply_cmd = ETHTOOL_MSG_TSCONFIG_GET_REPLY,
+ .hdr_attr = ETHTOOL_A_TSCONFIG_HEADER,
+ .req_info_size = sizeof(struct tsconfig_req_info),
+ .reply_data_size = sizeof(struct tsconfig_reply_data),
+
+ .prepare_data = tsconfig_prepare_data,
+ .reply_size = tsconfig_reply_size,
+ .fill_reply = tsconfig_fill_reply,
+
+ .set_validate = ethnl_set_tsconfig_validate,
+ .set = ethnl_set_tsconfig,
+ .set_ntf_cmd = ETHTOOL_MSG_TSCONFIG_NTF,
+};
