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