308 lines · c
1// SPDX-License-Identifier: GPL-2.02/* 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 8static 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 28static 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 38static 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 53static 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 71static 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 82static 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 94int 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 124int 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 153static 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 else162 return location;163 }164 165 if (location >= adapter->rxnfc_max)166 return -ENOSPC;167 168 return location;169}170 171static 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 183static 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 201int 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 263failed:264 mutex_unlock(&adapter->rxnfc_lock);265 kfree(rule);266 return retval;267}268 269int 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 292int 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 304void tsnep_rxnfc_cleanup(struct tsnep_adapter *adapter)305{306 tsnep_flush_rules(adapter);307}308