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