1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copyright (C) 2020 Felix Fietkau <nbd@nbd.name>
4 */
5
6 #include <linux/if_ether.h>
7 #include <linux/rhashtable.h>
8 #include <linux/ip.h>
9 #include <net/flow_offload.h>
10 #include <net/pkt_cls.h>
11 #include <net/dsa.h>
12 #include "mtk_eth_soc.h"
13
14 struct mtk_flow_data {
15 struct ethhdr eth;
16
17 union {
18 struct {
19 __be32 src_addr;
20 __be32 dst_addr;
21 } v4;
22 };
23
24 __be16 src_port;
25 __be16 dst_port;
26
27 struct {
28 u16 id;
29 __be16 proto;
30 u8 num;
31 } vlan;
32 struct {
33 u16 sid;
34 u8 num;
35 } pppoe;
36 };
37
38 struct mtk_flow_entry {
39 struct rhash_head node;
40 unsigned long cookie;
41 u16 hash;
42 };
43
44 static const struct rhashtable_params mtk_flow_ht_params = {
45 .head_offset = offsetof(struct mtk_flow_entry, node),
46 .key_offset = offsetof(struct mtk_flow_entry, cookie),
47 .key_len = sizeof(unsigned long),
48 .automatic_shrinking = true,
49 };
50
51 static u32
mtk_eth_timestamp(struct mtk_eth * eth)52 mtk_eth_timestamp(struct mtk_eth *eth)
53 {
54 return mtk_r32(eth, 0x0010) & MTK_FOE_IB1_BIND_TIMESTAMP;
55 }
56
57 static int
mtk_flow_set_ipv4_addr(struct mtk_foe_entry * foe,struct mtk_flow_data * data,bool egress)58 mtk_flow_set_ipv4_addr(struct mtk_foe_entry *foe, struct mtk_flow_data *data,
59 bool egress)
60 {
61 return mtk_foe_entry_set_ipv4_tuple(foe, egress,
62 data->v4.src_addr, data->src_port,
63 data->v4.dst_addr, data->dst_port);
64 }
65
66 static void
mtk_flow_offload_mangle_eth(const struct flow_action_entry * act,void * eth)67 mtk_flow_offload_mangle_eth(const struct flow_action_entry *act, void *eth)
68 {
69 void *dest = eth + act->mangle.offset;
70 const void *src = &act->mangle.val;
71
72 if (act->mangle.offset > 8)
73 return;
74
75 if (act->mangle.mask == 0xffff) {
76 src += 2;
77 dest += 2;
78 }
79
80 memcpy(dest, src, act->mangle.mask ? 2 : 4);
81 }
82
83
84 static int
mtk_flow_mangle_ports(const struct flow_action_entry * act,struct mtk_flow_data * data)85 mtk_flow_mangle_ports(const struct flow_action_entry *act,
86 struct mtk_flow_data *data)
87 {
88 u32 val = ntohl(act->mangle.val);
89
90 switch (act->mangle.offset) {
91 case 0:
92 if (act->mangle.mask == ~htonl(0xffff))
93 data->dst_port = cpu_to_be16(val);
94 else
95 data->src_port = cpu_to_be16(val >> 16);
96 break;
97 case 2:
98 data->dst_port = cpu_to_be16(val);
99 break;
100 default:
101 return -EINVAL;
102 }
103
104 return 0;
105 }
106
107 static int
mtk_flow_mangle_ipv4(const struct flow_action_entry * act,struct mtk_flow_data * data)108 mtk_flow_mangle_ipv4(const struct flow_action_entry *act,
109 struct mtk_flow_data *data)
110 {
111 __be32 *dest;
112
113 switch (act->mangle.offset) {
114 case offsetof(struct iphdr, saddr):
115 dest = &data->v4.src_addr;
116 break;
117 case offsetof(struct iphdr, daddr):
118 dest = &data->v4.dst_addr;
119 break;
120 default:
121 return -EINVAL;
122 }
123
124 memcpy(dest, &act->mangle.val, sizeof(u32));
125
126 return 0;
127 }
128
129 static int
mtk_flow_get_dsa_port(struct net_device ** dev)130 mtk_flow_get_dsa_port(struct net_device **dev)
131 {
132 #if IS_ENABLED(CONFIG_NET_DSA)
133 struct dsa_port *dp;
134
135 dp = dsa_port_from_netdev(*dev);
136 if (IS_ERR(dp))
137 return -ENODEV;
138
139 if (dp->cpu_dp->tag_ops->proto != DSA_TAG_PROTO_MTK)
140 return -ENODEV;
141
142 *dev = dp->cpu_dp->master;
143
144 return dp->index;
145 #else
146 return -ENODEV;
147 #endif
148 }
149
150 static int
mtk_flow_set_output_device(struct mtk_eth * eth,struct mtk_foe_entry * foe,struct net_device * dev)151 mtk_flow_set_output_device(struct mtk_eth *eth, struct mtk_foe_entry *foe,
152 struct net_device *dev)
153 {
154 int pse_port, dsa_port;
155
156 dsa_port = mtk_flow_get_dsa_port(&dev);
157 if (dsa_port >= 0)
158 mtk_foe_entry_set_dsa(foe, dsa_port);
159
160 if (dev == eth->netdev[0])
161 pse_port = 1;
162 else if (dev == eth->netdev[1])
163 pse_port = 2;
164 else
165 return -EOPNOTSUPP;
166
167 mtk_foe_entry_set_pse_port(foe, pse_port);
168
169 return 0;
170 }
171
172 static int
mtk_flow_offload_replace(struct mtk_eth * eth,struct flow_cls_offload * f)173 mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
174 {
175 struct flow_rule *rule = flow_cls_offload_flow_rule(f);
176 struct flow_action_entry *act;
177 struct mtk_flow_data data = {};
178 struct mtk_foe_entry foe;
179 struct net_device *odev = NULL;
180 struct mtk_flow_entry *entry;
181 int offload_type = 0;
182 u16 addr_type = 0;
183 u32 timestamp;
184 u8 l4proto = 0;
185 int err = 0;
186 int hash;
187 int i;
188
189 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_META)) {
190 struct flow_match_meta match;
191
192 flow_rule_match_meta(rule, &match);
193 } else {
194 return -EOPNOTSUPP;
195 }
196
197 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) {
198 struct flow_match_control match;
199
200 flow_rule_match_control(rule, &match);
201 addr_type = match.key->addr_type;
202 } else {
203 return -EOPNOTSUPP;
204 }
205
206 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
207 struct flow_match_basic match;
208
209 flow_rule_match_basic(rule, &match);
210 l4proto = match.key->ip_proto;
211 } else {
212 return -EOPNOTSUPP;
213 }
214
215 flow_action_for_each(i, act, &rule->action) {
216 switch (act->id) {
217 case FLOW_ACTION_MANGLE:
218 if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH)
219 mtk_flow_offload_mangle_eth(act, &data.eth);
220 break;
221 case FLOW_ACTION_REDIRECT:
222 odev = act->dev;
223 break;
224 case FLOW_ACTION_CSUM:
225 break;
226 case FLOW_ACTION_VLAN_PUSH:
227 if (data.vlan.num == 1 ||
228 act->vlan.proto != htons(ETH_P_8021Q))
229 return -EOPNOTSUPP;
230
231 data.vlan.id = act->vlan.vid;
232 data.vlan.proto = act->vlan.proto;
233 data.vlan.num++;
234 break;
235 case FLOW_ACTION_VLAN_POP:
236 break;
237 case FLOW_ACTION_PPPOE_PUSH:
238 if (data.pppoe.num == 1)
239 return -EOPNOTSUPP;
240
241 data.pppoe.sid = act->pppoe.sid;
242 data.pppoe.num++;
243 break;
244 default:
245 return -EOPNOTSUPP;
246 }
247 }
248
249 switch (addr_type) {
250 case FLOW_DISSECTOR_KEY_IPV4_ADDRS:
251 offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT;
252 break;
253 default:
254 return -EOPNOTSUPP;
255 }
256
257 if (!is_valid_ether_addr(data.eth.h_source) ||
258 !is_valid_ether_addr(data.eth.h_dest))
259 return -EINVAL;
260
261 err = mtk_foe_entry_prepare(&foe, offload_type, l4proto, 0,
262 data.eth.h_source,
263 data.eth.h_dest);
264 if (err)
265 return err;
266
267 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) {
268 struct flow_match_ports ports;
269
270 flow_rule_match_ports(rule, &ports);
271 data.src_port = ports.key->src;
272 data.dst_port = ports.key->dst;
273 } else {
274 return -EOPNOTSUPP;
275 }
276
277 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
278 struct flow_match_ipv4_addrs addrs;
279
280 flow_rule_match_ipv4_addrs(rule, &addrs);
281
282 data.v4.src_addr = addrs.key->src;
283 data.v4.dst_addr = addrs.key->dst;
284
285 mtk_flow_set_ipv4_addr(&foe, &data, false);
286 }
287
288 flow_action_for_each(i, act, &rule->action) {
289 if (act->id != FLOW_ACTION_MANGLE)
290 continue;
291
292 switch (act->mangle.htype) {
293 case FLOW_ACT_MANGLE_HDR_TYPE_TCP:
294 case FLOW_ACT_MANGLE_HDR_TYPE_UDP:
295 err = mtk_flow_mangle_ports(act, &data);
296 break;
297 case FLOW_ACT_MANGLE_HDR_TYPE_IP4:
298 err = mtk_flow_mangle_ipv4(act, &data);
299 break;
300 case FLOW_ACT_MANGLE_HDR_TYPE_ETH:
301 /* handled earlier */
302 break;
303 default:
304 return -EOPNOTSUPP;
305 }
306
307 if (err)
308 return err;
309 }
310
311 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
312 err = mtk_flow_set_ipv4_addr(&foe, &data, true);
313 if (err)
314 return err;
315 }
316
317 if (data.vlan.num == 1) {
318 if (data.vlan.proto != htons(ETH_P_8021Q))
319 return -EOPNOTSUPP;
320
321 mtk_foe_entry_set_vlan(&foe, data.vlan.id);
322 }
323 if (data.pppoe.num == 1)
324 mtk_foe_entry_set_pppoe(&foe, data.pppoe.sid);
325
326 err = mtk_flow_set_output_device(eth, &foe, odev);
327 if (err)
328 return err;
329
330 entry = kzalloc(sizeof(*entry), GFP_KERNEL);
331 if (!entry)
332 return -ENOMEM;
333
334 entry->cookie = f->cookie;
335 timestamp = mtk_eth_timestamp(eth);
336 hash = mtk_foe_entry_commit(ð->ppe, &foe, timestamp);
337 if (hash < 0) {
338 err = hash;
339 goto free;
340 }
341
342 entry->hash = hash;
343 err = rhashtable_insert_fast(ð->flow_table, &entry->node,
344 mtk_flow_ht_params);
345 if (err < 0)
346 goto clear_flow;
347
348 return 0;
349 clear_flow:
350 mtk_foe_entry_clear(ð->ppe, hash);
351 free:
352 kfree(entry);
353 return err;
354 }
355
356 static int
mtk_flow_offload_destroy(struct mtk_eth * eth,struct flow_cls_offload * f)357 mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
358 {
359 struct mtk_flow_entry *entry;
360
361 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
362 mtk_flow_ht_params);
363 if (!entry)
364 return -ENOENT;
365
366 mtk_foe_entry_clear(ð->ppe, entry->hash);
367 rhashtable_remove_fast(ð->flow_table, &entry->node,
368 mtk_flow_ht_params);
369 kfree(entry);
370
371 return 0;
372 }
373
374 static int
mtk_flow_offload_stats(struct mtk_eth * eth,struct flow_cls_offload * f)375 mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f)
376 {
377 struct mtk_flow_entry *entry;
378 int timestamp;
379 u32 idle;
380
381 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
382 mtk_flow_ht_params);
383 if (!entry)
384 return -ENOENT;
385
386 timestamp = mtk_foe_entry_timestamp(ð->ppe, entry->hash);
387 if (timestamp < 0)
388 return -ETIMEDOUT;
389
390 idle = mtk_eth_timestamp(eth) - timestamp;
391 f->stats.lastused = jiffies - idle * HZ;
392
393 return 0;
394 }
395
396 static DEFINE_MUTEX(mtk_flow_offload_mutex);
397
398 static int
mtk_eth_setup_tc_block_cb(enum tc_setup_type type,void * type_data,void * cb_priv)399 mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
400 {
401 struct flow_cls_offload *cls = type_data;
402 struct net_device *dev = cb_priv;
403 struct mtk_mac *mac = netdev_priv(dev);
404 struct mtk_eth *eth = mac->hw;
405 int err;
406
407 if (!tc_can_offload(dev))
408 return -EOPNOTSUPP;
409
410 if (type != TC_SETUP_CLSFLOWER)
411 return -EOPNOTSUPP;
412
413 mutex_lock(&mtk_flow_offload_mutex);
414 switch (cls->command) {
415 case FLOW_CLS_REPLACE:
416 err = mtk_flow_offload_replace(eth, cls);
417 break;
418 case FLOW_CLS_DESTROY:
419 err = mtk_flow_offload_destroy(eth, cls);
420 break;
421 case FLOW_CLS_STATS:
422 err = mtk_flow_offload_stats(eth, cls);
423 break;
424 default:
425 err = -EOPNOTSUPP;
426 break;
427 }
428 mutex_unlock(&mtk_flow_offload_mutex);
429
430 return err;
431 }
432
433 static int
mtk_eth_setup_tc_block(struct net_device * dev,struct flow_block_offload * f)434 mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f)
435 {
436 struct mtk_mac *mac = netdev_priv(dev);
437 struct mtk_eth *eth = mac->hw;
438 static LIST_HEAD(block_cb_list);
439 struct flow_block_cb *block_cb;
440 flow_setup_cb_t *cb;
441
442 if (!eth->ppe.foe_table)
443 return -EOPNOTSUPP;
444
445 if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
446 return -EOPNOTSUPP;
447
448 cb = mtk_eth_setup_tc_block_cb;
449 f->driver_block_list = &block_cb_list;
450
451 switch (f->command) {
452 case FLOW_BLOCK_BIND:
453 block_cb = flow_block_cb_lookup(f->block, cb, dev);
454 if (block_cb) {
455 flow_block_cb_incref(block_cb);
456 return 0;
457 }
458 block_cb = flow_block_cb_alloc(cb, dev, dev, NULL);
459 if (IS_ERR(block_cb))
460 return PTR_ERR(block_cb);
461
462 flow_block_cb_add(block_cb, f);
463 list_add_tail(&block_cb->driver_list, &block_cb_list);
464 return 0;
465 case FLOW_BLOCK_UNBIND:
466 block_cb = flow_block_cb_lookup(f->block, cb, dev);
467 if (!block_cb)
468 return -ENOENT;
469
470 if (flow_block_cb_decref(block_cb)) {
471 flow_block_cb_remove(block_cb, f);
472 list_del(&block_cb->driver_list);
473 }
474 return 0;
475 default:
476 return -EOPNOTSUPP;
477 }
478 }
479
mtk_eth_setup_tc(struct net_device * dev,enum tc_setup_type type,void * type_data)480 int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
481 void *type_data)
482 {
483 if (type == TC_SETUP_FT)
484 return mtk_eth_setup_tc_block(dev, type_data);
485
486 return -EOPNOTSUPP;
487 }
488
mtk_eth_offload_init(struct mtk_eth * eth)489 int mtk_eth_offload_init(struct mtk_eth *eth)
490 {
491 if (!eth->ppe.foe_table)
492 return 0;
493
494 return rhashtable_init(ð->flow_table, &mtk_flow_ht_params);
495 }
496