Index: head/sys/dev/etherswitch/mtkswitch/mtkswitch.c
===================================================================
--- head/sys/dev/etherswitch/mtkswitch/mtkswitch.c
+++ head/sys/dev/etherswitch/mtkswitch/mtkswitch.c
@@ -0,0 +1,668 @@
+/*-
+ * Copyright (c) 2016 Stanislav Galabov.
+ * Copyright (c) 2011-2012 Stefan Bethke.
+ * Copyright (c) 2012 Adrian Chadd.
+ * 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/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/ofw/ofw_bus_subr.h>
+
+#include "mdio_if.h"
+#include "miibus_if.h"
+#include "etherswitch_if.h"
+
+#define DEBUG
+
+#if defined(DEBUG)
+static SYSCTL_NODE(_debug, OID_AUTO, mtkswitch, CTLFLAG_RD, 0, "mtkswitch");
+#endif
+
+static inline int mtkswitch_portforphy(int phy);
+static int mtkswitch_ifmedia_upd(struct ifnet *ifp);
+static void mtkswitch_ifmedia_sts(struct ifnet *ifp, struct ifmediareq *ifmr);
+static void mtkswitch_tick(void *arg);
+
+static const struct ofw_compat_data compat_data[] = {
+	{ "ralink,rt3050-esw",		MTK_SWITCH_RT3050 },
+	{ "ralink,rt3352-esw",		MTK_SWITCH_RT3352 },
+	{ "ralink,rt5350-esw",		MTK_SWITCH_RT5350 },
+	{ "mediatek,mt7620-gsw",	MTK_SWITCH_MT7620 },
+	{ "mediatek,mt7621-gsw",	MTK_SWITCH_MT7621 },
+	{ "mediatek,mt7628-esw",	MTK_SWITCH_MT7628 },
+
+	/* Sentinel */
+	{ NULL,				MTK_SWITCH_NONE }
+};
+
+static int
+mtkswitch_probe(device_t dev)
+{
+	struct mtkswitch_softc *sc;
+	mtk_switch_type switch_type;
+
+	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)
+		return (ENXIO);
+
+	sc = device_get_softc(dev);
+	bzero(sc, sizeof(*sc));
+	sc->sc_switchtype = switch_type;
+
+	device_set_desc_copy(dev, "MTK Switch Driver");
+
+	return (0);
+}
+
+static int
+mtkswitch_attach_phys(struct mtkswitch_softc *sc)
+{
+	int phy, err = 0;
+	char name[IFNAMSIZ];
+
+	/* PHYs need an interface, so we generate a dummy one */
+	snprintf(name, IFNAMSIZ, "%sport", device_get_nameunit(sc->sc_dev));
+	for (phy = 0; phy < sc->numphys; phy++) {
+		if ((sc->phymap & (1u << phy)) == 0) {
+			sc->ifp[phy] = NULL;
+			sc->ifname[phy] = NULL;
+			sc->miibus[phy] = NULL;
+			continue;
+		}
+		sc->ifp[phy] = if_alloc(IFT_ETHER);
+		sc->ifp[phy]->if_softc = sc;
+		sc->ifp[phy]->if_flags |= IFF_UP | IFF_BROADCAST |
+		    IFF_DRV_RUNNING | IFF_SIMPLEX;
+		sc->ifname[phy] = malloc(strlen(name) + 1, M_DEVBUF, M_WAITOK);
+		bcopy(name, sc->ifname[phy], strlen(name) + 1);
+		if_initname(sc->ifp[phy], sc->ifname[phy],
+		    mtkswitch_portforphy(phy));
+		err = mii_attach(sc->sc_dev, &sc->miibus[phy], sc->ifp[phy],
+		    mtkswitch_ifmedia_upd, mtkswitch_ifmedia_sts,
+		    BMSR_DEFCAPMASK, phy, MII_OFFSET_ANY, 0);
+		if (err != 0) {
+			device_printf(sc->sc_dev,
+			    "attaching PHY %d failed\n",
+			    phy);
+		} else {
+			DPRINTF(sc->sc_dev, "%s attached to pseudo interface "
+			    "%s\n", device_get_nameunit(sc->miibus[phy]),
+			    sc->ifp[phy]->if_xname);
+		}
+	}
+	return (err);
+}
+
+static int
+mtkswitch_set_vlan_mode(struct mtkswitch_softc *sc, uint32_t mode)
+{
+
+	/* Check for invalid modes. */
+	if ((mode & sc->info.es_vlan_caps) != mode)
+		return (EINVAL);
+
+	sc->vlan_mode = mode;
+
+	/* Reset VLANs. */
+	sc->hal.mtkswitch_vlan_init_hw(sc);
+
+	return (0);
+}
+
+static int
+mtkswitch_attach(device_t dev)
+{
+	struct mtkswitch_softc *sc;
+	int err = 0;
+	int port, rid;
+
+	sc = device_get_softc(dev);
+
+	/* sc->sc_switchtype is already decided in mtkswitch_probe() */
+	sc->numports = MTKSWITCH_MAX_PORTS;
+	sc->numphys = MTKSWITCH_MAX_PHYS;
+	sc->cpuport = MTKSWITCH_CPU_PORT;
+	sc->sc_dev = dev;
+
+	/* Attach switch related functions */
+	if (sc->sc_switchtype == MTK_SWITCH_NONE) {
+		device_printf(dev, "Unknown switch type\n");
+		return (ENXIO);
+	}
+
+	if (sc->sc_switchtype == MTK_SWITCH_MT7620 ||
+	    sc->sc_switchtype == MTK_SWITCH_MT7621)
+		mtk_attach_switch_mt7620(sc);
+	else
+		mtk_attach_switch_rt3050(sc);
+
+	/* Allocate resources */
+	rid = 0;
+	sc->sc_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->sc_res == NULL) {
+		device_printf(dev, "could not map memory\n");
+		return (ENXIO);
+	}
+
+	mtx_init(&sc->sc_mtx, "mtkswitch", NULL, MTX_DEF);
+
+	/* Reset the switch */
+	if (sc->hal.mtkswitch_reset(sc)) {
+		DPRINTF(dev, "%s: mtkswitch_reset: failed\n", __func__);
+		return (ENXIO);
+	}
+
+	err = sc->hal.mtkswitch_hw_setup(sc);
+	DPRINTF(dev, "%s: hw_setup: err=%d\n", __func__, err);
+	if (err != 0)
+		return (err);
+
+	err = sc->hal.mtkswitch_hw_global_setup(sc);
+	DPRINTF(dev, "%s: hw_global_setup: err=%d\n", __func__, err);
+	if (err != 0)
+		return (err);
+
+	/* Initialize the switch ports */
+	for (port = 0; port < sc->numports; port++) {
+		sc->hal.mtkswitch_port_init(sc, port);
+	}
+
+	/* Attach the PHYs and complete the bus enumeration */
+	err = mtkswitch_attach_phys(sc);
+	DPRINTF(dev, "%s: attach_phys: err=%d\n", __func__, err);
+	if (err != 0)
+		return (err);
+
+	/* Default to ingress filters off. */
+	err = mtkswitch_set_vlan_mode(sc, ETHERSWITCH_VLAN_DOT1Q);
+	DPRINTF(dev, "%s: set_vlan_mode: err=%d\n", __func__, err);
+	if (err != 0)
+		return (err);
+
+	bus_generic_probe(dev);
+	bus_enumerate_hinted_children(dev);
+	err = bus_generic_attach(dev);
+	DPRINTF(dev, "%s: bus_generic_attach: err=%d\n", __func__, err);
+	if (err != 0)
+		return (err);
+
+	callout_init_mtx(&sc->callout_tick, &sc->sc_mtx, 0);
+
+	MTKSWITCH_LOCK(sc);
+	mtkswitch_tick(sc);
+	MTKSWITCH_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+mtkswitch_detach(device_t dev)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+	int phy;
+
+	callout_drain(&sc->callout_tick);
+
+	for (phy = 0; phy < MTKSWITCH_MAX_PHYS; phy++) {
+		if (sc->miibus[phy] != NULL)
+			device_delete_child(dev, sc->miibus[phy]);
+		if (sc->ifp[phy] != NULL)
+			if_free(sc->ifp[phy]);
+		free(sc->ifname[phy], M_DEVBUF);
+	}
+
+	bus_generic_detach(dev);
+	mtx_destroy(&sc->sc_mtx);
+
+	return (0);
+}
+
+/* PHY <-> port mapping is currently 1:1 */
+static inline int
+mtkswitch_portforphy(int phy)
+{
+
+	return (phy);
+}
+
+static inline int
+mtkswitch_phyforport(int port)
+{
+
+	return (port);
+}
+
+static inline struct mii_data *
+mtkswitch_miiforport(struct mtkswitch_softc *sc, int port)
+{
+	int phy = mtkswitch_phyforport(port);
+
+	if (phy < 0 || phy >= MTKSWITCH_MAX_PHYS || sc->miibus[phy] == NULL)
+		return (NULL);
+
+	return (device_get_softc(sc->miibus[phy]));
+}
+
+static inline struct ifnet *
+mtkswitch_ifpforport(struct mtkswitch_softc *sc, int port)
+{
+	int phy = mtkswitch_phyforport(port);
+
+	if (phy < 0 || phy >= MTKSWITCH_MAX_PHYS)
+		return (NULL);
+
+	return (sc->ifp[phy]);
+}
+
+/*
+ * Convert port status to ifmedia.
+ */
+static void
+mtkswitch_update_ifmedia(uint32_t portstatus, u_int *media_status,
+    u_int *media_active)
+{
+	*media_active = IFM_ETHER;
+	*media_status = IFM_AVALID;
+
+	if ((portstatus & MTKSWITCH_LINK_UP) != 0)
+		*media_status |= IFM_ACTIVE;
+	else {
+		*media_active |= IFM_NONE;
+		return;
+	}
+
+	switch (portstatus & MTKSWITCH_SPEED_MASK) {
+	case MTKSWITCH_SPEED_10:
+		*media_active |= IFM_10_T;
+		break;
+	case MTKSWITCH_SPEED_100:
+		*media_active |= IFM_100_TX;
+		break;
+	case MTKSWITCH_SPEED_1000:
+		*media_active |= IFM_1000_T;
+		break;
+	}
+
+	if ((portstatus & MTKSWITCH_DUPLEX) != 0)
+		*media_active |= IFM_FDX;
+	else
+		*media_active |= IFM_HDX;
+
+	if ((portstatus & MTKSWITCH_TXFLOW) != 0)
+		*media_active |= IFM_ETH_TXPAUSE;
+	if ((portstatus & MTKSWITCH_RXFLOW) != 0)
+		*media_active |= IFM_ETH_RXPAUSE;
+}
+
+static void
+mtkswitch_miipollstat(struct mtkswitch_softc *sc)
+{
+	struct mii_data *mii;
+	struct mii_softc *miisc;
+	uint32_t portstatus;
+	int i, port_flap = 0;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+
+	for (i = 0; i < sc->numphys; i++) {
+		if (sc->miibus[i] == NULL)
+			continue;
+		mii = device_get_softc(sc->miibus[i]);
+		portstatus = sc->hal.mtkswitch_get_port_status(sc,
+		    mtkswitch_portforphy(i));
+
+		/* If a port has flapped - mark it so we can flush the ATU */
+		if (((mii->mii_media_status & IFM_ACTIVE) == 0 &&
+		    (portstatus & MTKSWITCH_LINK_UP) != 0) ||
+		    ((mii->mii_media_status & IFM_ACTIVE) != 0 &&
+		    (portstatus & MTKSWITCH_LINK_UP) == 0)) {
+			port_flap = 1;
+		}
+
+		mtkswitch_update_ifmedia(portstatus, &mii->mii_media_status,
+		    &mii->mii_media_active);
+		LIST_FOREACH(miisc, &mii->mii_phys, mii_list) {
+			if (IFM_INST(mii->mii_media.ifm_cur->ifm_media) !=
+			    miisc->mii_inst)
+				continue;
+			mii_phy_update(miisc, MII_POLLSTAT);
+		}
+	}
+
+	if (port_flap)
+		sc->hal.mtkswitch_atu_flush(sc);
+}
+
+static void
+mtkswitch_tick(void *arg)
+{
+	struct mtkswitch_softc *sc = arg;
+
+	mtkswitch_miipollstat(sc);
+	callout_reset(&sc->callout_tick, hz, mtkswitch_tick, sc);
+}
+
+static void
+mtkswitch_lock(device_t dev)
+{
+        struct mtkswitch_softc *sc = device_get_softc(dev);
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+	MTKSWITCH_LOCK(sc);
+}
+
+static void
+mtkswitch_unlock(device_t dev)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	MTKSWITCH_UNLOCK(sc);
+}
+
+static etherswitch_info_t *
+mtkswitch_getinfo(device_t dev)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+
+	return (&sc->info);
+}
+
+static inline int
+mtkswitch_is_cpuport(struct mtkswitch_softc *sc, int port)
+{
+
+	return (sc->cpuport == port);
+}
+
+static int
+mtkswitch_getport(device_t dev, etherswitch_port_t *p)
+{       
+	struct mtkswitch_softc *sc;
+	struct mii_data *mii;
+	struct ifmediareq *ifmr;
+	int err;
+
+	sc = device_get_softc(dev);
+	if (p->es_port < 0 || p->es_port > sc->info.es_nports)
+		return (ENXIO);
+
+	err = sc->hal.mtkswitch_port_vlan_get(sc, p);
+	if (err != 0)
+		return (err);
+
+	mii = mtkswitch_miiforport(sc, p->es_port);
+	if (mtkswitch_is_cpuport(sc, p->es_port)) {
+		/* fill in fixed values for CPU port */
+		/* XXX is this valid in all cases? */
+		p->es_flags |= ETHERSWITCH_PORT_CPU;
+		ifmr = &p->es_ifmr;
+		ifmr->ifm_count = 0;
+		ifmr->ifm_current = ifmr->ifm_active =
+		    IFM_ETHER | IFM_1000_T | IFM_FDX;
+		ifmr->ifm_mask = 0;
+		ifmr->ifm_status = IFM_ACTIVE | IFM_AVALID;
+	} else if (mii != NULL) {
+		err = ifmedia_ioctl(mii->mii_ifp, &p->es_ifr,
+		    &mii->mii_media, SIOCGIFMEDIA);
+		if (err)
+			return (err);
+	} else {
+		ifmr = &p->es_ifmr;
+		ifmr->ifm_count = 0;
+		ifmr->ifm_current = ifmr->ifm_active = IFM_NONE;
+		ifmr->ifm_mask = 0;
+		ifmr->ifm_status = 0;
+	}
+	return (0);
+}
+
+static int
+mtkswitch_setport(device_t dev, etherswitch_port_t *p)
+{       
+	int err;
+	struct mtkswitch_softc *sc;
+	struct ifmedia *ifm;
+	struct mii_data *mii;
+	struct ifnet *ifp;
+
+	sc = device_get_softc(dev);
+	if (p->es_port < 0 || p->es_port > sc->info.es_nports)
+		return (ENXIO);
+        
+	/* Port flags. */ 
+	if (sc->vlan_mode == ETHERSWITCH_VLAN_DOT1Q) {
+		err = sc->hal.mtkswitch_port_vlan_setup(sc, p);
+		if (err)
+			return (err);
+	}
+
+	/* Do not allow media changes on CPU port. */
+	if (mtkswitch_is_cpuport(sc, p->es_port))
+		return (0);
+
+	mii = mtkswitch_miiforport(sc, p->es_port);
+	if (mii == NULL)
+		return (ENXIO);
+
+	ifp = mtkswitch_ifpforport(sc, p->es_port);
+
+	ifm = &mii->mii_media;
+	return (ifmedia_ioctl(ifp, &p->es_ifr, ifm, SIOCSIFMEDIA));
+}
+
+static void
+mtkswitch_statchg(device_t dev)
+{
+
+	DPRINTF(dev, "%s\n", __func__);
+}
+
+static int
+mtkswitch_ifmedia_upd(struct ifnet *ifp)
+{
+	struct mtkswitch_softc *sc = ifp->if_softc;
+	struct mii_data *mii = mtkswitch_miiforport(sc, ifp->if_dunit);
+        
+	if (mii == NULL)
+		return (ENXIO);
+	mii_mediachg(mii);
+	return (0);
+}
+
+static void
+mtkswitch_ifmedia_sts(struct ifnet *ifp, struct ifmediareq *ifmr)
+{
+	struct mtkswitch_softc *sc = ifp->if_softc;
+	struct mii_data *mii = mtkswitch_miiforport(sc, ifp->if_dunit);
+
+	DPRINTF(sc->sc_dev, "%s\n", __func__);
+
+	if (mii == NULL)
+		return;
+	mii_pollstat(mii); 
+	ifmr->ifm_active = mii->mii_media_active;
+	ifmr->ifm_status = mii->mii_media_status;
+}
+
+static int
+mtkswitch_getconf(device_t dev, etherswitch_conf_t *conf)
+{
+	struct mtkswitch_softc *sc;
+
+	sc = device_get_softc(dev);
+
+	/* Return the VLAN mode. */
+	conf->cmd = ETHERSWITCH_CONF_VLAN_MODE;
+	conf->vlan_mode = sc->vlan_mode;
+
+	return (0);
+}
+
+static int
+mtkswitch_setconf(device_t dev, etherswitch_conf_t *conf)
+{
+	struct mtkswitch_softc *sc;
+	int err;
+        
+	sc = device_get_softc(dev);
+        
+	/* Set the VLAN mode. */
+	if (conf->cmd & ETHERSWITCH_CONF_VLAN_MODE) {
+		err = mtkswitch_set_vlan_mode(sc, conf->vlan_mode);
+		if (err != 0)
+			return (err);
+	}
+
+	return (0);
+}
+
+static int
+mtkswitch_getvgroup(device_t dev, etherswitch_vlangroup_t *e)
+{
+        struct mtkswitch_softc *sc = device_get_softc(dev);
+
+        return (sc->hal.mtkswitch_vlan_getvgroup(sc, e));
+}
+
+static int
+mtkswitch_setvgroup(device_t dev, etherswitch_vlangroup_t *e)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+
+	return (sc->hal.mtkswitch_vlan_setvgroup(sc, e));
+}
+
+static int
+mtkswitch_readphy(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)
+{
+	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)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+
+	return (sc->hal.mtkswitch_reg_read(dev, addr));
+}
+
+static int
+mtkswitch_writereg(device_t dev, int addr, int value)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+
+	return (sc->hal.mtkswitch_reg_write(dev, addr, value));
+}
+
+static device_method_t mtkswitch_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		mtkswitch_probe),
+	DEVMETHOD(device_attach,	mtkswitch_attach),
+	DEVMETHOD(device_detach,	mtkswitch_detach),
+
+	/* bus interface */
+	DEVMETHOD(bus_add_child,	device_add_child_ordered),
+
+	/* MII interface */
+	DEVMETHOD(miibus_readreg,	mtkswitch_readphy),
+	DEVMETHOD(miibus_writereg,	mtkswitch_writephy),
+	DEVMETHOD(miibus_statchg,	mtkswitch_statchg),
+
+	/* MDIO interface */
+	DEVMETHOD(mdio_readreg,		mtkswitch_readphy),
+	DEVMETHOD(mdio_writereg,	mtkswitch_writephy),
+
+	/* ehterswitch interface */
+	DEVMETHOD(etherswitch_lock,	mtkswitch_lock),
+	DEVMETHOD(etherswitch_unlock,	mtkswitch_unlock),
+	DEVMETHOD(etherswitch_getinfo,	mtkswitch_getinfo),
+	DEVMETHOD(etherswitch_readreg,	mtkswitch_readreg),
+	DEVMETHOD(etherswitch_writereg,	mtkswitch_writereg),
+	DEVMETHOD(etherswitch_readphyreg,	mtkswitch_readphy),
+	DEVMETHOD(etherswitch_writephyreg,	mtkswitch_writephy),
+	DEVMETHOD(etherswitch_getport,	mtkswitch_getport),
+	DEVMETHOD(etherswitch_setport,	mtkswitch_setport),
+	DEVMETHOD(etherswitch_getvgroup,	mtkswitch_getvgroup),
+	DEVMETHOD(etherswitch_setvgroup,	mtkswitch_setvgroup),
+	DEVMETHOD(etherswitch_getconf,	mtkswitch_getconf),
+	DEVMETHOD(etherswitch_setconf,	mtkswitch_setconf),
+
+	DEVMETHOD_END
+};
+
+DEFINE_CLASS_0(mtkswitch, mtkswitch_driver, mtkswitch_methods,
+    sizeof(struct mtkswitch_softc));
+static devclass_t mtkswitch_devclass;
+
+DRIVER_MODULE(mtkswitch, simplebus, mtkswitch_driver, mtkswitch_devclass, 0, 0);
+DRIVER_MODULE(miibus, mtkswitch, miibus_driver, miibus_devclass, 0, 0);
+DRIVER_MODULE(mdio, mtkswitch, mdio_driver, mdio_devclass, 0, 0);
+DRIVER_MODULE(etherswitch, mtkswitch, etherswitch_driver, etherswitch_devclass,
+    0, 0);
+MODULE_VERSION(mtkswitch, 1);
+MODULE_DEPEND(mtkswitch, miibus, 1, 1, 1);
+MODULE_DEPEND(mtkswitch, etherswitch, 1, 1, 1);
Index: head/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7620.h
===================================================================
--- head/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7620.h
+++ head/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7620.h
@@ -0,0 +1,122 @@
+/*-
+ * Copyright (c) 2016 Stanislav Galabov.
+ * 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_MT7620_H__
+#define	__MTKSWITCH_MT7620_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_PORT_MEMBER(p)	((1u<<16)<<(p))
+#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_VTIM(v)	((((v) >> 1) * 4) + 0x100)
+#define		VTIM_OFF(v)	(((v) & 1) ? 12 : 0)
+#define		VTIM_MASK	0xfff
+
+#define	MTKSWITCH_PIAC	0x7004
+#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_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_IPG_CFG_RND	(1u<<18)
+#define		PMCR_CFG_DEFAULT	(PMCR_BACKPR_EN | PMCR_BKOFF_EN | \
+		    PMCR_MAC_RX_EN | PMCR_MAC_TX_EN | PMCR_IPG_CFG_RND)
+
+#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_LAN_VID	0x001
+#define	MTKSWITCH_WAN_VID	0x002
+#define	MTKSWITCH_INVALID_VID	0xfff
+
+#define	MTKSWITCH_LAN_FID	1
+#define	MTKSWITCH_WAN_FID	2
+
+#endif	/* __MTKSWITCH_MT7620_H__ */
Index: head/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7620.c
===================================================================
--- head/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7620.c
+++ head/sys/dev/etherswitch/mtkswitch/mtkswitch_mt7620.c
@@ -0,0 +1,563 @@
+/*-
+ * Copyright (c) 2016 Stanislav Galabov.
+ * 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_mt7620.h>
+
+static int
+mtkswitch_phy_read_locked(struct mtkswitch_softc *sc, int phy, int reg)
+{
+	uint32_t data;
+        
+	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 = 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)
+{
+
+	MTKSWITCH_WRITE(sc, MTKSWITCH_PIAC, PIAC_PHY_ACS_ST | PIAC_MDIO_ST |
+	    (reg << PIAC_MDIO_REG_ADDR_OFF) | (phy << PIAC_MDIO_PHY_ADDR_OFF) |
+	    (val & PIAC_MDIO_RW_DATA_MASK) | PIAC_MDIO_CMD_WRITE);
+	while (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)
+{
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	return (MTKSWITCH_READ(sc, reg));
+}
+
+static uint32_t
+mtkswitch_reg_write32(struct mtkswitch_softc *sc, int reg, uint32_t val)
+{
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	MTKSWITCH_WRITE(sc, reg, val);
+	return (0);
+}
+
+static uint32_t
+mtkswitch_reg_read32_mt7621(struct mtkswitch_softc *sc, int reg)
+{
+	uint32_t low, hi;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	mtkswitch_phy_write_locked(sc, MTKSWITCH_GLOBAL_PHY,
+	    MTKSWITCH_GLOBAL_REG, MTKSWITCH_REG_ADDR(reg));
+	low = mtkswitch_phy_read_locked(sc, MTKSWITCH_GLOBAL_PHY,
+	    MTKSWITCH_REG_LO(reg));
+	hi = mtkswitch_phy_read_locked(sc, MTKSWITCH_GLOBAL_PHY,
+	    MTKSWITCH_REG_HI(reg));;
+	return (low | (hi << 16));
+}
+
+static uint32_t
+mtkswitch_reg_write32_mt7621(struct mtkswitch_softc *sc, int reg, uint32_t val)
+{
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	mtkswitch_phy_write_locked(sc, MTKSWITCH_GLOBAL_PHY,
+	    MTKSWITCH_GLOBAL_REG, MTKSWITCH_REG_ADDR(reg));
+	mtkswitch_phy_write_locked(sc, MTKSWITCH_GLOBAL_PHY,
+	    MTKSWITCH_REG_LO(reg), MTKSWITCH_VAL_LO(val));
+	mtkswitch_phy_write_locked(sc, 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));
+	if (MTKSWITCH_IS_HI16(reg))
+		return (MTKSWITCH_HI16(val));
+	return (MTKSWITCH_LO16(val));
+}
+
+static int
+mtkswitch_reg_write(device_t dev, int reg, int val)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+	uint32_t tmp;
+
+	tmp = sc->hal.mtkswitch_read(sc, MTKSWITCH_REG32(reg));
+	if (MTKSWITCH_IS_HI16(reg)) {
+		tmp &= MTKSWITCH_LO16_MSK;
+		tmp |= MTKSWITCH_TO_HI16(val);
+	} else {
+		tmp &= MTKSWITCH_HI16_MSK;
+		tmp |= MTKSWITCH_TO_LO16(val);
+	}
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_REG32(reg), tmp);
+
+	return (0);
+}
+
+static int
+mtkswitch_reset(struct mtkswitch_softc *sc)
+{
+
+	/* We don't reset the switch for now */
+	return (0);
+}
+
+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.
+	 */
+
+	/* 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 */
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_PCR(port), PCR_PORT_VLAN_SECURE);
+
+	/* 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);
+
+	/* Set port's MAC to default settings */
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_PMCR(port), PMCR_CFG_DEFAULT);
+}
+
+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 void
+mtkswitch_vlan_init_hw(struct mtkswitch_softc *sc)
+{
+	uint32_t val, vid, 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);
+		if (sc->sc_switchtype == MTK_SWITCH_MT7620) {
+			val = sc->hal.mtkswitch_read(sc, MTKSWITCH_VTIM(i));
+			val &= (VTIM_MASK << VTIM_OFF(i));
+			val |= ((i + 1) << VTIM_OFF(i));
+			sc->hal.mtkswitch_write(sc, MTKSWITCH_VTIM(i), val);
+		}
+	}
+
+	/* Now, add all ports as untagged members of VLAN 1 */
+	if (sc->sc_switchtype == MTK_SWITCH_MT7620) {
+		/* MT7620 uses vid index instead of actual vid */
+		vid = 0;
+	} else {
+		/* MT7621 uses the vid itself */
+		vid = 1;
+	}
+	val = VAWD1_IVL_MAC | VAWD1_VTAG_EN | VAWD1_VALID;
+	for (i = 0; i < sc->info.es_nports; i++)
+		val |= VAWD1_PORT_MEMBER(i);
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_VAWD1, val);
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_VAWD2, 0);
+	val = VTCR_BUSY | VTCR_FUNC_VID_WRITE | vid;
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_VTCR, val);
+
+	/* 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);
+
+	if ((sc->vlan_mode != ETHERSWITCH_VLAN_DOT1Q) ||
+	    (v->es_vlangroup > sc->info.es_nvlangroups))
+		return (EINVAL);
+
+	/* 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);
+	if (sc->sc_switchtype == MTK_SWITCH_MT7620) {
+		v->es_vid = (sc->hal.mtkswitch_read(sc,
+		    MTKSWITCH_VTIM(v->es_vlangroup)) >>
+		    VTIM_OFF(v->es_vlangroup)) & VTIM_MASK;
+	} else {
+		v->es_vid = v->es_vlangroup;
+	}
+
+	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_vlangroup & 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)
+{
+	uint32_t val, i, vid;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+
+	if ((sc->vlan_mode != ETHERSWITCH_VLAN_DOT1Q) ||
+	    (v->es_vlangroup > sc->info.es_nvlangroups))
+		return (EINVAL);
+
+	/* We currently don't support FID */
+	if (v->es_fid != 0)
+		return (EINVAL);
+
+	MTKSWITCH_LOCK(sc);
+	while (sc->hal.mtkswitch_read(sc, MTKSWITCH_VTCR) & VTCR_BUSY);
+	if (sc->sc_switchtype == MTK_SWITCH_MT7620) {
+		val = sc->hal.mtkswitch_read(sc,
+		    MTKSWITCH_VTIM(v->es_vlangroup));
+		val &= (VTIM_MASK << VTIM_OFF(v->es_vlangroup));
+		val |= ((v->es_vid & VTIM_MASK) << VTIM_OFF(v->es_vlangroup));
+		sc->hal.mtkswitch_write(sc, MTKSWITCH_VTIM(v->es_vlangroup),
+		    val);
+		vid = v->es_vlangroup;
+	} else
+		vid = v->es_vid;
+
+	/* We use FID 0 */
+	val = VAWD1_IVL_MAC | VAWD1_VTAG_EN | VAWD1_VALID;
+	val |= ((v->es_member_ports & VAWD1_MEMBER_MASK) << VAWD1_MEMBER_OFF);
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_VAWD1, val);
+
+	/* Set tagged ports */
+	val = 0;
+	for (i = 0; i < sc->info.es_nports; i++)
+		if (((1<<i) & v->es_untagged_ports) == 0)
+			val |= VAWD2_PORT_TAGGED(i);
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_VAWD2, val);
+
+	/* 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);
+
+	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);
+	sc->hal.mtkswitch_write(sc, MTKSWITCH_PPBV2(port), val);
+
+	return (0);
+}
+
+extern void
+mtk_attach_switch_mt7620(struct mtkswitch_softc *sc)
+{
+
+	sc->portmap = 0x7f;
+	sc->phymap = 0x1f;
+
+	sc->info.es_nports = 7;
+	sc->info.es_vlan_caps = ETHERSWITCH_VLAN_DOT1Q;
+	sc->info.es_nvlangroups = 16;
+	sprintf(sc->info.es_name, "Mediatek GSW");
+
+	if (sc->sc_switchtype == MTK_SWITCH_MT7621) {
+		sc->hal.mtkswitch_read = mtkswitch_reg_read32_mt7621;
+		sc->hal.mtkswitch_write = mtkswitch_reg_write32_mt7621;
+		sc->info.es_nvlangroups = 4096;
+	} else {
+		sc->hal.mtkswitch_read = mtkswitch_reg_read32;
+		sc->hal.mtkswitch_write = mtkswitch_reg_write32;
+	}
+
+	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;
+}
Index: head/sys/dev/etherswitch/mtkswitch/mtkswitch_rt3050.h
===================================================================
--- head/sys/dev/etherswitch/mtkswitch/mtkswitch_rt3050.h
+++ head/sys/dev/etherswitch/mtkswitch/mtkswitch_rt3050.h
@@ -0,0 +1,88 @@
+/*-
+ * Copyright (c) 2016 Stanislav Galabov.
+ * 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_RT3050_H__
+#define	__MTKSWITCH_RT3050_H__
+
+#define	MTKSWITCH_PVID(p)	((((p) >> 1) * 4) + 0x40)
+#define		PVID_OFF(p)	(((p) & 1) ? 12 : 0)
+#define		PVID_MASK	0xfff
+
+#define	MTKSWITCH_VLANI(v)	((((v) >> 1) * 4) + 0x50)
+#define		VLANI_OFF(v)	(((v) & 1) ? 12 : 0)
+#define		VLANI_MASK	0xfff
+
+#define MTKSWITCH_VMSC(x)	((((x) >> 2) * 4) + 0x70)
+#define		VMSC_OFF(x)	((x & 3) * 8)
+#define		VMSC_MASK	0xff
+
+#define	MTKSWITCH_POA		0x0080
+#define		POA_PRT_DPX(x)	((1<<9)<<(x))
+#define		POA_FE_SPEED(x) ((1<<0)<<(x))
+#define		POA_GE_SPEED(v, x)	((((v)>>5)>>(((x)-5)*2)) & 0x3)
+#define		POA_FE_XFC(x)	((1<<16)<<(x))
+#define		POA_GE_XFC(v, x)	((((v)>>21)>>(((x)-5)*2)) & 0x3)
+#define		POA_PRT_LINK(x)	((1<<25)<<(x))
+#define		POA_GE_XFC_TX_MSK	0x2
+#define		POA_GE_XFC_RX_MSK	0x1
+#define		POA_GE_SPEED_10		0x0
+#define		POA_GE_SPEED_100	0x1
+#define		POA_GE_SPEED_1000	0x2
+
+#define	MTKSWITCH_FPA		0x0084
+#define		FPA_ALL_AUTO	0x00000000
+
+#define	MTKSWITCH_POC2		0x0098
+#define		POC2_UNTAG_PORT(x)	(1 << (x))
+#define		POC2_UNTAG_VLAN		(1 << 15)
+
+#define	MTKSWITCH_STRT		0x00a0
+#define		STRT_RESET	0xffffffff
+
+#define	MTKSWITCH_PCR0		0x00c0
+#define		PCR0_WRITE	(1<<13)
+#define		PCR0_READ	(1<<14)
+#define		PCR0_ACTIVE	(PCR0_WRITE | PCR0_READ)
+#define		PCR0_REG(x)	(((x) & 0x1f) << 8)
+#define		PCR0_PHY(x)	((x) & 0x1f)
+#define		PCR0_DATA(x)	(((x) & 0xffff) << 16)
+
+#define	MTKSWITCH_PCR1		0x00c4
+#define		PCR1_DATA_OFF	16
+#define		PCR1_DATA_MASK	0xffff
+
+#define	MTKSWITCH_SGC2		0x00e4
+#define		SGC2_DOUBLE_TAG_PORT(x)	(1 << (x))
+
+#define	MTKSWITCH_VUB(x)	((((x) >> 2) * 4) + 0x100)
+#define		VUB_OFF(x)	((x & 3) * 7)
+#define		VUB_MASK	0x7f
+
+#define	MTKSWITCH_PORT_IS_100M(x)	((x) < 5)
+
+#endif	/* __MTKSWITCH_RT3050_H__ */
Index: head/sys/dev/etherswitch/mtkswitch/mtkswitch_rt3050.c
===================================================================
--- head/sys/dev/etherswitch/mtkswitch/mtkswitch_rt3050.c
+++ head/sys/dev/etherswitch/mtkswitch/mtkswitch_rt3050.c
@@ -0,0 +1,498 @@
+/*-
+ * Copyright (c) 2016 Stanislav Galabov.
+ * 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_rt3050.h>
+
+static int
+mtkswitch_reg_read(device_t dev, int reg)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+	uint32_t val;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	val = MTKSWITCH_READ(sc, MTKSWITCH_REG32(reg));
+	if (MTKSWITCH_IS_HI16(reg))
+		return (MTKSWITCH_HI16(val));
+	return (MTKSWITCH_LO16(val));
+}
+
+static int
+mtkswitch_reg_write(device_t dev, int reg, int val)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+	uint32_t tmp;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	tmp = MTKSWITCH_READ(sc, MTKSWITCH_REG32(reg));
+	if (MTKSWITCH_IS_HI16(reg)) {
+		tmp &= MTKSWITCH_LO16_MSK;
+		tmp |= MTKSWITCH_TO_HI16(val);
+	} else {
+		tmp &= MTKSWITCH_HI16_MSK;
+		tmp |= MTKSWITCH_TO_LO16(val);
+	}
+	MTKSWITCH_WRITE(sc, MTKSWITCH_REG32(reg), tmp);
+
+	return (0);
+}
+
+static int
+mtkswitch_phy_read(device_t dev, int phy, int reg)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+	int val;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+	MTKSWITCH_LOCK(sc);
+	while (MTKSWITCH_READ(sc, MTKSWITCH_PCR0) & PCR0_ACTIVE);
+	MTKSWITCH_WRITE(sc, MTKSWITCH_PCR0, PCR0_READ | PCR0_REG(reg) |
+	    PCR0_PHY(phy));
+	while (MTKSWITCH_READ(sc, MTKSWITCH_PCR0) & PCR0_ACTIVE);
+	val = (MTKSWITCH_READ(sc, MTKSWITCH_PCR1) >> PCR1_DATA_OFF) &
+	    PCR1_DATA_MASK;
+	MTKSWITCH_UNLOCK(sc);
+	return (val);
+}
+
+static int
+mtkswitch_phy_write(device_t dev, int phy, int reg, int val)
+{
+	struct mtkswitch_softc *sc = device_get_softc(dev);
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+	MTKSWITCH_LOCK(sc);
+	while (MTKSWITCH_READ(sc, MTKSWITCH_PCR0) & PCR0_ACTIVE);
+	MTKSWITCH_WRITE(sc, MTKSWITCH_PCR0, PCR0_WRITE | PCR0_REG(reg) |
+	    PCR0_PHY(phy) | PCR0_DATA(val));
+	while (MTKSWITCH_READ(sc, MTKSWITCH_PCR0) & PCR0_ACTIVE);
+	MTKSWITCH_UNLOCK(sc);
+	return (0);
+}
+
+static int
+mtkswitch_reset(struct mtkswitch_softc *sc)
+{
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+	MTKSWITCH_LOCK(sc);
+	MTKSWITCH_WRITE(sc, MTKSWITCH_STRT, STRT_RESET);
+	while (MTKSWITCH_READ(sc, MTKSWITCH_STRT) != 0);
+	MTKSWITCH_UNLOCK(sc);
+
+	return (0);
+}
+
+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.
+	 */
+
+	/* Called early and hence unlocked */
+	/* Set ports 0-4 to auto negotiation */
+	MTKSWITCH_WRITE(sc, MTKSWITCH_FPA, FPA_ALL_AUTO);
+
+	return (0);
+}
+
+static int
+mtkswitch_hw_global_setup(struct mtkswitch_softc *sc)
+{
+
+	/* Called early and hence unlocked */
+	return (0);
+}
+
+static void
+mtkswitch_port_init(struct mtkswitch_softc *sc, int port)
+{
+	/* Called early and hence unlocked */
+	/* Do nothing - ports are set to auto negotiation in hw_setup */
+}
+
+static uint32_t
+mtkswitch_get_port_status(struct mtkswitch_softc *sc, int port)
+{
+	uint32_t val, res;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	res = 0;
+	val = MTKSWITCH_READ(sc, MTKSWITCH_POA);
+
+	if (val & POA_PRT_LINK(port))
+		res |= MTKSWITCH_LINK_UP;
+	if (val & POA_PRT_DPX(port))
+		res |= MTKSWITCH_DUPLEX;
+
+	if (MTKSWITCH_PORT_IS_100M(port)) {
+		if (val & POA_FE_SPEED(port))
+			res |= MTKSWITCH_SPEED_100;
+		if (val & POA_FE_XFC(port))
+			res |= (MTKSWITCH_TXFLOW | MTKSWITCH_RXFLOW);
+	} else {
+		switch (POA_GE_SPEED(val, port)) {
+		case POA_GE_SPEED_10:
+			res |= MTKSWITCH_SPEED_10;
+			break;
+		case POA_GE_SPEED_100:
+			res |= MTKSWITCH_SPEED_100;
+			break;
+		case POA_GE_SPEED_1000:
+			res |= MTKSWITCH_SPEED_1000;
+			break;
+		}
+
+		val = POA_GE_XFC(val, port);
+		if (val & POA_GE_XFC_TX_MSK)
+			res |= MTKSWITCH_TXFLOW;
+		if (val & POA_GE_XFC_RX_MSK)
+			res |= MTKSWITCH_RXFLOW;
+	}
+
+	return (res);
+}
+
+static int
+mtkswitch_atu_flush(struct mtkswitch_softc *sc)
+{
+	return (0);
+}
+
+static int
+mtkswitch_port_vlan_setup(struct mtkswitch_softc *sc, etherswitch_port_t *p)
+{
+	uint32_t val;
+	int err, invert = 0;
+
+	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);
+		}
+	}
+
+	/* Mutually exclusive */
+	if (p->es_flags & ETHERSWITCH_PORT_ADDTAG &&
+	    p->es_flags & ETHERSWITCH_PORT_STRIPTAG) {
+		invert = 1;
+	}
+
+	val = MTKSWITCH_READ(sc, MTKSWITCH_SGC2);
+	if (p->es_flags & ETHERSWITCH_PORT_DOUBLE_TAG)
+		val |= SGC2_DOUBLE_TAG_PORT(p->es_port);
+	else
+		val &= ~SGC2_DOUBLE_TAG_PORT(p->es_port);
+	MTKSWITCH_WRITE(sc, MTKSWITCH_SGC2, val);
+
+	val = MTKSWITCH_READ(sc, MTKSWITCH_POC2);
+	if (invert) {
+		if (val & POC2_UNTAG_PORT(p->es_port))
+			val &= ~POC2_UNTAG_PORT(p->es_port);
+		else
+			val |= POC2_UNTAG_PORT(p->es_port);
+	} else if (p->es_flags & ETHERSWITCH_PORT_STRIPTAG)
+		val |= POC2_UNTAG_PORT(p->es_port);
+	else
+		val &= ~POC2_UNTAG_PORT(p->es_port);
+	MTKSWITCH_WRITE(sc, MTKSWITCH_POC2, val);
+	MTKSWITCH_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+mtkswitch_port_vlan_get(struct mtkswitch_softc *sc, etherswitch_port_t *p)
+{
+	uint32_t val;
+
+	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 */
+	p->es_flags = 0;
+	val = MTKSWITCH_READ(sc, MTKSWITCH_SGC2);
+	if (val & SGC2_DOUBLE_TAG_PORT(p->es_port))
+		p->es_flags |= ETHERSWITCH_PORT_DOUBLE_TAG;
+
+	val = MTKSWITCH_READ(sc, MTKSWITCH_POC2);
+	if (val & POC2_UNTAG_PORT(p->es_port))
+		p->es_flags |= ETHERSWITCH_PORT_STRIPTAG;
+	else
+		p->es_flags |= ETHERSWITCH_PORT_ADDTAG;
+
+	MTKSWITCH_UNLOCK(sc);
+
+	return (0);
+}
+
+static void
+mtkswitch_vlan_init_hw(struct mtkswitch_softc *sc)
+{
+	uint32_t val, vid;
+	int i;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+	MTKSWITCH_LOCK(sc);
+
+	/* Reset everything to defaults first */
+	for (i = 0; i < sc->info.es_nvlangroups; i++) {
+		/* Remove all VLAN members and untag info, if any */
+		if (i % 4 == 0) {
+			MTKSWITCH_WRITE(sc, MTKSWITCH_VMSC(i), 0);
+			if (sc->sc_switchtype != MTK_SWITCH_RT3050)
+				MTKSWITCH_WRITE(sc, MTKSWITCH_VUB(i), 0);
+		}
+		/* Reset to default VIDs */
+		val = MTKSWITCH_READ(sc, MTKSWITCH_VLANI(i));
+		val &= ~(VLANI_MASK << VLANI_OFF(i));
+		val |= ((i + 1) << VLANI_OFF(i));
+		MTKSWITCH_WRITE(sc, MTKSWITCH_VLANI(i), val);
+	}
+
+	/* Now, add all ports as untagged members to VLAN1 */
+	vid = 0;
+	val = MTKSWITCH_READ(sc, MTKSWITCH_VMSC(vid));
+	val &= ~(VMSC_MASK << VMSC_OFF(vid));
+	val |= (((1<<sc->numports)-1) << VMSC_OFF(vid));
+	MTKSWITCH_WRITE(sc, MTKSWITCH_VMSC(vid), val);
+	if (sc->sc_switchtype != MTK_SWITCH_RT3050) {
+		val = MTKSWITCH_READ(sc, MTKSWITCH_VUB(vid));
+		val &= ~(VUB_MASK << VUB_OFF(vid));
+		val |= (((1<<sc->numports)-1) << VUB_OFF(vid));
+		MTKSWITCH_WRITE(sc, MTKSWITCH_VUB(vid), val);
+	}
+	val = MTKSWITCH_READ(sc, MTKSWITCH_POC2);
+	if (sc->sc_switchtype != MTK_SWITCH_RT3050)
+		val |= POC2_UNTAG_VLAN;
+	val |= ((1<<sc->numports)-1);
+	MTKSWITCH_WRITE(sc, MTKSWITCH_POC2, val);
+
+	/* only the first vlangroup is valid */
+	sc->valid_vlans = (1<<0);
+
+	/* Set all port PVIDs to 1 */
+	vid = 1;
+	for (i = 0; i < sc->info.es_nports; i++) {
+		val = MTKSWITCH_READ(sc, MTKSWITCH_PVID(i));
+		val &= ~(PVID_MASK << PVID_OFF(i));
+		val |= (vid << PVID_OFF(i));
+		MTKSWITCH_WRITE(sc, MTKSWITCH_PVID(i), val);
+	}
+
+	MTKSWITCH_UNLOCK(sc);
+}
+
+static int
+mtkswitch_vlan_getvgroup(struct mtkswitch_softc *sc, etherswitch_vlangroup_t *v)
+{
+	uint32_t val;
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+
+	if ((sc->vlan_mode != ETHERSWITCH_VLAN_DOT1Q) ||
+	    (v->es_vlangroup > sc->info.es_nvlangroups))
+		return (EINVAL);
+
+	/* Reset the member ports. */
+	v->es_untagged_ports = 0;
+	v->es_member_ports = 0;
+
+	/* Not supported */
+	v->es_fid = 0;
+
+	/* Vlan ID */
+	v->es_vid = 0;
+	if ((sc->valid_vlans & (1<<v->es_vlangroup)) == 0)
+		return (0);
+
+	MTKSWITCH_LOCK(sc);
+	v->es_vid = (MTKSWITCH_READ(sc, MTKSWITCH_VLANI(v->es_vlangroup)) >>
+	    VLANI_OFF(v->es_vlangroup)) & VLANI_MASK;
+	v->es_vid |= ETHERSWITCH_VID_VALID;
+
+	/* Member ports */
+	v->es_member_ports = v->es_untagged_ports =
+	    (MTKSWITCH_READ(sc, MTKSWITCH_VMSC(v->es_vlangroup)) >>
+	    VMSC_OFF(v->es_vlangroup)) & VMSC_MASK;
+
+	val = MTKSWITCH_READ(sc, MTKSWITCH_POC2);
+
+	if ((val & POC2_UNTAG_VLAN) && sc->sc_switchtype != MTK_SWITCH_RT3050) {
+		val = (MTKSWITCH_READ(sc, MTKSWITCH_VUB(v->es_vlangroup)) >>
+		    VUB_OFF(v->es_vlangroup)) & VUB_MASK;
+	} else {
+		val &= VUB_MASK;
+	}
+	v->es_untagged_ports &= val;
+
+	MTKSWITCH_UNLOCK(sc);
+	return (0);
+}
+
+static int
+mtkswitch_vlan_setvgroup(struct mtkswitch_softc *sc, etherswitch_vlangroup_t *v)
+{
+	uint32_t val, tmp;
+
+	if ((sc->vlan_mode != ETHERSWITCH_VLAN_DOT1Q) ||
+	    (v->es_vlangroup > sc->info.es_nvlangroups))
+		return (EINVAL);
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+	MTKSWITCH_LOCK(sc);
+	/* First, see if we can accomodate the request at all */
+	val = MTKSWITCH_READ(sc, MTKSWITCH_POC2);
+	if ((val & POC2_UNTAG_VLAN) == 0 ||
+	    sc->sc_switchtype == MTK_SWITCH_RT3050) {
+		val &= VUB_MASK;
+		tmp = v->es_untagged_ports & v->es_member_ports;
+		if (val != tmp) {
+			/* Cannot accomodate request */
+			MTKSWITCH_UNLOCK(sc);
+			return (ENOTSUP);
+		}
+	} else {
+		/* Prefer per-Vlan untag and set its members */
+		val = MTKSWITCH_READ(sc, MTKSWITCH_VUB(v->es_vlangroup));
+		val &= ~(VUB_MASK << VUB_OFF(v->es_vlangroup));
+		val |= (((v->es_untagged_ports) & VUB_MASK) <<
+		    VUB_OFF(v->es_vlangroup));
+		MTKSWITCH_WRITE(sc, MTKSWITCH_VUB(v->es_vlangroup), val);
+	}
+
+	/* Set VID */
+	val = MTKSWITCH_READ(sc, MTKSWITCH_VLANI(v->es_vlangroup));
+	val &= ~(VLANI_MASK << VLANI_OFF(v->es_vlangroup));
+	val |= (v->es_vid & VLANI_MASK) << VLANI_OFF(v->es_vlangroup);
+	MTKSWITCH_WRITE(sc, MTKSWITCH_VLANI(v->es_vlangroup), val);
+
+	/* Set members */
+	val = MTKSWITCH_READ(sc, MTKSWITCH_VMSC(v->es_vlangroup));
+	val &= ~(VMSC_MASK << VMSC_OFF(v->es_vlangroup));
+	val |= (v->es_member_ports << VMSC_OFF(v->es_vlangroup));
+	MTKSWITCH_WRITE(sc, MTKSWITCH_VMSC(v->es_vlangroup), val);
+
+	sc->valid_vlans |= (1<<v->es_vlangroup);
+
+	MTKSWITCH_UNLOCK(sc);
+	return (0);
+}
+
+static int
+mtkswitch_vlan_get_pvid(struct mtkswitch_softc *sc, int port, int *pvid)
+{
+
+	MTKSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	*pvid = (MTKSWITCH_READ(sc, MTKSWITCH_PVID(port)) >> PVID_OFF(port)) &
+	    PVID_MASK;
+
+	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 = MTKSWITCH_READ(sc, MTKSWITCH_PVID(port));
+	val &= ~(PVID_MASK << PVID_OFF(port));
+	val |= (pvid & PVID_MASK) << PVID_OFF(port);
+	MTKSWITCH_WRITE(sc, MTKSWITCH_PVID(port), val);
+	
+	return (0);
+}
+
+extern void
+mtk_attach_switch_rt3050(struct mtkswitch_softc *sc)
+{
+
+	sc->portmap = 0x7f;
+	sc->phymap = 0x1f;
+
+	sc->info.es_nports = 7;
+	sc->info.es_vlan_caps = ETHERSWITCH_VLAN_DOT1Q;
+	sc->info.es_nvlangroups = 16;
+	sprintf(sc->info.es_name, "Ralink ESW");
+
+	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;
+}
Index: head/sys/dev/etherswitch/mtkswitch/mtkswitchvar.h
===================================================================
--- head/sys/dev/etherswitch/mtkswitch/mtkswitchvar.h
+++ head/sys/dev/etherswitch/mtkswitch/mtkswitchvar.h
@@ -0,0 +1,164 @@
+/*-
+ * Copyright (c) 2016 Stanislav Galabov.
+ * 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	__MTKSWITCHVAR_H__
+#define	__MTKSWITCHVAR_H__
+
+typedef enum {
+	MTK_SWITCH_NONE,
+	MTK_SWITCH_RT3050,
+	MTK_SWITCH_RT3352,
+	MTK_SWITCH_RT5350,
+	MTK_SWITCH_MT7620,
+	MTK_SWITCH_MT7621,
+	MTK_SWITCH_MT7628,
+} mtk_switch_type;
+
+#define	MTK_IS_SWITCH(_sc, _type)		\
+	    (!!((_sc)->sc_switchtype == MTK_SWITCH_ ## _type))
+
+#define	MTKSWITCH_MAX_PORTS	7
+#define MTKSWITCH_MAX_PHYS	7
+#define	MTKSWITCH_CPU_PORT	6
+
+#define	MTKSWITCH_LINK_UP	(1<<0)
+#define	MTKSWITCH_SPEED_MASK	(3<<1)
+#define	MTKSWITCH_SPEED_10	(0<<1)
+#define MTKSWITCH_SPEED_100	(1<<1)
+#define	MTKSWITCH_SPEED_1000	(2<<1)
+#define	MTKSWITCH_DUPLEX	(1<<3)
+#define MTKSWITCH_TXFLOW	(1<<4)
+#define MTKSWITCH_RXFLOW	(1<<5)
+
+struct mtkswitch_softc {
+	struct mtx	sc_mtx;
+	device_t	sc_dev;
+	struct resource *sc_res;
+	int		numphys;
+	uint32_t	phymap;
+	int		numports;
+	uint32_t	portmap;
+	int		cpuport;
+	uint32_t	valid_vlans;
+	mtk_switch_type	sc_switchtype;
+	char		*ifname[MTKSWITCH_MAX_PHYS];
+	device_t	miibus[MTKSWITCH_MAX_PHYS];
+	struct ifnet	*ifp[MTKSWITCH_MAX_PHYS];
+	struct callout	callout_tick;
+	etherswitch_info_t info;
+
+	uint32_t	vlan_mode;
+
+	struct {
+		/* Global setup */
+		int (* mtkswitch_reset) (struct mtkswitch_softc *);
+		int (* mtkswitch_hw_setup) (struct mtkswitch_softc *);
+		int (* mtkswitch_hw_global_setup) (struct mtkswitch_softc *);
+
+		/* Port functions */
+		void (* mtkswitch_port_init) (struct mtkswitch_softc *, int);
+		uint32_t (* mtkswitch_get_port_status)
+		    (struct mtkswitch_softc *, int);
+
+		/* ATU functions */
+		int (* mtkswitch_atu_flush) (struct mtkswitch_softc *);
+
+		/* VLAN functions */
+		int (* mtkswitch_port_vlan_setup) (struct mtkswitch_softc *,
+		    etherswitch_port_t *);
+		int (* mtkswitch_port_vlan_get) (struct mtkswitch_softc *,
+		    etherswitch_port_t *);
+		void (* mtkswitch_vlan_init_hw) (struct mtkswitch_softc *);
+		int (* mtkswitch_vlan_getvgroup) (struct mtkswitch_softc *,
+		    etherswitch_vlangroup_t *);
+		int (* mtkswitch_vlan_setvgroup) (struct mtkswitch_softc *,
+		    etherswitch_vlangroup_t *);
+		int (* mtkswitch_vlan_get_pvid) (struct mtkswitch_softc *,
+		    int, int *);
+		int (* mtkswitch_vlan_set_pvid) (struct mtkswitch_softc *,
+		    int, int);
+
+		/* PHY functions */
+		int (* mtkswitch_phy_read) (device_t, int, int);
+		int (* mtkswitch_phy_write) (device_t, int, int, int);
+
+		/* Register functions */
+		int (* mtkswitch_reg_read) (device_t, int);
+		int (* mtkswitch_reg_write) (device_t, int, int);
+
+		/* Internal register access functions */
+		uint32_t (* mtkswitch_read) (struct mtkswitch_softc *, int);
+		uint32_t (* mtkswitch_write) (struct mtkswitch_softc *, int,
+		    uint32_t);
+	} hal;
+};
+
+#define	MTKSWITCH_LOCK(_sc)			\
+	    mtx_lock(&(_sc)->sc_mtx)
+#define	MTKSWITCH_UNLOCK(_sc)			\
+	    mtx_unlock(&(_sc)->sc_mtx)
+#define	MTKSWITCH_LOCK_ASSERT(_sc, _what)	\
+	    mtx_assert(&(_sc)->sc_mtx, (_what))
+#define	MTKSWITCH_TRYLOCK(_sc)			\
+	    mtx_trylock(&(_sc)->sc_mtx)
+
+#define	MTKSWITCH_READ(_sc, _reg)		\
+	    bus_read_4((_sc)->sc_res, (_reg))
+#define MTKSWITCH_WRITE(_sc, _reg, _val)	\
+	    bus_write_4((_sc)->sc_res, (_reg), (_val))
+#define	MTKSWITCH_MOD(_sc, _reg, _clr, _set)	\
+	    MTKSWITCH_WRITE((_sc), (_reg),	\
+	        ((MTKSWITCH_READ((_sc), (_reg)) & ~(_clr)) | (_set))
+
+#define	MTKSWITCH_REG32(addr)	((addr) & ~(0x3))
+#define	MTKSWITCH_IS_HI16(addr)	(((addr) & 0x3) > 0x1)
+#define	MTKSWITCH_HI16(x)	(((x) >> 16) & 0xffff)
+#define	MTKSWITCH_LO16(x)	((x) & 0xffff)
+#define	MTKSWITCH_TO_HI16(x)	(((x) & 0xffff) << 16)
+#define	MTKSWITCH_TO_LO16(x)	((x) & 0xffff)
+#define	MTKSWITCH_HI16_MSK	0xffff0000
+#define MTKSWITCH_LO16_MSK	0x0000ffff
+
+#if defined(DEBUG)
+#define	DPRINTF(dev, args...)	device_printf(dev, args)
+#define	DEVERR(dev, err, fmt, args...)	do {	\
+	    if (err != 0) device_printf(dev, fmt, err, args);	\
+	} while (0)
+#define	DEBUG_INCRVAR(var)		do {	\
+	    var++;				\
+	} while (0)
+#else
+#define	DPRINTF(dev, args...)
+#define	DEVERR(dev, err, fmt, args...)
+#define	DEBUG_INCRVAR(var)
+#endif
+
+extern void mtk_attach_switch_rt3050(struct mtkswitch_softc *);
+extern void mtk_attach_switch_mt7620(struct mtkswitch_softc *);
+
+#endif	/* __MTKSWITCHVAR_H__ */