1 // SPDX-License-Identifier: GPL-2.0-only 2 3 #include <linux/phy.h> 4 #include <linux/ethtool_netlink.h> 5 6 #include "netlink.h" 7 #include "common.h" 8 9 struct plca_req_info { 10 struct ethnl_req_info base; 11 }; 12 13 struct plca_reply_data { 14 struct ethnl_reply_data base; 15 struct phy_plca_cfg plca_cfg; 16 struct phy_plca_status plca_st; 17 }; 18 19 // Helpers ------------------------------------------------------------------ // 20 21 #define PLCA_REPDATA(__reply_base) \ 22 container_of(__reply_base, struct plca_reply_data, base) 23 24 static void plca_update_sint(int *dst, const struct nlattr *attr, 25 bool *mod) 26 { 27 if (!attr) 28 return; 29 30 *dst = nla_get_u32(attr); 31 *mod = true; 32 } 33 34 // PLCA get configuration message ------------------------------------------- // 35 36 const struct nla_policy ethnl_plca_get_cfg_policy[] = { 37 [ETHTOOL_A_PLCA_HEADER] = 38 NLA_POLICY_NESTED(ethnl_header_policy), 39 }; 40 41 static int plca_get_cfg_prepare_data(const struct ethnl_req_info *req_base, 42 struct ethnl_reply_data *reply_base, 43 struct genl_info *info) 44 { 45 struct plca_reply_data *data = PLCA_REPDATA(reply_base); 46 struct net_device *dev = reply_base->dev; 47 const struct ethtool_phy_ops *ops; 48 int ret; 49 50 // check that the PHY device is available and connected 51 if (!dev->phydev) { 52 ret = -EOPNOTSUPP; 53 goto out; 54 } 55 56 // note: rtnl_lock is held already by ethnl_default_doit 57 ops = ethtool_phy_ops; 58 if (!ops || !ops->get_plca_cfg) { 59 ret = -EOPNOTSUPP; 60 goto out; 61 } 62 63 ret = ethnl_ops_begin(dev); 64 if (ret < 0) 65 goto out; 66 67 memset(&data->plca_cfg, 0xff, 68 sizeof_field(struct plca_reply_data, plca_cfg)); 69 70 ret = ops->get_plca_cfg(dev->phydev, &data->plca_cfg); 71 ethnl_ops_complete(dev); 72 73 out: 74 return ret; 75 } 76 77 static int plca_get_cfg_reply_size(const struct ethnl_req_info *req_base, 78 const struct ethnl_reply_data *reply_base) 79 { 80 return nla_total_size(sizeof(u16)) + /* _VERSION */ 81 nla_total_size(sizeof(u8)) + /* _ENABLED */ 82 nla_total_size(sizeof(u32)) + /* _NODE_CNT */ 83 nla_total_size(sizeof(u32)) + /* _NODE_ID */ 84 nla_total_size(sizeof(u32)) + /* _TO_TIMER */ 85 nla_total_size(sizeof(u32)) + /* _BURST_COUNT */ 86 nla_total_size(sizeof(u32)); /* _BURST_TIMER */ 87 } 88 89 static int plca_get_cfg_fill_reply(struct sk_buff *skb, 90 const struct ethnl_req_info *req_base, 91 const struct ethnl_reply_data *reply_base) 92 { 93 const struct plca_reply_data *data = PLCA_REPDATA(reply_base); 94 const struct phy_plca_cfg *plca = &data->plca_cfg; 95 96 if ((plca->version >= 0 && 97 nla_put_u16(skb, ETHTOOL_A_PLCA_VERSION, plca->version)) || 98 (plca->enabled >= 0 && 99 nla_put_u8(skb, ETHTOOL_A_PLCA_ENABLED, !!plca->enabled)) || 100 (plca->node_id >= 0 && 101 nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_ID, plca->node_id)) || 102 (plca->node_cnt >= 0 && 103 nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_CNT, plca->node_cnt)) || 104 (plca->to_tmr >= 0 && 105 nla_put_u32(skb, ETHTOOL_A_PLCA_TO_TMR, plca->to_tmr)) || 106 (plca->burst_cnt >= 0 && 107 nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_CNT, plca->burst_cnt)) || 108 (plca->burst_tmr >= 0 && 109 nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_TMR, plca->burst_tmr))) 110 return -EMSGSIZE; 111 112 return 0; 113 }; 114 115 const struct ethnl_request_ops ethnl_plca_cfg_request_ops = { 116 .request_cmd = ETHTOOL_MSG_PLCA_GET_CFG, 117 .reply_cmd = ETHTOOL_MSG_PLCA_GET_CFG_REPLY, 118 .hdr_attr = ETHTOOL_A_PLCA_HEADER, 119 .req_info_size = sizeof(struct plca_req_info), 120 .reply_data_size = sizeof(struct plca_reply_data), 121 122 .prepare_data = plca_get_cfg_prepare_data, 123 .reply_size = plca_get_cfg_reply_size, 124 .fill_reply = plca_get_cfg_fill_reply, 125 }; 126 127 // PLCA set configuration message ------------------------------------------- // 128 129 const struct nla_policy ethnl_plca_set_cfg_policy[] = { 130 [ETHTOOL_A_PLCA_HEADER] = 131 NLA_POLICY_NESTED(ethnl_header_policy), 132 [ETHTOOL_A_PLCA_ENABLED] = NLA_POLICY_MAX(NLA_U8, 1), 133 [ETHTOOL_A_PLCA_NODE_ID] = NLA_POLICY_MAX(NLA_U32, 255), 134 [ETHTOOL_A_PLCA_NODE_CNT] = NLA_POLICY_RANGE(NLA_U32, 1, 255), 135 [ETHTOOL_A_PLCA_TO_TMR] = NLA_POLICY_MAX(NLA_U32, 255), 136 [ETHTOOL_A_PLCA_BURST_CNT] = NLA_POLICY_MAX(NLA_U32, 255), 137 [ETHTOOL_A_PLCA_BURST_TMR] = NLA_POLICY_MAX(NLA_U32, 255), 138 }; 139 140 int ethnl_set_plca_cfg(struct sk_buff *skb, struct genl_info *info) 141 { 142 struct ethnl_req_info req_info = {}; 143 struct nlattr **tb = info->attrs; 144 const struct ethtool_phy_ops *ops; 145 struct phy_plca_cfg plca_cfg; 146 struct net_device *dev; 147 bool mod = false; 148 int ret; 149 150 ret = ethnl_parse_header_dev_get(&req_info, 151 tb[ETHTOOL_A_PLCA_HEADER], 152 genl_info_net(info), info->extack, 153 true); 154 if (ret < 0) 155 return ret; 156 157 dev = req_info.dev; 158 159 rtnl_lock(); 160 161 // check that the PHY device is available and connected 162 if (!dev->phydev) { 163 ret = -EOPNOTSUPP; 164 goto out_rtnl; 165 } 166 167 ops = ethtool_phy_ops; 168 if (!ops || !ops->set_plca_cfg) { 169 ret = -EOPNOTSUPP; 170 goto out_rtnl; 171 } 172 173 ret = ethnl_ops_begin(dev); 174 if (ret < 0) 175 goto out_rtnl; 176 177 memset(&plca_cfg, 0xff, sizeof(plca_cfg)); 178 plca_update_sint(&plca_cfg.enabled, tb[ETHTOOL_A_PLCA_ENABLED], &mod); 179 plca_update_sint(&plca_cfg.node_id, tb[ETHTOOL_A_PLCA_NODE_ID], &mod); 180 plca_update_sint(&plca_cfg.node_cnt, tb[ETHTOOL_A_PLCA_NODE_CNT], &mod); 181 plca_update_sint(&plca_cfg.to_tmr, tb[ETHTOOL_A_PLCA_TO_TMR], &mod); 182 plca_update_sint(&plca_cfg.burst_cnt, tb[ETHTOOL_A_PLCA_BURST_CNT], 183 &mod); 184 plca_update_sint(&plca_cfg.burst_tmr, tb[ETHTOOL_A_PLCA_BURST_TMR], 185 &mod); 186 187 ret = 0; 188 if (!mod) 189 goto out_ops; 190 191 ret = ops->set_plca_cfg(dev->phydev, &plca_cfg, info->extack); 192 if (ret < 0) 193 goto out_ops; 194 195 ethtool_notify(dev, ETHTOOL_MSG_PLCA_NTF, NULL); 196 197 out_ops: 198 ethnl_ops_complete(dev); 199 out_rtnl: 200 rtnl_unlock(); 201 ethnl_parse_header_dev_put(&req_info); 202 203 return ret; 204 } 205 206 // PLCA get status message -------------------------------------------------- // 207 208 const struct nla_policy ethnl_plca_get_status_policy[] = { 209 [ETHTOOL_A_PLCA_HEADER] = 210 NLA_POLICY_NESTED(ethnl_header_policy), 211 }; 212 213 static int plca_get_status_prepare_data(const struct ethnl_req_info *req_base, 214 struct ethnl_reply_data *reply_base, 215 struct genl_info *info) 216 { 217 struct plca_reply_data *data = PLCA_REPDATA(reply_base); 218 struct net_device *dev = reply_base->dev; 219 const struct ethtool_phy_ops *ops; 220 int ret; 221 222 // check that the PHY device is available and connected 223 if (!dev->phydev) { 224 ret = -EOPNOTSUPP; 225 goto out; 226 } 227 228 // note: rtnl_lock is held already by ethnl_default_doit 229 ops = ethtool_phy_ops; 230 if (!ops || !ops->get_plca_status) { 231 ret = -EOPNOTSUPP; 232 goto out; 233 } 234 235 ret = ethnl_ops_begin(dev); 236 if (ret < 0) 237 goto out; 238 239 memset(&data->plca_st, 0xff, 240 sizeof_field(struct plca_reply_data, plca_st)); 241 242 ret = ops->get_plca_status(dev->phydev, &data->plca_st); 243 ethnl_ops_complete(dev); 244 out: 245 return ret; 246 } 247 248 static int plca_get_status_reply_size(const struct ethnl_req_info *req_base, 249 const struct ethnl_reply_data *reply_base) 250 { 251 return nla_total_size(sizeof(u8)); /* _STATUS */ 252 } 253 254 static int plca_get_status_fill_reply(struct sk_buff *skb, 255 const struct ethnl_req_info *req_base, 256 const struct ethnl_reply_data *reply_base) 257 { 258 const struct plca_reply_data *data = PLCA_REPDATA(reply_base); 259 const u8 status = data->plca_st.pst; 260 261 if (nla_put_u8(skb, ETHTOOL_A_PLCA_STATUS, !!status)) 262 return -EMSGSIZE; 263 264 return 0; 265 }; 266 267 const struct ethnl_request_ops ethnl_plca_status_request_ops = { 268 .request_cmd = ETHTOOL_MSG_PLCA_GET_STATUS, 269 .reply_cmd = ETHTOOL_MSG_PLCA_GET_STATUS_REPLY, 270 .hdr_attr = ETHTOOL_A_PLCA_HEADER, 271 .req_info_size = sizeof(struct plca_req_info), 272 .reply_data_size = sizeof(struct plca_reply_data), 273 274 .prepare_data = plca_get_status_prepare_data, 275 .reply_size = plca_get_status_reply_size, 276 .fill_reply = plca_get_status_fill_reply, 277 }; 278