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, 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 
58 static 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 
80 static 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 
89 static 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 
119 static 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 
141 static 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 		/* no barrier */
157 		slave_write(cpsw->slaves + (priv->emac_port - 1), pvid, reg);
158 	} else {
159 		/* CPU port */
160 		port_vlan_reg = &cpsw->host_port_regs->port_vlan;
161 		writel(pvid, port_vlan_reg);
162 	}
163 }
164 
165 static 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 
212 static 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 	/* We don't care for the return value here, error is returned only if
230 	 * the unicast entry is not present
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 	/* We don't care for the return value here, error is returned only if
240 	 * the multicast entry is not present
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 
250 static 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 
267 static 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 
290 static 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 
313 static 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 
341 static 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 
368 static 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 
380 static 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 /* called under rcu_read_lock() */
433 static 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 
481 err_addr_alloc:
482 	kfree(switchdev_work);
483 	return NOTIFY_BAD;
484 }
485 
486 static struct notifier_block cpsw_switchdev_notifier = {
487 	.notifier_call = cpsw_switchdev_event,
488 };
489 
490 static 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 
519 static struct notifier_block cpsw_switchdev_bl_notifier = {
520 	.notifier_call = cpsw_switchdev_blocking_event,
521 };
522 
523 int 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 
544 void 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