1 // SPDX-License-Identifier: GPL-2.0-only 2 3 #include <linux/net_tstamp.h> 4 #include <linux/phy.h> 5 #include <linux/ptp_clock_kernel.h> 6 #include <net/netdev_lock.h> 7 8 #include "bitset.h" 9 #include "common.h" 10 #include "netlink.h" 11 #include "ts.h" 12 #include "../core/dev.h" 13 14 struct tsconfig_req_info { 15 struct ethnl_req_info base; 16 }; 17 18 struct tsconfig_reply_data { 19 struct ethnl_reply_data base; 20 struct hwtstamp_provider_desc hwprov_desc; 21 struct { 22 u32 tx_type; 23 u32 rx_filter; 24 u32 flags; 25 } hwtst_config; 26 }; 27 28 #define TSCONFIG_REPDATA(__reply_base) \ 29 container_of(__reply_base, struct tsconfig_reply_data, base) 30 31 const struct nla_policy ethnl_tsconfig_get_policy[ETHTOOL_A_TSCONFIG_HEADER + 1] = { 32 [ETHTOOL_A_TSCONFIG_HEADER] = 33 NLA_POLICY_NESTED(ethnl_header_policy), 34 }; 35 36 static int tsconfig_prepare_data(const struct ethnl_req_info *req_base, 37 struct ethnl_reply_data *reply_base, 38 const struct genl_info *info) 39 { 40 struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base); 41 struct hwtstamp_provider *hwprov = NULL; 42 struct net_device *dev = reply_base->dev; 43 struct kernel_hwtstamp_config cfg = {}; 44 int ret; 45 46 if (!dev->netdev_ops->ndo_hwtstamp_get && 47 !phy_is_default_hwtstamp(dev->phydev) && 48 !netdev_ops_lock_dereference(dev->hwprov, dev)) 49 return -EOPNOTSUPP; 50 51 ret = ethnl_ops_begin(dev); 52 if (ret < 0) 53 return ret; 54 55 ret = dev_get_hwtstamp_phylib(dev, &cfg); 56 if (ret) 57 goto out; 58 59 data->hwtst_config.tx_type = BIT(cfg.tx_type); 60 data->hwtst_config.rx_filter = BIT(cfg.rx_filter); 61 data->hwtst_config.flags = cfg.flags; 62 63 data->hwprov_desc.index = -1; 64 hwprov = netdev_ops_lock_dereference(dev->hwprov, dev); 65 if (hwprov) { 66 data->hwprov_desc.index = hwprov->desc.index; 67 data->hwprov_desc.qualifier = hwprov->desc.qualifier; 68 } else { 69 struct kernel_ethtool_ts_info ts_info = {}; 70 71 ts_info.phc_index = -1; 72 ret = __ethtool_get_ts_info(dev, &ts_info); 73 if (ret) 74 goto out; 75 76 if (ts_info.phc_index == -1) { 77 ret = -ENODEV; 78 goto out; 79 } 80 81 data->hwprov_desc.index = ts_info.phc_index; 82 data->hwprov_desc.qualifier = ts_info.phc_qualifier; 83 } 84 85 out: 86 ethnl_ops_complete(dev); 87 return ret; 88 } 89 90 static int tsconfig_reply_size(const struct ethnl_req_info *req_base, 91 const struct ethnl_reply_data *reply_base) 92 { 93 const struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base); 94 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 95 int len = 0; 96 int ret; 97 98 BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32); 99 BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32); 100 BUILD_BUG_ON(__HWTSTAMP_FLAG_CNT > 32); 101 102 if (data->hwtst_config.flags) { 103 ret = ethnl_bitset32_size(&data->hwtst_config.flags, 104 NULL, __HWTSTAMP_FLAG_CNT, 105 ts_flags_names, compact); 106 if (ret < 0) 107 return ret; 108 len += ret; /* _TSCONFIG_HWTSTAMP_FLAGS */ 109 } 110 111 if (data->hwtst_config.tx_type) { 112 ret = ethnl_bitset32_size(&data->hwtst_config.tx_type, 113 NULL, __HWTSTAMP_TX_CNT, 114 ts_tx_type_names, compact); 115 if (ret < 0) 116 return ret; 117 len += ret; /* _TSCONFIG_TX_TYPES */ 118 } 119 if (data->hwtst_config.rx_filter) { 120 ret = ethnl_bitset32_size(&data->hwtst_config.rx_filter, 121 NULL, __HWTSTAMP_FILTER_CNT, 122 ts_rx_filter_names, compact); 123 if (ret < 0) 124 return ret; 125 len += ret; /* _TSCONFIG_RX_FILTERS */ 126 } 127 128 if (data->hwprov_desc.index >= 0) 129 /* _TSCONFIG_HWTSTAMP_PROVIDER */ 130 len += nla_total_size(0) + 131 2 * nla_total_size(sizeof(u32)); 132 133 return len; 134 } 135 136 static int tsconfig_fill_reply(struct sk_buff *skb, 137 const struct ethnl_req_info *req_base, 138 const struct ethnl_reply_data *reply_base) 139 { 140 const struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base); 141 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 142 int ret; 143 144 if (data->hwtst_config.flags) { 145 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS, 146 &data->hwtst_config.flags, NULL, 147 __HWTSTAMP_FLAG_CNT, 148 ts_flags_names, compact); 149 if (ret < 0) 150 return ret; 151 } 152 153 if (data->hwtst_config.tx_type) { 154 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_TX_TYPES, 155 &data->hwtst_config.tx_type, NULL, 156 __HWTSTAMP_TX_CNT, 157 ts_tx_type_names, compact); 158 if (ret < 0) 159 return ret; 160 } 161 162 if (data->hwtst_config.rx_filter) { 163 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_RX_FILTERS, 164 &data->hwtst_config.rx_filter, 165 NULL, __HWTSTAMP_FILTER_CNT, 166 ts_rx_filter_names, compact); 167 if (ret < 0) 168 return ret; 169 } 170 171 if (data->hwprov_desc.index >= 0) { 172 struct nlattr *nest; 173 174 nest = nla_nest_start(skb, ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER); 175 if (!nest) 176 return -EMSGSIZE; 177 178 if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX, 179 data->hwprov_desc.index) || 180 nla_put_u32(skb, 181 ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER, 182 data->hwprov_desc.qualifier)) { 183 nla_nest_cancel(skb, nest); 184 return -EMSGSIZE; 185 } 186 187 nla_nest_end(skb, nest); 188 } 189 return 0; 190 } 191 192 /* TSCONFIG_SET */ 193 const struct nla_policy ethnl_tsconfig_set_policy[ETHTOOL_A_TSCONFIG_MAX + 1] = { 194 [ETHTOOL_A_TSCONFIG_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy), 195 [ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER] = 196 NLA_POLICY_NESTED(ethnl_ts_hwtst_prov_policy), 197 [ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS] = { .type = NLA_NESTED }, 198 [ETHTOOL_A_TSCONFIG_RX_FILTERS] = { .type = NLA_NESTED }, 199 [ETHTOOL_A_TSCONFIG_TX_TYPES] = { .type = NLA_NESTED }, 200 }; 201 202 static int tsconfig_send_reply(struct net_device *dev, struct genl_info *info) 203 { 204 struct tsconfig_reply_data *reply_data; 205 struct tsconfig_req_info *req_info; 206 struct sk_buff *rskb; 207 void *reply_payload; 208 int reply_len = 0; 209 int ret; 210 211 req_info = kzalloc_obj(*req_info); 212 if (!req_info) 213 return -ENOMEM; 214 reply_data = kmalloc_obj(*reply_data); 215 if (!reply_data) { 216 kfree(req_info); 217 return -ENOMEM; 218 } 219 220 netdev_assert_locked_ops_compat(dev); 221 reply_data->base.dev = dev; 222 ret = tsconfig_prepare_data(&req_info->base, &reply_data->base, info); 223 if (ret < 0) 224 goto err_cleanup; 225 226 ret = tsconfig_reply_size(&req_info->base, &reply_data->base); 227 if (ret < 0) 228 goto err_cleanup; 229 230 reply_len = ret + ethnl_reply_header_size(); 231 rskb = ethnl_reply_init(reply_len, dev, ETHTOOL_MSG_TSCONFIG_SET_REPLY, 232 ETHTOOL_A_TSCONFIG_HEADER, info, &reply_payload); 233 if (!rskb) { 234 ret = -ENOMEM; 235 goto err_cleanup; 236 } 237 238 ret = tsconfig_fill_reply(rskb, &req_info->base, &reply_data->base); 239 if (ret < 0) 240 goto err_free_msg; 241 242 genlmsg_end(rskb, reply_payload); 243 ret = genlmsg_reply(rskb, info); 244 rskb = NULL; 245 246 err_free_msg: 247 nlmsg_free(rskb); 248 err_cleanup: 249 kfree(reply_data); 250 kfree(req_info); 251 return ret; 252 } 253 254 static struct hwtstamp_provider * 255 tsconfig_set_hwprov_from_desc(struct net_device *dev, 256 struct genl_info *info, 257 struct hwtstamp_provider_desc *hwprov_desc) 258 { 259 struct kernel_ethtool_ts_info ts_info; 260 struct hwtstamp_provider *hwprov; 261 struct nlattr **tb = info->attrs; 262 struct phy_device *phy = NULL; 263 enum hwtstamp_source source; 264 int ret; 265 266 ret = ethtool_net_get_ts_info_by_phc(dev, &ts_info, hwprov_desc); 267 if (!ret && dev->netdev_ops->ndo_hwtstamp_set) { 268 /* Found */ 269 source = HWTSTAMP_SOURCE_NETDEV; 270 } else { 271 phy = ethtool_phy_get_ts_info_by_phc(dev, &ts_info, hwprov_desc); 272 if (IS_ERR(phy)) { 273 if (PTR_ERR(phy) == -ENODEV) 274 NL_SET_ERR_MSG_ATTR(info->extack, 275 tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER], 276 "phc not in this net device topology"); 277 return ERR_CAST(phy); 278 } 279 280 source = HWTSTAMP_SOURCE_PHYLIB; 281 } 282 283 hwprov = kzalloc_obj(*hwprov); 284 if (!hwprov) 285 return ERR_PTR(-ENOMEM); 286 287 hwprov->desc.index = hwprov_desc->index; 288 hwprov->desc.qualifier = hwprov_desc->qualifier; 289 hwprov->source = source; 290 hwprov->phydev = phy; 291 292 return hwprov; 293 } 294 295 static int ethnl_set_tsconfig(struct ethnl_req_info *req_base, 296 struct genl_info *info) 297 { 298 struct kernel_hwtstamp_config hwtst_config = {0}; 299 bool hwprov_mod = false, config_mod = false; 300 struct hwtstamp_provider *hwprov = NULL; 301 struct net_device *dev = req_base->dev; 302 struct nlattr **tb = info->attrs; 303 int ret; 304 305 if (!netif_device_present(dev)) 306 return -ENODEV; 307 308 if (!dev->netdev_ops->ndo_hwtstamp_set && 309 !phy_is_default_hwtstamp(dev->phydev) && 310 !netdev_ops_lock_dereference(dev->hwprov, dev)) 311 return -EOPNOTSUPP; 312 313 if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER]) { 314 struct hwtstamp_provider_desc __hwprov_desc = {.index = -1}; 315 struct hwtstamp_provider *__hwprov; 316 317 __hwprov = netdev_ops_lock_dereference(dev->hwprov, dev); 318 if (__hwprov) { 319 __hwprov_desc.index = __hwprov->desc.index; 320 __hwprov_desc.qualifier = __hwprov->desc.qualifier; 321 } 322 323 ret = ts_parse_hwtst_provider(tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER], 324 &__hwprov_desc, info->extack, 325 &hwprov_mod); 326 if (ret < 0) 327 return ret; 328 329 if (hwprov_mod) { 330 hwprov = tsconfig_set_hwprov_from_desc(dev, info, 331 &__hwprov_desc); 332 if (IS_ERR(hwprov)) 333 return PTR_ERR(hwprov); 334 } 335 } 336 337 /* Get current hwtstamp config if we are not changing the 338 * hwtstamp source. It will be zeroed in the other case. 339 */ 340 if (!hwprov_mod) { 341 ret = dev_get_hwtstamp_phylib(dev, &hwtst_config); 342 if (ret < 0 && ret != -EOPNOTSUPP) 343 goto err_free_hwprov; 344 } 345 346 /* Get the hwtstamp config from netlink */ 347 if (tb[ETHTOOL_A_TSCONFIG_TX_TYPES]) { 348 u32 req_tx_type; 349 350 req_tx_type = BIT(hwtst_config.tx_type); 351 ret = ethnl_update_bitset32(&req_tx_type, 352 __HWTSTAMP_TX_CNT, 353 tb[ETHTOOL_A_TSCONFIG_TX_TYPES], 354 ts_tx_type_names, info->extack, 355 &config_mod); 356 if (ret < 0) 357 goto err_free_hwprov; 358 359 /* Select exactly one tx type at a time */ 360 if (hweight32(req_tx_type) != 1) { 361 NL_SET_BAD_ATTR(info->extack, 362 tb[ETHTOOL_A_TSCONFIG_TX_TYPES]); 363 ret = -EINVAL; 364 goto err_free_hwprov; 365 } 366 367 hwtst_config.tx_type = ffs(req_tx_type) - 1; 368 } 369 370 if (tb[ETHTOOL_A_TSCONFIG_RX_FILTERS]) { 371 u32 req_rx_filter; 372 373 req_rx_filter = BIT(hwtst_config.rx_filter); 374 ret = ethnl_update_bitset32(&req_rx_filter, 375 __HWTSTAMP_FILTER_CNT, 376 tb[ETHTOOL_A_TSCONFIG_RX_FILTERS], 377 ts_rx_filter_names, info->extack, 378 &config_mod); 379 if (ret < 0) 380 goto err_free_hwprov; 381 382 /* Select exactly one rx filter at a time */ 383 if (hweight32(req_rx_filter) != 1) { 384 NL_SET_BAD_ATTR(info->extack, 385 tb[ETHTOOL_A_TSCONFIG_RX_FILTERS]); 386 ret = -EINVAL; 387 goto err_free_hwprov; 388 } 389 390 hwtst_config.rx_filter = ffs(req_rx_filter) - 1; 391 } 392 393 if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS]) { 394 ret = ethnl_update_bitset32(&hwtst_config.flags, 395 __HWTSTAMP_FLAG_CNT, 396 tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS], 397 ts_flags_names, info->extack, 398 &config_mod); 399 if (ret < 0) 400 goto err_free_hwprov; 401 } 402 403 ret = net_hwtstamp_validate(&hwtst_config); 404 if (ret) 405 goto err_free_hwprov; 406 407 if (hwprov_mod) { 408 struct kernel_hwtstamp_config zero_config = {0}; 409 struct hwtstamp_provider *__hwprov; 410 411 /* Disable current time stamping if we try to enable 412 * another one 413 */ 414 ret = dev_set_hwtstamp_phylib(dev, &zero_config, info->extack); 415 if (ret < 0) 416 goto err_free_hwprov; 417 418 /* Change the selected hwtstamp source */ 419 __hwprov = rcu_replace_pointer(dev->hwprov, hwprov, 420 netdev_is_locked_ops_compat(dev)); 421 if (__hwprov) 422 kfree_rcu(__hwprov, rcu_head); 423 } 424 425 if (config_mod) { 426 ret = dev_set_hwtstamp_phylib(dev, &hwtst_config, 427 info->extack); 428 if (ret < 0) 429 return ret; 430 } 431 432 ret = tsconfig_send_reply(dev, info); 433 if (ret && ret != -EOPNOTSUPP) { 434 NL_SET_ERR_MSG(info->extack, 435 "error while reading the new configuration set"); 436 return ret; 437 } 438 439 /* tsconfig has no notification */ 440 return 0; 441 442 err_free_hwprov: 443 kfree(hwprov); 444 445 return ret; 446 } 447 448 const struct ethnl_request_ops ethnl_tsconfig_request_ops = { 449 .request_cmd = ETHTOOL_MSG_TSCONFIG_GET, 450 .reply_cmd = ETHTOOL_MSG_TSCONFIG_GET_REPLY, 451 .hdr_attr = ETHTOOL_A_TSCONFIG_HEADER, 452 .req_info_size = sizeof(struct tsconfig_req_info), 453 .reply_data_size = sizeof(struct tsconfig_reply_data), 454 455 .prepare_data = tsconfig_prepare_data, 456 .reply_size = tsconfig_reply_size, 457 .fill_reply = tsconfig_fill_reply, 458 459 .set = ethnl_set_tsconfig, 460 }; 461