1 // SPDX-License-Identifier: GPL-2.0 2 /* Copyright (C) 2022 Gerhard Engleder <gerhard@engleder-embedded.com> */ 3 4 #include "tsnep.h" 5 6 #define ETHER_TYPE_FULL_MASK ((__force __be16)~0) 7 8 static void tsnep_enable_rule(struct tsnep_adapter *adapter, 9 struct tsnep_rxnfc_rule *rule) 10 { 11 u8 rx_assign; 12 void __iomem *addr; 13 14 rx_assign = TSNEP_RX_ASSIGN_ACTIVE; 15 rx_assign |= (rule->queue_index << TSNEP_RX_ASSIGN_QUEUE_SHIFT) & 16 TSNEP_RX_ASSIGN_QUEUE_MASK; 17 18 addr = adapter->addr + TSNEP_RX_ASSIGN_ETHER_TYPE + 19 TSNEP_RX_ASSIGN_ETHER_TYPE_OFFSET * rule->location; 20 iowrite16(rule->filter.ether_type, addr); 21 22 /* enable rule after all settings are done */ 23 addr = adapter->addr + TSNEP_RX_ASSIGN + 24 TSNEP_RX_ASSIGN_OFFSET * rule->location; 25 iowrite8(rx_assign, addr); 26 } 27 28 static void tsnep_disable_rule(struct tsnep_adapter *adapter, 29 struct tsnep_rxnfc_rule *rule) 30 { 31 void __iomem *addr; 32 33 addr = adapter->addr + TSNEP_RX_ASSIGN + 34 TSNEP_RX_ASSIGN_OFFSET * rule->location; 35 iowrite8(0, addr); 36 } 37 38 static struct tsnep_rxnfc_rule *tsnep_get_rule(struct tsnep_adapter *adapter, 39 int location) 40 { 41 struct tsnep_rxnfc_rule *rule; 42 43 list_for_each_entry(rule, &adapter->rxnfc_rules, list) { 44 if (rule->location == location) 45 return rule; 46 if (rule->location > location) 47 break; 48 } 49 50 return NULL; 51 } 52 53 static void tsnep_add_rule(struct tsnep_adapter *adapter, 54 struct tsnep_rxnfc_rule *rule) 55 { 56 struct tsnep_rxnfc_rule *pred, *cur; 57 58 tsnep_enable_rule(adapter, rule); 59 60 pred = NULL; 61 list_for_each_entry(cur, &adapter->rxnfc_rules, list) { 62 if (cur->location >= rule->location) 63 break; 64 pred = cur; 65 } 66 67 list_add(&rule->list, pred ? &pred->list : &adapter->rxnfc_rules); 68 adapter->rxnfc_count++; 69 } 70 71 static void tsnep_delete_rule(struct tsnep_adapter *adapter, 72 struct tsnep_rxnfc_rule *rule) 73 { 74 tsnep_disable_rule(adapter, rule); 75 76 list_del(&rule->list); 77 adapter->rxnfc_count--; 78 79 kfree(rule); 80 } 81 82 static void tsnep_flush_rules(struct tsnep_adapter *adapter) 83 { 84 struct tsnep_rxnfc_rule *rule, *tmp; 85 86 mutex_lock(&adapter->rxnfc_lock); 87 88 list_for_each_entry_safe(rule, tmp, &adapter->rxnfc_rules, list) 89 tsnep_delete_rule(adapter, rule); 90 91 mutex_unlock(&adapter->rxnfc_lock); 92 } 93 94 int tsnep_rxnfc_get_rule(struct tsnep_adapter *adapter, 95 struct ethtool_rxnfc *cmd) 96 { 97 struct ethtool_rx_flow_spec *fsp = &cmd->fs; 98 struct tsnep_rxnfc_rule *rule = NULL; 99 100 cmd->data = adapter->rxnfc_max; 101 102 mutex_lock(&adapter->rxnfc_lock); 103 104 rule = tsnep_get_rule(adapter, fsp->location); 105 if (!rule) { 106 mutex_unlock(&adapter->rxnfc_lock); 107 108 return -ENOENT; 109 } 110 111 fsp->flow_type = ETHER_FLOW; 112 fsp->ring_cookie = rule->queue_index; 113 114 if (rule->filter.type == TSNEP_RXNFC_ETHER_TYPE) { 115 fsp->h_u.ether_spec.h_proto = htons(rule->filter.ether_type); 116 fsp->m_u.ether_spec.h_proto = ETHER_TYPE_FULL_MASK; 117 } 118 119 mutex_unlock(&adapter->rxnfc_lock); 120 121 return 0; 122 } 123 124 int tsnep_rxnfc_get_all(struct tsnep_adapter *adapter, 125 struct ethtool_rxnfc *cmd, 126 u32 *rule_locs) 127 { 128 struct tsnep_rxnfc_rule *rule; 129 int count = 0; 130 131 cmd->data = adapter->rxnfc_max; 132 133 mutex_lock(&adapter->rxnfc_lock); 134 135 list_for_each_entry(rule, &adapter->rxnfc_rules, list) { 136 if (count == cmd->rule_cnt) { 137 mutex_unlock(&adapter->rxnfc_lock); 138 139 return -EMSGSIZE; 140 } 141 142 rule_locs[count] = rule->location; 143 count++; 144 } 145 146 mutex_unlock(&adapter->rxnfc_lock); 147 148 cmd->rule_cnt = count; 149 150 return 0; 151 } 152 153 static int tsnep_rxnfc_find_location(struct tsnep_adapter *adapter) 154 { 155 struct tsnep_rxnfc_rule *tmp; 156 int location = 0; 157 158 list_for_each_entry(tmp, &adapter->rxnfc_rules, list) { 159 if (tmp->location == location) 160 location++; 161 else 162 return location; 163 } 164 165 if (location >= adapter->rxnfc_max) 166 return -ENOSPC; 167 168 return location; 169 } 170 171 static void tsnep_rxnfc_init_rule(struct tsnep_rxnfc_rule *rule, 172 const struct ethtool_rx_flow_spec *fsp) 173 { 174 INIT_LIST_HEAD(&rule->list); 175 176 rule->queue_index = fsp->ring_cookie; 177 rule->location = fsp->location; 178 179 rule->filter.type = TSNEP_RXNFC_ETHER_TYPE; 180 rule->filter.ether_type = ntohs(fsp->h_u.ether_spec.h_proto); 181 } 182 183 static int tsnep_rxnfc_check_rule(struct tsnep_adapter *adapter, 184 struct tsnep_rxnfc_rule *rule) 185 { 186 struct net_device *dev = adapter->netdev; 187 struct tsnep_rxnfc_rule *tmp; 188 189 list_for_each_entry(tmp, &adapter->rxnfc_rules, list) { 190 if (!memcmp(&rule->filter, &tmp->filter, sizeof(rule->filter)) && 191 tmp->location != rule->location) { 192 netdev_dbg(dev, "rule already exists\n"); 193 194 return -EEXIST; 195 } 196 } 197 198 return 0; 199 } 200 201 int tsnep_rxnfc_add_rule(struct tsnep_adapter *adapter, 202 struct ethtool_rxnfc *cmd) 203 { 204 struct net_device *netdev = adapter->netdev; 205 struct ethtool_rx_flow_spec *fsp = 206 (struct ethtool_rx_flow_spec *)&cmd->fs; 207 struct tsnep_rxnfc_rule *rule, *old_rule; 208 int retval; 209 210 /* only EtherType is supported */ 211 if (fsp->flow_type != ETHER_FLOW || 212 !is_zero_ether_addr(fsp->m_u.ether_spec.h_dest) || 213 !is_zero_ether_addr(fsp->m_u.ether_spec.h_source) || 214 fsp->m_u.ether_spec.h_proto != ETHER_TYPE_FULL_MASK) { 215 netdev_dbg(netdev, "only ethernet protocol is supported\n"); 216 217 return -EOPNOTSUPP; 218 } 219 220 if (fsp->ring_cookie > 221 (TSNEP_RX_ASSIGN_QUEUE_MASK >> TSNEP_RX_ASSIGN_QUEUE_SHIFT)) { 222 netdev_dbg(netdev, "invalid action\n"); 223 224 return -EINVAL; 225 } 226 227 if (fsp->location != RX_CLS_LOC_ANY && 228 fsp->location >= adapter->rxnfc_max) { 229 netdev_dbg(netdev, "invalid location\n"); 230 231 return -EINVAL; 232 } 233 234 rule = kzalloc(sizeof(*rule), GFP_KERNEL); 235 if (!rule) 236 return -ENOMEM; 237 238 mutex_lock(&adapter->rxnfc_lock); 239 240 if (fsp->location == RX_CLS_LOC_ANY) { 241 retval = tsnep_rxnfc_find_location(adapter); 242 if (retval < 0) 243 goto failed; 244 fsp->location = retval; 245 } 246 247 tsnep_rxnfc_init_rule(rule, fsp); 248 249 retval = tsnep_rxnfc_check_rule(adapter, rule); 250 if (retval) 251 goto failed; 252 253 old_rule = tsnep_get_rule(adapter, fsp->location); 254 if (old_rule) 255 tsnep_delete_rule(adapter, old_rule); 256 257 tsnep_add_rule(adapter, rule); 258 259 mutex_unlock(&adapter->rxnfc_lock); 260 261 return 0; 262 263 failed: 264 mutex_unlock(&adapter->rxnfc_lock); 265 kfree(rule); 266 return retval; 267 } 268 269 int tsnep_rxnfc_del_rule(struct tsnep_adapter *adapter, 270 struct ethtool_rxnfc *cmd) 271 { 272 struct ethtool_rx_flow_spec *fsp = 273 (struct ethtool_rx_flow_spec *)&cmd->fs; 274 struct tsnep_rxnfc_rule *rule; 275 276 mutex_lock(&adapter->rxnfc_lock); 277 278 rule = tsnep_get_rule(adapter, fsp->location); 279 if (!rule) { 280 mutex_unlock(&adapter->rxnfc_lock); 281 282 return -ENOENT; 283 } 284 285 tsnep_delete_rule(adapter, rule); 286 287 mutex_unlock(&adapter->rxnfc_lock); 288 289 return 0; 290 } 291 292 int tsnep_rxnfc_init(struct tsnep_adapter *adapter) 293 { 294 int i; 295 296 /* disable all rules */ 297 for (i = 0; i < adapter->rxnfc_max; 298 i += sizeof(u32) / TSNEP_RX_ASSIGN_OFFSET) 299 iowrite32(0, adapter->addr + TSNEP_RX_ASSIGN + i); 300 301 return 0; 302 } 303 304 void tsnep_rxnfc_cleanup(struct tsnep_adapter *adapter) 305 { 306 tsnep_flush_rules(adapter); 307 } 308