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 "netlink.h" 7 #include "common.h" 8 #include "bitset.h" 9 #include "../core/dev.h" 10 #include "ts.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 return -ENODEV; 74 75 data->hwprov_desc.index = ts_info.phc_index; 76 data->hwprov_desc.qualifier = ts_info.phc_qualifier; 77 } 78 79 out: 80 ethnl_ops_complete(dev); 81 return ret; 82 } 83 84 static int tsconfig_reply_size(const struct ethnl_req_info *req_base, 85 const struct ethnl_reply_data *reply_base) 86 { 87 const struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base); 88 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 89 int len = 0; 90 int ret; 91 92 BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32); 93 BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32); 94 BUILD_BUG_ON(__HWTSTAMP_FLAG_CNT > 32); 95 96 if (data->hwtst_config.flags) { 97 ret = ethnl_bitset32_size(&data->hwtst_config.flags, 98 NULL, __HWTSTAMP_FLAG_CNT, 99 ts_flags_names, compact); 100 if (ret < 0) 101 return ret; 102 len += ret; /* _TSCONFIG_HWTSTAMP_FLAGS */ 103 } 104 105 if (data->hwtst_config.tx_type) { 106 ret = ethnl_bitset32_size(&data->hwtst_config.tx_type, 107 NULL, __HWTSTAMP_TX_CNT, 108 ts_tx_type_names, compact); 109 if (ret < 0) 110 return ret; 111 len += ret; /* _TSCONFIG_TX_TYPES */ 112 } 113 if (data->hwtst_config.rx_filter) { 114 ret = ethnl_bitset32_size(&data->hwtst_config.rx_filter, 115 NULL, __HWTSTAMP_FILTER_CNT, 116 ts_rx_filter_names, compact); 117 if (ret < 0) 118 return ret; 119 len += ret; /* _TSCONFIG_RX_FILTERS */ 120 } 121 122 if (data->hwprov_desc.index >= 0) 123 /* _TSCONFIG_HWTSTAMP_PROVIDER */ 124 len += nla_total_size(0) + 125 2 * nla_total_size(sizeof(u32)); 126 127 return len; 128 } 129 130 static int tsconfig_fill_reply(struct sk_buff *skb, 131 const struct ethnl_req_info *req_base, 132 const struct ethnl_reply_data *reply_base) 133 { 134 const struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base); 135 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 136 int ret; 137 138 if (data->hwtst_config.flags) { 139 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS, 140 &data->hwtst_config.flags, NULL, 141 __HWTSTAMP_FLAG_CNT, 142 ts_flags_names, compact); 143 if (ret < 0) 144 return ret; 145 } 146 147 if (data->hwtst_config.tx_type) { 148 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_TX_TYPES, 149 &data->hwtst_config.tx_type, NULL, 150 __HWTSTAMP_TX_CNT, 151 ts_tx_type_names, compact); 152 if (ret < 0) 153 return ret; 154 } 155 156 if (data->hwtst_config.rx_filter) { 157 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_RX_FILTERS, 158 &data->hwtst_config.rx_filter, 159 NULL, __HWTSTAMP_FILTER_CNT, 160 ts_rx_filter_names, compact); 161 if (ret < 0) 162 return ret; 163 } 164 165 if (data->hwprov_desc.index >= 0) { 166 struct nlattr *nest; 167 168 nest = nla_nest_start(skb, ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER); 169 if (!nest) 170 return -EMSGSIZE; 171 172 if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX, 173 data->hwprov_desc.index) || 174 nla_put_u32(skb, 175 ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER, 176 data->hwprov_desc.qualifier)) { 177 nla_nest_cancel(skb, nest); 178 return -EMSGSIZE; 179 } 180 181 nla_nest_end(skb, nest); 182 } 183 return 0; 184 } 185 186 /* TSCONFIG_SET */ 187 const struct nla_policy ethnl_tsconfig_set_policy[ETHTOOL_A_TSCONFIG_MAX + 1] = { 188 [ETHTOOL_A_TSCONFIG_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy), 189 [ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER] = 190 NLA_POLICY_NESTED(ethnl_ts_hwtst_prov_policy), 191 [ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS] = { .type = NLA_NESTED }, 192 [ETHTOOL_A_TSCONFIG_RX_FILTERS] = { .type = NLA_NESTED }, 193 [ETHTOOL_A_TSCONFIG_TX_TYPES] = { .type = NLA_NESTED }, 194 }; 195 196 static int tsconfig_send_reply(struct net_device *dev, struct genl_info *info) 197 { 198 struct tsconfig_reply_data *reply_data; 199 struct tsconfig_req_info *req_info; 200 struct sk_buff *rskb; 201 void *reply_payload; 202 int reply_len = 0; 203 int ret; 204 205 req_info = kzalloc(sizeof(*req_info), GFP_KERNEL); 206 if (!req_info) 207 return -ENOMEM; 208 reply_data = kmalloc(sizeof(*reply_data), GFP_KERNEL); 209 if (!reply_data) { 210 kfree(req_info); 211 return -ENOMEM; 212 } 213 214 ASSERT_RTNL(); 215 reply_data->base.dev = dev; 216 ret = tsconfig_prepare_data(&req_info->base, &reply_data->base, info); 217 if (ret < 0) 218 goto err_cleanup; 219 220 ret = tsconfig_reply_size(&req_info->base, &reply_data->base); 221 if (ret < 0) 222 goto err_cleanup; 223 224 reply_len = ret + ethnl_reply_header_size(); 225 rskb = ethnl_reply_init(reply_len, dev, ETHTOOL_MSG_TSCONFIG_SET_REPLY, 226 ETHTOOL_A_TSCONFIG_HEADER, info, &reply_payload); 227 if (!rskb) 228 goto err_cleanup; 229 230 ret = tsconfig_fill_reply(rskb, &req_info->base, &reply_data->base); 231 if (ret < 0) 232 goto err_cleanup; 233 234 genlmsg_end(rskb, reply_payload); 235 ret = genlmsg_reply(rskb, info); 236 237 err_cleanup: 238 kfree(reply_data); 239 kfree(req_info); 240 return ret; 241 } 242 243 static int ethnl_set_tsconfig_validate(struct ethnl_req_info *req_base, 244 struct genl_info *info) 245 { 246 const struct net_device_ops *ops = req_base->dev->netdev_ops; 247 248 if (!ops->ndo_hwtstamp_set || !ops->ndo_hwtstamp_get) 249 return -EOPNOTSUPP; 250 251 return 1; 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) { 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(sizeof(*hwprov), GFP_KERNEL); 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 BUILD_BUG_ON(__HWTSTAMP_TX_CNT >= 32); 306 BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT >= 32); 307 BUILD_BUG_ON(__HWTSTAMP_FLAG_CNT > 32); 308 309 if (!netif_device_present(dev)) 310 return -ENODEV; 311 312 if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER]) { 313 struct hwtstamp_provider_desc __hwprov_desc = {.index = -1}; 314 struct hwtstamp_provider *__hwprov; 315 316 __hwprov = rtnl_dereference(dev->hwprov); 317 if (__hwprov) { 318 __hwprov_desc.index = __hwprov->desc.index; 319 __hwprov_desc.qualifier = __hwprov->desc.qualifier; 320 } 321 322 ret = ts_parse_hwtst_provider(tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER], 323 &__hwprov_desc, info->extack, 324 &hwprov_mod); 325 if (ret < 0) 326 return ret; 327 328 if (hwprov_mod) { 329 hwprov = tsconfig_set_hwprov_from_desc(dev, info, 330 &__hwprov_desc); 331 if (IS_ERR(hwprov)) 332 return PTR_ERR(hwprov); 333 } 334 } 335 336 /* Get current hwtstamp config if we are not changing the 337 * hwtstamp source. It will be zeroed in the other case. 338 */ 339 if (!hwprov_mod) { 340 ret = dev_get_hwtstamp_phylib(dev, &hwtst_config); 341 if (ret < 0 && ret != -EOPNOTSUPP) 342 goto err_free_hwprov; 343 } 344 345 /* Get the hwtstamp config from netlink */ 346 if (tb[ETHTOOL_A_TSCONFIG_TX_TYPES]) { 347 u32 req_tx_type; 348 349 req_tx_type = BIT(hwtst_config.tx_type); 350 ret = ethnl_update_bitset32(&req_tx_type, 351 __HWTSTAMP_TX_CNT, 352 tb[ETHTOOL_A_TSCONFIG_TX_TYPES], 353 ts_tx_type_names, info->extack, 354 &config_mod); 355 if (ret < 0) 356 goto err_free_hwprov; 357 358 /* Select only one tx type at a time */ 359 if (ffs(req_tx_type) != fls(req_tx_type)) { 360 ret = -EINVAL; 361 goto err_free_hwprov; 362 } 363 364 hwtst_config.tx_type = ffs(req_tx_type) - 1; 365 } 366 367 if (tb[ETHTOOL_A_TSCONFIG_RX_FILTERS]) { 368 u32 req_rx_filter; 369 370 req_rx_filter = BIT(hwtst_config.rx_filter); 371 ret = ethnl_update_bitset32(&req_rx_filter, 372 __HWTSTAMP_FILTER_CNT, 373 tb[ETHTOOL_A_TSCONFIG_RX_FILTERS], 374 ts_rx_filter_names, info->extack, 375 &config_mod); 376 if (ret < 0) 377 goto err_free_hwprov; 378 379 /* Select only one rx filter at a time */ 380 if (ffs(req_rx_filter) != fls(req_rx_filter)) { 381 ret = -EINVAL; 382 goto err_free_hwprov; 383 } 384 385 hwtst_config.rx_filter = ffs(req_rx_filter) - 1; 386 } 387 388 if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS]) { 389 ret = ethnl_update_bitset32(&hwtst_config.flags, 390 __HWTSTAMP_FLAG_CNT, 391 tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS], 392 ts_flags_names, info->extack, 393 &config_mod); 394 if (ret < 0) 395 goto err_free_hwprov; 396 } 397 398 ret = net_hwtstamp_validate(&hwtst_config); 399 if (ret) 400 goto err_free_hwprov; 401 402 if (hwprov_mod) { 403 struct kernel_hwtstamp_config zero_config = {0}; 404 struct hwtstamp_provider *__hwprov; 405 406 /* Disable current time stamping if we try to enable 407 * another one 408 */ 409 ret = dev_set_hwtstamp_phylib(dev, &zero_config, info->extack); 410 if (ret < 0) 411 goto err_free_hwprov; 412 413 /* Change the selected hwtstamp source */ 414 __hwprov = rcu_replace_pointer_rtnl(dev->hwprov, hwprov); 415 if (__hwprov) 416 kfree_rcu(__hwprov, rcu_head); 417 } 418 419 if (config_mod) { 420 ret = dev_set_hwtstamp_phylib(dev, &hwtst_config, 421 info->extack); 422 if (ret < 0) 423 return ret; 424 } 425 426 if (hwprov_mod || config_mod) { 427 ret = tsconfig_send_reply(dev, info); 428 if (ret && ret != -EOPNOTSUPP) { 429 NL_SET_ERR_MSG(info->extack, 430 "error while reading the new configuration set"); 431 return ret; 432 } 433 } 434 435 /* tsconfig has no notification */ 436 return 0; 437 438 err_free_hwprov: 439 kfree(hwprov); 440 441 return ret; 442 } 443 444 const struct ethnl_request_ops ethnl_tsconfig_request_ops = { 445 .request_cmd = ETHTOOL_MSG_TSCONFIG_GET, 446 .reply_cmd = ETHTOOL_MSG_TSCONFIG_GET_REPLY, 447 .hdr_attr = ETHTOOL_A_TSCONFIG_HEADER, 448 .req_info_size = sizeof(struct tsconfig_req_info), 449 .reply_data_size = sizeof(struct tsconfig_reply_data), 450 451 .prepare_data = tsconfig_prepare_data, 452 .reply_size = tsconfig_reply_size, 453 .fill_reply = tsconfig_fill_reply, 454 455 .set_validate = ethnl_set_tsconfig_validate, 456 .set = ethnl_set_tsconfig, 457 }; 458