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