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