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