D55013.diff
No OneTemporary
Actions

Size

51 KB

Referenced Files

None

Subscribers

None

D55013.diff
View Options

	diff --git a/sys/arm64/conf/std.rockchip b/sys/arm64/conf/std.rockchip
	--- a/sys/arm64/conf/std.rockchip
	+++ b/sys/arm64/conf/std.rockchip
	@@ -41,6 +41,9 @@
	device dwc # Synopsys DesignWare GMAC controller
	device dwc_rk # Rockchip Designware
	device eqos # Synopsys Designware Ethernet QoS controller
	+device etherswitch # Enable etherswitch support
	+device mtkswitch # Enable mediatek 7531 switch
	+device mdio

	# IO Domains
	device rk_iodomain
	diff --git a/sys/conf/files b/sys/conf/files
	--- a/sys/conf/files
	+++ b/sys/conf/files
	@@ -1697,6 +1697,10 @@
	dev/etherswitch/rtl8366/rtl8366rb.c optional rtl8366rb
	dev/etherswitch/e6000sw/e6000sw.c optional e6000sw fdt
	dev/etherswitch/e6000sw/e6060sw.c optional e6060sw
	+dev/etherswitch/mtkswitch/mtkswitch.c optional mtkswitch
	+dev/etherswitch/mtkswitch/mtkswitch_mt7531.c optional mtkswitch
	+dev/etherswitch/mtkswitch/mtkswitch_mt7620.c optional mtkswitch
	+dev/etherswitch/mtkswitch/mtkswitch_rt3050.c optional mtkswitch
	dev/etherswitch/infineon/adm6996fc.c optional adm6996fc
	dev/etherswitch/micrel/ksz8995ma.c optional ksz8995ma
	dev/etherswitch/ukswitch/ukswitch.c optional ukswitch
	diff --git a/sys/dev/eqos/if_eqos.c b/sys/dev/eqos/if_eqos.c
	--- a/sys/dev/eqos/if_eqos.c
	+++ b/sys/dev/eqos/if_eqos.c
	@@ -59,9 +59,13 @@

	#include <dev/mii/mii.h>
	#include <dev/mii/miivar.h>
	+#include <dev/fdt/fdt_common.h>
	+
	+#include <dev/mdio/mdio.h>

	#include "miibus_if.h"
	#include "if_eqos_if.h"
	+#include "mdio_if.h"

	#ifdef FDT
	#include <dev/ofw/openfirm.h>
	@@ -172,22 +176,13 @@
	}

	static void
	-eqos_miibus_statchg(device_t dev)
	+eqos_mac_change(struct eqos_softc *sc, uint32_t media)
	{
	- struct eqos_softc *sc = device_get_softc(dev);
	- struct mii_data *mii = device_get_softc(sc->miibus);
	uint32_t reg;

	- EQOS_ASSERT_LOCKED(sc);
	-
	- if (mii->mii_media_status & IFM_ACTIVE)
	- sc->link_up = true;
	- else
	- sc->link_up = false;
	-
	reg = RD4(sc, GMAC_MAC_CONFIGURATION);

	- switch (IFM_SUBTYPE(mii->mii_media_active)) {
	+ switch (IFM_SUBTYPE(media)) {
	case IFM_10_T:
	reg \|= GMAC_MAC_CONFIGURATION_PS;
	reg &= ~GMAC_MAC_CONFIGURATION_FES;
	@@ -211,28 +206,56 @@
	return;
	}

	- if ((IFM_OPTIONS(mii->mii_media_active) & IFM_FDX))
	+ if ((IFM_OPTIONS(media) & IFM_FDX))
	reg \|= GMAC_MAC_CONFIGURATION_DM;
	else
	reg &= ~GMAC_MAC_CONFIGURATION_DM;

	WR4(sc, GMAC_MAC_CONFIGURATION, reg);

	- IF_EQOS_SET_SPEED(dev, IFM_SUBTYPE(mii->mii_media_active));
	+ IF_EQOS_SET_SPEED(sc->dev, IFM_SUBTYPE(media));

	WR4(sc, GMAC_MAC_1US_TIC_COUNTER, (sc->csr_clock / 1000000) - 1);
	}

	+static void
	+eqos_miibus_statchg(device_t dev)
	+{
	+ struct eqos_softc *sc = device_get_softc(dev);
	+ struct mii_data *mii = device_get_softc(sc->miibus);
	+
	+ KASSERT(sc->phy_attached == 1, ("wqos_tick while PHY not attached"));
	+
	+ EQOS_ASSERT_LOCKED(sc);
	+
	+ if (mii->mii_media_status & IFM_ACTIVE)
	+ sc->link_up = true;
	+ else
	+ sc->link_up = false;
	+
	+ eqos_mac_change(sc, mii->mii_media_active);
	+}
	+
	static void
	eqos_media_status(if_t ifp, struct ifmediareq *ifmr)
	{
	struct eqos_softc *sc = if_getsoftc(ifp);
	- struct mii_data *mii = device_get_softc(sc->miibus);
	+ struct mii_data *mii;

	EQOS_LOCK(sc);
	+
	+ if (!sc->phy_attached) {
	+ ifmr->ifm_active = IFM_1000_T \| IFM_FDX \| IFM_ETHER;
	+ ifmr->ifm_status = IFM_AVALID \| IFM_ACTIVE;
	+ goto out_unlock;
	+ }
	+
	+ mii = device_get_softc(sc->miibus);
	mii_pollstat(mii);
	ifmr->ifm_active = mii->mii_media_active;
	ifmr->ifm_status = mii->mii_media_status;
	+
	+ out_unlock:
	EQOS_UNLOCK(sc);
	}

	@@ -240,11 +263,17 @@
	eqos_media_change(if_t ifp)
	{
	struct eqos_softc *sc = if_getsoftc(ifp);
	- int error;
	+ int error = 0;

	+ /*
	+ * Do not do anything for switch here
	+ */
	+
	+ if (sc->phy_attached) {
	EQOS_LOCK(sc);
	error = mii_mediachg(device_get_softc(sc->miibus));
	EQOS_UNLOCK(sc);
	+ }
	return (error);
	}

	@@ -578,8 +607,16 @@

	if_setdrvflagbits(ifp, IFF_DRV_RUNNING, IFF_DRV_OACTIVE);

	+ if (sc->switch_attached) {
	+ sc->link_up = true;
	+ eqos_mac_change(sc, IFM_ETHER \| IFM_1000_T \| IFM_FDX);
	+ }
	+
	+ if (sc->phy_attached) {
	+ mii = device_get_softc(sc->miibus);
	mii_mediachg(mii);
	callout_reset(&sc->callout, hz, eqos_tick, sc);
	+ }

	EQOS_UNLOCK(sc);
	}
	@@ -1145,6 +1182,84 @@
	return (0);
	}

	+static int
	+eqos_mdio_writereg(device_t dev, int phy, int reg, int val)
	+{
	+ struct eqos_softc *sc = device_get_softc(dev);
	+ uint32_t addr;
	+ int retry;
	+
	+ KASSERT(sc != NULL, ("NULL softc ptr!"));
	+
	+ WR4(sc, GMAC_MAC_MDIO_DATA, val);
	+
	+ addr = sc->csr_clock_range \|
	+ (phy << GMAC_MAC_MDIO_ADDRESS_PA_SHIFT) \|
	+ (reg << GMAC_MAC_MDIO_ADDRESS_RDA_SHIFT) \|
	+ GMAC_MAC_MDIO_ADDRESS_GOC_WRITE \| GMAC_MAC_MDIO_ADDRESS_GB;
	+ WR4(sc, GMAC_MAC_MDIO_ADDRESS, addr);
	+
	+ DELAY(100);
	+
	+ for (retry = MII_BUSY_RETRY; retry > 0; retry--) {
	+ addr = RD4(sc, GMAC_MAC_MDIO_ADDRESS);
	+ if (!(addr & GMAC_MAC_MDIO_ADDRESS_GB))
	+ break;
	+ DELAY(10);
	+ }
	+ if (!retry) {
	+ device_printf(dev, "phy write timeout, phy=%d reg=%d\n",
	+ phy, reg);
	+ return (ETIMEDOUT);
	+ }
	+ return (0);
	+}
	+
	+static int
	+eqos_mdio_readreg(device_t dev, int phy, int reg)
	+{
	+ struct eqos_softc *sc = device_get_softc(dev);
	+ uint32_t addr;
	+ int retry, val;
	+
	+
	+ KASSERT(sc != NULL, ("NULL softc ptr!"));
	+
	+ addr = sc->csr_clock_range \|
	+ (phy << GMAC_MAC_MDIO_ADDRESS_PA_SHIFT) \|
	+ (reg << GMAC_MAC_MDIO_ADDRESS_RDA_SHIFT) \|
	+ GMAC_MAC_MDIO_ADDRESS_GOC_READ \| GMAC_MAC_MDIO_ADDRESS_GB;
	+ WR4(sc, GMAC_MAC_MDIO_ADDRESS, addr);
	+
	+ DELAY(100);
	+
	+ for (retry = MII_BUSY_RETRY; retry > 0; retry--) {
	+ addr = RD4(sc, GMAC_MAC_MDIO_ADDRESS);
	+ if (!(addr & GMAC_MAC_MDIO_ADDRESS_GB)) {
	+ val = RD4(sc, GMAC_MAC_MDIO_DATA) & 0xFFFF;
	+ break;
	+ }
	+ DELAY(10);
	+ }
	+ if (!retry) {
	+ device_printf(dev, "phy read timeout, phy=%d reg=%d\n",
	+ phy, reg);
	+ return (ETIMEDOUT);
	+ }
	+ return (val);
	+}
	+
	+static boolean_t
	+eqos_has_switch(device_t dev)
	+{
	+#ifdef FDT
	+ phandle_t node;
	+ node = ofw_bus_get_node(dev);
	+ return (fdt_find_ethernet_prop_switch(node, OF_finddevice("/")));
	+#endif
	+ return (false);
	+}
	+
	static int
	eqos_attach(device_t dev)
	{
	@@ -1155,6 +1270,8 @@
	u_int userver, snpsver;
	int error;
	int n;
	+ int phy;
	+ phandle_t node;

	/* default values */
	sc->thresh_dma_mode = false;
	@@ -1175,6 +1292,11 @@
	return (error);

	sc->dev = dev;
	+
	+ sc->phy_attached = 0;
	+ sc->switch_attached = 0;
	+ node = ofw_bus_get_node(dev);
	+
	ver = RD4(sc, GMAC_MAC_VERSION);
	userver = (ver & GMAC_MAC_VERSION_USERVER_MASK) >>
	GMAC_MAC_VERSION_USERVER_SHIFT;
	@@ -1241,12 +1363,36 @@
	if_setcapenable(ifp, if_getcapabilities(ifp));

	/* Attach MII driver */
	+ if (fdt_get_phyaddr(node, sc->dev, &phy, NULL) == 0) {
	if ((error = mii_attach(sc->dev, &sc->miibus, ifp, eqos_media_change,
	- eqos_media_status, BMSR_DEFCAPMASK, MII_PHY_ANY,
	+ eqos_media_status, BMSR_DEFCAPMASK, phy,
	MII_OFFSET_ANY, 0))) {
	device_printf(sc->dev, "PHY attach failed\n");
	return (ENXIO);
	}
	+ sc->phy_attached = 1;
	+ } else {
	+ /* Fixed-link, use predefined values */
	+ ifmedia_init(&sc->eqos_ifmedia, 0,
	+ eqos_media_change,
	+ eqos_media_status);
	+ ifmedia_add(&sc->eqos_ifmedia,
	+ IFM_ETHER \| IFM_1000_T \| IFM_FDX,
	+ 0, NULL);
	+ ifmedia_set(&sc->eqos_ifmedia,
	+ IFM_ETHER \| IFM_1000_T \| IFM_FDX);
	+
	+ if (eqos_has_switch(dev)) {
	+ device_t child;
	+ child = device_add_child(dev, "mdio", -1);
	+ bus_attach_children(dev);
	+ bus_attach_children(child);
	+ device_printf(dev, "Switch attached.\n");
	+ sc->switch_attached = 1;
	+ } else {
	+ device_printf(dev, "PHY not attached.\n");
	+ }
	+ }

	/* Attach ethernet interface */
	ether_ifattach(ifp, eaddr);
	diff --git a/sys/dev/eqos/if_eqos_var.h b/sys/dev/eqos/if_eqos_var.h
	--- a/sys/dev/eqos/if_eqos_var.h
	+++ b/sys/dev/eqos/if_eqos_var.h
	@@ -92,6 +92,7 @@
	uint32_t ttc;
	uint32_t rtc;

	+ struct ifmedia eqos_ifmedia;
	struct ifnet *ifp;
	device_t miibus;
	struct mtx lock;
	@@ -99,6 +100,9 @@

	struct eqos_ring tx;
	struct eqos_ring rx;
	+
	+ int phy_attached;
	+ int switch_attached;
	};

	DECLARE_CLASS(eqos_driver);
	diff --git a/sys/dev/etherswitch/mtkswitch/mtkswitch.c b/sys/dev/etherswitch/mtkswitch/mtkswitch.c
	--- a/sys/dev/etherswitch/mtkswitch/mtkswitch.c
	+++ b/sys/dev/etherswitch/mtkswitch/mtkswitch.c
	@@ -1,4 +1,5 @@
	/*-
	+ * Copyright (c) 2026 Martin Filla
	* Copyright (c) 2016 Stanislav Galabov.
	* Copyright (c) 2011-2012 Stefan Bethke.
	* Copyright (c) 2012 Adrian Chadd.
	@@ -38,6 +39,7 @@
	#include <sys/sockio.h>
	#include <sys/sysctl.h>
	#include <sys/systm.h>
	+#include <sys/rman.h>

	#include <net/if.h>
	#include <net/if_var.h>
	@@ -78,31 +80,60 @@
	{ "mediatek,mt7620-gsw", MTK_SWITCH_MT7620 },
	{ "mediatek,mt7621-gsw", MTK_SWITCH_MT7621 },
	{ "mediatek,mt7628-esw", MTK_SWITCH_MT7628 },
	+ { "mediatek,mt7531", MTK_SWITCH_MT7531},

	/* Sentinel */
	{ NULL, MTK_SWITCH_NONE }
	};

	+static void
	+mtkswitch_identify(driver_t *driver, device_t parent)
	+{
	+ if (device_find_child(parent, "mtkswitch", -1) == NULL)
	+ BUS_ADD_CHILD(parent, 0, "mtkswitch", -1);
	+}
	+
	static int
	mtkswitch_probe(device_t dev)
	{
	struct mtkswitch_softc *sc;
	mtk_switch_type switch_type;
	+ phandle_t switch_node;
	+
	+ sc = device_get_softc(dev);
	+ bzero(sc, sizeof(*sc));

	if (!ofw_bus_status_okay(dev))
	return (ENXIO);

	switch_type = ofw_bus_search_compatible(dev, compat_data)->ocd_data;
	if (switch_type == MTK_SWITCH_NONE)
	+ {
	+ // TODO: remove this hack
	+ switch_node = ofw_bus_find_compatible(OF_finddevice("/"),
	+ "mediatek,mt7531");
	+ if (switch_node == MTK_SWITCH_NONE \|\| switch_node == 0)
	+ {
	return (ENXIO);
	-
	- sc = device_get_softc(dev);
	- bzero(sc, sizeof(*sc));
	+ }
	+ else
	+ {
	+ if (bootverbose)
	+ {
	+ device_printf(dev, "Found switch_node: 0x%x\n", switch_node);
	+ }
	+
	+ sc->sc_switchtype = MTK_SWITCH_MT7531;
	+ }
	+ }
	+ else
	+ {
	sc->sc_switchtype = switch_type;
	+ }

	device_set_desc(dev, "MTK Switch Driver");

	- return (0);
	+ return (BUS_PROBE_DEFAULT);
	}

	static int
	@@ -172,7 +203,22 @@
	/* sc->sc_switchtype is already decided in mtkswitch_probe() */
	sc->numports = MTKSWITCH_MAX_PORTS;
	sc->numphys = MTKSWITCH_MAX_PHYS;
	- sc->cpuport = MTKSWITCH_CPU_PORT;
	+ if (sc->sc_switchtype == MTK_SWITCH_MT7621 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_MT7621 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_MT7628 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT3050 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT3352 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT5350)
	+ {
	+ /* mt7621/21/28/3050/3352/5350 has 6 ports */
	+ sc->cpuport = 6;
	+ }
	+ else if (sc->sc_switchtype == MTK_SWITCH_MT7531)
	+ {
	+ /bpi-r2-pro has cpu port 5/
	+ sc->cpuport = 5;
	+ }
	+
	sc->sc_dev = dev;

	/* Attach switch related functions */
	@@ -182,11 +228,21 @@
	}

	if (sc->sc_switchtype == MTK_SWITCH_MT7620 \|\|
	- sc->sc_switchtype == MTK_SWITCH_MT7621)
	+ sc->sc_switchtype == MTK_SWITCH_MT7621) {
	mtk_attach_switch_mt7620(sc);
	- else
	+ } else if (sc->sc_switchtype == MTK_SWITCH_MT7531) {
	+ mtk_attach_switch_mt7531(sc);
	+ } else {
	mtk_attach_switch_rt3050(sc);
	+ }

	+ if (sc->sc_switchtype == MTK_SWITCH_MT7621 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_MT7621 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_MT7628 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT3050 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT3352 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT5350)
	+ {
	/* Allocate resources */
	rid = 0;
	sc->sc_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
	@@ -195,6 +251,7 @@
	device_printf(dev, "could not map memory\n");
	return (ENXIO);
	}
	+ }

	mtx_init(&sc->sc_mtx, "mtkswitch", NULL, MTX_DEF);

	@@ -232,7 +289,17 @@
	return (err);

	bus_identify_children(dev);
	+
	+ if (sc->sc_switchtype == MTK_SWITCH_MT7621 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_MT7621 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_MT7628 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT3050 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT3352 \|\|
	+ sc->sc_switchtype == MTK_SWITCH_RT5350)
	+ {
	bus_enumerate_hinted_children(dev);
	+ }
	+
	bus_attach_children(dev);

	callout_init_mtx(&sc->callout_tick, &sc->sc_mtx, 0);
	@@ -241,9 +308,14 @@
	mtkswitch_tick(sc);
	MTKSWITCH_UNLOCK(sc);

	+ if (sc->sc_switchtype == MTK_SWITCH_MT7531) {
	+ mt7531_sysctl_attach(sc);
	+ }
	+
	return (0);
	}

	+
	static int
	mtkswitch_detach(device_t dev)
	{
	@@ -591,6 +663,14 @@
	return (sc->hal.mtkswitch_phy_read(dev, phy, reg));
	}

	+static int
	+mtkswitch_readphy_mii(device_t dev, int phy, int reg)
	+{
	+ struct mtkswitch_softc *sc = device_get_softc(dev);
	+
	+ return (sc->hal.mtkswitch_phy_read(dev, phy, reg));
	+}
	+
	static int
	mtkswitch_writephy(device_t dev, int phy, int reg, int val)
	{
	@@ -599,6 +679,14 @@
	return (sc->hal.mtkswitch_phy_write(dev, phy, reg, val));
	}

	+static int
	+mtkswitch_writephy_mii(device_t dev, int phy, int reg, int val)
	+{
	+ struct mtkswitch_softc *sc = device_get_softc(dev);
	+
	+ return (sc->hal.mtkswitch_phy_write(dev, phy, reg, val));
	+}
	+
	static int
	mtkswitch_readreg(device_t dev, int addr)
	{
	@@ -617,6 +705,7 @@

	static device_method_t mtkswitch_methods[] = {
	/* Device interface */
	+ DEVMETHOD(device_identify, mtkswitch_identify),
	DEVMETHOD(device_probe, mtkswitch_probe),
	DEVMETHOD(device_attach, mtkswitch_attach),
	DEVMETHOD(device_detach, mtkswitch_detach),
	@@ -625,13 +714,13 @@
	DEVMETHOD(bus_add_child, device_add_child_ordered),

	/* MII interface */
	- DEVMETHOD(miibus_readreg, mtkswitch_readphy),
	- DEVMETHOD(miibus_writereg, mtkswitch_writephy),
	+ DEVMETHOD(miibus_readreg, mtkswitch_readphy_mii),
	+ DEVMETHOD(miibus_writereg, mtkswitch_writephy_mii),
	DEVMETHOD(miibus_statchg, mtkswitch_statchg),

	/* MDIO interface */
	- DEVMETHOD(mdio_readreg, mtkswitch_readphy),
	- DEVMETHOD(mdio_writereg, mtkswitch_writephy),
	+ //DEVMETHOD(mdio_readreg, mtkswitch_readphy),
	+ //DEVMETHOD(mdio_writereg, mtkswitch_writephy),

	/* ehterswitch interface */
	DEVMETHOD(etherswitch_lock, mtkswitch_lock),
	@@ -648,16 +737,28 @@
	DEVMETHOD(etherswitch_getconf, mtkswitch_getconf),
	DEVMETHOD(etherswitch_setconf, mtkswitch_setconf),

	+ DEVMETHOD(etherswitch_fetch_table, mt7531_atu_fetch_table),
	+ DEVMETHOD(etherswitch_fetch_table_entry, mt7531_atu_fetch_table_entry),
	+
	DEVMETHOD_END
	};

	DEFINE_CLASS_0(mtkswitch, mtkswitch_driver, mtkswitch_methods,
	sizeof(struct mtkswitch_softc));

	-DRIVER_MODULE(mtkswitch, simplebus, mtkswitch_driver, 0, 0);
	+//#ifndef MT7531
	+//DRIVER_MODULE(mtkswitch, simplebus, mtkswitch_driver, 0, 0);
	+//DRIVER_MODULE(miibus, mtkswitch, miibus_driver, 0, 0);
	+//DRIVER_MODULE(mdio, mtkswitch, mdio_driver, 0, 0);
	+//DRIVER_MODULE(etherswitch, mtkswitch, etherswitch_driver, 0, 0);
	+//MODULE_VERSION(mtkswitch, 1);
	+//MODULE_DEPEND(mtkswitch, miibus, 1, 1, 1);
	+//MODULE_DEPEND(mtkswitch, etherswitch, 1, 1, 1);
	+//MODULE_DEPEND(mtkswitch, mdio, 1, 1, 1); //
	+//#else
	+DRIVER_MODULE(mtkswitch, mdio, mtkswitch_driver, 0, 0);
	DRIVER_MODULE(miibus, mtkswitch, miibus_driver, 0, 0);
	-DRIVER_MODULE(mdio, mtkswitch, mdio_driver, 0, 0);
	DRIVER_MODULE(etherswitch, mtkswitch, etherswitch_driver, 0, 0);
	MODULE_VERSION(mtkswitch, 1);
	-MODULE_DEPEND(mtkswitch, miibus, 1, 1, 1);
	-MODULE_DEPEND(mtkswitch, etherswitch, 1, 1, 1);
	+MODULE_DEPEND(mtkswitch, mdio, 1, 1, 1);
	+//#endif
	\ No newline at end of file
	diff --git a/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7531.h b/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7531.h
	new file mode 100644
	--- /dev/null
	+++ b/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7531.h
	@@ -0,0 +1,178 @@
	+/*-
	+ * Copyright (c) 2023 Priit Trees.
	+ * All rights reserved.
	+ *
	+ * Redistribution and use in source and binary forms, with or without
	+ * modification, are permitted provided that the following conditions
	+ * are met:
	+ * 1. Redistributions of source code must retain the above copyright
	+ * notice, this list of conditions and the following disclaimer.
	+ * 2. Redistributions in binary form must reproduce the above copyright
	+ * notice, this list of conditions and the following disclaimer in the
	+ * documentation and/or other materials provided with the distribution.
	+ *
	+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
	+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
	+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
	+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
	+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
	+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
	+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
	+ * SUCH DAMAGE.
	+ *
	+ * $FreeBSD$
	+ */
	+
	+#ifndef __MTKSWITCH_MT7631_H__
	+#define __MTKSWITCH_MT7631_H__
	+
	+#define MTKSWITCH_ATC 0x0080
	+#define ATC_BUSY (1u<<15)
	+#define ATC_AC_MAT_NON_STATIC_MACS (4u<<8)
	+#define ATC_AC_CMD_CLEAN (2u<<0)
	+
	+#define MTKSWITCH_VTCR 0x0090
	+#define VTCR_BUSY (1u<<31)
	+#define VTCR_FUNC_VID_READ (0u<<12)
	+#define VTCR_FUNC_VID_WRITE (1u<<12)
	+#define VTCR_FUNC_VID_INVALID (2u<<12)
	+#define VTCR_FUNC_VID_VALID (3u<<12)
	+#define VTCR_IDX_INVALID (1u<<16)
	+#define VTCR_VID_MASK 0xfff
	+
	+#define MTKSWITCH_VAWD1 0x0094
	+#define VAWD1_IVL_MAC (1u<<30)
	+#define VAWD1_VTAG_EN (1u<<28)
	+#define VAWD1_MEMBER_OFF 16
	+#define VAWD1_MEMBER_MASK 0xff
	+#define VAWD1_FID_OFFSET 1
	+#define VAWD1_VALID (1u<<0)
	+
	+#define MTKSWITCH_VAWD2 0x0098
	+#define VAWD2_PORT_UNTAGGED(p) (0u<<((p)*2))
	+#define VAWD2_PORT_TAGGED(p) (2u<<((p)*2))
	+#define VAWD2_PORT_MASK(p) (3u<<((p)*2))
	+
	+#define MTKSWITCH_PIAC 0x701C
	+#define PIAC_PHY_ACS_ST (1u<<31)
	+#define PIAC_MDIO_REG_ADDR_OFF 25
	+#define PIAC_MDIO_PHY_ADDR_OFF 20
	+#define PIAC_MDIO_CMD_WRITE (1u<<18)
	+#define PIAC_MDIO_CMD_READ (2u<<18)
	+#define PIAC_MDIO_ST (1u<<16)
	+#define PIAC_MDIO_RW_DATA_MASK 0xffff
	+
	+#define MTKSWITCH_PORTREG(r, p) ((r) + ((p) * 0x100))
	+
	+#define MTKSWITCH_PCR(x) MTKSWITCH_PORTREG(0x2004, (x))
	+#define PCR_PORT_VLAN_SECURE (3u<<0)
	+
	+#define MTKSWITCH_PVC(x) MTKSWITCH_PORTREG(0x2010, (x))
	+#define PVC_VLAN_ATTR_MASK (3u<<6)
	+
	+#define MTKSWITCH_PPBV1(x) MTKSWITCH_PORTREG(0x2014, (x))
	+#define MTKSWITCH_PPBV2(x) MTKSWITCH_PORTREG(0x2018, (x))
	+#define PPBV_VID(v) (((v)<<16) \| (v))
	+#define PPBV_VID_FROM_REG(x) ((x) & 0xfff)
	+#define PPBV_VID_MASK 0xfff
	+
	+#define MTKSWITCH_PMCR(x) MTKSWITCH_PORTREG(0x3000, (x))
	+#define PMCR_FORCE_LINK (1u<<0)
	+#define PMCR_FORCE_DPX (1u<<1)
	+#define PMCR_FORCE_SPD_1000 (2u<<2)
	+#define PMCR_FORCE_TX_FC (1u<<4)
	+#define PMCR_FORCE_RX_FC (1u<<5)
	+#define PMCR_BACKPR_EN (1u<<8)
	+#define PMCR_BKOFF_EN (1u<<9)
	+#define PMCR_MAC_RX_EN (1u<<13)
	+#define PMCR_MAC_TX_EN (1u<<14)
	+#define PMCR_MAC_MODE (1u<<16)
	+#define PMCR_IPG_CFG_RND (1u<<18)
	+#define MT7531_PMCR_FORCE_TX_FC (1u<<27)
	+#define MT7531_PMCR_FORCE_RX_FC (1u<<28)
	+#define MT7531_PMCR_FORCE_DPX (1u<<29)
	+#define MT7531_PMCR_FORCE_SPX (1u<<30)
	+#define MT7531_PMCR_FORCE_LINK (1u<<31)
	+#define MT7631_PMCR_FORCE_MODE (MT7531_PMCR_FORCE_TX_FC \| \
	+ MT7531_PMCR_FORCE_RX_FC \| MT7531_PMCR_FORCE_DPX \| \
	+ MT7531_PMCR_FORCE_SPX \| MT7531_PMCR_FORCE_LINK)
	+#define PMCR_CFG_DEFAULT (PMCR_BACKPR_EN \| PMCR_BKOFF_EN \| \
	+ PMCR_MAC_RX_EN \| PMCR_MAC_TX_EN \| PMCR_IPG_CFG_RND \| \
	+ PMCR_FORCE_RX_FC \| PMCR_FORCE_TX_FC)
	+
	+#define MTKSWITCH_PMSR(x) MTKSWITCH_PORTREG(0x3008, (x))
	+#define PMSR_MAC_LINK_STS (1u<<0)
	+#define PMSR_MAC_DPX_STS (1u<<1)
	+#define PMSR_MAC_SPD_STS (3u<<2)
	+#define PMSR_MAC_SPD(x) (((x)>>2) & 0x3)
	+#define PMSR_MAC_SPD_10 0
	+#define PMSR_MAC_SPD_100 1
	+#define PMSR_MAC_SPD_1000 2
	+#define PMSR_TX_FC_STS (1u<<4)
	+#define PMSR_RX_FC_STS (1u<<5)
	+#define MTKSWITCH_REG_ADDR(r) (((r) >> 6) & 0x3ff)
	+#define MTKSWITCH_REG_LO(r) (((r) >> 2) & 0xf)
	+#define MTKSWITCH_REG_HI(r) (1 << 4)
	+#define MTKSWITCH_VAL_LO(v) ((v) & 0xffff)
	+#define MTKSWITCH_VAL_HI(v) (((v) >> 16) & 0xffff)
	+#define MTKSWITCH_GLOBAL_PHY 31
	+#define MTKSWITCH_GLOBAL_REG 31
	+
	+#define MTKSWITCH_STRAP 0x7800
	+#define MTKSWITCH_SWSTRAP 0x7804
	+#define STRAP_TM_STRAP (1u<<0)
	+#define STRAP_EEPROM_MODE (1u<<1)
	+#define STRAP_PWR_ON_LIGHT (1u<<2)
	+#define STRAP_PWR_ON_PLL (1u<<3)
	+#define STRAP_EEE (1u<<4)
	+#define STRAP_AUTO_EEPROM (1u<<5)
	+#define STRAP_PHY (1u<<6)
	+#define STRAP_XTAL (1u<<7) /* 1 25MHz 0 40MHz */
	+#define SWSTRAP_CH_STRAP (1u<<9)
	+
	+#define MT7531_TOP_SIG_SR 0x780c
	+#define PAD_DUAL_SGMII (1u << 1)
	+#define PAD_MCM_SMI (1u << 0)
	+
	+#define MTKSWITCH_CREV 0x781C
	+#define CREV_CHIP_NAME(x) ((x >> 16) & 0xFFFF)
	+#define CREV_CHIP_REV(x) (x & 0xF)
	+
	+#define MT7531_PLLGP 0x7820
	+#define PLLGP_COREPLL (1u << 2)
	+#define PLLGP_SW_CLKSW (1u << 1)
	+#define PLLGP_SW_PLLGP (1u << 0)
	+
	+#define MT7531_PLLGP_CR0 0x78a8
	+#define PLLGP_RG_COREPLL (1u << 22)
	+#define PLLGP_RG_COREPLL_POSDIV_S 23
	+#define PLLGP_RG_COREPLL_POSDIV_M 0x3800000
	+#define PLLGP_RG_COREPLL_SDM_PCW_S 1
	+#define PLLGP_RG_COREPLL_SDM_PCW_M 0x3ffffe
	+#define MT7531_XTAL_25MHZ 0x140000
	+#define MT7531_XTAL_40MHZ 0x190000
	+#define PLLGP_RG_COREPLL_SDM_PCW_CHG (1u << 0)
	+
	+/* RGMII and SGMII PLL clock */
	+#define MT7531_ANA_PLLGP_CR2 0x78b0
	+#define MT7531_ANA_PLLGP_CR5 0x78bc
	+
	+#define MT7531_ATC 0x80
	+#define MT7531_AC_CMD_RD 0x00
	+#define MT7531_AC_CMD_RW 0x01
	+#define MT7531_AC_CMD_CLN 0x02
	+#define MT7531_AC_CMD_SSC 0x04
	+#define MT7531_AC_CMD_NSC 0x05
	+
	+#define MT7531_ATC_ADDR_INVLD (1u<<12)
	+#define MT7531_ATC_SRCH_HIT (1u<<13)
	+#define MT7531_ATC_SRCH_END (1u<<14)
	+#define MT7531_ATC_BUSY (1u<<15)
	+#define MT7531_TSRA1 0x84
	+#define MT7531_TSRA2 0x88
	+#define MT7531_TSRD 0x8C
	+
	+#endif /* __MTKSWITCH_MT7531_H__ */
	\ No newline at end of file
	diff --git a/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7531.c b/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7531.c
	new file mode 100644
	--- /dev/null
	+++ b/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7531.c
	@@ -0,0 +1,946 @@
	+/*-
	+ * Copyright (c) 2023 Priit Trees
	+ * All rights reserved.
	+ *
	+ * Redistribution and use in source and binary forms, with or without
	+ * modification, are permitted provided that the following conditions
	+ * are met:
	+ * 1. Redistributions of source code must retain the above copyright
	+ * notice, this list of conditions and the following disclaimer.
	+ * 2. Redistributions in binary form must reproduce the above copyright
	+ * notice, this list of conditions and the following disclaimer in the
	+ * documentation and/or other materials provided with the distribution.
	+ *
	+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
	+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
	+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
	+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
	+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
	+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
	+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
	+ * SUCH DAMAGE.
	+ *
	+ * $FreeBSD$
	+ */
	+
	+#include <sys/param.h>
	+#include <sys/bus.h>
	+#include <sys/errno.h>
	+#include <sys/kernel.h>
	+#include <sys/lock.h>
	+#include <sys/malloc.h>
	+#include <sys/module.h>
	+#include <sys/mutex.h>
	+#include <sys/rman.h>
	+#include <sys/socket.h>
	+#include <sys/sockio.h>
	+#include <sys/sysctl.h>
	+#include <sys/systm.h>
	+
	+#include <net/if.h>
	+#include <net/if_var.h>
	+#include <net/ethernet.h>
	+#include <net/if_media.h>
	+#include <net/if_types.h>
	+
	+#include <machine/bus.h>
	+#include <dev/mii/mii.h>
	+#include <dev/mii/miivar.h>
	+#include <dev/mdio/mdio.h>
	+
	+#include <dev/etherswitch/etherswitch.h>
	+#include <dev/etherswitch/mtkswitch/mtkswitchvar.h>
	+#include <dev/etherswitch/mtkswitch/mtkswitch_mt7531.h>
	+
	+#include "etherswitch_if.h"
	+#include "miibus_if.h"
	+#include "mdio_if.h"
	+
	+#define MDIO_READ(dev, addr, reg) \
	+ MDIO_READREG(device_get_parent(dev), (addr), (reg))
	+#define MDIO_WRITE(dev, addr, reg, val) \
	+ MDIO_WRITEREG(device_get_parent(dev), (addr), (reg), (val))
	+
	+static int
	+mtkswitch_phy_read_locked(struct mtkswitch_softc *sc, int phy, int reg)
	+{
	+ uint32_t data;
	+
	+ while (sc->hal.mtkswitch_read(sc, MTKSWITCH_PIAC) & PIAC_PHY_ACS_ST);
	+
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_PIAC,
	+ PIAC_PHY_ACS_ST \| PIAC_MDIO_ST \| (reg << PIAC_MDIO_REG_ADDR_OFF) \|
	+ (phy << PIAC_MDIO_PHY_ADDR_OFF) \| PIAC_MDIO_CMD_READ);
	+
	+ while ((data = sc->hal.mtkswitch_read(sc,MTKSWITCH_PIAC)) & PIAC_PHY_ACS_ST);
	+
	+ return ((int)(data & PIAC_MDIO_RW_DATA_MASK));
	+
	+}
	+
	+static int
	+mtkswitch_phy_read(device_t dev, int phy, int reg)
	+{
	+ struct mtkswitch_softc *sc = device_get_softc(dev);
	+ int data;
	+
	+ if ((phy < 0 \|\| phy >= 32) \|\| (reg < 0 \|\| reg >= 32))
	+ return (ENXIO);
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
	+ MTKSWITCH_LOCK(sc);
	+ data = mtkswitch_phy_read_locked(sc, phy, reg);
	+ MTKSWITCH_UNLOCK(sc);
	+
	+ return (data);
	+}
	+
	+static int
	+mtkswitch_phy_write_locked(struct mtkswitch_softc *sc, int phy, int reg,
	+ int val)
	+{
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_PIAC,
	+ PIAC_PHY_ACS_ST \| PIAC_MDIO_ST \| (reg << PIAC_MDIO_REG_ADDR_OFF) \|
	+ (phy << PIAC_MDIO_PHY_ADDR_OFF) \| PIAC_MDIO_CMD_WRITE \|
	+ (val & PIAC_MDIO_RW_DATA_MASK));
	+ while (sc->hal.mtkswitch_read(sc, MTKSWITCH_PIAC) & PIAC_PHY_ACS_ST);
	+
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_phy_write(device_t dev, int phy, int reg, int val)
	+{
	+ struct mtkswitch_softc *sc = device_get_softc(dev);
	+ int res;
	+
	+ if ((phy < 0 \|\| phy >= 32) \|\| (reg < 0 \|\| reg >= 32))
	+ return (ENXIO);
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
	+ MTKSWITCH_LOCK(sc);
	+ res = mtkswitch_phy_write_locked(sc, phy, reg, val);
	+ MTKSWITCH_UNLOCK(sc);
	+
	+ return (res);
	+}
	+
	+static uint32_t
	+mtkswitch_reg_read32(struct mtkswitch_softc *sc, int reg)
	+{
	+ uint32_t low, hi;
	+
	+ MDIO_WRITE(sc->sc_dev, MTKSWITCH_GLOBAL_PHY,
	+ MTKSWITCH_GLOBAL_REG, MTKSWITCH_REG_ADDR(reg));
	+ low = MDIO_READ(sc->sc_dev, MTKSWITCH_GLOBAL_PHY,
	+ MTKSWITCH_REG_LO(reg));
	+ hi = MDIO_READ(sc->sc_dev, MTKSWITCH_GLOBAL_PHY,
	+ MTKSWITCH_REG_HI(reg));
	+ return (low \| (hi << 16));
	+}
	+
	+static uint32_t
	+mtkswitch_reg_write32(struct mtkswitch_softc *sc, int reg, uint32_t val)
	+{
	+
	+ MDIO_WRITE(sc->sc_dev, MTKSWITCH_GLOBAL_PHY,
	+ MTKSWITCH_GLOBAL_REG, MTKSWITCH_REG_ADDR(reg));
	+ MDIO_WRITE(sc->sc_dev, MTKSWITCH_GLOBAL_PHY,
	+ MTKSWITCH_REG_LO(reg), MTKSWITCH_VAL_LO(val));
	+ MDIO_WRITE(sc->sc_dev, MTKSWITCH_GLOBAL_PHY,
	+ MTKSWITCH_REG_HI(reg), MTKSWITCH_VAL_HI(val));
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_reg_read(device_t dev, int reg)
	+{
	+
	+ struct mtkswitch_softc *sc = device_get_softc(dev);
	+ uint32_t val;
	+
	+ val = sc->hal.mtkswitch_read(sc, MTKSWITCH_REG32(reg));
	+ return val;
	+}
	+
	+static int
	+mtkswitch_reg_write(device_t dev, int reg, int val)
	+{
	+ struct mtkswitch_softc *sc = device_get_softc(dev);
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_REG32(reg), val);
	+
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_reset(struct mtkswitch_softc *sc)
	+{
	+
	+ /* We don't reset the switch for now */
	+ return (0);
	+}
	+
	+static void
	+mtkswitch_setup_xtal(struct mtkswitch_softc *sc, uint32_t xtal)
	+{
	+ uint32_t val;
	+
	+ /* Step 1 : Disable MT7531 COREPLL */
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP);
	+ val &= ~PLLGP_COREPLL;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP, val);
	+
	+ /* Step 2: switch to XTAL output */
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP);
	+ val \|= PLLGP_SW_CLKSW;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP, val);
	+
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP_CR0);
	+ val &= ~PLLGP_RG_COREPLL;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP_CR0, val);
	+
	+ /* Step 3: disable PLLGP and enable program PLLGP */
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP);
	+ val \|= PLLGP_SW_PLLGP;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP, val);
	+
	+ /* Step 4: program COREPLL output frequency to 500MHz */
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP_CR0);
	+ val &= ~PLLGP_RG_COREPLL_POSDIV_M;
	+ val \|= 2 << PLLGP_RG_COREPLL_POSDIV_S;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP_CR0, val);
	+ DELAY(35);
	+
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP_CR0);
	+ val &= ~PLLGP_RG_COREPLL_SDM_PCW_M;
	+ val \|= xtal << PLLGP_RG_COREPLL_SDM_PCW_S;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP_CR0, val);
	+
	+ /* Set feedback divide ratio update signal to high */
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP_CR0);
	+ val \|= PLLGP_RG_COREPLL_SDM_PCW_CHG;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP_CR0, val);
	+ /* Wait for at least 16 XTAL clocks */
	+ DELAY(20);
	+
	+ /* Step 5: set feedback divide ratio update signal to low */
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP_CR0);
	+ val &= ~PLLGP_RG_COREPLL_SDM_PCW_CHG;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP_CR0, val);
	+
	+ /* Enable 325M clock for SGMII */
	+ sc->hal.mtkswitch_write(sc, MT7531_ANA_PLLGP_CR5, 0xad0000);
	+
	+ /* Enable 250SSC clock for RGMII */
	+ sc->hal.mtkswitch_write(sc, MT7531_ANA_PLLGP_CR2, 0x4f40000);
	+
	+ /* Step 6: Enable MT7531 PLL */
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP_CR0);
	+ val \|= PLLGP_RG_COREPLL;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP_CR0, val);
	+
	+ val = sc->hal.mtkswitch_read(sc, MT7531_PLLGP);
	+ val \|= PLLGP_COREPLL;
	+ sc->hal.mtkswitch_write(sc, MT7531_PLLGP, val);
	+ DELAY(35);
	+}
	+
	+static int
	+mtkswitch_hw_setup(struct mtkswitch_softc *sc)
	+{
	+
	+ /*
	+ * TODO: parse the device tree and see if we need to configure
	+ * ports, etc. differently. For now we fallback to defaults.
	+ */
	+
	+ uint32_t crev, topsig, val, xtal;
	+
	+ crev = sc->hal.mtkswitch_read(sc, MTKSWITCH_CREV);
	+ topsig = sc->hal.mtkswitch_read(sc, MT7531_TOP_SIG_SR);
	+
	+ /* Print chip name and revision. In future need
	+ * to move other place and mayby can use it.
	+ */
	+ device_printf(sc->sc_dev, "chip %s rev 0x%x\n",
	+ topsig & PAD_DUAL_SGMII ? "MT7531AE" : "MT7531BE",
	+ CREV_CHIP_REV(crev));
	+
	+ /* MT7531AE has got two SGMII units. One for port 5, one for port 6.
	+ * MT7531BE has got only one SGMII unit which is for port 6.
	+ */
	+ if (topsig & PAD_DUAL_SGMII)
	+ return (0);
	+
	+ /* on linux rev is checked. if it bigger than 0
	+ * and SMI is enabled then 40MHz other 25MHz
	+ * else hwstrap is set then 25MHz other 40MHz
	+ */
	+
	+ val = sc->hal.mtkswitch_read(sc, MTKSWITCH_STRAP);
	+ if (val & STRAP_XTAL) /* starp is set 25MHz */
	+ xtal = MT7531_XTAL_25MHZ;
	+ else /* starp is not set. Then is used 40MHz */
	+ xtal = MT7531_XTAL_40MHZ;
	+
	+ mtkswitch_setup_xtal(sc, xtal);
	+
	+ /* Called early and hence unlocked */
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_hw_global_setup(struct mtkswitch_softc *sc)
	+{
	+ /* Currently does nothing */
	+
	+ /* Called early and hence unlocked */
	+ return (0);
	+}
	+
	+static void
	+mtkswitch_port_init(struct mtkswitch_softc *sc, int port)
	+{
	+ uint32_t val;
	+
	+ /* Called early and hence unlocked */
	+
	+ /* Set the port to secure mode */
	+ val = sc->hal.mtkswitch_read(sc, MTKSWITCH_PCR(port));
	+ val \|= PCR_PORT_VLAN_SECURE;
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_PCR(port), val);
	+
	+ /* Set port's vlan_attr to user port */
	+ val = sc->hal.mtkswitch_read(sc, MTKSWITCH_PVC(port));
	+ val &= ~PVC_VLAN_ATTR_MASK;
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_PVC(port), val);
	+
	+ val = PMCR_CFG_DEFAULT;
	+ if (port == sc->cpuport){
	+ val \|= PMCR_FORCE_LINK \| PMCR_FORCE_DPX \| PMCR_FORCE_SPD_1000 \|
	+ MT7631_PMCR_FORCE_MODE \| PMCR_MAC_MODE;
	+ }
	+ /* Set port's MAC to default settings */
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_PMCR(port), val);
	+}
	+
	+static uint32_t
	+mtkswitch_get_port_status(struct mtkswitch_softc *sc, int port)
	+{
	+ uint32_t val, res, tmp;
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+ res = 0;
	+ val = sc->hal.mtkswitch_read(sc, MTKSWITCH_PMSR(port));
	+
	+ if (val & PMSR_MAC_LINK_STS)
	+ res \|= MTKSWITCH_LINK_UP;
	+ if (val & PMSR_MAC_DPX_STS)
	+ res \|= MTKSWITCH_DUPLEX;
	+ tmp = PMSR_MAC_SPD(val);
	+ if (tmp == 0)
	+ res \|= MTKSWITCH_SPEED_10;
	+ else if (tmp == 1)
	+ res \|= MTKSWITCH_SPEED_100;
	+ else if (tmp == 2)
	+ res \|= MTKSWITCH_SPEED_1000;
	+ if (val & PMSR_TX_FC_STS)
	+ res \|= MTKSWITCH_TXFLOW;
	+ if (val & PMSR_RX_FC_STS)
	+ res \|= MTKSWITCH_RXFLOW;
	+
	+ return (res);
	+}
	+
	+static int
	+mtkswitch_atu_flush(struct mtkswitch_softc *sc)
	+{
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+
	+ /* Flush all non-static MAC addresses */
	+ while (sc->hal.mtkswitch_read(sc, MTKSWITCH_ATC) & ATC_BUSY);
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_ATC, ATC_BUSY \|
	+ ATC_AC_MAT_NON_STATIC_MACS \| ATC_AC_CMD_CLEAN);
	+ while (sc->hal.mtkswitch_read(sc, MTKSWITCH_ATC) & ATC_BUSY);
	+
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_port_vlan_setup(struct mtkswitch_softc sc, etherswitch_port_t p)
	+{
	+ int err;
	+
	+ /*
	+ * Port behaviour wrt tag/untag/stack is currently defined per-VLAN.
	+ * So we say we don't support it here.
	+ */
	+ if ((p->es_flags & (ETHERSWITCH_PORT_DOUBLE_TAG \|
	+ ETHERSWITCH_PORT_ADDTAG \| ETHERSWITCH_PORT_STRIPTAG)) != 0)
	+ return (ENOTSUP);
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
	+ MTKSWITCH_LOCK(sc);
	+
	+ /* Set the PVID */
	+ if (p->es_pvid != 0) {
	+ err = sc->hal.mtkswitch_vlan_set_pvid(sc, p->es_port,
	+ p->es_pvid);
	+ if (err != 0) {
	+ MTKSWITCH_UNLOCK(sc);
	+ return (err);
	+ }
	+ }
	+
	+ MTKSWITCH_UNLOCK(sc);
	+
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_port_vlan_get(struct mtkswitch_softc sc, etherswitch_port_t p)
	+{
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
	+ MTKSWITCH_LOCK(sc);
	+
	+ /* Retrieve the PVID */
	+ sc->hal.mtkswitch_vlan_get_pvid(sc, p->es_port, &p->es_pvid);
	+
	+ /*
	+ * Port flags are not supported at the moment.
	+ * Port's tag/untag/stack behaviour is defined per-VLAN.
	+ */
	+ p->es_flags = 0;
	+
	+ MTKSWITCH_UNLOCK(sc);
	+
	+ return (0);
	+}
	+
	+static void
	+mtkswitch_invalidate_vlan(struct mtkswitch_softc *sc, uint32_t vid)
	+{
	+
	+ while (sc->hal.mtkswitch_read(sc, MTKSWITCH_VTCR) & VTCR_BUSY);
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_VTCR, VTCR_BUSY \|
	+ VTCR_FUNC_VID_INVALID \| (vid & VTCR_VID_MASK));
	+ while (sc->hal.mtkswitch_read(sc, MTKSWITCH_VTCR) & VTCR_BUSY);
	+}
	+
	+static int
	+mtkswitch_update_vlan_entry(struct mtkswitch_softc *sc, uint16_t vid,
	+ uint8_t members, uint16_t untag)
	+{
	+ uint32_t val;
	+
	+ while (sc->hal.mtkswitch_read(sc, MTKSWITCH_VTCR) & VTCR_BUSY);
	+
	+ /* We use FID 0 */
	+ val = VAWD1_IVL_MAC \| VAWD1_VTAG_EN \| VAWD1_VALID \|
	+ ((members & VAWD1_MEMBER_MASK) << VAWD1_MEMBER_OFF);
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_VAWD1, val);
	+
	+ /* Set tagged ports */
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_VAWD2, (untag &0xFFFF));
	+
	+ /* Write the VLAN entry */
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_VTCR, VTCR_BUSY \|
	+ VTCR_FUNC_VID_WRITE \| (vid & VTCR_VID_MASK));
	+ while ((val = sc->hal.mtkswitch_read(sc, MTKSWITCH_VTCR)) & VTCR_BUSY);
	+ return (val);
	+}
	+
	+static void
	+mtkswitch_vlan_init_hw(struct mtkswitch_softc *sc)
	+{
	+ uint8_t members = 0;
	+ uint32_t i;
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
	+ MTKSWITCH_LOCK(sc);
	+ /* Reset all VLANs to defaults first */
	+ for (i = 0; i < sc->info.es_nvlangroups; i++) {
	+ mtkswitch_invalidate_vlan(sc, i);
	+ }
	+
	+ /* Now, add all ports as untagged members of VLAN 1 */
	+ for (i = 0; i < sc->info.es_nports; i++)
	+ members \|= ((1u)<<(i));
	+
	+ mtkswitch_update_vlan_entry(sc, 1, members, 0);
	+
	+ /* Reset internal VLAN table. */
	+ for (i = 0; i < nitems(sc->vlans); i++)
	+ sc->vlans[i] = 0;
	+
	+ sc->vlans[0] = 1;
	+
	+ /* Set all port PVIDs to 1 */
	+ for (i = 0; i < sc->info.es_nports; i++) {
	+ sc->hal.mtkswitch_vlan_set_pvid(sc, i, 1);
	+ }
	+
	+ MTKSWITCH_UNLOCK(sc);
	+}
	+
	+static int
	+mtkswitch_vlan_getvgroup(struct mtkswitch_softc sc, etherswitch_vlangroup_t v)
	+{
	+ uint32_t val, i;
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
	+
	+ /* Reset the member ports. */
	+ v->es_untagged_ports = 0;
	+ v->es_member_ports = 0;
	+
	+ /* Not supported for now */
	+ v->es_fid = 0;
	+
	+ MTKSWITCH_LOCK(sc);
	+ v->es_vid = sc->vlans[v->es_vlangroup];
	+
	+ if (v->es_vid == 0)
	+ {
	+ MTKSWITCH_UNLOCK(sc);
	+ return (0);
	+ }
	+ while (sc->hal.mtkswitch_read(sc, MTKSWITCH_VTCR) & VTCR_BUSY);
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_VTCR, VTCR_BUSY \|
	+ VTCR_FUNC_VID_READ \| (v->es_vid & VTCR_VID_MASK));
	+ while ((val = sc->hal.mtkswitch_read(sc, MTKSWITCH_VTCR)) & VTCR_BUSY);
	+ if (val & VTCR_IDX_INVALID) {
	+ MTKSWITCH_UNLOCK(sc);
	+ return (0);
	+ }
	+
	+ val = sc->hal.mtkswitch_read(sc, MTKSWITCH_VAWD1);
	+ if (val & VAWD1_VALID)
	+ v->es_vid \|= ETHERSWITCH_VID_VALID;
	+ else {
	+ MTKSWITCH_UNLOCK(sc);
	+ return (0);
	+ }
	+ v->es_member_ports = (val >> VAWD1_MEMBER_OFF) & VAWD1_MEMBER_MASK;
	+
	+ val = sc->hal.mtkswitch_read(sc, MTKSWITCH_VAWD2);
	+ for (i = 0; i < sc->info.es_nports; i++) {
	+ if ((val & VAWD2_PORT_MASK(i)) == VAWD2_PORT_UNTAGGED(i))
	+ v->es_untagged_ports \|= (1<<i);
	+ }
	+
	+ MTKSWITCH_UNLOCK(sc);
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_vlan_setvgroup(struct mtkswitch_softc sc, etherswitch_vlangroup_t v)
	+{
	+ uint16_t untagged_ports = 0;
	+ uint32_t val;
	+ int i, vlan;
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
	+
	+ vlan = v->es_vid & ETHERSWITCH_VID_MASK;
	+ if (vlan == 0)
	+ {
	+ mtkswitch_invalidate_vlan(sc, sc->vlans[v->es_vlangroup]);
	+ sc->vlans[v->es_vlangroup] = 0;
	+ return (0);
	+ }
	+
	+ /* Is this VLAN already in table ? */
	+ for (i = 0; i < sc->info.es_nvlangroups; i++)
	+ if (i != v->es_vlangroup && vlan == sc->vlans[i])
	+ return (EINVAL);
	+
	+ sc->vlans[v->es_vlangroup] = vlan;
	+
	+ /* We currently don't support FID */
	+ if (v->es_fid != 0)
	+ return (EINVAL);
	+
	+ MTKSWITCH_LOCK(sc);
	+ /* Set tagged ports and Write the VLAN entry*/
	+ for (i = 0; i < sc->info.es_nports; i++)
	+ if (((1<<i) & v->es_untagged_ports) == 0)
	+ untagged_ports \|= VAWD2_PORT_TAGGED(i);
	+
	+ val = mtkswitch_update_vlan_entry(sc, v->es_vid, v->es_member_ports,
	+ untagged_ports);
	+ MTKSWITCH_UNLOCK(sc);
	+
	+ if (val & VTCR_IDX_INVALID)
	+ return (EINVAL);
	+
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_vlan_get_pvid(struct mtkswitch_softc sc, int port, int pvid)
	+{
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+
	+ *pvid = sc->hal.mtkswitch_read(sc, MTKSWITCH_PPBV1(port));
	+ pvid = PPBV_VID_FROM_REG(pvid);
	+
	+ return (0);
	+}
	+
	+static int
	+mtkswitch_vlan_set_pvid(struct mtkswitch_softc *sc, int port, int pvid)
	+{
	+ uint32_t val;
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+ val = PPBV_VID(pvid & PPBV_VID_MASK);
	+ sc->hal.mtkswitch_write(sc, MTKSWITCH_PPBV1(port), val);
	+
	+ return (0);
	+}
	+
	+extern void
	+mtk_attach_switch_mt7531(struct mtkswitch_softc *sc)
	+{
	+
	+ sc->portmap = 0x7f;
	+
	+ /* bpi-r2-pro port 0 not connected */
	+ sc->phymap = 0x1f;
	+
	+ sc->info.es_nports = 7;
	+ sc->info.es_vlan_caps = ETHERSWITCH_VLAN_DOT1Q;
	+ sprintf(sc->info.es_name, "Mediatek GSW");
	+ sc->hal.mtkswitch_read = mtkswitch_reg_read32;
	+ sc->hal.mtkswitch_write = mtkswitch_reg_write32;
	+ sc->info.es_nvlangroups = 4096;
	+ sc->hal.mtkswitch_reset = mtkswitch_reset;
	+ sc->hal.mtkswitch_hw_setup = mtkswitch_hw_setup;
	+ sc->hal.mtkswitch_hw_global_setup = mtkswitch_hw_global_setup;
	+ sc->hal.mtkswitch_port_init = mtkswitch_port_init;
	+ sc->hal.mtkswitch_get_port_status = mtkswitch_get_port_status;
	+ sc->hal.mtkswitch_atu_flush = mtkswitch_atu_flush;
	+ sc->hal.mtkswitch_port_vlan_setup = mtkswitch_port_vlan_setup;
	+ sc->hal.mtkswitch_port_vlan_get = mtkswitch_port_vlan_get;
	+ sc->hal.mtkswitch_vlan_init_hw = mtkswitch_vlan_init_hw;
	+ sc->hal.mtkswitch_vlan_getvgroup = mtkswitch_vlan_getvgroup;
	+ sc->hal.mtkswitch_vlan_setvgroup = mtkswitch_vlan_setvgroup;
	+ sc->hal.mtkswitch_vlan_get_pvid = mtkswitch_vlan_get_pvid;
	+ sc->hal.mtkswitch_vlan_set_pvid = mtkswitch_vlan_set_pvid;
	+ sc->hal.mtkswitch_phy_read = mtkswitch_phy_read;
	+ sc->hal.mtkswitch_phy_write = mtkswitch_phy_write;
	+ sc->hal.mtkswitch_reg_read = mtkswitch_reg_read;
	+ sc->hal.mtkswitch_reg_write = mtkswitch_reg_write;
	+}
	+
	+#define MT7530_PORT_MIB_COUNTER(x) (0x4000 + (x) * 0x100)
	+
	+struct mt7530_mib_desc {
	+ unsigned int size;
	+ unsigned int offset;
	+ const char *name;
	+ const char *desc;
	+};
	+
	+#define MIB_DESC(_s, _o, _n, _d)\
	+{ \
	+ .size = (_s), \
	+ .offset = (_o), \
	+ .name = (_n), \
	+ .desc = (_d), \
	+}
	+
	+// vaata üle
	+static const
	+struct mt7530_mib_desc mt7530_mib[] = {
	+ MIB_DESC(1, 0x00, "tx_drop", "Transmit droped frames"),
	+ MIB_DESC(1, 0x04, "tx_crcerrs", "Transmit CRC errors"),
	+ MIB_DESC(1, 0x08, "tx_ucast_frames", "Transmit good unicast frames"),
	+ MIB_DESC(1, 0x0c, "tx_mcast_frames", "Transmit good multicast frames"),
	+ MIB_DESC(1, 0x10, "tx_bcast_frames", "Transmit good broadcast frames"),
	+ MIB_DESC(1, 0x14, "tx_colls", "Transmit collisions"),
	+ MIB_DESC(1, 0x18, "tx_single_colls", "Transmit single collisions"),
	+ MIB_DESC(1, 0x1c, "tx_multi_colls", "Transmit multiple collisions"),
	+ MIB_DESC(1, 0x20, "tx_deferred", "Transmit deferred frames"),
	+ MIB_DESC(1, 0x24, "tx_late_colls", "Transmit late collisions"),
	+ MIB_DESC(1, 0x28, "tx_excess_colls", "Transmit excessive collisions"),
	+ MIB_DESC(1, 0x2c, "tx_pause_frames", "Transmit pause frames"),
	+ MIB_DESC(1, 0x30, "tx_frames_64", "Transmit 64 bytes frames"),
	+ MIB_DESC(1, 0x34, "tx_frames_65_127", "Transmit 65 to 127 bytes frames"),
	+ MIB_DESC(1, 0x38, "tx_frames_128_255", "Transmit 128 to 255 bytes frames"),
	+ MIB_DESC(1, 0x3c, "tx_frames_256_511", "Transmit 256 to 511 bytes frames"),
	+ MIB_DESC(1, 0x40, "tx_frames_512_1023", "Transmit 512 to 1023 bytes frames"),
	+ MIB_DESC(1, 0x44, "tx_frame_1024_max", "Transmit 1024 to max bytes frames"),
	+ MIB_DESC(2, 0x48, "tx_bytes", "Transmit good bytes"),
	+ MIB_DESC(1, 0x60, "rx_drop", "Receive droped frames"),
	+ MIB_DESC(1, 0x64, "rx_pkts_filtered", "Receive frames is filtered"),
	+ MIB_DESC(1, 0x68, "rx_ucast_frames", "Receive unicast frames"),
	+ MIB_DESC(1, 0x6c, "rx_mcast_frames", "Receive multicast frames"),
	+ MIB_DESC(1, 0x70, "rx_bcast_frames", "Receive broadcast frames"),
	+ MIB_DESC(1, 0x74, "rx_align_errs", "Receive alignment errors"),
	+ MIB_DESC(1, 0x78, "rx_crcerrs", "Receive CRC errors"),
	+ MIB_DESC(1, 0x7c, "rx_runts", "Receive undersized frames"),
	+ MIB_DESC(1, 0x80, "rx_fragments", "Receive fragmented frames"),
	+ MIB_DESC(1, 0x84, "rx_oversize_frames", "Receive oversize frames"),
	+ MIB_DESC(1, 0x88, "rx_jabbers", "Receive jabbers frames"),
	+ MIB_DESC(1, 0x8c, "rx_pause_frames", "Receive pause control frames"),
	+ MIB_DESC(1, 0x90, "rx_frames_64", "Receive 64 bytes frames"),
	+ MIB_DESC(1, 0x94, "rx_frames_65_127", "Receive 65 to 127 bytes frames"),
	+ MIB_DESC(1, 0x98, "rx_frames_128_255", "Receive 128 to 255 bytes frames"),
	+ MIB_DESC(1, 0x9c, "rx_frames_256_511", "Receive 256 to 511 bytes frames"),
	+ MIB_DESC(1, 0xa0, "rx_frames_512_1023", "Receive 512 to 1023 bytes frames"),
	+ MIB_DESC(1, 0xa4, "rx_frame_1024_max", "Receive 1024 to max bytes frames"),
	+ MIB_DESC(2, 0xa8, "rx_bytes", "Receive good bytes"),
	+ MIB_DESC(1, 0xb0, "rx_ctrl_drop", "Receive droped frames"),
	+ MIB_DESC(1, 0xb4, "rx_ingress_drop", "Receive droped by ingress rate limited"),
	+ MIB_DESC(1, 0xb8, "rx_arl_drop", "Receive droped by ACL"),
	+};
	+
	+static int64_t
	+mt7531_hw_port_mib_read_count(struct mtkswitch_softc *sc, int port, int index)
	+{
	+ const struct mt7530_mib_desc *mib;
	+ uint64_t val;
	+ uint32_t reg, hi;
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+
	+ mib = &mt7530_mib[index];
	+ reg = MT7530_PORT_MIB_COUNTER(port) + mib->offset;
	+
	+ val = sc->hal.mtkswitch_read(sc, reg);
	+ if (mib->size == 2) {
	+ hi = sc->hal.mtkswitch_read(sc, reg + 4);
	+ val \|= ((uint64_t) hi << 32);
	+ }
	+
	+ return val;
	+}
	+
	+static int
	+mt7531_hw_port_mib_clear(struct mtkswitch_softc *sc, int port,
	+ int index, uint32_t val)
	+{
	+ uint32_t reg;
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+
	+ reg = MT7530_PORT_MIB_COUNTER(port) + index;
	+
	+ return sc->hal.mtkswitch_write(sc, reg, val);
	+}
	+
	+struct mt7531_sysctl_mib {
	+ struct mtkswitch_softc *sc;
	+ int index;
	+ int port;
	+};
	+
	+static int
	+mt7531_sysctl_port_mib_read_count(SYSCTL_HANDLER_ARGS)
	+{
	+ struct mtkswitch_softc *sc;
	+ uint64_t val = 0;
	+ uint32_t reg = (uint32_t)arg2;
	+ uint32_t port = ((reg >> 16) & 0xffff);
	+ uint32_t index = (reg & 0xffff);
	+
	+ sc = (struct mtkswitch_softc *)arg1;
	+ if (sc == NULL)
	+ return (EINVAL);
	+
	+ if (index < 0 \|\| index > nitems(mt7530_mib))
	+ return (EINVAL);
	+
	+ if (port < 0 \|\| port > MTKSWITCH_MAX_PORTS)
	+ return (EINVAL);
	+
	+ //MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+ MTKSWITCH_LOCK(sc);
	+ val = mt7531_hw_port_mib_read_count(sc, port, index);
	+ MTKSWITCH_UNLOCK(sc);
	+
	+ return sysctl_handle_64(oidp, &val, 0, req);
	+}
	+
	+static int
	+mt7531_sysctl_port_mib_clear_count(SYSCTL_HANDLER_ARGS)
	+{
	+ struct mtkswitch_softc *sc;
	+ uint32_t val = 0;
	+ uint32_t reg = (uint32_t)arg2;
	+ uint32_t port = ((reg >> 16) & 0xffff);
	+ uint32_t index = (reg & 0xffff);
	+ int error;
	+
	+ sc = (struct mtkswitch_softc *)arg1;
	+ if (sc == NULL)
	+ return (EINVAL);
	+
	+ //if (index < 0 \|\| index > nitems(mt7530_mib))
	+ // return (EINVAL);
	+
	+ if (port < 0 \|\| port > MTKSWITCH_MAX_PORTS)
	+ return (EINVAL);
	+
	+ error = sysctl_handle_32(oidp, &val, 0, req);
	+
	+ if (error \|\| !req->newptr)
	+ return (error);
	+
	+ //MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+ MTKSWITCH_LOCK(sc);
	+ mt7531_hw_port_mib_clear(sc, port, index, val);
	+ MTKSWITCH_UNLOCK(sc);
	+
	+ return (0);
	+}
	+
	+int
	+mt7531_sysctl_attach(struct mtkswitch_softc *sc)
	+{
	+ struct sysctl_ctx_list *ctx;
	+ struct sysctl_oid *tree;
	+ struct sysctl_oid *ptree;
	+ struct sysctl_oid_list *children;
	+ struct sysctl_oid_list *pchildren;
	+ struct sysctl_oid_list *ichildren;
	+
	+ char port_num_buf[32];
	+ uint32_t reg;
	+ int index, port;
	+
	+ ctx = device_get_sysctl_ctx(sc->sc_dev);
	+ children = SYSCTL_CHILDREN(device_get_sysctl_tree(sc->sc_dev));
	+
	+ tree = SYSCTL_ADD_NODE(ctx, children, OID_AUTO, "port",
	+ CTLFLAG_RD \| CTLFLAG_MPSAFE, 0,
	+ "ethernet stndard mib counters of ports");
	+ pchildren = SYSCTL_CHILDREN(tree);
	+
	+ for (port = 0; port < MTKSWITCH_MAX_PORTS; port++) {
	+ snprintf(port_num_buf, sizeof(port_num_buf), "%d", port);
	+ ptree = SYSCTL_ADD_NODE(ctx, pchildren, port,
	+ port_num_buf, CTLFLAG_RD \| CTLFLAG_MPSAFE,
	+ NULL, "port mib counters");
	+ ichildren = SYSCTL_CHILDREN(ptree);
	+ for (index = 0; index < nitems(mt7530_mib); index++) {
	+ reg = ((port << 16) \| index);
	+
	+ SYSCTL_ADD_PROC(ctx, ichildren, index,
	+ mt7530_mib[index].name,
	+ CTLTYPE_U64 \| CTLFLAG_RD \| CTLFLAG_MPSAFE,
	+ sc, reg, mt7531_sysctl_port_mib_read_count,
	+ "LU", mt7530_mib[index].desc);
	+ }
	+ reg = ((port << 16) \| 0xD0);
	+ SYSCTL_ADD_PROC(ctx, ichildren, OID_AUTO,
	+ "clear_tx", CTLTYPE_UINT \| CTLFLAG_WR \| CTLFLAG_MPSAFE,
	+ sc, reg, mt7531_sysctl_port_mib_clear_count,
	+ "IU", "Clear TX Counters");
	+ reg = ((port << 16) \| 0xD4);
	+ SYSCTL_ADD_PROC(ctx, ichildren, OID_AUTO,
	+ "clear_rx", CTLTYPE_UINT \| CTLFLAG_WR \| CTLFLAG_MPSAFE,
	+ sc, reg, mt7531_sysctl_port_mib_clear_count,
	+ "IU", "Clear RX Counters");
	+ }
	+ return (0);
	+}
	+
	+static int
	+mt7531_arl_fetch_entry(struct mtkswitch_softc sc, etherswitch_atu_entry_t e)
	+{
	+
	+ uint32_t tsra1, tsra2, tsrd;
	+
	+ tsra1 = sc->hal.mtkswitch_read(sc, MT7531_TSRA1);
	+ tsra2 = sc->hal.mtkswitch_read(sc, MT7531_TSRA2);
	+ tsrd = sc->hal.mtkswitch_read(sc, MT7531_TSRD);
	+
	+ /* MAC address */
	+ e->es_macaddr[5] = ((tsra2 >> 16) & 0xFF);
	+ e->es_macaddr[4] = ((tsra2 >> 24) & 0xFF);
	+ e->es_macaddr[3] = ((tsra1 >> 0) & 0xFF);
	+ e->es_macaddr[2] = ((tsra1 >> 8) & 0xFF);
	+ e->es_macaddr[1] = ((tsra1 >> 16) & 0xFF);
	+ e->es_macaddr[0] = ((tsra1 >> 24) & 0xFF);
	+
	+ /* Bitmask of ports this entry is for */
	+ e->es_portmask = ((tsrd >> 4) & 0xFF);
	+
	+ return 0;
	+}
	+
	+static void mt7531_is_atc_busy(struct mtkswitch_softc *sc)
	+{
	+ uint32_t val;
	+ do {
	+ val = sc->hal.mtkswitch_read(sc,MT7531_ATC);
	+ } while(val & MT7531_ATC_BUSY);
	+}
	+
	+int
	+mt7531_atu_fetch_table(device_t dev, etherswitch_atu_table_t *table)
	+{
	+ struct mtkswitch_softc *sc;
	+ int nitems;
	+ uint32_t val;
	+
	+ sc = device_get_softc(dev);
	+
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+
	+ memset(&sc->atu.entries, 0, sizeof(sc->atu.entries));
	+
	+ table->es_nitems = 0;
	+ nitems = 0;
	+
	+ MTKSWITCH_LOCK(sc);
	+ sc->atu.count = 0;
	+
	+ sc->hal.mtkswitch_write(sc, MT7531_ATC,
	+ (MT7531_ATC_BUSY \| MT7531_AC_CMD_SSC));
	+ mt7531_is_atc_busy(sc);
	+ val = sc->hal.mtkswitch_read(sc, MT7531_ATC);
	+ while(!((val) & MT7531_ATC_SRCH_END)) {
	+ mt7531_arl_fetch_entry(sc, &sc->atu.entries[nitems]);
	+ sc->atu.entries[nitems].id = nitems;
	+ nitems++;
	+ sc->hal.mtkswitch_write(sc, MT7531_ATC,
	+ (MT7531_ATC_BUSY \| MT7531_AC_CMD_NSC));
	+ mt7531_is_atc_busy(sc);
	+ val = sc->hal.mtkswitch_read(sc, MT7531_ATC);
	+ }
	+ sc->atu.count = nitems;
	+ table->es_nitems = nitems;
	+ MTKSWITCH_UNLOCK(sc);
	+
	+ return (0);
	+}
	+
	+int
	+mt7531_atu_fetch_table_entry(device_t dev, etherswitch_atu_entry_t *e)
	+{
	+ struct mtkswitch_softc *sc;
	+ int id, err = 0;
	+ sc = device_get_softc(dev);
	+ MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
	+
	+ id = e->id;
	+
	+ MTKSWITCH_LOCK(sc);
	+ if (id > sc->atu.count) {
	+ err = ENOENT;
	+ goto done;
	+ }
	+ memcpy(e, &sc->atu.entries[id], sizeof(*e));
	+ done:
	+ MTKSWITCH_UNLOCK(sc);
	+ return (err);
	+}
	\ No newline at end of file
	diff --git a/sys/dev/etherswitch/mtkswitch/mtkswitchvar.h b/sys/dev/etherswitch/mtkswitch/mtkswitchvar.h
	--- a/sys/dev/etherswitch/mtkswitch/mtkswitchvar.h
	+++ b/sys/dev/etherswitch/mtkswitch/mtkswitchvar.h
	@@ -35,6 +35,7 @@
	MTK_SWITCH_MT7620,
	MTK_SWITCH_MT7621,
	MTK_SWITCH_MT7628,
	+ MTK_SWITCH_MT7531,
	} mtk_switch_type;

	#define MTK_IS_SWITCH(_sc, _type) \
	@@ -42,7 +43,10 @@

	#define MTKSWITCH_MAX_PORTS 7
	#define MTKSWITCH_MAX_PHYS 7
	-#define MTKSWITCH_CPU_PORT 6
	+#define MTKSWITCH_NUM_VLANS 4096
	+/* Size of the ALR table in hardware */
	+#define MTKSWITCH_NUM_ARL_ENTRIES 4096
	+

	#define MTKSWITCH_LINK_UP (1<<0)
	#define MTKSWITCH_SPEED_MASK (3<<1)
	@@ -70,6 +74,13 @@
	struct callout callout_tick;
	etherswitch_info_t info;

	+ int vlans[MTKSWITCH_NUM_VLANS];
	+ /* ARL (address resolution table) */
	+ struct {
	+ int count;
	+ etherswitch_atu_entry_t entries[MTKSWITCH_NUM_ARL_ENTRIES];
	+ } atu;
	+
	uint32_t vlan_mode;

	struct {
	@@ -158,5 +169,9 @@

	extern void mtk_attach_switch_rt3050(struct mtkswitch_softc *);
	extern void mtk_attach_switch_mt7620(struct mtkswitch_softc *);
	+extern void mtk_attach_switch_mt7531(struct mtkswitch_softc *);
	+extern int mt7531_sysctl_attach(struct mtkswitch_softc *sc);
	+extern int mt7531_atu_fetch_table(device_t dev, etherswitch_atu_table_t *table);
	+extern int mt7531_atu_fetch_table_entry(device_t dev, etherswitch_atu_entry_t *e);

	#endif /* __MTKSWITCHVAR_H__ */
	diff --git a/sys/dev/fdt/fdt_common.h b/sys/dev/fdt/fdt_common.h
	--- a/sys/dev/fdt/fdt_common.h
	+++ b/sys/dev/fdt/fdt_common.h
	@@ -75,6 +75,7 @@
	int fdt_foreach_mem_region(fdt_mem_region_cb, void *);
	int fdt_foreach_reserved_mem(fdt_mem_region_cb, void *);
	int fdt_foreach_reserved_region(fdt_mem_region_cb, void *);
	+boolean_t fdt_find_ethernet_prop_switch(phandle_t ethernet, phandle_t node);
	int fdt_get_phyaddr(phandle_t, device_t, int , void *);
	int fdt_get_range(phandle_t, int, u_long , u_long );
	int fdt_immr_addr(vm_offset_t);
	diff --git a/sys/dev/fdt/fdt_common.c b/sys/dev/fdt/fdt_common.c
	--- a/sys/dev/fdt/fdt_common.c
	+++ b/sys/dev/fdt/fdt_common.c
	@@ -338,6 +338,27 @@
	return (0);
	}

	+boolean_t
	+fdt_find_ethernet_prop_switch(phandle_t ethernet, phandle_t node)
	+{
	+ boolean_t ret;
	+ phandle_t child, switch_handle, switch_eth;
	+ for (child = OF_child(node); child != 0; child = OF_peer(child)) {
	+ if (OF_getencprop(child, "ethernet", (void*)&switch_handle,
	+ sizeof(switch_handle)) > 0){
	+ if (switch_handle > 0) {
	+ switch_eth = OF_node_from_xref(switch_handle);
	+ if (switch_eth == ethernet)
	+ return (true);
	+ }
	+ }
	+ ret = fdt_find_ethernet_prop_switch(ethernet, child);
	+ if (ret != 0)
	+ return (ret);
	+ }
	+ return (false);
	+}
	+
	int
	fdt_get_phyaddr(phandle_t node, device_t dev, int phy_addr, void *phy_sc)
	{
	diff --git a/sys/modules/mtkswitch/Makefile b/sys/modules/mtkswitch/Makefile
	new file mode 100644
	--- /dev/null
	+++ b/sys/modules/mtkswitch/Makefile
	@@ -0,0 +1,11 @@
	+# $FreeBSD$
	+
	+.PATH: ${SRCTOP}/sys/dev/etherswitch/mtkswitch
	+
	+KMOD= mtkswitch
	+SRCS= mtkswitch.c mtkswitch_mt7531.c
	+
	+SRCS+= bus_if.h etherswitch_if.h mdio_if.h miibus_if.h ofw_bus_if.h
	+
	+.include <bsd.kmod.mk>
	+

File Metadata

Mime Type: text/plain
Expires: Sun, Apr 19, 4:38 AM (16 m, 52 s)
Storage Engine: blob
Storage Format: Raw Data
Storage Handle: 31746359
Default Alt Text: D55013.diff (51 KB)

D55013.diffNo OneTemporaryActions

D55013.diffView Options

File Metadata

Event Timeline

D55013.diff
No OneTemporary
Actions

D55013.diff
View Options