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 (!req_base->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(req_base->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 const struct ethtool_phy_ops *ops; 145 struct nlattr **tb = info->attrs; 146 struct phy_plca_cfg plca_cfg; 147 bool mod = false; 148 int ret; 149 150 // check that the PHY device is available and connected 151 if (!req_info->phydev) 152 return -EOPNOTSUPP; 153 154 ops = ethtool_phy_ops; 155 if (!ops || !ops->set_plca_cfg) 156 return -EOPNOTSUPP; 157 158 memset(&plca_cfg, 0xff, sizeof(plca_cfg)); 159 plca_update_sint(&plca_cfg.enabled, tb, ETHTOOL_A_PLCA_ENABLED, &mod); 160 plca_update_sint(&plca_cfg.node_id, tb, ETHTOOL_A_PLCA_NODE_ID, &mod); 161 plca_update_sint(&plca_cfg.node_cnt, tb, ETHTOOL_A_PLCA_NODE_CNT, &mod); 162 plca_update_sint(&plca_cfg.to_tmr, tb, ETHTOOL_A_PLCA_TO_TMR, &mod); 163 plca_update_sint(&plca_cfg.burst_cnt, tb, ETHTOOL_A_PLCA_BURST_CNT, 164 &mod); 165 plca_update_sint(&plca_cfg.burst_tmr, tb, ETHTOOL_A_PLCA_BURST_TMR, 166 &mod); 167 if (!mod) 168 return 0; 169 170 ret = ops->set_plca_cfg(req_info->phydev, &plca_cfg, info->extack); 171 return ret < 0 ? ret : 1; 172 } 173 174 const struct ethnl_request_ops ethnl_plca_cfg_request_ops = { 175 .request_cmd = ETHTOOL_MSG_PLCA_GET_CFG, 176 .reply_cmd = ETHTOOL_MSG_PLCA_GET_CFG_REPLY, 177 .hdr_attr = ETHTOOL_A_PLCA_HEADER, 178 .req_info_size = sizeof(struct plca_req_info), 179 .reply_data_size = sizeof(struct plca_reply_data), 180 181 .prepare_data = plca_get_cfg_prepare_data, 182 .reply_size = plca_get_cfg_reply_size, 183 .fill_reply = plca_get_cfg_fill_reply, 184 185 .set = ethnl_set_plca, 186 .set_ntf_cmd = ETHTOOL_MSG_PLCA_NTF, 187 }; 188 189 // PLCA get status message -------------------------------------------------- // 190 191 const struct nla_policy ethnl_plca_get_status_policy[] = { 192 [ETHTOOL_A_PLCA_HEADER] = 193 NLA_POLICY_NESTED(ethnl_header_policy), 194 }; 195 196 static int plca_get_status_prepare_data(const struct ethnl_req_info *req_base, 197 struct ethnl_reply_data *reply_base, 198 const struct genl_info *info) 199 { 200 struct plca_reply_data *data = PLCA_REPDATA(reply_base); 201 struct net_device *dev = reply_base->dev; 202 const struct ethtool_phy_ops *ops; 203 int ret; 204 205 // check that the PHY device is available and connected 206 if (!req_base->phydev) { 207 ret = -EOPNOTSUPP; 208 goto out; 209 } 210 211 // note: rtnl_lock is held already by ethnl_default_doit 212 ops = ethtool_phy_ops; 213 if (!ops || !ops->get_plca_status) { 214 ret = -EOPNOTSUPP; 215 goto out; 216 } 217 218 ret = ethnl_ops_begin(dev); 219 if (ret < 0) 220 goto out; 221 222 memset(&data->plca_st, 0xff, 223 sizeof_field(struct plca_reply_data, plca_st)); 224 225 ret = ops->get_plca_status(req_base->phydev, &data->plca_st); 226 ethnl_ops_complete(dev); 227 out: 228 return ret; 229 } 230 231 static int plca_get_status_reply_size(const struct ethnl_req_info *req_base, 232 const struct ethnl_reply_data *reply_base) 233 { 234 return nla_total_size(sizeof(u8)); /* _STATUS */ 235 } 236 237 static int plca_get_status_fill_reply(struct sk_buff *skb, 238 const struct ethnl_req_info *req_base, 239 const struct ethnl_reply_data *reply_base) 240 { 241 const struct plca_reply_data *data = PLCA_REPDATA(reply_base); 242 const u8 status = data->plca_st.pst; 243 244 if (nla_put_u8(skb, ETHTOOL_A_PLCA_STATUS, !!status)) 245 return -EMSGSIZE; 246 247 return 0; 248 }; 249 250 const struct ethnl_request_ops ethnl_plca_status_request_ops = { 251 .request_cmd = ETHTOOL_MSG_PLCA_GET_STATUS, 252 .reply_cmd = ETHTOOL_MSG_PLCA_GET_STATUS_REPLY, 253 .hdr_attr = ETHTOOL_A_PLCA_HEADER, 254 .req_info_size = sizeof(struct plca_req_info), 255 .reply_data_size = sizeof(struct plca_reply_data), 256 257 .prepare_data = plca_get_status_prepare_data, 258 .reply_size = plca_get_status_reply_size, 259 .fill_reply = plca_get_status_fill_reply, 260 }; 261