1
2
3
4
5
6
7
8
9#include <linux/etherdevice.h>
10#include <linux/if_bridge.h>
11#include <linux/netdevice.h>
12#include <linux/workqueue.h>
13#include <net/switchdev.h>
14
15#include "cpsw.h"
16#include "cpsw_ale.h"
17#include "cpsw_priv.h"
18#include "cpsw_switchdev.h"
19
20struct cpsw_switchdev_event_work {
21 struct work_struct work;
22 struct switchdev_notifier_fdb_info fdb_info;
23 struct cpsw_priv *priv;
24 unsigned long event;
25};
26
27static int cpsw_port_stp_state_set(struct cpsw_priv *priv, u8 state)
28{
29 struct cpsw_common *cpsw = priv->cpsw;
30 u8 cpsw_state;
31 int ret = 0;
32
33 switch (state) {
34 case BR_STATE_FORWARDING:
35 cpsw_state = ALE_PORT_STATE_FORWARD;
36 break;
37 case BR_STATE_LEARNING:
38 cpsw_state = ALE_PORT_STATE_LEARN;
39 break;
40 case BR_STATE_DISABLED:
41 cpsw_state = ALE_PORT_STATE_DISABLE;
42 break;
43 case BR_STATE_LISTENING:
44 case BR_STATE_BLOCKING:
45 cpsw_state = ALE_PORT_STATE_BLOCK;
46 break;
47 default:
48 return -EOPNOTSUPP;
49 }
50
51 ret = cpsw_ale_control_set(cpsw->ale, priv->emac_port,
52 ALE_PORT_STATE, cpsw_state);
53 dev_dbg(priv->dev, "ale state: %u\n", cpsw_state);
54
55 return ret;
56}
57
58static int cpsw_port_attr_br_flags_set(struct cpsw_priv *priv,
59 struct net_device *orig_dev,
60 struct switchdev_brport_flags flags)
61{
62 struct cpsw_common *cpsw = priv->cpsw;
63
64 if (flags.mask & BR_MCAST_FLOOD) {
65 bool unreg_mcast_add = false;
66
67 if (flags.val & BR_MCAST_FLOOD)
68 unreg_mcast_add = true;
69
70 dev_dbg(priv->dev, "BR_MCAST_FLOOD: %d port %u\n",
71 unreg_mcast_add, priv->emac_port);
72
73 cpsw_ale_set_unreg_mcast(cpsw->ale, BIT(priv->emac_port),
74 unreg_mcast_add);
75 }
76
77 return 0;
78}
79
80static int cpsw_port_attr_br_flags_pre_set(struct net_device *netdev,
81 struct switchdev_brport_flags flags)
82{
83 if (flags.mask & ~(BR_LEARNING | BR_MCAST_FLOOD))
84 return -EINVAL;
85
86 return 0;
87}
88
89static int cpsw_port_attr_set(struct net_device *ndev, const void *ctx,
90 const struct switchdev_attr *attr,
91 struct netlink_ext_ack *extack)
92{
93 struct cpsw_priv *priv = netdev_priv(ndev);
94 int ret;
95
96 dev_dbg(priv->dev, "attr: id %u port: %u\n", attr->id, priv->emac_port);
97
98 switch (attr->id) {
99 case SWITCHDEV_ATTR_ID_PORT_PRE_BRIDGE_FLAGS:
100 ret = cpsw_port_attr_br_flags_pre_set(ndev,
101 attr->u.brport_flags);
102 break;
103 case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
104 ret = cpsw_port_stp_state_set(priv, attr->u.stp_state);
105 dev_dbg(priv->dev, "stp state: %u\n", attr->u.stp_state);
106 break;
107 case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
108 ret = cpsw_port_attr_br_flags_set(priv, attr->orig_dev,
109 attr->u.brport_flags);
110 break;
111 default:
112 ret = -EOPNOTSUPP;
113 break;
114 }
115
116 return ret;
117}
118
119static u16 cpsw_get_pvid(struct cpsw_priv *priv)
120{
121 struct cpsw_common *cpsw = priv->cpsw;
122 u32 __iomem *port_vlan_reg;
123 u32 pvid;
124
125 if (priv->emac_port) {
126 int reg = CPSW2_PORT_VLAN;
127
128 if (cpsw->version == CPSW_VERSION_1)
129 reg = CPSW1_PORT_VLAN;
130 pvid = slave_read(cpsw->slaves + (priv->emac_port - 1), reg);
131 } else {
132 port_vlan_reg = &cpsw->host_port_regs->port_vlan;
133 pvid = readl(port_vlan_reg);
134 }
135
136 pvid = pvid & 0xfff;
137
138 return pvid;
139}
140
141static void cpsw_set_pvid(struct cpsw_priv *priv, u16 vid, bool cfi, u32 cos)
142{
143 struct cpsw_common *cpsw = priv->cpsw;
144 void __iomem *port_vlan_reg;
145 u32 pvid;
146
147 pvid = vid;
148 pvid |= cfi ? BIT(12) : 0;
149 pvid |= (cos & 0x7) << 13;
150
151 if (priv->emac_port) {
152 int reg = CPSW2_PORT_VLAN;
153
154 if (cpsw->version == CPSW_VERSION_1)
155 reg = CPSW1_PORT_VLAN;
156
157 slave_write(cpsw->slaves + (priv->emac_port - 1), pvid, reg);
158 } else {
159
160 port_vlan_reg = &cpsw->host_port_regs->port_vlan;
161 writel(pvid, port_vlan_reg);
162 }
163}
164
165static int cpsw_port_vlan_add(struct cpsw_priv *priv, bool untag, bool pvid,
166 u16 vid, struct net_device *orig_dev)
167{
168 bool cpu_port = netif_is_bridge_master(orig_dev);
169 struct cpsw_common *cpsw = priv->cpsw;
170 int unreg_mcast_mask = 0;
171 int reg_mcast_mask = 0;
172 int untag_mask = 0;
173 int port_mask;
174 int ret = 0;
175 u32 flags;
176
177 if (cpu_port) {
178 port_mask = BIT(HOST_PORT_NUM);
179 flags = orig_dev->flags;
180 unreg_mcast_mask = port_mask;
181 } else {
182 port_mask = BIT(priv->emac_port);
183 flags = priv->ndev->flags;
184 }
185
186 if (flags & IFF_MULTICAST)
187 reg_mcast_mask = port_mask;
188
189 if (untag)
190 untag_mask = port_mask;
191
192 ret = cpsw_ale_vlan_add_modify(cpsw->ale, vid, port_mask, untag_mask,
193 reg_mcast_mask, unreg_mcast_mask);
194 if (ret) {
195 dev_err(priv->dev, "Unable to add vlan\n");
196 return ret;
197 }
198
199 if (cpu_port)
200 cpsw_ale_add_ucast(cpsw->ale, priv->mac_addr,
201 HOST_PORT_NUM, ALE_VLAN, vid);
202 if (!pvid)
203 return ret;
204
205 cpsw_set_pvid(priv, vid, 0, 0);
206
207 dev_dbg(priv->dev, "VID add: %s: vid:%u ports:%X\n",
208 priv->ndev->name, vid, port_mask);
209 return ret;
210}
211
212static int cpsw_port_vlan_del(struct cpsw_priv *priv, u16 vid,
213 struct net_device *orig_dev)
214{
215 bool cpu_port = netif_is_bridge_master(orig_dev);
216 struct cpsw_common *cpsw = priv->cpsw;
217 int port_mask;
218 int ret = 0;
219
220 if (cpu_port)
221 port_mask = BIT(HOST_PORT_NUM);
222 else
223 port_mask = BIT(priv->emac_port);
224
225 ret = cpsw_ale_vlan_del_modify(cpsw->ale, vid, port_mask);
226 if (ret != 0)
227 return ret;
228
229
230
231
232 if (cpu_port)
233 cpsw_ale_del_ucast(cpsw->ale, priv->mac_addr,
234 HOST_PORT_NUM, ALE_VLAN, vid);
235
236 if (vid == cpsw_get_pvid(priv))
237 cpsw_set_pvid(priv, 0, 0, 0);
238
239
240
241
242 cpsw_ale_del_mcast(cpsw->ale, priv->ndev->broadcast,
243 port_mask, ALE_VLAN, vid);
244 dev_dbg(priv->dev, "VID del: %s: vid:%u ports:%X\n",
245 priv->ndev->name, vid, port_mask);
246
247 return ret;
248}
249
250static int cpsw_port_vlans_add(struct cpsw_priv *priv,
251 const struct switchdev_obj_port_vlan *vlan)
252{
253 bool untag = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
254 struct net_device *orig_dev = vlan->obj.orig_dev;
255 bool cpu_port = netif_is_bridge_master(orig_dev);
256 bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
257
258 dev_dbg(priv->dev, "VID add: %s: vid:%u flags:%X\n",
259 priv->ndev->name, vlan->vid, vlan->flags);
260
261 if (cpu_port && !(vlan->flags & BRIDGE_VLAN_INFO_BRENTRY))
262 return 0;
263
264 return cpsw_port_vlan_add(priv, untag, pvid, vlan->vid, orig_dev);
265}
266
267static int cpsw_port_mdb_add(struct cpsw_priv *priv,
268 struct switchdev_obj_port_mdb *mdb)
269
270{
271 struct net_device *orig_dev = mdb->obj.orig_dev;
272 bool cpu_port = netif_is_bridge_master(orig_dev);
273 struct cpsw_common *cpsw = priv->cpsw;
274 int port_mask;
275 int err;
276
277 if (cpu_port)
278 port_mask = BIT(HOST_PORT_NUM);
279 else
280 port_mask = BIT(priv->emac_port);
281
282 err = cpsw_ale_add_mcast(cpsw->ale, mdb->addr, port_mask,
283 ALE_VLAN, mdb->vid, 0);
284 dev_dbg(priv->dev, "MDB add: %s: vid %u:%pM ports: %X\n",
285 priv->ndev->name, mdb->vid, mdb->addr, port_mask);
286
287 return err;
288}
289
290static int cpsw_port_mdb_del(struct cpsw_priv *priv,
291 struct switchdev_obj_port_mdb *mdb)
292
293{
294 struct net_device *orig_dev = mdb->obj.orig_dev;
295 bool cpu_port = netif_is_bridge_master(orig_dev);
296 struct cpsw_common *cpsw = priv->cpsw;
297 int del_mask;
298 int err;
299
300 if (cpu_port)
301 del_mask = BIT(HOST_PORT_NUM);
302 else
303 del_mask = BIT(priv->emac_port);
304
305 err = cpsw_ale_del_mcast(cpsw->ale, mdb->addr, del_mask,
306 ALE_VLAN, mdb->vid);
307 dev_dbg(priv->dev, "MDB del: %s: vid %u:%pM ports: %X\n",
308 priv->ndev->name, mdb->vid, mdb->addr, del_mask);
309
310 return err;
311}
312
313static int cpsw_port_obj_add(struct net_device *ndev, const void *ctx,
314 const struct switchdev_obj *obj,
315 struct netlink_ext_ack *extack)
316{
317 struct switchdev_obj_port_vlan *vlan = SWITCHDEV_OBJ_PORT_VLAN(obj);
318 struct switchdev_obj_port_mdb *mdb = SWITCHDEV_OBJ_PORT_MDB(obj);
319 struct cpsw_priv *priv = netdev_priv(ndev);
320 int err = 0;
321
322 dev_dbg(priv->dev, "obj_add: id %u port: %u\n",
323 obj->id, priv->emac_port);
324
325 switch (obj->id) {
326 case SWITCHDEV_OBJ_ID_PORT_VLAN:
327 err = cpsw_port_vlans_add(priv, vlan);
328 break;
329 case SWITCHDEV_OBJ_ID_PORT_MDB:
330 case SWITCHDEV_OBJ_ID_HOST_MDB:
331 err = cpsw_port_mdb_add(priv, mdb);
332 break;
333 default:
334 err = -EOPNOTSUPP;
335 break;
336 }
337
338 return err;
339}
340
341static int cpsw_port_obj_del(struct net_device *ndev, const void *ctx,
342 const struct switchdev_obj *obj)
343{
344 struct switchdev_obj_port_vlan *vlan = SWITCHDEV_OBJ_PORT_VLAN(obj);
345 struct switchdev_obj_port_mdb *mdb = SWITCHDEV_OBJ_PORT_MDB(obj);
346 struct cpsw_priv *priv = netdev_priv(ndev);
347 int err = 0;
348
349 dev_dbg(priv->dev, "obj_del: id %u port: %u\n",
350 obj->id, priv->emac_port);
351
352 switch (obj->id) {
353 case SWITCHDEV_OBJ_ID_PORT_VLAN:
354 err = cpsw_port_vlan_del(priv, vlan->vid, vlan->obj.orig_dev);
355 break;
356 case SWITCHDEV_OBJ_ID_PORT_MDB:
357 case SWITCHDEV_OBJ_ID_HOST_MDB:
358 err = cpsw_port_mdb_del(priv, mdb);
359 break;
360 default:
361 err = -EOPNOTSUPP;
362 break;
363 }
364
365 return err;
366}
367
368static void cpsw_fdb_offload_notify(struct net_device *ndev,
369 struct switchdev_notifier_fdb_info *rcv)
370{
371 struct switchdev_notifier_fdb_info info = {};
372
373 info.addr = rcv->addr;
374 info.vid = rcv->vid;
375 info.offloaded = true;
376 call_switchdev_notifiers(SWITCHDEV_FDB_OFFLOADED,
377 ndev, &info.info, NULL);
378}
379
380static void cpsw_switchdev_event_work(struct work_struct *work)
381{
382 struct cpsw_switchdev_event_work *switchdev_work =
383 container_of(work, struct cpsw_switchdev_event_work, work);
384 struct cpsw_priv *priv = switchdev_work->priv;
385 struct switchdev_notifier_fdb_info *fdb;
386 struct cpsw_common *cpsw = priv->cpsw;
387 int port = priv->emac_port;
388
389 rtnl_lock();
390 switch (switchdev_work->event) {
391 case SWITCHDEV_FDB_ADD_TO_DEVICE:
392 fdb = &switchdev_work->fdb_info;
393
394 dev_dbg(cpsw->dev, "cpsw_fdb_add: MACID = %pM vid = %u flags = %u %u -- port %d\n",
395 fdb->addr, fdb->vid, fdb->added_by_user,
396 fdb->offloaded, port);
397
398 if (!fdb->added_by_user || fdb->is_local)
399 break;
400 if (memcmp(priv->mac_addr, (u8 *)fdb->addr, ETH_ALEN) == 0)
401 port = HOST_PORT_NUM;
402
403 cpsw_ale_add_ucast(cpsw->ale, (u8 *)fdb->addr, port,
404 fdb->vid ? ALE_VLAN : 0, fdb->vid);
405 cpsw_fdb_offload_notify(priv->ndev, fdb);
406 break;
407 case SWITCHDEV_FDB_DEL_TO_DEVICE:
408 fdb = &switchdev_work->fdb_info;
409
410 dev_dbg(cpsw->dev, "cpsw_fdb_del: MACID = %pM vid = %u flags = %u %u -- port %d\n",
411 fdb->addr, fdb->vid, fdb->added_by_user,
412 fdb->offloaded, port);
413
414 if (!fdb->added_by_user || fdb->is_local)
415 break;
416 if (memcmp(priv->mac_addr, (u8 *)fdb->addr, ETH_ALEN) == 0)
417 port = HOST_PORT_NUM;
418
419 cpsw_ale_del_ucast(cpsw->ale, (u8 *)fdb->addr, port,
420 fdb->vid ? ALE_VLAN : 0, fdb->vid);
421 break;
422 default:
423 break;
424 }
425 rtnl_unlock();
426
427 kfree(switchdev_work->fdb_info.addr);
428 kfree(switchdev_work);
429 dev_put(priv->ndev);
430}
431
432
433static int cpsw_switchdev_event(struct notifier_block *unused,
434 unsigned long event, void *ptr)
435{
436 struct net_device *ndev = switchdev_notifier_info_to_dev(ptr);
437 struct switchdev_notifier_fdb_info *fdb_info = ptr;
438 struct cpsw_switchdev_event_work *switchdev_work;
439 struct cpsw_priv *priv = netdev_priv(ndev);
440 int err;
441
442 if (event == SWITCHDEV_PORT_ATTR_SET) {
443 err = switchdev_handle_port_attr_set(ndev, ptr,
444 cpsw_port_dev_check,
445 cpsw_port_attr_set);
446 return notifier_from_errno(err);
447 }
448
449 if (!cpsw_port_dev_check(ndev))
450 return NOTIFY_DONE;
451
452 switchdev_work = kzalloc(sizeof(*switchdev_work), GFP_ATOMIC);
453 if (WARN_ON(!switchdev_work))
454 return NOTIFY_BAD;
455
456 INIT_WORK(&switchdev_work->work, cpsw_switchdev_event_work);
457 switchdev_work->priv = priv;
458 switchdev_work->event = event;
459
460 switch (event) {
461 case SWITCHDEV_FDB_ADD_TO_DEVICE:
462 case SWITCHDEV_FDB_DEL_TO_DEVICE:
463 memcpy(&switchdev_work->fdb_info, ptr,
464 sizeof(switchdev_work->fdb_info));
465 switchdev_work->fdb_info.addr = kzalloc(ETH_ALEN, GFP_ATOMIC);
466 if (!switchdev_work->fdb_info.addr)
467 goto err_addr_alloc;
468 ether_addr_copy((u8 *)switchdev_work->fdb_info.addr,
469 fdb_info->addr);
470 dev_hold(ndev);
471 break;
472 default:
473 kfree(switchdev_work);
474 return NOTIFY_DONE;
475 }
476
477 queue_work(system_long_wq, &switchdev_work->work);
478
479 return NOTIFY_DONE;
480
481err_addr_alloc:
482 kfree(switchdev_work);
483 return NOTIFY_BAD;
484}
485
486static struct notifier_block cpsw_switchdev_notifier = {
487 .notifier_call = cpsw_switchdev_event,
488};
489
490static int cpsw_switchdev_blocking_event(struct notifier_block *unused,
491 unsigned long event, void *ptr)
492{
493 struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
494 int err;
495
496 switch (event) {
497 case SWITCHDEV_PORT_OBJ_ADD:
498 err = switchdev_handle_port_obj_add(dev, ptr,
499 cpsw_port_dev_check,
500 cpsw_port_obj_add);
501 return notifier_from_errno(err);
502 case SWITCHDEV_PORT_OBJ_DEL:
503 err = switchdev_handle_port_obj_del(dev, ptr,
504 cpsw_port_dev_check,
505 cpsw_port_obj_del);
506 return notifier_from_errno(err);
507 case SWITCHDEV_PORT_ATTR_SET:
508 err = switchdev_handle_port_attr_set(dev, ptr,
509 cpsw_port_dev_check,
510 cpsw_port_attr_set);
511 return notifier_from_errno(err);
512 default:
513 break;
514 }
515
516 return NOTIFY_DONE;
517}
518
519static struct notifier_block cpsw_switchdev_bl_notifier = {
520 .notifier_call = cpsw_switchdev_blocking_event,
521};
522
523int cpsw_switchdev_register_notifiers(struct cpsw_common *cpsw)
524{
525 int ret = 0;
526
527 ret = register_switchdev_notifier(&cpsw_switchdev_notifier);
528 if (ret) {
529 dev_err(cpsw->dev, "register switchdev notifier fail ret:%d\n",
530 ret);
531 return ret;
532 }
533
534 ret = register_switchdev_blocking_notifier(&cpsw_switchdev_bl_notifier);
535 if (ret) {
536 dev_err(cpsw->dev, "register switchdev blocking notifier ret:%d\n",
537 ret);
538 unregister_switchdev_notifier(&cpsw_switchdev_notifier);
539 }
540
541 return ret;
542}
543
544void cpsw_switchdev_unregister_notifiers(struct cpsw_common *cpsw)
545{
546 unregister_switchdev_blocking_notifier(&cpsw_switchdev_bl_notifier);
547 unregister_switchdev_notifier(&cpsw_switchdev_notifier);
548}
549
