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