[linux-2.6-block.git] / drivers / net / ethernet / mscc / ocelot_board.c

// SPDX-License-Identifier: (GPL-2.0 OR MIT)
/*
 * Microsemi Ocelot Switch driver
 *
 * Copyright (c) 2017 Microsemi Corporation
 */
#include <linux/interrupt.h>
#include <linux/module.h>
#include <linux/of_net.h>
#include <linux/netdevice.h>
#include <linux/of_mdio.h>
#include <linux/of_platform.h>
#include <linux/mfd/syscon.h>
#include <linux/skbuff.h>
#include <net/switchdev.h>

#include "ocelot.h"

#define IFH_EXTRACT_BITFIELD64(x, o, w) (((x) >> (o)) & GENMASK_ULL((w) - 1, 0))

static int ocelot_parse_ifh(u32 *_ifh, struct frame_info *info)
{
	u8 llen, wlen;
	u64 ifh[2];

	ifh[0] = be64_to_cpu(((__force __be64 *)_ifh)[0]);
	ifh[1] = be64_to_cpu(((__force __be64 *)_ifh)[1]);

	wlen = IFH_EXTRACT_BITFIELD64(ifh[0], 7,  8);
	llen = IFH_EXTRACT_BITFIELD64(ifh[0], 15,  6);

	info->len = OCELOT_BUFFER_CELL_SZ * wlen + llen - 80;

	info->timestamp = IFH_EXTRACT_BITFIELD64(ifh[0], 21, 32);

	info->port = IFH_EXTRACT_BITFIELD64(ifh[1], 43, 4);

	info->tag_type = IFH_EXTRACT_BITFIELD64(ifh[1], 16,  1);
	info->vid = IFH_EXTRACT_BITFIELD64(ifh[1], 0,  12);

	return 0;
}

static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
				u32 *rval)
{
	u32 val;
	u32 bytes_valid;

	val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
	if (val == XTR_NOT_READY) {
		if (ifh)
			return -EIO;

		do {
			val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
		} while (val == XTR_NOT_READY);
	}

	switch (val) {
	case XTR_ABORT:
		return -EIO;
	case XTR_EOF_0:
	case XTR_EOF_1:
	case XTR_EOF_2:
	case XTR_EOF_3:
	case XTR_PRUNED:
		bytes_valid = XTR_VALID_BYTES(val);
		val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
		if (val == XTR_ESCAPE)
			*rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
		else
			*rval = val;

		return bytes_valid;
	case XTR_ESCAPE:
		*rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);

		return 4;
	default:
		*rval = val;

		return 4;
	}
}

static irqreturn_t ocelot_xtr_irq_handler(int irq, void *arg)
{
	struct ocelot *ocelot = arg;
	int i = 0, grp = 0;
	int err = 0;

	if (!(ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp)))
		return IRQ_NONE;

	do {
		struct skb_shared_hwtstamps *shhwtstamps;
		u64 tod_in_ns, full_ts_in_ns;
		struct frame_info info = {};
		struct net_device *dev;
		u32 ifh[4], val, *buf;
		struct timespec64 ts;
		int sz, len, buf_len;
		struct sk_buff *skb;

		for (i = 0; i < IFH_LEN; i++) {
			err = ocelot_rx_frame_word(ocelot, grp, true, &ifh[i]);
			if (err != 4)
				break;
		}

		if (err != 4)
			break;

		ocelot_parse_ifh(ifh, &info);

		dev = ocelot->ports[info.port]->dev;

		skb = netdev_alloc_skb(dev, info.len);

		if (unlikely(!skb)) {
			netdev_err(dev, "Unable to allocate sk_buff\n");
			err = -ENOMEM;
			break;
		}
		buf_len = info.len - ETH_FCS_LEN;
		buf = (u32 *)skb_put(skb, buf_len);

		len = 0;
		do {
			sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
			*buf++ = val;
			len += sz;
		} while (len < buf_len);

		/* Read the FCS */
		sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
		/* Update the statistics if part of the FCS was read before */
		len -= ETH_FCS_LEN - sz;

		if (unlikely(dev->features & NETIF_F_RXFCS)) {
			buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
			*buf = val;
		}

		if (sz < 0) {
			err = sz;
			break;
		}

		if (ocelot->ptp) {
			ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);

			tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
			if ((tod_in_ns & 0xffffffff) < info.timestamp)
				full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) |
						info.timestamp;
			else
				full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) |
						info.timestamp;

			shhwtstamps = skb_hwtstamps(skb);
			memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
			shhwtstamps->hwtstamp = full_ts_in_ns;
		}

		/* Everything we see on an interface that is in the HW bridge
		 * has already been forwarded.
		 */
		if (ocelot->bridge_mask & BIT(info.port))
			skb->offload_fwd_mark = 1;

		skb->protocol = eth_type_trans(skb, dev);
		netif_rx(skb);
		dev->stats.rx_bytes += len;
		dev->stats.rx_packets++;
	} while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp));

	if (err)
		while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp))
			ocelot_read_rix(ocelot, QS_XTR_RD, grp);

	return IRQ_HANDLED;
}

static irqreturn_t ocelot_ptp_rdy_irq_handler(int irq, void *arg)
{
	int budget = OCELOT_PTP_QUEUE_SZ;
	struct ocelot *ocelot = arg;

	while (budget--) {
		struct skb_shared_hwtstamps shhwtstamps;
		struct list_head *pos, *tmp;
		struct sk_buff *skb = NULL;
		struct ocelot_skb *entry;
		struct ocelot_port *port;
		struct timespec64 ts;
		u32 val, id, txport;

		val = ocelot_read(ocelot, SYS_PTP_STATUS);

		/* Check if a timestamp can be retrieved */
		if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
			break;

		WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);

		/* Retrieve the ts ID and Tx port */
		id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
		txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);

		/* Retrieve its associated skb */
		port = ocelot->ports[txport];

		list_for_each_safe(pos, tmp, &port->skbs) {
			entry = list_entry(pos, struct ocelot_skb, head);
			if (entry->id != id)
				continue;

			skb = entry->skb;

			list_del(pos);
			kfree(entry);
		}

		/* Next ts */
		ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);

		if (unlikely(!skb))
			continue;

		/* Get the h/w timestamp */
		ocelot_get_hwtimestamp(ocelot, &ts);

		/* Set the timestamp into the skb */
		memset(&shhwtstamps, 0, sizeof(shhwtstamps));
		shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
		skb_tstamp_tx(skb, &shhwtstamps);

		dev_kfree_skb_any(skb);
	}

	return IRQ_HANDLED;
}

static const struct of_device_id mscc_ocelot_match[] = {
	{ .compatible = "mscc,vsc7514-switch" },
	{ }
};
MODULE_DEVICE_TABLE(of, mscc_ocelot_match);

static int mscc_ocelot_probe(struct platform_device *pdev)
{
	struct device_node *np = pdev->dev.of_node;
	struct device_node *ports, *portnp;
	int err, irq_xtr, irq_ptp_rdy;
	struct ocelot *ocelot;
	struct regmap *hsio;
	unsigned int i;
	u32 val;

	struct {
		enum ocelot_target id;
		char *name;
		u8 optional:1;
	} res[] = {
		{ SYS, "sys" },
		{ REW, "rew" },
		{ QSYS, "qsys" },
		{ ANA, "ana" },
		{ QS, "qs" },
		{ S2, "s2" },
		{ PTP, "ptp", 1 },
	};

	if (!np && !pdev->dev.platform_data)
		return -ENODEV;

	ocelot = devm_kzalloc(&pdev->dev, sizeof(*ocelot), GFP_KERNEL);
	if (!ocelot)
		return -ENOMEM;

	platform_set_drvdata(pdev, ocelot);
	ocelot->dev = &pdev->dev;

	for (i = 0; i < ARRAY_SIZE(res); i++) {
		struct regmap *target;

		target = ocelot_io_platform_init(ocelot, pdev, res[i].name);
		if (IS_ERR(target)) {
			if (res[i].optional) {
				ocelot->targets[res[i].id] = NULL;
				continue;
			}

			return PTR_ERR(target);
		}

		ocelot->targets[res[i].id] = target;
	}

	hsio = syscon_regmap_lookup_by_compatible("mscc,ocelot-hsio");
	if (IS_ERR(hsio)) {
		dev_err(&pdev->dev, "missing hsio syscon\n");
		return PTR_ERR(hsio);
	}

	ocelot->targets[HSIO] = hsio;

	err = ocelot_chip_init(ocelot);
	if (err)
		return err;

	irq_xtr = platform_get_irq_byname(pdev, "xtr");
	if (irq_xtr < 0)
		return -ENODEV;

	err = devm_request_threaded_irq(&pdev->dev, irq_xtr, NULL,
					ocelot_xtr_irq_handler, IRQF_ONESHOT,
					"frame extraction", ocelot);
	if (err)
		return err;

	irq_ptp_rdy = platform_get_irq_byname(pdev, "ptp_rdy");
	if (irq_ptp_rdy > 0 && ocelot->targets[PTP]) {
		err = devm_request_threaded_irq(&pdev->dev, irq_ptp_rdy, NULL,
						ocelot_ptp_rdy_irq_handler,
						IRQF_ONESHOT, "ptp ready",
						ocelot);
		if (err)
			return err;

		/* Both the PTP interrupt and the PTP bank are available */
		ocelot->ptp = 1;
	}

	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_INIT], 1);
	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_ENA], 1);

	do {
		msleep(1);
		regmap_field_read(ocelot->regfields[SYS_RESET_CFG_MEM_INIT],
				  &val);
	} while (val);

	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_ENA], 1);
	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_CORE_ENA], 1);

	ocelot->num_cpu_ports = 1; /* 1 port on the switch, two groups */

	ports = of_get_child_by_name(np, "ethernet-ports");
	if (!ports) {
		dev_err(&pdev->dev, "no ethernet-ports child node found\n");
		return -ENODEV;
	}

	ocelot->num_phys_ports = of_get_child_count(ports);

	ocelot->ports = devm_kcalloc(&pdev->dev, ocelot->num_phys_ports,
				     sizeof(struct ocelot_port *), GFP_KERNEL);

	INIT_LIST_HEAD(&ocelot->multicast);
	ocelot_init(ocelot);

	for_each_available_child_of_node(ports, portnp) {
		struct device_node *phy_node;
		struct phy_device *phy;
		struct resource *res;
		struct phy *serdes;
		void __iomem *regs;
		char res_name[8];
		int phy_mode;
		u32 port;

		if (of_property_read_u32(portnp, "reg", &port))
			continue;

		snprintf(res_name, sizeof(res_name), "port%d", port);

		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
						   res_name);
		regs = devm_ioremap_resource(&pdev->dev, res);
		if (IS_ERR(regs))
			continue;

		phy_node = of_parse_phandle(portnp, "phy-handle", 0);
		if (!phy_node)
			continue;

		phy = of_phy_find_device(phy_node);
		of_node_put(phy_node);
		if (!phy)
			continue;

		err = ocelot_probe_port(ocelot, port, regs, phy);
		if (err) {
			of_node_put(portnp);
			goto out_put_ports;
		}

		phy_mode = of_get_phy_mode(portnp);
		if (phy_mode < 0)
			ocelot->ports[port]->phy_mode = PHY_INTERFACE_MODE_NA;
		else
			ocelot->ports[port]->phy_mode = phy_mode;

		switch (ocelot->ports[port]->phy_mode) {
		case PHY_INTERFACE_MODE_NA:
			continue;
		case PHY_INTERFACE_MODE_SGMII:
			break;
		case PHY_INTERFACE_MODE_QSGMII:
			/* Ensure clock signals and speed is set on all
			 * QSGMII links
			 */
			ocelot_port_writel(ocelot->ports[port],
					   DEV_CLOCK_CFG_LINK_SPEED
					   (OCELOT_SPEED_1000),
					   DEV_CLOCK_CFG);
			break;
		default:
			dev_err(ocelot->dev,
				"invalid phy mode for port%d, (Q)SGMII only\n",
				port);
			of_node_put(portnp);
			err = -EINVAL;
			goto out_put_ports;
		}

		serdes = devm_of_phy_get(ocelot->dev, portnp, NULL);
		if (IS_ERR(serdes)) {
			err = PTR_ERR(serdes);
			if (err == -EPROBE_DEFER)
				dev_dbg(ocelot->dev, "deferring probe\n");
			else
				dev_err(ocelot->dev,
					"missing SerDes phys for port%d\n",
					port);

			of_node_put(portnp);
			goto out_put_ports;
		}

		ocelot->ports[port]->serdes = serdes;
	}

	register_netdevice_notifier(&ocelot_netdevice_nb);
	register_switchdev_notifier(&ocelot_switchdev_nb);
	register_switchdev_blocking_notifier(&ocelot_switchdev_blocking_nb);

	dev_info(&pdev->dev, "Ocelot switch probed\n");

out_put_ports:
	of_node_put(ports);
	return err;
}

static int mscc_ocelot_remove(struct platform_device *pdev)
{
	struct ocelot *ocelot = platform_get_drvdata(pdev);

	ocelot_deinit(ocelot);
	unregister_switchdev_blocking_notifier(&ocelot_switchdev_blocking_nb);
	unregister_switchdev_notifier(&ocelot_switchdev_nb);
	unregister_netdevice_notifier(&ocelot_netdevice_nb);

	return 0;
}

static struct platform_driver mscc_ocelot_driver = {
	.probe = mscc_ocelot_probe,
	.remove = mscc_ocelot_remove,
	.driver = {
		.name = "ocelot-switch",
		.of_match_table = mscc_ocelot_match,
	},
};

module_platform_driver(mscc_ocelot_driver);

MODULE_DESCRIPTION("Microsemi Ocelot switch driver");
MODULE_AUTHOR("Alexandre Belloni <alexandre.belloni@bootlin.com>");
MODULE_LICENSE("Dual MIT/GPL");
Commit	Line	Data
a556c76a AB	1	// SPDX-License-Identifier: (GPL-2.0 OR MIT)
	2	/*
	3	* Microsemi Ocelot Switch driver
	4	*
	5	* Copyright (c) 2017 Microsemi Corporation
	6	*/
	7	#include <linux/interrupt.h>
	8	#include <linux/module.h>
71e32a20	9	#include <linux/of_net.h>
a556c76a AB	10	#include <linux/netdevice.h>
	11	#include <linux/of_mdio.h>
	12	#include <linux/of_platform.h>
19aedfbe	13	#include <linux/mfd/syscon.h>
a556c76a	14	#include <linux/skbuff.h>
0e332c85	15	#include <net/switchdev.h>
a556c76a AB	16
	17	#include "ocelot.h"
	18
d8c964dc AT	19	#define IFH_EXTRACT_BITFIELD64(x, o, w) (((x) >> (o)) & GENMASK_ULL((w) - 1, 0))
	20
	21	static int ocelot_parse_ifh(u32 _ifh, struct frame_info info)
a556c76a	22	{
a556c76a	23	u8 llen, wlen;
d8c964dc AT	24	u64 ifh[2];
	25
	26	ifh[0] = be64_to_cpu(((__force __be64 *)_ifh)[0]);
	27	ifh[1] = be64_to_cpu(((__force __be64 *)_ifh)[1]);
a556c76a	28
d8c964dc AT	29	wlen = IFH_EXTRACT_BITFIELD64(ifh[0], 7, 8);
d8c964dc AT	30	llen = IFH_EXTRACT_BITFIELD64(ifh[0], 15, 6);
a556c76a	31
a556c76a AB	32	info->len = OCELOT_BUFFER_CELL_SZ * wlen + llen - 80;
a556c76a AB	33
4e3b0468 AT	34	info->timestamp = IFH_EXTRACT_BITFIELD64(ifh[0], 21, 32);
4e3b0468 AT	35
d8c964dc	36	info->port = IFH_EXTRACT_BITFIELD64(ifh[1], 43, 4);
a556c76a	37
d8c964dc AT	38	info->tag_type = IFH_EXTRACT_BITFIELD64(ifh[1], 16, 1);
d8c964dc AT	39	info->vid = IFH_EXTRACT_BITFIELD64(ifh[1], 0, 12);
a556c76a AB	40
	41	return 0;
	42	}
	43
	44	static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
	45	u32 *rval)
	46	{
	47	u32 val;
	48	u32 bytes_valid;
	49
	50	val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
	51	if (val == XTR_NOT_READY) {
	52	if (ifh)
	53	return -EIO;
	54
	55	do {
	56	val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
	57	} while (val == XTR_NOT_READY);
	58	}
	59
	60	switch (val) {
	61	case XTR_ABORT:
	62	return -EIO;
	63	case XTR_EOF_0:
	64	case XTR_EOF_1:
	65	case XTR_EOF_2:
	66	case XTR_EOF_3:
	67	case XTR_PRUNED:
	68	bytes_valid = XTR_VALID_BYTES(val);
	69	val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
	70	if (val == XTR_ESCAPE)
	71	*rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
	72	else
	73	*rval = val;
	74
	75	return bytes_valid;
	76	case XTR_ESCAPE:
	77	*rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
	78
	79	return 4;
	80	default:
	81	*rval = val;
	82
	83	return 4;
	84	}
	85	}
	86
	87	static irqreturn_t ocelot_xtr_irq_handler(int irq, void *arg)
	88	{
	89	struct ocelot *ocelot = arg;
	90	int i = 0, grp = 0;
	91	int err = 0;
	92
	93	if (!(ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp)))
	94	return IRQ_NONE;
	95
	96	do {
4e3b0468 AT	97	struct skb_shared_hwtstamps *shhwtstamps;
	98	u64 tod_in_ns, full_ts_in_ns;
	99	struct frame_info info = {};
a556c76a	100	struct net_device *dev;
4e3b0468 AT	101	u32 ifh[4], val, *buf;
4e3b0468 AT	102	struct timespec64 ts;
652ef42c	103	int sz, len, buf_len;
4e3b0468	104	struct sk_buff *skb;
a556c76a AB	105
	106	for (i = 0; i < IFH_LEN; i++) {
	107	err = ocelot_rx_frame_word(ocelot, grp, true, &ifh[i]);
	108	if (err != 4)
	109	break;
	110	}
	111
	112	if (err != 4)
	113	break;
	114
	115	ocelot_parse_ifh(ifh, &info);
	116
	117	dev = ocelot->ports[info.port]->dev;
	118
	119	skb = netdev_alloc_skb(dev, info.len);
	120
	121	if (unlikely(!skb)) {
	122	netdev_err(dev, "Unable to allocate sk_buff\n");
	123	err = -ENOMEM;
	124	break;
	125	}
652ef42c AT	126	buf_len = info.len - ETH_FCS_LEN;
652ef42c AT	127	buf = (u32 *)skb_put(skb, buf_len);
a556c76a AB	128
	129	len = 0;
	130	do {
	131	sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
	132	*buf++ = val;
	133	len += sz;
652ef42c AT	134	} while (len < buf_len);
652ef42c AT	135
60f8e67d	136	/* Read the FCS */
652ef42c AT	137	sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
	138	/* Update the statistics if part of the FCS was read before */
	139	len -= ETH_FCS_LEN - sz;
a556c76a	140
60f8e67d AT	141	if (unlikely(dev->features & NETIF_F_RXFCS)) {
	142	buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
	143	*buf = val;
	144	}
	145
a556c76a AB	146	if (sz < 0) {
	147	err = sz;
	148	break;
	149	}
	150
4e3b0468 AT	151	if (ocelot->ptp) {
	152	ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
	153
	154	tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
	155	if ((tod_in_ns & 0xffffffff) < info.timestamp)
	156	full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) \|
	157	info.timestamp;
	158	else
	159	full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) \|
	160	info.timestamp;
	161
	162	shhwtstamps = skb_hwtstamps(skb);
	163	memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
	164	shhwtstamps->hwtstamp = full_ts_in_ns;
	165	}
	166
a556c76a AB	167	/* Everything we see on an interface that is in the HW bridge
	168	* has already been forwarded.
	169	*/
	170	if (ocelot->bridge_mask & BIT(info.port))
	171	skb->offload_fwd_mark = 1;
	172
	173	skb->protocol = eth_type_trans(skb, dev);
	174	netif_rx(skb);
	175	dev->stats.rx_bytes += len;
	176	dev->stats.rx_packets++;
	177	} while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp));
	178
	179	if (err)
	180	while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp))
	181	ocelot_read_rix(ocelot, QS_XTR_RD, grp);
	182
	183	return IRQ_HANDLED;
	184	}
	185
4e3b0468 AT	186	static irqreturn_t ocelot_ptp_rdy_irq_handler(int irq, void *arg)
	187	{
	188	int budget = OCELOT_PTP_QUEUE_SZ;
	189	struct ocelot *ocelot = arg;
	190
	191	while (budget--) {
	192	struct skb_shared_hwtstamps shhwtstamps;
	193	struct list_head pos, tmp;
	194	struct sk_buff *skb = NULL;
	195	struct ocelot_skb *entry;
	196	struct ocelot_port *port;
	197	struct timespec64 ts;
	198	u32 val, id, txport;
	199
	200	val = ocelot_read(ocelot, SYS_PTP_STATUS);
	201
	202	/* Check if a timestamp can be retrieved */
	203	if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
	204	break;
	205
	206	WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
	207
	208	/* Retrieve the ts ID and Tx port */
	209	id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
	210	txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
	211
	212	/* Retrieve its associated skb */
	213	port = ocelot->ports[txport];
	214
	215	list_for_each_safe(pos, tmp, &port->skbs) {
	216	entry = list_entry(pos, struct ocelot_skb, head);
	217	if (entry->id != id)
	218	continue;
	219
	220	skb = entry->skb;
	221
	222	list_del(pos);
	223	kfree(entry);
	224	}
	225
	226	/* Next ts */
	227	ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
	228
	229	if (unlikely(!skb))
	230	continue;
	231
	232	/* Get the h/w timestamp */
	233	ocelot_get_hwtimestamp(ocelot, &ts);
	234
	235	/* Set the timestamp into the skb */
	236	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
	237	shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
	238	skb_tstamp_tx(skb, &shhwtstamps);
	239
	240	dev_kfree_skb_any(skb);
	241	}
	242
	243	return IRQ_HANDLED;
	244	}
	245
a556c76a AB	246	static const struct of_device_id mscc_ocelot_match[] = {
	247	{ .compatible = "mscc,vsc7514-switch" },
	248	{ }
	249	};
	250	MODULE_DEVICE_TABLE(of, mscc_ocelot_match);
	251
	252	static int mscc_ocelot_probe(struct platform_device *pdev)
	253	{
a556c76a AB	254	struct device_node *np = pdev->dev.of_node;
a556c76a AB	255	struct device_node ports, portnp;
4e3b0468	256	int err, irq_xtr, irq_ptp_rdy;
a556c76a	257	struct ocelot *ocelot;
19aedfbe	258	struct regmap *hsio;
4e3b0468	259	unsigned int i;
a556c76a AB	260	u32 val;
	261
	262	struct {
	263	enum ocelot_target id;
	264	char *name;
45bce171	265	u8 optional:1;
a556c76a AB	266	} res[] = {
	267	{ SYS, "sys" },
	268	{ REW, "rew" },
	269	{ QSYS, "qsys" },
	270	{ ANA, "ana" },
	271	{ QS, "qs" },
b5962294	272	{ S2, "s2" },
45bce171	273	{ PTP, "ptp", 1 },
a556c76a AB	274	};
	275
	276	if (!np && !pdev->dev.platform_data)
	277	return -ENODEV;
	278
	279	ocelot = devm_kzalloc(&pdev->dev, sizeof(*ocelot), GFP_KERNEL);
	280	if (!ocelot)
	281	return -ENOMEM;
	282
	283	platform_set_drvdata(pdev, ocelot);
	284	ocelot->dev = &pdev->dev;
	285
	286	for (i = 0; i < ARRAY_SIZE(res); i++) {
	287	struct regmap *target;
	288
	289	target = ocelot_io_platform_init(ocelot, pdev, res[i].name);
45bce171 AT	290	if (IS_ERR(target)) {
	291	if (res[i].optional) {
	292	ocelot->targets[res[i].id] = NULL;
	293	continue;
	294	}
	295
a556c76a	296	return PTR_ERR(target);
45bce171	297	}
a556c76a AB	298
	299	ocelot->targets[res[i].id] = target;
	300	}
	301
19aedfbe QS	302	hsio = syscon_regmap_lookup_by_compatible("mscc,ocelot-hsio");
	303	if (IS_ERR(hsio)) {
	304	dev_err(&pdev->dev, "missing hsio syscon\n");
	305	return PTR_ERR(hsio);
	306	}
	307
	308	ocelot->targets[HSIO] = hsio;
	309
a556c76a AB	310	err = ocelot_chip_init(ocelot);
	311	if (err)
	312	return err;
	313
4e3b0468 AT	314	irq_xtr = platform_get_irq_byname(pdev, "xtr");
4e3b0468 AT	315	if (irq_xtr < 0)
a556c76a AB	316	return -ENODEV;
a556c76a AB	317
4e3b0468	318	err = devm_request_threaded_irq(&pdev->dev, irq_xtr, NULL,
a556c76a AB	319	ocelot_xtr_irq_handler, IRQF_ONESHOT,
	320	"frame extraction", ocelot);
	321	if (err)
	322	return err;
	323
4e3b0468 AT	324	irq_ptp_rdy = platform_get_irq_byname(pdev, "ptp_rdy");
	325	if (irq_ptp_rdy > 0 && ocelot->targets[PTP]) {
	326	err = devm_request_threaded_irq(&pdev->dev, irq_ptp_rdy, NULL,
	327	ocelot_ptp_rdy_irq_handler,
	328	IRQF_ONESHOT, "ptp ready",
	329	ocelot);
	330	if (err)
	331	return err;
	332
	333	/* Both the PTP interrupt and the PTP bank are available */
	334	ocelot->ptp = 1;
	335	}
	336
a556c76a AB	337	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_INIT], 1);
	338	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_ENA], 1);
	339
	340	do {
	341	msleep(1);
	342	regmap_field_read(ocelot->regfields[SYS_RESET_CFG_MEM_INIT],
	343	&val);
	344	} while (val);
	345
	346	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_ENA], 1);
	347	regmap_field_write(ocelot->regfields[SYS_RESET_CFG_CORE_ENA], 1);
	348
	349	ocelot->num_cpu_ports = 1; /* 1 port on the switch, two groups */
	350
	351	ports = of_get_child_by_name(np, "ethernet-ports");
	352	if (!ports) {
	353	dev_err(&pdev->dev, "no ethernet-ports child node found\n");
	354	return -ENODEV;
	355	}
	356
	357	ocelot->num_phys_ports = of_get_child_count(ports);
	358
	359	ocelot->ports = devm_kcalloc(&pdev->dev, ocelot->num_phys_ports,
	360	sizeof(struct ocelot_port *), GFP_KERNEL);
	361
	362	INIT_LIST_HEAD(&ocelot->multicast);
	363	ocelot_init(ocelot);
	364
a556c76a AB	365	for_each_available_child_of_node(ports, portnp) {
	366	struct device_node *phy_node;
	367	struct phy_device *phy;
	368	struct resource *res;
71e32a20	369	struct phy *serdes;
a556c76a AB	370	void __iomem *regs;
a556c76a AB	371	char res_name[8];
084e5bb1	372	int phy_mode;
a556c76a AB	373	u32 port;
	374
	375	if (of_property_read_u32(portnp, "reg", &port))
	376	continue;
	377
	378	snprintf(res_name, sizeof(res_name), "port%d", port);
	379
	380	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
	381	res_name);
	382	regs = devm_ioremap_resource(&pdev->dev, res);
	383	if (IS_ERR(regs))
	384	continue;
	385
	386	phy_node = of_parse_phandle(portnp, "phy-handle", 0);
	387	if (!phy_node)
	388	continue;
	389
	390	phy = of_phy_find_device(phy_node);
d2c50b1c	391	of_node_put(phy_node);
a556c76a AB	392	if (!phy)
	393	continue;
	394
	395	err = ocelot_probe_port(ocelot, port, regs, phy);
64fc973d ND	396	if (err) {
64fc973d ND	397	of_node_put(portnp);
d2c50b1c	398	goto out_put_ports;
64fc973d	399	}
71e32a20	400
084e5bb1 KSK	401	phy_mode = of_get_phy_mode(portnp);
084e5bb1 KSK	402	if (phy_mode < 0)
71e32a20 QS	403	ocelot->ports[port]->phy_mode = PHY_INTERFACE_MODE_NA;
71e32a20 QS	404	else
084e5bb1	405	ocelot->ports[port]->phy_mode = phy_mode;
71e32a20 QS	406
	407	switch (ocelot->ports[port]->phy_mode) {
	408	case PHY_INTERFACE_MODE_NA:
	409	continue;
	410	case PHY_INTERFACE_MODE_SGMII:
71e32a20 QS	411	break;
71e32a20 QS	412	case PHY_INTERFACE_MODE_QSGMII:
084e5bb1 KSK	413	/* Ensure clock signals and speed is set on all
	414	* QSGMII links
	415	*/
	416	ocelot_port_writel(ocelot->ports[port],
	417	DEV_CLOCK_CFG_LINK_SPEED
	418	(OCELOT_SPEED_1000),
	419	DEV_CLOCK_CFG);
71e32a20 QS	420	break;
	421	default:
	422	dev_err(ocelot->dev,
	423	"invalid phy mode for port%d, (Q)SGMII only\n",
	424	port);
64fc973d	425	of_node_put(portnp);
d2c50b1c WY	426	err = -EINVAL;
d2c50b1c WY	427	goto out_put_ports;
71e32a20 QS	428	}
	429
	430	serdes = devm_of_phy_get(ocelot->dev, portnp, NULL);
	431	if (IS_ERR(serdes)) {
	432	err = PTR_ERR(serdes);
	433	if (err == -EPROBE_DEFER)
	434	dev_dbg(ocelot->dev, "deferring probe\n");
	435	else
	436	dev_err(ocelot->dev,
	437	"missing SerDes phys for port%d\n",
	438	port);
	439
d2c50b1c WY	440	of_node_put(portnp);
d2c50b1c WY	441	goto out_put_ports;
a556c76a	442	}
71e32a20 QS	443
71e32a20 QS	444	ocelot->ports[port]->serdes = serdes;
a556c76a AB	445	}
	446
	447	register_netdevice_notifier(&ocelot_netdevice_nb);
56da64bc	448	register_switchdev_notifier(&ocelot_switchdev_nb);
0e332c85	449	register_switchdev_blocking_notifier(&ocelot_switchdev_blocking_nb);
a556c76a AB	450
	451	dev_info(&pdev->dev, "Ocelot switch probed\n");
	452
d2c50b1c WY	453	out_put_ports:
d2c50b1c WY	454	of_node_put(ports);
a556c76a AB	455	return err;
	456	}
	457
	458	static int mscc_ocelot_remove(struct platform_device *pdev)
	459	{
	460	struct ocelot *ocelot = platform_get_drvdata(pdev);
	461
	462	ocelot_deinit(ocelot);
0e332c85	463	unregister_switchdev_blocking_notifier(&ocelot_switchdev_blocking_nb);
56da64bc	464	unregister_switchdev_notifier(&ocelot_switchdev_nb);
a556c76a AB	465	unregister_netdevice_notifier(&ocelot_netdevice_nb);
	466
	467	return 0;
	468	}
	469
	470	static struct platform_driver mscc_ocelot_driver = {
	471	.probe = mscc_ocelot_probe,
	472	.remove = mscc_ocelot_remove,
	473	.driver = {
	474	.name = "ocelot-switch",
	475	.of_match_table = mscc_ocelot_match,
	476	},
	477	};
	478
	479	module_platform_driver(mscc_ocelot_driver);
	480
	481	MODULE_DESCRIPTION("Microsemi Ocelot switch driver");
	482	MODULE_AUTHOR("Alexandre Belloni <alexandre.belloni@bootlin.com>");
	483	MODULE_LICENSE("Dual MIT/GPL");