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