drivers: net: ti: am65-cpsw-nuss: implement ndo_fdb_dump

When the cpsw is operated in switch-mode allow dumping of the ale entries.
This way userspace can read the fdb without special means (ti switch-config)

Signed-off-by: Jan Sondhauss <jan.sondhauss@wago.com>

drivers/net/ethernet/ti/am65-cpsw-nuss.c

@@ -1467,6 +1467,82 @@ static void am65_cpsw_nuss_ndo_get_stats(struct net_device *dev,
 	stats->tx_dropped	= dev->stats.tx_dropped;
 }
 
+struct am65_cpsw_dump_ctx {
+	struct net_device *dev;
+	struct sk_buff *skb;
+	struct netlink_callback *cb;
+	int idx;
+};
+
+static int am65_cpsw_fdb_do_dump(const unsigned char *addr, u16 vid,
+				 bool is_static, void *data)
+{
+	struct am65_cpsw_dump_ctx *dump = data;
+	u32 portid = NETLINK_CB(dump->cb->skb).portid;
+	u32 seq = dump->cb->nlh->nlmsg_seq;
+	struct nlmsghdr *nlh;
+	struct ndmsg *ndm;
+
+	if (dump->idx < dump->cb->args[2])
+		goto skip;
+
+	nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH, sizeof(*ndm),
+			NLM_F_MULTI);
+	if (!nlh)
+		return -EMSGSIZE;
+
+	ndm = nlmsg_data(nlh);
+	ndm->ndm_family = AF_BRIDGE;
+	ndm->ndm_pad1 = 0;
+	ndm->ndm_pad2 = 0;
+	ndm->ndm_flags = NTF_SELF;
+	ndm->ndm_type = 0;
+	ndm->ndm_ifindex = dump->dev->ifindex;
+	ndm->ndm_state = is_static ? NUD_NOARP : NUD_REACHABLE;
+
+	if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
+		goto nla_put_failure;
+
+	if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
+		goto nla_put_failure;
+
+	nlmsg_end(dump->skb, nlh);
+
+skip:
+	dump->idx++;
+	return 0;
+
+nla_put_failure:
+	nlmsg_cancel(dump->skb, nlh);
+	return -EMSGSIZE;
+}
+
+static int am65_cpsw_ndo_fdb_dump(struct sk_buff *skb,
+				  struct netlink_callback *cb,
+				  struct net_device *ndev,
+				  struct net_device *filter_dev, int *idx)
+{
+	struct am65_cpsw_common *common = am65_ndev_to_common(ndev);
+	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
+	int ret;
+	struct am65_cpsw_dump_ctx dump = {
+		.dev = ndev,
+		.skb = skb,
+		.cb = cb,
+		.idx = *idx,
+	};
+
+	if (common->is_emac_mode)
+		return -EOPNOTSUPP;
+
+	ret = cpsw_ale_fdb_dump(common->ale, port->port_id,
+				am65_cpsw_fdb_do_dump, &dump);
+
+	*idx = dump.idx;
+
+	return ret;
+}
+
 static const struct net_device_ops am65_cpsw_nuss_netdev_ops = {
 	.ndo_open		= am65_cpsw_nuss_ndo_slave_open,
 	.ndo_stop		= am65_cpsw_nuss_ndo_slave_stop,
@@ -1481,6 +1557,7 @@ static const struct net_device_ops am65_cpsw_nuss_netdev_ops = {
 	.ndo_eth_ioctl		= am65_cpsw_nuss_ndo_slave_ioctl,
 	.ndo_setup_tc           = am65_cpsw_qos_ndo_setup_tc,
 	.ndo_set_tx_maxrate	= am65_cpsw_qos_ndo_tx_p0_set_maxrate,
+	.ndo_fdb_dump		= am65_cpsw_ndo_fdb_dump,
 };
 
 static void am65_cpsw_disable_phy(struct phy *phy)

drivers/net/ethernet/ti/cpsw_ale.c

@@ -1478,6 +1478,51 @@ struct cpsw_ale *cpsw_ale_create(struct cpsw_ale_params *params)
 	return ale;
 }
 
+int cpsw_ale_fdb_dump(struct cpsw_ale *ale, int port,
+		      cpsw_ale_fdb_dump_cb_t *cb, void *data)
+{
+	u32 entry[ALE_ENTRY_WORDS];
+	u8 addr[ETH_ALEN];
+	int entry_type;
+	bool is_static;
+	int vid;
+	int i;
+	int ret = 0;
+	int ucast_type;
+
+	for (i = 0; i < ale->params.ale_entries; i++) {
+		cpsw_ale_read(ale, i, entry);
+
+		if (port != cpsw_ale_get_port_num(entry, ale->port_num_bits))
+			continue;
+
+		entry_type = cpsw_ale_get_entry_type(entry);
+
+		if (entry_type == ALE_TYPE_FREE)
+			continue;
+		if (entry_type == ALE_TYPE_VLAN)
+			continue;
+		if (cpsw_ale_get_mcast(entry))
+			continue;
+		ucast_type = cpsw_ale_get_ucast_type(entry);
+		if (ucast_type == ALE_UCAST_UNTOUCHED)
+			continue;
+
+		vid = 0;
+		if (entry_type == ALE_TYPE_VLAN_ADDR)
+			vid = cpsw_ale_get_vlan_id(entry);
+
+		cpsw_ale_get_addr(entry, addr);
+		is_static = ucast_type == ALE_UCAST_PERSISTANT;
+
+		ret = cb(addr, vid, is_static , data);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
 void cpsw_ale_dump(struct cpsw_ale *ale, u32 *data)
 {
 	int i;

drivers/net/ethernet/ti/cpsw_ale.h

@@ -84,6 +84,8 @@ enum cpsw_ale_port_state {
 	ALE_PORT_STATE_FORWARD	= 0x03,
 };
 
+typedef int cpsw_ale_fdb_dump_cb_t(const unsigned char *addr, u16 vid,
+			      bool is_static, void *data);
 /* ALE unicast entry flags - passed into cpsw_ale_add_ucast() */
 #define ALE_SECURE			BIT(0)
 #define ALE_BLOCKED			BIT(1)
@@ -127,6 +129,7 @@ int cpsw_ale_control_get(struct cpsw_ale *ale, int port, int control);
 int cpsw_ale_control_set(struct cpsw_ale *ale, int port,
 			 int control, int value);
 void cpsw_ale_dump(struct cpsw_ale *ale, u32 *data);
+int cpsw_ale_fdb_dump(struct cpsw_ale *ale, int port, cpsw_ale_fdb_dump_cb_t *cb, void *data);
 void cpsw_ale_restore(struct cpsw_ale *ale, u32 *data);
 u32 cpsw_ale_get_num_entries(struct cpsw_ale *ale);