[linux-2.6-block.git] / drivers / net / ethernet / ti / cpsw_switchdev.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Texas Instruments switchdev Driver
 *
 * Copyright (C) 2019 Texas Instruments
 *
 */

#include <linux/etherdevice.h>
#include <linux/if_bridge.h>
#include <linux/netdevice.h>
#include <linux/workqueue.h>
#include <net/switchdev.h>

#include "cpsw.h"
#include "cpsw_ale.h"
#include "cpsw_priv.h"
#include "cpsw_switchdev.h"

struct cpsw_switchdev_event_work {
	struct work_struct work;
	struct switchdev_notifier_fdb_info fdb_info;
	struct cpsw_priv *priv;
	unsigned long event;
};

static int cpsw_port_stp_state_set(struct cpsw_priv *priv, u8 state)
{
	struct cpsw_common *cpsw = priv->cpsw;
	u8 cpsw_state;
	int ret = 0;

	switch (state) {
	case BR_STATE_FORWARDING:
		cpsw_state = ALE_PORT_STATE_FORWARD;
		break;
	case BR_STATE_LEARNING:
		cpsw_state = ALE_PORT_STATE_LEARN;
		break;
	case BR_STATE_DISABLED:
		cpsw_state = ALE_PORT_STATE_DISABLE;
		break;
	case BR_STATE_LISTENING:
	case BR_STATE_BLOCKING:
		cpsw_state = ALE_PORT_STATE_BLOCK;
		break;
	default:
		return -EOPNOTSUPP;
	}

	ret = cpsw_ale_control_set(cpsw->ale, priv->emac_port,
				   ALE_PORT_STATE, cpsw_state);
	dev_dbg(priv->dev, "ale state: %u\n", cpsw_state);

	return ret;
}

static int cpsw_port_attr_br_flags_set(struct cpsw_priv *priv,
				       struct net_device *orig_dev,
				       struct switchdev_brport_flags flags)
{
	struct cpsw_common *cpsw = priv->cpsw;

	if (flags.mask & BR_MCAST_FLOOD) {
		bool unreg_mcast_add = false;

		if (flags.val & BR_MCAST_FLOOD)
			unreg_mcast_add = true;

		dev_dbg(priv->dev, "BR_MCAST_FLOOD: %d port %u\n",
			unreg_mcast_add, priv->emac_port);

		cpsw_ale_set_unreg_mcast(cpsw->ale, BIT(priv->emac_port),
					 unreg_mcast_add);
	}

	return 0;
}

static int cpsw_port_attr_br_flags_pre_set(struct net_device *netdev,
					   struct switchdev_brport_flags flags)
{
	if (flags.mask & ~(BR_LEARNING | BR_MCAST_FLOOD))
		return -EINVAL;

	return 0;
}

static int cpsw_port_attr_set(struct net_device *ndev, const void *ctx,
			      const struct switchdev_attr *attr,
			      struct netlink_ext_ack *extack)
{
	struct cpsw_priv *priv = netdev_priv(ndev);
	int ret;

	dev_dbg(priv->dev, "attr: id %u port: %u\n", attr->id, priv->emac_port);

	switch (attr->id) {
	case SWITCHDEV_ATTR_ID_PORT_PRE_BRIDGE_FLAGS:
		ret = cpsw_port_attr_br_flags_pre_set(ndev,
						      attr->u.brport_flags);
		break;
	case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
		ret = cpsw_port_stp_state_set(priv, attr->u.stp_state);
		dev_dbg(priv->dev, "stp state: %u\n", attr->u.stp_state);
		break;
	case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
		ret = cpsw_port_attr_br_flags_set(priv, attr->orig_dev,
						  attr->u.brport_flags);
		break;
	default:
		ret = -EOPNOTSUPP;
		break;
	}

	return ret;
}

static u16 cpsw_get_pvid(struct cpsw_priv *priv)
{
	struct cpsw_common *cpsw = priv->cpsw;
	u32 __iomem *port_vlan_reg;
	u32 pvid;

	if (priv->emac_port) {
		int reg = CPSW2_PORT_VLAN;

		if (cpsw->version == CPSW_VERSION_1)
			reg = CPSW1_PORT_VLAN;
		pvid = slave_read(cpsw->slaves + (priv->emac_port - 1), reg);
	} else {
		port_vlan_reg = &cpsw->host_port_regs->port_vlan;
		pvid = readl(port_vlan_reg);
	}

	pvid = pvid & 0xfff;

	return pvid;
}

static void cpsw_set_pvid(struct cpsw_priv *priv, u16 vid, bool cfi, u32 cos)
{
	struct cpsw_common *cpsw = priv->cpsw;
	void __iomem *port_vlan_reg;
	u32 pvid;

	pvid = vid;
	pvid |= cfi ? BIT(12) : 0;
	pvid |= (cos & 0x7) << 13;

	if (priv->emac_port) {
		int reg = CPSW2_PORT_VLAN;

		if (cpsw->version == CPSW_VERSION_1)
			reg = CPSW1_PORT_VLAN;
		/* no barrier */
		slave_write(cpsw->slaves + (priv->emac_port - 1), pvid, reg);
	} else {
		/* CPU port */
		port_vlan_reg = &cpsw->host_port_regs->port_vlan;
		writel(pvid, port_vlan_reg);
	}
}

static int cpsw_port_vlan_add(struct cpsw_priv *priv, bool untag, bool pvid,
			      u16 vid, struct net_device *orig_dev)
{
	bool cpu_port = netif_is_bridge_master(orig_dev);
	struct cpsw_common *cpsw = priv->cpsw;
	int unreg_mcast_mask = 0;
	int reg_mcast_mask = 0;
	int untag_mask = 0;
	int port_mask;
	int ret = 0;
	u32 flags;

	if (cpu_port) {
		port_mask = BIT(HOST_PORT_NUM);
		flags = orig_dev->flags;
		unreg_mcast_mask = port_mask;
	} else {
		port_mask = BIT(priv->emac_port);
		flags = priv->ndev->flags;
	}

	if (flags & IFF_MULTICAST)
		reg_mcast_mask = port_mask;

	if (untag)
		untag_mask = port_mask;

	ret = cpsw_ale_vlan_add_modify(cpsw->ale, vid, port_mask, untag_mask,
				       reg_mcast_mask, unreg_mcast_mask);
	if (ret) {
		dev_err(priv->dev, "Unable to add vlan\n");
		return ret;
	}

	if (cpu_port)
		cpsw_ale_add_ucast(cpsw->ale, priv->mac_addr,
				   HOST_PORT_NUM, ALE_VLAN, vid);
	if (!pvid)
		return ret;

	cpsw_set_pvid(priv, vid, 0, 0);

	dev_dbg(priv->dev, "VID add: %s: vid:%u ports:%X\n",
		priv->ndev->name, vid, port_mask);
	return ret;
}

static int cpsw_port_vlan_del(struct cpsw_priv *priv, u16 vid,
			      struct net_device *orig_dev)
{
	bool cpu_port = netif_is_bridge_master(orig_dev);
	struct cpsw_common *cpsw = priv->cpsw;
	int port_mask;
	int ret = 0;

	if (cpu_port)
		port_mask = BIT(HOST_PORT_NUM);
	else
		port_mask = BIT(priv->emac_port);

	ret = cpsw_ale_vlan_del_modify(cpsw->ale, vid, port_mask);
	if (ret != 0)
		return ret;

	/* We don't care for the return value here, error is returned only if
	 * the unicast entry is not present
	 */
	if (cpu_port)
		cpsw_ale_del_ucast(cpsw->ale, priv->mac_addr,
				   HOST_PORT_NUM, ALE_VLAN, vid);

	if (vid == cpsw_get_pvid(priv))
		cpsw_set_pvid(priv, 0, 0, 0);

	/* We don't care for the return value here, error is returned only if
	 * the multicast entry is not present
	 */
	cpsw_ale_del_mcast(cpsw->ale, priv->ndev->broadcast,
			   port_mask, ALE_VLAN, vid);
	dev_dbg(priv->dev, "VID del: %s: vid:%u ports:%X\n",
		priv->ndev->name, vid, port_mask);

	return ret;
}

static int cpsw_port_vlans_add(struct cpsw_priv *priv,
			       const struct switchdev_obj_port_vlan *vlan)
{
	bool untag = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
	struct net_device *orig_dev = vlan->obj.orig_dev;
	bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;

	dev_dbg(priv->dev, "VID add: %s: vid:%u flags:%X\n",
		priv->ndev->name, vlan->vid, vlan->flags);

	return cpsw_port_vlan_add(priv, untag, pvid, vlan->vid, orig_dev);
}

static int cpsw_port_mdb_add(struct cpsw_priv *priv,
			     struct switchdev_obj_port_mdb *mdb)

{
	struct net_device *orig_dev = mdb->obj.orig_dev;
	bool cpu_port = netif_is_bridge_master(orig_dev);
	struct cpsw_common *cpsw = priv->cpsw;
	int port_mask;
	int err;

	if (cpu_port)
		port_mask = BIT(HOST_PORT_NUM);
	else
		port_mask = BIT(priv->emac_port);

	err = cpsw_ale_add_mcast(cpsw->ale, mdb->addr, port_mask,
				 ALE_VLAN, mdb->vid, 0);
	dev_dbg(priv->dev, "MDB add: %s: vid %u:%pM  ports: %X\n",
		priv->ndev->name, mdb->vid, mdb->addr, port_mask);

	return err;
}

static int cpsw_port_mdb_del(struct cpsw_priv *priv,
			     struct switchdev_obj_port_mdb *mdb)

{
	struct net_device *orig_dev = mdb->obj.orig_dev;
	bool cpu_port = netif_is_bridge_master(orig_dev);
	struct cpsw_common *cpsw = priv->cpsw;
	int del_mask;
	int err;

	if (cpu_port)
		del_mask = BIT(HOST_PORT_NUM);
	else
		del_mask = BIT(priv->emac_port);

	err = cpsw_ale_del_mcast(cpsw->ale, mdb->addr, del_mask,
				 ALE_VLAN, mdb->vid);
	dev_dbg(priv->dev, "MDB del: %s: vid %u:%pM  ports: %X\n",
		priv->ndev->name, mdb->vid, mdb->addr, del_mask);

	return err;
}

static int cpsw_port_obj_add(struct net_device *ndev, const void *ctx,
			     const struct switchdev_obj *obj,
			     struct netlink_ext_ack *extack)
{
	struct switchdev_obj_port_vlan *vlan = SWITCHDEV_OBJ_PORT_VLAN(obj);
	struct switchdev_obj_port_mdb *mdb = SWITCHDEV_OBJ_PORT_MDB(obj);
	struct cpsw_priv *priv = netdev_priv(ndev);
	int err = 0;

	dev_dbg(priv->dev, "obj_add: id %u port: %u\n",
		obj->id, priv->emac_port);

	switch (obj->id) {
	case SWITCHDEV_OBJ_ID_PORT_VLAN:
		err = cpsw_port_vlans_add(priv, vlan);
		break;
	case SWITCHDEV_OBJ_ID_PORT_MDB:
	case SWITCHDEV_OBJ_ID_HOST_MDB:
		err = cpsw_port_mdb_add(priv, mdb);
		break;
	default:
		err = -EOPNOTSUPP;
		break;
	}

	return err;
}

static int cpsw_port_obj_del(struct net_device *ndev, const void *ctx,
			     const struct switchdev_obj *obj)
{
	struct switchdev_obj_port_vlan *vlan = SWITCHDEV_OBJ_PORT_VLAN(obj);
	struct switchdev_obj_port_mdb *mdb = SWITCHDEV_OBJ_PORT_MDB(obj);
	struct cpsw_priv *priv = netdev_priv(ndev);
	int err = 0;

	dev_dbg(priv->dev, "obj_del: id %u port: %u\n",
		obj->id, priv->emac_port);

	switch (obj->id) {
	case SWITCHDEV_OBJ_ID_PORT_VLAN:
		err = cpsw_port_vlan_del(priv, vlan->vid, vlan->obj.orig_dev);
		break;
	case SWITCHDEV_OBJ_ID_PORT_MDB:
	case SWITCHDEV_OBJ_ID_HOST_MDB:
		err = cpsw_port_mdb_del(priv, mdb);
		break;
	default:
		err = -EOPNOTSUPP;
		break;
	}

	return err;
}

static void cpsw_fdb_offload_notify(struct net_device *ndev,
				    struct switchdev_notifier_fdb_info *rcv)
{
	struct switchdev_notifier_fdb_info info = {};

	info.addr = rcv->addr;
	info.vid = rcv->vid;
	info.offloaded = true;
	call_switchdev_notifiers(SWITCHDEV_FDB_OFFLOADED,
				 ndev, &info.info, NULL);
}

static void cpsw_switchdev_event_work(struct work_struct *work)
{
	struct cpsw_switchdev_event_work *switchdev_work =
		container_of(work, struct cpsw_switchdev_event_work, work);
	struct cpsw_priv *priv = switchdev_work->priv;
	struct switchdev_notifier_fdb_info *fdb;
	struct cpsw_common *cpsw = priv->cpsw;
	int port = priv->emac_port;

	rtnl_lock();
	switch (switchdev_work->event) {
	case SWITCHDEV_FDB_ADD_TO_DEVICE:
		fdb = &switchdev_work->fdb_info;

		dev_dbg(cpsw->dev, "cpsw_fdb_add: MACID = %pM vid = %u flags = %u %u -- port %d\n",
			fdb->addr, fdb->vid, fdb->added_by_user,
			fdb->offloaded, port);

		if (!fdb->added_by_user || fdb->is_local)
			break;
		if (memcmp(priv->mac_addr, (u8 *)fdb->addr, ETH_ALEN) == 0)
			port = HOST_PORT_NUM;

		cpsw_ale_add_ucast(cpsw->ale, (u8 *)fdb->addr, port,
				   fdb->vid ? ALE_VLAN : 0, fdb->vid);
		cpsw_fdb_offload_notify(priv->ndev, fdb);
		break;
	case SWITCHDEV_FDB_DEL_TO_DEVICE:
		fdb = &switchdev_work->fdb_info;

		dev_dbg(cpsw->dev, "cpsw_fdb_del: MACID = %pM vid = %u flags = %u %u -- port %d\n",
			fdb->addr, fdb->vid, fdb->added_by_user,
			fdb->offloaded, port);

		if (!fdb->added_by_user || fdb->is_local)
			break;
		if (memcmp(priv->mac_addr, (u8 *)fdb->addr, ETH_ALEN) == 0)
			port = HOST_PORT_NUM;

		cpsw_ale_del_ucast(cpsw->ale, (u8 *)fdb->addr, port,
				   fdb->vid ? ALE_VLAN : 0, fdb->vid);
		break;
	default:
		break;
	}
	rtnl_unlock();

	kfree(switchdev_work->fdb_info.addr);
	kfree(switchdev_work);
	dev_put(priv->ndev);
}

/* called under rcu_read_lock() */
static int cpsw_switchdev_event(struct notifier_block *unused,
				unsigned long event, void *ptr)
{
	struct net_device *ndev = switchdev_notifier_info_to_dev(ptr);
	struct switchdev_notifier_fdb_info *fdb_info = ptr;
	struct cpsw_switchdev_event_work *switchdev_work;
	struct cpsw_priv *priv = netdev_priv(ndev);
	int err;

	if (event == SWITCHDEV_PORT_ATTR_SET) {
		err = switchdev_handle_port_attr_set(ndev, ptr,
						     cpsw_port_dev_check,
						     cpsw_port_attr_set);
		return notifier_from_errno(err);
	}

	if (!cpsw_port_dev_check(ndev))
		return NOTIFY_DONE;

	switchdev_work = kzalloc(sizeof(*switchdev_work), GFP_ATOMIC);
	if (WARN_ON(!switchdev_work))
		return NOTIFY_BAD;

	INIT_WORK(&switchdev_work->work, cpsw_switchdev_event_work);
	switchdev_work->priv = priv;
	switchdev_work->event = event;

	switch (event) {
	case SWITCHDEV_FDB_ADD_TO_DEVICE:
	case SWITCHDEV_FDB_DEL_TO_DEVICE:
		memcpy(&switchdev_work->fdb_info, ptr,
		       sizeof(switchdev_work->fdb_info));
		switchdev_work->fdb_info.addr = kzalloc(ETH_ALEN, GFP_ATOMIC);
		if (!switchdev_work->fdb_info.addr)
			goto err_addr_alloc;
		ether_addr_copy((u8 *)switchdev_work->fdb_info.addr,
				fdb_info->addr);
		dev_hold(ndev);
		break;
	default:
		kfree(switchdev_work);
		return NOTIFY_DONE;
	}

	queue_work(system_long_wq, &switchdev_work->work);

	return NOTIFY_DONE;

err_addr_alloc:
	kfree(switchdev_work);
	return NOTIFY_BAD;
}

static struct notifier_block cpsw_switchdev_notifier = {
	.notifier_call = cpsw_switchdev_event,
};

static int cpsw_switchdev_blocking_event(struct notifier_block *unused,
					 unsigned long event, void *ptr)
{
	struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
	int err;

	switch (event) {
	case SWITCHDEV_PORT_OBJ_ADD:
		err = switchdev_handle_port_obj_add(dev, ptr,
						    cpsw_port_dev_check,
						    cpsw_port_obj_add);
		return notifier_from_errno(err);
	case SWITCHDEV_PORT_OBJ_DEL:
		err = switchdev_handle_port_obj_del(dev, ptr,
						    cpsw_port_dev_check,
						    cpsw_port_obj_del);
		return notifier_from_errno(err);
	case SWITCHDEV_PORT_ATTR_SET:
		err = switchdev_handle_port_attr_set(dev, ptr,
						     cpsw_port_dev_check,
						     cpsw_port_attr_set);
		return notifier_from_errno(err);
	default:
		break;
	}

	return NOTIFY_DONE;
}

static struct notifier_block cpsw_switchdev_bl_notifier = {
	.notifier_call = cpsw_switchdev_blocking_event,
};

int cpsw_switchdev_register_notifiers(struct cpsw_common *cpsw)
{
	int ret = 0;

	ret = register_switchdev_notifier(&cpsw_switchdev_notifier);
	if (ret) {
		dev_err(cpsw->dev, "register switchdev notifier fail ret:%d\n",
			ret);
		return ret;
	}

	ret = register_switchdev_blocking_notifier(&cpsw_switchdev_bl_notifier);
	if (ret) {
		dev_err(cpsw->dev, "register switchdev blocking notifier ret:%d\n",
			ret);
		unregister_switchdev_notifier(&cpsw_switchdev_notifier);
	}

	return ret;
}

void cpsw_switchdev_unregister_notifiers(struct cpsw_common *cpsw)
{
	unregister_switchdev_blocking_notifier(&cpsw_switchdev_bl_notifier);
	unregister_switchdev_notifier(&cpsw_switchdev_notifier);
}
Commit	Line	Data
111cf1ab IA	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
bae33f2b	27	static int cpsw_port_stp_state_set(struct cpsw_priv *priv, u8 state)
111cf1ab IA	28	{
	29	struct cpsw_common *cpsw = priv->cpsw;
	30	u8 cpsw_state;
	31	int ret = 0;
	32
111cf1ab IA	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,
111cf1ab	59	struct net_device *orig_dev,
e18f4c18	60	struct switchdev_brport_flags flags)
111cf1ab IA	61	{
111cf1ab IA	62	struct cpsw_common *cpsw = priv->cpsw;
111cf1ab	63
e18f4c18 VO	64	if (flags.mask & BR_MCAST_FLOOD) {
e18f4c18 VO	65	bool unreg_mcast_add = false;
111cf1ab	66
e18f4c18 VO	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	}
111cf1ab IA	76
	77	return 0;
	78	}
	79
	80	static int cpsw_port_attr_br_flags_pre_set(struct net_device *netdev,
e18f4c18	81	struct switchdev_brport_flags flags)
111cf1ab	82	{
e18f4c18	83	if (flags.mask & ~(BR_LEARNING \| BR_MCAST_FLOOD))
111cf1ab IA	84	return -EINVAL;
	85
	86	return 0;
	87	}
	88
69bfac96	89	static int cpsw_port_attr_set(struct net_device ndev, const void ctx,
4c08c586 VO	90	const struct switchdev_attr *attr,
4c08c586 VO	91	struct netlink_ext_ack *extack)
111cf1ab IA	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:
bae33f2b	100	ret = cpsw_port_attr_br_flags_pre_set(ndev,
111cf1ab IA	101	attr->u.brport_flags);
	102	break;
	103	case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
bae33f2b	104	ret = cpsw_port_stp_state_set(priv, attr->u.stp_state);
111cf1ab IA	105	dev_dbg(priv->dev, "stp state: %u\n", attr->u.stp_state);
	106	break;
	107	case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
bae33f2b	108	ret = cpsw_port_attr_br_flags_set(priv, attr->orig_dev,
111cf1ab IA	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
82882bd5	225	ret = cpsw_ale_vlan_del_modify(cpsw->ale, vid, port_mask);
111cf1ab IA	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,
ffb68fc5	251	const struct switchdev_obj_port_vlan *vlan)
111cf1ab IA	252	{
	253	bool untag = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
	254	struct net_device *orig_dev = vlan->obj.orig_dev;
111cf1ab	255	bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
111cf1ab IA	256
111cf1ab IA	257	dev_dbg(priv->dev, "VID add: %s: vid:%u flags:%X\n",
b7a9e0da	258	priv->ndev->name, vlan->vid, vlan->flags);
111cf1ab	259
b7a9e0da	260	return cpsw_port_vlan_add(priv, untag, pvid, vlan->vid, orig_dev);
111cf1ab IA	261	}
	262
	263	static int cpsw_port_mdb_add(struct cpsw_priv *priv,
ffb68fc5	264	struct switchdev_obj_port_mdb *mdb)
111cf1ab IA	265
	266	{
	267	struct net_device *orig_dev = mdb->obj.orig_dev;
	268	bool cpu_port = netif_is_bridge_master(orig_dev);
	269	struct cpsw_common *cpsw = priv->cpsw;
	270	int port_mask;
	271	int err;
	272
111cf1ab IA	273	if (cpu_port)
	274	port_mask = BIT(HOST_PORT_NUM);
	275	else
	276	port_mask = BIT(priv->emac_port);
	277
	278	err = cpsw_ale_add_mcast(cpsw->ale, mdb->addr, port_mask,
	279	ALE_VLAN, mdb->vid, 0);
	280	dev_dbg(priv->dev, "MDB add: %s: vid %u:%pM ports: %X\n",
	281	priv->ndev->name, mdb->vid, mdb->addr, port_mask);
	282
	283	return err;
	284	}
	285
	286	static int cpsw_port_mdb_del(struct cpsw_priv *priv,
	287	struct switchdev_obj_port_mdb *mdb)
	288
	289	{
	290	struct net_device *orig_dev = mdb->obj.orig_dev;
	291	bool cpu_port = netif_is_bridge_master(orig_dev);
	292	struct cpsw_common *cpsw = priv->cpsw;
	293	int del_mask;
	294	int err;
	295
	296	if (cpu_port)
	297	del_mask = BIT(HOST_PORT_NUM);
	298	else
	299	del_mask = BIT(priv->emac_port);
	300
	301	err = cpsw_ale_del_mcast(cpsw->ale, mdb->addr, del_mask,
	302	ALE_VLAN, mdb->vid);
	303	dev_dbg(priv->dev, "MDB del: %s: vid %u:%pM ports: %X\n",
	304	priv->ndev->name, mdb->vid, mdb->addr, del_mask);
	305
	306	return err;
	307	}
	308
69bfac96	309	static int cpsw_port_obj_add(struct net_device ndev, const void ctx,
111cf1ab	310	const struct switchdev_obj *obj,
111cf1ab IA	311	struct netlink_ext_ack *extack)
	312	{
	313	struct switchdev_obj_port_vlan *vlan = SWITCHDEV_OBJ_PORT_VLAN(obj);
	314	struct switchdev_obj_port_mdb *mdb = SWITCHDEV_OBJ_PORT_MDB(obj);
	315	struct cpsw_priv *priv = netdev_priv(ndev);
	316	int err = 0;
	317
	318	dev_dbg(priv->dev, "obj_add: id %u port: %u\n",
	319	obj->id, priv->emac_port);
	320
	321	switch (obj->id) {
	322	case SWITCHDEV_OBJ_ID_PORT_VLAN:
ffb68fc5	323	err = cpsw_port_vlans_add(priv, vlan);
111cf1ab IA	324	break;
	325	case SWITCHDEV_OBJ_ID_PORT_MDB:
	326	case SWITCHDEV_OBJ_ID_HOST_MDB:
ffb68fc5	327	err = cpsw_port_mdb_add(priv, mdb);
111cf1ab IA	328	break;
	329	default:
	330	err = -EOPNOTSUPP;
	331	break;
	332	}
	333
	334	return err;
	335	}
	336
69bfac96	337	static int cpsw_port_obj_del(struct net_device ndev, const void ctx,
111cf1ab IA	338	const struct switchdev_obj *obj)
	339	{
	340	struct switchdev_obj_port_vlan *vlan = SWITCHDEV_OBJ_PORT_VLAN(obj);
	341	struct switchdev_obj_port_mdb *mdb = SWITCHDEV_OBJ_PORT_MDB(obj);
	342	struct cpsw_priv *priv = netdev_priv(ndev);
	343	int err = 0;
	344
	345	dev_dbg(priv->dev, "obj_del: id %u port: %u\n",
	346	obj->id, priv->emac_port);
	347
	348	switch (obj->id) {
	349	case SWITCHDEV_OBJ_ID_PORT_VLAN:
b7a9e0da	350	err = cpsw_port_vlan_del(priv, vlan->vid, vlan->obj.orig_dev);
111cf1ab IA	351	break;
	352	case SWITCHDEV_OBJ_ID_PORT_MDB:
	353	case SWITCHDEV_OBJ_ID_HOST_MDB:
	354	err = cpsw_port_mdb_del(priv, mdb);
	355	break;
	356	default:
	357	err = -EOPNOTSUPP;
	358	break;
	359	}
	360
	361	return err;
	362	}
	363
	364	static void cpsw_fdb_offload_notify(struct net_device *ndev,
	365	struct switchdev_notifier_fdb_info *rcv)
	366	{
c35b57ce	367	struct switchdev_notifier_fdb_info info = {};
111cf1ab IA	368
	369	info.addr = rcv->addr;
	370	info.vid = rcv->vid;
	371	info.offloaded = true;
	372	call_switchdev_notifiers(SWITCHDEV_FDB_OFFLOADED,
	373	ndev, &info.info, NULL);
	374	}
	375
	376	static void cpsw_switchdev_event_work(struct work_struct *work)
	377	{
	378	struct cpsw_switchdev_event_work *switchdev_work =
	379	container_of(work, struct cpsw_switchdev_event_work, work);
	380	struct cpsw_priv *priv = switchdev_work->priv;
	381	struct switchdev_notifier_fdb_info *fdb;
	382	struct cpsw_common *cpsw = priv->cpsw;
	383	int port = priv->emac_port;
	384
	385	rtnl_lock();
	386	switch (switchdev_work->event) {
	387	case SWITCHDEV_FDB_ADD_TO_DEVICE:
	388	fdb = &switchdev_work->fdb_info;
	389
	390	dev_dbg(cpsw->dev, "cpsw_fdb_add: MACID = %pM vid = %u flags = %u %u -- port %d\n",
	391	fdb->addr, fdb->vid, fdb->added_by_user,
	392	fdb->offloaded, port);
	393
2c4eca3e	394	if (!fdb->added_by_user \|\| fdb->is_local)
111cf1ab IA	395	break;
	396	if (memcmp(priv->mac_addr, (u8 *)fdb->addr, ETH_ALEN) == 0)
	397	port = HOST_PORT_NUM;
	398
	399	cpsw_ale_add_ucast(cpsw->ale, (u8 *)fdb->addr, port,
	400	fdb->vid ? ALE_VLAN : 0, fdb->vid);
	401	cpsw_fdb_offload_notify(priv->ndev, fdb);
	402	break;
	403	case SWITCHDEV_FDB_DEL_TO_DEVICE:
	404	fdb = &switchdev_work->fdb_info;
	405
	406	dev_dbg(cpsw->dev, "cpsw_fdb_del: MACID = %pM vid = %u flags = %u %u -- port %d\n",
	407	fdb->addr, fdb->vid, fdb->added_by_user,
	408	fdb->offloaded, port);
	409
2c4eca3e	410	if (!fdb->added_by_user \|\| fdb->is_local)
111cf1ab IA	411	break;
	412	if (memcmp(priv->mac_addr, (u8 *)fdb->addr, ETH_ALEN) == 0)
	413	port = HOST_PORT_NUM;
	414
	415	cpsw_ale_del_ucast(cpsw->ale, (u8 *)fdb->addr, port,
	416	fdb->vid ? ALE_VLAN : 0, fdb->vid);
	417	break;
	418	default:
	419	break;
	420	}
	421	rtnl_unlock();
	422
	423	kfree(switchdev_work->fdb_info.addr);
	424	kfree(switchdev_work);
	425	dev_put(priv->ndev);
	426	}
	427
	428	/* called under rcu_read_lock() */
	429	static int cpsw_switchdev_event(struct notifier_block *unused,
	430	unsigned long event, void *ptr)
	431	{
	432	struct net_device *ndev = switchdev_notifier_info_to_dev(ptr);
	433	struct switchdev_notifier_fdb_info *fdb_info = ptr;
	434	struct cpsw_switchdev_event_work *switchdev_work;
	435	struct cpsw_priv *priv = netdev_priv(ndev);
	436	int err;
	437
	438	if (event == SWITCHDEV_PORT_ATTR_SET) {
	439	err = switchdev_handle_port_attr_set(ndev, ptr,
	440	cpsw_port_dev_check,
	441	cpsw_port_attr_set);
	442	return notifier_from_errno(err);
	443	}
	444
	445	if (!cpsw_port_dev_check(ndev))
	446	return NOTIFY_DONE;
	447
	448	switchdev_work = kzalloc(sizeof(*switchdev_work), GFP_ATOMIC);
	449	if (WARN_ON(!switchdev_work))
	450	return NOTIFY_BAD;
	451
	452	INIT_WORK(&switchdev_work->work, cpsw_switchdev_event_work);
	453	switchdev_work->priv = priv;
	454	switchdev_work->event = event;
	455
	456	switch (event) {
	457	case SWITCHDEV_FDB_ADD_TO_DEVICE:
	458	case SWITCHDEV_FDB_DEL_TO_DEVICE:
	459	memcpy(&switchdev_work->fdb_info, ptr,
	460	sizeof(switchdev_work->fdb_info));
	461	switchdev_work->fdb_info.addr = kzalloc(ETH_ALEN, GFP_ATOMIC);
	462	if (!switchdev_work->fdb_info.addr)
	463	goto err_addr_alloc;
	464	ether_addr_copy((u8 *)switchdev_work->fdb_info.addr,
	465	fdb_info->addr);
	466	dev_hold(ndev);
	467	break;
	468	default:
	469	kfree(switchdev_work);
	470	return NOTIFY_DONE;
	471	}
	472
	473	queue_work(system_long_wq, &switchdev_work->work);
	474
475	return NOTIFY_DONE;
476
477	err_addr_alloc:
478	kfree(switchdev_work);
479	return NOTIFY_BAD;
480	}
481
482	static struct notifier_block cpsw_switchdev_notifier = {
483	.notifier_call = cpsw_switchdev_event,
484	};
485
486	static int cpsw_switchdev_blocking_event(struct notifier_block *unused,
487	unsigned long event, void *ptr)
488	{
489	struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
490	int err;
491
492	switch (event) {
493	case SWITCHDEV_PORT_OBJ_ADD:
494	err = switchdev_handle_port_obj_add(dev, ptr,
495	cpsw_port_dev_check,
496	cpsw_port_obj_add);
497	return notifier_from_errno(err);
498	case SWITCHDEV_PORT_OBJ_DEL:
499	err = switchdev_handle_port_obj_del(dev, ptr,
500	cpsw_port_dev_check,
501	cpsw_port_obj_del);
502	return notifier_from_errno(err);
503	case SWITCHDEV_PORT_ATTR_SET:
504	err = switchdev_handle_port_attr_set(dev, ptr,
505	cpsw_port_dev_check,
506	cpsw_port_attr_set);
507	return notifier_from_errno(err);
508	default:
509	break;
510	}
511
512	return NOTIFY_DONE;
513	}
514
515	static struct notifier_block cpsw_switchdev_bl_notifier = {
516	.notifier_call = cpsw_switchdev_blocking_event,
517	};
518
519	int cpsw_switchdev_register_notifiers(struct cpsw_common *cpsw)
520	{
521	int ret = 0;
522
523	ret = register_switchdev_notifier(&cpsw_switchdev_notifier);
524	if (ret) {
525	dev_err(cpsw->dev, "register switchdev notifier fail ret:%d\n",
526	ret);
527	return ret;
528	}
529
530	ret = register_switchdev_blocking_notifier(&cpsw_switchdev_bl_notifier);
531	if (ret) {
532	dev_err(cpsw->dev, "register switchdev blocking notifier ret:%d\n",
533	ret);
534	unregister_switchdev_notifier(&cpsw_switchdev_notifier);
535	}
536
537	return ret;
538	}
539
540	void cpsw_switchdev_unregister_notifiers(struct cpsw_common *cpsw)
541	{
542	unregister_switchdev_blocking_notifier(&cpsw_switchdev_bl_notifier);
543	unregister_switchdev_notifier(&cpsw_switchdev_notifier);
544	}