Add the support for 802.1q and port based vlans for arswitch.

Tested on: RB450G (standalone ar8316), RSPRO (standalone ar8316) and
TPLink MR-3220 (ar724x integrated switch).

Approved by:	adrian (mentor)
Obtained from:	zrouter

11 files changed, 660 insertions, 70 deletions

diff --git a/sys/dev/etherswitch/arswitch/arswitch.c b/sys/dev/etherswitch/arswitch/arswitch.c
index 0490580..41139a3 100644
--- a/sys/dev/etherswitch/arswitch/arswitch.c
+++ b/sys/dev/etherswitch/arswitch/arswitch.c
@@ -58,6 +58,7 @@
 #include <dev/etherswitch/arswitch/arswitchvar.h>
 #include <dev/etherswitch/arswitch/arswitch_reg.h>
 #include <dev/etherswitch/arswitch/arswitch_phy.h>
+#include <dev/etherswitch/arswitch/arswitch_vlans.h>
 
 #include <dev/etherswitch/arswitch/arswitch_7240.h>
 #include <dev/etherswitch/arswitch/arswitch_8216.h>
@@ -162,6 +163,74 @@ arswitch_attach_phys(struct arswitch_softc *sc)
 }
 
 static int
+arswitch_reset(device_t dev)
+{
+
+	arswitch_writereg(dev, AR8X16_REG_MASK_CTRL,
+	    AR8X16_MASK_CTRL_SOFT_RESET);
+	DELAY(1000);
+	if (arswitch_readreg(dev, AR8X16_REG_MASK_CTRL) &
+	    AR8X16_MASK_CTRL_SOFT_RESET) {
+		device_printf(dev, "unable to reset switch\n");
+		return (-1);
+	}
+	return (0);
+}
+
+static int
+arswitch_set_vlan_mode(struct arswitch_softc *sc, uint32_t mode)
+{
+
+	/* Check for invalid modes. */
+	if ((mode & sc->info.es_vlan_caps) != mode)
+		return (EINVAL);
+
+	switch (mode) {
+	case ETHERSWITCH_VLAN_DOT1Q:
+		sc->vlan_mode = ETHERSWITCH_VLAN_DOT1Q;
+		break;
+	case ETHERSWITCH_VLAN_PORT:
+		sc->vlan_mode = ETHERSWITCH_VLAN_PORT;
+		break;
+	default:
+		sc->vlan_mode = 0;
+	};
+
+	/* Reset VLANs. */
+	arswitch_reset_vlans(sc);
+
+	return (0);
+}
+
+static void
+arswitch_ports_init(struct arswitch_softc *sc)
+{
+	int port;
+
+	/* Port0 - CPU */
+	arswitch_writereg(sc->sc_dev, AR8X16_REG_PORT_STS(0),
+	    (AR8X16_IS_SWITCH(sc, AR8216) ?
+	    AR8X16_PORT_STS_SPEED_100 : AR8X16_PORT_STS_SPEED_1000) |
+	    (AR8X16_IS_SWITCH(sc, AR8216) ? 0 : AR8X16_PORT_STS_RXFLOW) |
+	    (AR8X16_IS_SWITCH(sc, AR8216) ? 0 : AR8X16_PORT_STS_TXFLOW) |
+	    AR8X16_PORT_STS_RXMAC |
+	    AR8X16_PORT_STS_TXMAC |
+	    AR8X16_PORT_STS_DUPLEX);
+	arswitch_writereg(sc->sc_dev, AR8X16_REG_PORT_CTRL(0),
+	    arswitch_readreg(sc->sc_dev, AR8X16_REG_PORT_CTRL(0)) &
+	    ~AR8X16_PORT_CTRL_HEADER);
+
+	for (port = 1; port <= sc->numphys; port++) { 
+		/* Set ports to auto negotiation. */
+		arswitch_writereg(sc->sc_dev, AR8X16_REG_PORT_STS(port),
+		    AR8X16_PORT_STS_LINK_AUTO);
+		arswitch_writereg(sc->sc_dev, AR8X16_REG_PORT_CTRL(port),
+		    arswitch_readreg(sc->sc_dev, AR8X16_REG_PORT_CTRL(port)) &
+		    ~AR8X16_PORT_CTRL_HEADER);
+	}
+}
+
+static int
 arswitch_attach(device_t dev)
 {
 	struct arswitch_softc *sc;
@@ -190,11 +259,8 @@ arswitch_attach(device_t dev)
 	else
 		return (ENXIO);
 
-	/*
-	 * XXX these two should be part of the switch attach function
-	 */
+	/* Common defaults. */
 	sc->info.es_nports = 5; /* XXX technically 6, but 6th not used */
-	sc->info.es_nvlangroups = 16;
 
 	/* XXX Defaults for externally connected AR8316 */
 	sc->numphys = 4;
@@ -211,34 +277,20 @@ arswitch_attach(device_t dev)
 	(void) resource_int_value(device_get_name(dev), device_get_unit(dev),
 	    "is_gmii", &sc->is_gmii);
 
-	/*
-	 * This requires much more setup depending upon each chip, including:
-	 *
-	 * + Proper reinitialisation of the PHYs;
-	 * + Initialising the VLAN table;
-	 * + Initialising the port access table and CPU flood/broadcast
-	 *   configuration;
-	 * + Other things I haven't yet thought of.
-	 */
-#ifdef NOTYET
-	arswitch_writereg(dev, AR8X16_REG_MASK_CTRL,
-	    AR8X16_MASK_CTRL_SOFT_RESET);
-	DELAY(1000);
-	if (arswitch_readreg(dev, AR8X16_REG_MASK_CTRL) &
-	    AR8X16_MASK_CTRL_SOFT_RESET) {
-		device_printf(dev, "unable to reset switch\n");
-		return (ENXIO);
-	}
-#endif
+	if (sc->numphys > AR8X16_NUM_PHYS)
+		sc->numphys = AR8X16_NUM_PHYS;
 
-	err = sc->hal.arswitch_hw_setup(sc);
-	if (err != 0)
-		return (err);
+	/* Reset the switch. */
+	if (arswitch_reset(dev))
+		return (ENXIO);
 
 	err = sc->hal.arswitch_hw_global_setup(sc);
 	if (err != 0)
 		return (err);
 
+	/* Initialize the switch ports. */
+	arswitch_ports_init(sc);
+
 	/*
 	 * Attach the PHYs and complete the bus enumeration.
 	 */
@@ -246,6 +298,15 @@ arswitch_attach(device_t dev)
 	if (err != 0)
 		return (err);
 
+	/* Default to ingress filters off. */
+	err = arswitch_set_vlan_mode(sc, 0);
+	if (err != 0)
+		return (err);
+
+	err = sc->hal.arswitch_hw_setup(sc);
+	if (err != 0)
+		return (err);
+
 	bus_generic_probe(dev);
 	bus_enumerate_hinted_children(dev);
 	err = bus_generic_attach(dev);
@@ -428,19 +489,37 @@ arswitch_getinfo(device_t dev)
 static int
 arswitch_getport(device_t dev, etherswitch_port_t *p)
 {
-	struct arswitch_softc *sc = device_get_softc(dev);
+	struct arswitch_softc *sc;
+	struct ifmediareq *ifmr;
 	struct mii_data *mii;
-	struct ifmediareq *ifmr = &p->es_ifmr;
+	uint32_t reg;
 	int err;
-	
-	if (p->es_port < 0 || p->es_port >= AR8X16_NUM_PORTS)
+
+	sc = device_get_softc(dev);
+	if (p->es_port < 0 || p->es_port > sc->numphys)
 		return (ENXIO);
-	p->es_pvid = 0;
+
+	ARSWITCH_LOCK(sc);
+
+	/* Retrieve the PVID. */
+	arswitch_get_pvid(sc, p->es_port, &p->es_pvid);
+
+	/* Port flags. */
+	reg = arswitch_readreg(sc->sc_dev, AR8X16_REG_PORT_CTRL(p->es_port));
+	if (reg & AR8X16_PORT_CTRL_DOUBLE_TAG)
+		p->es_flags |= ETHERSWITCH_PORT_DOUBLE_TAG;
+	reg >>= AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_SHIFT;
+	if ((reg & 0x3) == AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_ADD)
+		p->es_flags |= ETHERSWITCH_PORT_ADDTAG;
+	if ((reg & 0x3) == AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_STRIP)
+		p->es_flags |= ETHERSWITCH_PORT_STRIPTAG;
+	ARSWITCH_UNLOCK(sc);
 
 	mii = arswitch_miiforport(sc, p->es_port);
 	if (p->es_port == 0) {
 		/* fill in fixed values for CPU port */
 		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;
@@ -457,29 +536,58 @@ arswitch_getport(device_t dev, etherswitch_port_t *p)
 	return (0);
 }
 
-/*
- * XXX doesn't yet work?
- */
 static int
 arswitch_setport(device_t dev, etherswitch_port_t *p)
 {
 	int err;
+	uint32_t reg;
 	struct arswitch_softc *sc;
 	struct ifmedia *ifm;
 	struct mii_data *mii;
 	struct ifnet *ifp;
 
-	/*
-	 * XXX check the sc numphys, or the #define ?
-	 */
-	if (p->es_port < 0 || p->es_port >= AR8X16_NUM_PHYS)
+	sc = device_get_softc(dev);
+	if (p->es_port < 0 || p->es_port > sc->numphys)
 		return (ENXIO);
 
-	sc = device_get_softc(dev);
+	/* Port flags. */
+	if (sc->vlan_mode == ETHERSWITCH_VLAN_DOT1Q) {
 
-	/*
-	 * XXX TODO: don't set the CPU port?
-	 */
+		ARSWITCH_LOCK(sc);
+		/* Set the PVID. */
+		if (p->es_pvid != 0)
+			arswitch_set_pvid(sc, p->es_port, p->es_pvid);
+
+		/* Mutually exclusive. */
+		if (p->es_flags & ETHERSWITCH_PORT_ADDTAG &&
+		    p->es_flags & ETHERSWITCH_PORT_STRIPTAG) {
+			ARSWITCH_UNLOCK(sc);
+			return (EINVAL);
+		}
+
+		reg = 0;
+		if (p->es_flags & ETHERSWITCH_PORT_DOUBLE_TAG)
+			reg |= AR8X16_PORT_CTRL_DOUBLE_TAG;
+		if (p->es_flags & ETHERSWITCH_PORT_ADDTAG)
+			reg |= AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_ADD <<
+			    AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_SHIFT;
+		if (p->es_flags & ETHERSWITCH_PORT_STRIPTAG)
+			reg |= AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_STRIP <<
+			    AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_SHIFT;
+
+		err = arswitch_modifyreg(sc->sc_dev,
+		    AR8X16_REG_PORT_CTRL(p->es_port),
+		    0x3 << AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_SHIFT |
+		    AR8X16_PORT_CTRL_DOUBLE_TAG, reg);
+
+		ARSWITCH_UNLOCK(sc);
+		if (err)
+			return (err);
+        }
+
+	/* Do not allow media changes on CPU port. */
+	if (p->es_port == AR8X16_PORT_CPU)
+		return (0);
 
 	mii = arswitch_miiforport(sc, p->es_port);
 	if (mii == NULL)
@@ -488,28 +596,7 @@ arswitch_setport(device_t dev, etherswitch_port_t *p)
 	ifp = arswitch_ifpforport(sc, p->es_port);
 
 	ifm = &mii->mii_media;
-	err = ifmedia_ioctl(ifp, &p->es_ifr, ifm, SIOCSIFMEDIA);
-	return (err);
-}
-
-static int
-arswitch_getvgroup(device_t dev, etherswitch_vlangroup_t *vg)
-{
-
-	/* XXX not implemented yet */
-	vg->es_vid = 0;
-	vg->es_member_ports = 0;
-	vg->es_untagged_ports = 0;
-	vg->es_fid = 0;
-	return (0);
-}
-
-static int
-arswitch_setvgroup(device_t dev, etherswitch_vlangroup_t *vg)
-{
-
-	/* XXX not implemented yet */
-	return (0);
+	return (ifmedia_ioctl(ifp, &p->es_ifr, ifm, SIOCSIFMEDIA));
 }
 
 static void
@@ -546,6 +633,38 @@ arswitch_ifmedia_sts(struct ifnet *ifp, struct ifmediareq *ifmr)
 	ifmr->ifm_status = mii->mii_media_status;
 }
 
+static int
+arswitch_getconf(device_t dev, etherswitch_conf_t *conf)
+{
+	struct arswitch_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
+arswitch_setconf(device_t dev, etherswitch_conf_t *conf)
+{
+	struct arswitch_softc *sc;
+	int err;
+
+	sc = device_get_softc(dev);
+
+	/* Set the VLAN mode. */
+	if (conf->cmd & ETHERSWITCH_CONF_VLAN_MODE) {
+		err = arswitch_set_vlan_mode(sc, conf->vlan_mode);
+		if (err != 0)
+			return (err);
+	}
+
+	return (0);
+}
+
 static device_method_t arswitch_methods[] = {
 	/* Device interface */
 	DEVMETHOD(device_probe,		arswitch_probe),
@@ -576,6 +695,8 @@ static device_method_t arswitch_methods[] = {
 	DEVMETHOD(etherswitch_setport,	arswitch_setport),
 	DEVMETHOD(etherswitch_getvgroup,	arswitch_getvgroup),
 	DEVMETHOD(etherswitch_setvgroup,	arswitch_setvgroup),
+	DEVMETHOD(etherswitch_getconf,	arswitch_getconf),
+	DEVMETHOD(etherswitch_setconf,	arswitch_setconf),
 
 	DEVMETHOD_END
 };
diff --git a/sys/dev/etherswitch/arswitch/arswitch_7240.c b/sys/dev/etherswitch/arswitch/arswitch_7240.c
index 63a8910..f3750bf 100644
--- a/sys/dev/etherswitch/arswitch/arswitch_7240.c
+++ b/sys/dev/etherswitch/arswitch/arswitch_7240.c
@@ -71,10 +71,6 @@ static int
 ar7240_hw_setup(struct arswitch_softc *sc)
 {
 
-	/* Enable CPU port; disable mirror port */
-	arswitch_writereg(sc->sc_dev, AR8X16_REG_CPU_PORT,
-	    AR8X16_CPU_PORT_EN | AR8X16_CPU_MIRROR_DIS);
-
 	return (0);
 }
 
@@ -85,6 +81,10 @@ static int
 ar7240_hw_global_setup(struct arswitch_softc *sc)
 {
 
+	/* Enable CPU port; disable mirror port */
+	arswitch_writereg(sc->sc_dev, AR8X16_REG_CPU_PORT,
+	    AR8X16_CPU_PORT_EN | AR8X16_CPU_MIRROR_DIS);
+
 	/* Setup TAG priority mapping */
 	arswitch_writereg(sc->sc_dev, AR8X16_REG_TAG_PRIO, 0xfa50);
 
@@ -133,4 +133,9 @@ ar7240_attach(struct arswitch_softc *sc)
 
 	sc->hal.arswitch_hw_setup = ar7240_hw_setup;
 	sc->hal.arswitch_hw_global_setup = ar7240_hw_global_setup;
+
+	/* Set the switch vlan capabilities. */
+	sc->info.es_vlan_caps = ETHERSWITCH_VLAN_DOT1Q |
+	    ETHERSWITCH_VLAN_PORT | ETHERSWITCH_VLAN_DOUBLE_TAG;
+	sc->info.es_nvlangroups = AR8X16_MAX_VLANS;
 }
diff --git a/sys/dev/etherswitch/arswitch/arswitch_8216.c b/sys/dev/etherswitch/arswitch/arswitch_8216.c
index eb49aba..72b2327 100644
--- a/sys/dev/etherswitch/arswitch/arswitch_8216.c
+++ b/sys/dev/etherswitch/arswitch/arswitch_8216.c
@@ -89,4 +89,6 @@ ar8216_attach(struct arswitch_softc *sc)
 
 	sc->hal.arswitch_hw_setup = ar8216_hw_setup;
 	sc->hal.arswitch_hw_global_setup = ar8216_hw_global_setup;
+
+	sc->info.es_nvlangroups = 0;
 }
diff --git a/sys/dev/etherswitch/arswitch/arswitch_8226.c b/sys/dev/etherswitch/arswitch/arswitch_8226.c
index 336120a..43d2281 100644
--- a/sys/dev/etherswitch/arswitch/arswitch_8226.c
+++ b/sys/dev/etherswitch/arswitch/arswitch_8226.c
@@ -89,4 +89,6 @@ ar8226_attach(struct arswitch_softc *sc)
 
 	sc->hal.arswitch_hw_setup = ar8226_hw_setup;
 	sc->hal.arswitch_hw_global_setup = ar8226_hw_global_setup;
+
+	sc->info.es_nvlangroups = 0;
 }
diff --git a/sys/dev/etherswitch/arswitch/arswitch_8316.c b/sys/dev/etherswitch/arswitch/arswitch_8316.c
index 3f01aed..91dd72d 100644
--- a/sys/dev/etherswitch/arswitch/arswitch_8316.c
+++ b/sys/dev/etherswitch/arswitch/arswitch_8316.c
@@ -127,7 +127,18 @@ static int
 ar8316_hw_global_setup(struct arswitch_softc *sc)
 {
 
-	arswitch_writereg(sc->sc_dev, 0x38, 0xc000050e);
+	arswitch_writereg(sc->sc_dev, 0x38, AR8X16_MAGIC);
+
+	/* Enable CPU port and disable mirror port. */
+	arswitch_writereg(sc->sc_dev, AR8X16_REG_CPU_PORT,
+	    AR8X16_CPU_PORT_EN | AR8X16_CPU_MIRROR_DIS);
+
+	/* Setup TAG priority mapping. */
+	arswitch_writereg(sc->sc_dev, AR8X16_REG_TAG_PRIO, 0xfa50);
+
+	/* Enable ARP frame acknowledge. */
+	arswitch_modifyreg(sc->sc_dev, AR8X16_REG_AT_CTRL, 0,
+	    AR8X16_AT_CTRL_ARP_EN);
 
 	/*
 	 * Flood address table misses to all ports, and enable forwarding of
@@ -136,9 +147,14 @@ ar8316_hw_global_setup(struct arswitch_softc *sc)
 	arswitch_writereg(sc->sc_dev, AR8X16_REG_FLOOD_MASK,
 	    AR8X16_FLOOD_MASK_BCAST_TO_CPU | 0x003f003f);
 
+	/* Enable jumbo frames. */
 	arswitch_modifyreg(sc->sc_dev, AR8X16_REG_GLOBAL_CTRL,
 	    AR8316_GLOBAL_CTRL_MTU_MASK, 9018 + 8 + 2);
 
+	/* Setup service TAG. */
+	arswitch_modifyreg(sc->sc_dev, AR8X16_REG_SERVICE_TAG,
+	    AR8X16_SERVICE_TAG_MASK, 0);
+
 	return (0);
 }
 
@@ -148,4 +164,9 @@ ar8316_attach(struct arswitch_softc *sc)
 
 	sc->hal.arswitch_hw_setup = ar8316_hw_setup;
 	sc->hal.arswitch_hw_global_setup = ar8316_hw_global_setup;
+
+	/* Set the switch vlan capabilities. */
+	sc->info.es_vlan_caps = ETHERSWITCH_VLAN_DOT1Q |
+	    ETHERSWITCH_VLAN_PORT | ETHERSWITCH_VLAN_DOUBLE_TAG;
+	sc->info.es_nvlangroups = AR8X16_MAX_VLANS;
 }
diff --git a/sys/dev/etherswitch/arswitch/arswitch_reg.c b/sys/dev/etherswitch/arswitch/arswitch_reg.c
index 2c31c9f..5413406 100644
--- a/sys/dev/etherswitch/arswitch/arswitch_reg.c
+++ b/sys/dev/etherswitch/arswitch/arswitch_reg.c
@@ -178,3 +178,24 @@ arswitch_modifyreg(device_t dev, int addr, int mask, int set)
 	value |= set;
 	return (arswitch_writereg(dev, addr, value));
 }
+
+int
+arswitch_waitreg(device_t dev, int addr, int mask, int val, int timeout)
+{
+	int err, v;
+
+	err = -1;
+	while (1) {
+		v = arswitch_readreg(dev, addr);
+		v &= mask;
+		if (v == val) {
+			err = 0;
+			break;
+		}
+		if (!timeout)
+			break;
+		DELAY(1);
+		timeout--;
+	}
+	return (err);
+}
diff --git a/sys/dev/etherswitch/arswitch/arswitch_reg.h b/sys/dev/etherswitch/arswitch/arswitch_reg.h
index fdac59f..0167e2c 100644
--- a/sys/dev/etherswitch/arswitch/arswitch_reg.h
+++ b/sys/dev/etherswitch/arswitch/arswitch_reg.h
@@ -34,6 +34,7 @@ extern	void arswitch_writedbg(device_t dev, int phy, uint16_t dbg_addr,
 extern	int arswitch_readreg(device_t dev, int addr);
 extern	int arswitch_writereg(device_t dev, int addr, int value);
 extern	int arswitch_modifyreg(device_t dev, int addr, int mask, int set);
+extern	int arswitch_waitreg(device_t, int, int, int, int);
 
 extern	int arswitch_readreg_lsb(device_t dev, int addr);
 extern	int arswitch_readreg_msb(device_t dev, int addr);
diff --git a/sys/dev/etherswitch/arswitch/arswitch_vlans.c b/sys/dev/etherswitch/arswitch/arswitch_vlans.c
new file mode 100644
index 0000000..2307f710
--- /dev/null
+++ b/sys/dev/etherswitch/arswitch/arswitch_vlans.c
@@ -0,0 +1,375 @@
+/*-
+ * Copyright (c) 2013 Luiz Otavio O Souza.
+ * 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/systm.h>
+#include <sys/socket.h>
+
+#include <net/if.h>
+
+#include <dev/mii/mii.h>
+
+#include <dev/etherswitch/etherswitch.h>
+#include <dev/etherswitch/arswitch/arswitchreg.h>
+#include <dev/etherswitch/arswitch/arswitchvar.h>
+#include <dev/etherswitch/arswitch/arswitch_reg.h>
+#include <dev/etherswitch/arswitch/arswitch_vlans.h>
+
+#include "mdio_if.h"
+#include "miibus_if.h"
+#include "etherswitch_if.h"
+
+static int
+arswitch_vlan_op(struct arswitch_softc *sc, uint32_t op, uint32_t vid,
+	uint32_t data)
+{
+	int err;
+
+	if (arswitch_waitreg(sc->sc_dev, AR8X16_REG_VLAN_CTRL,
+	    AR8X16_VLAN_ACTIVE, 0, 5))
+		return (EBUSY);
+
+	/* Load the vlan data if needed. */
+	if (op == AR8X16_VLAN_OP_LOAD) {
+		err = arswitch_writereg(sc->sc_dev, AR8X16_REG_VLAN_DATA,
+		    (data & AR8X16_VLAN_MEMBER) | AR8X16_VLAN_VALID);
+		if (err)
+			return (err);
+	}
+
+	if (vid != 0)
+		op |= ((vid & ETHERSWITCH_VID_MASK) << AR8X16_VLAN_VID_SHIFT);
+	op |= AR8X16_VLAN_ACTIVE;
+	arswitch_writereg(sc->sc_dev, AR8X16_REG_VLAN_CTRL, op);
+
+	/* Wait for command processing. */
+	if (arswitch_waitreg(sc->sc_dev, AR8X16_REG_VLAN_CTRL,
+	    AR8X16_VLAN_ACTIVE, 0, 5))
+		return (EBUSY);
+
+	return (0);
+}
+
+static int
+arswitch_flush_dot1q_vlan(struct arswitch_softc *sc)
+{
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	return (arswitch_vlan_op(sc, AR8X16_VLAN_OP_FLUSH, 0, 0));
+}
+
+static int
+arswitch_purge_dot1q_vlan(struct arswitch_softc *sc, int vid)
+{
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	return (arswitch_vlan_op(sc, AR8X16_VLAN_OP_PURGE, vid, 0));
+}
+
+static int
+arswitch_get_dot1q_vlan(struct arswitch_softc *sc, uint32_t *ports, int vid)
+{
+	uint32_t reg;
+	int err;
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	err = arswitch_vlan_op(sc, AR8X16_VLAN_OP_GET, vid, 0);
+	if (err)
+		return (err);
+
+	reg = arswitch_readreg(sc->sc_dev, AR8X16_REG_VLAN_DATA);
+	if ((reg & AR8X16_VLAN_VALID) == 0) {
+		*ports = 0;
+		return (EINVAL);
+	}
+	reg &= ((1 << (sc->numphys + 1)) - 1);
+	*ports = reg;
+	return (0);
+}
+
+static int
+arswitch_set_dot1q_vlan(struct arswitch_softc *sc, uint32_t ports, int vid)
+{
+	int err;
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	err = arswitch_vlan_op(sc, AR8X16_VLAN_OP_LOAD, vid, ports);
+	if (err)
+		return (err);
+	return (0);
+}
+
+static int
+arswitch_get_port_vlan(struct arswitch_softc *sc, uint32_t *ports, int vid)
+{
+	int port;
+	uint32_t reg;
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	/* For port based vlans the vlanid is the same as the port index. */
+	port = vid & ETHERSWITCH_VID_MASK;
+	reg = arswitch_readreg(sc->sc_dev, AR8X16_REG_PORT_VLAN(port));
+	*ports = (reg >> AR8X16_PORT_VLAN_DEST_PORTS_SHIFT);
+	*ports &= AR8X16_VLAN_MEMBER;
+	return (0);
+}
+
+static int
+arswitch_set_port_vlan(struct arswitch_softc *sc, uint32_t ports, int vid)
+{
+	int err, port;
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	/* For port based vlans the vlanid is the same as the port index. */
+	port = vid & ETHERSWITCH_VID_MASK;
+	err = arswitch_modifyreg(sc->sc_dev, AR8X16_REG_PORT_VLAN(port),
+	    AR8X16_VLAN_MEMBER << AR8X16_PORT_VLAN_DEST_PORTS_SHIFT,
+	    (ports & AR8X16_VLAN_MEMBER) << AR8X16_PORT_VLAN_DEST_PORTS_SHIFT);
+	if (err)
+		return (err);
+	return (0);
+}
+
+/*
+ * Reset vlans to default state.
+ */
+void
+arswitch_reset_vlans(struct arswitch_softc *sc)
+{
+	uint32_t ports;
+	int i, j;
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+
+	ARSWITCH_LOCK(sc);
+
+	/* Reset all vlan data. */
+	memset(sc->vid, 0, sizeof(sc->vid));
+
+	/* Disable the QinQ and egress filters for all ports. */
+	for (i = 0; i <= sc->numphys; i++) {
+		if (arswitch_modifyreg(sc->sc_dev, AR8X16_REG_PORT_CTRL(i),
+		    0x3 << AR8X16_PORT_CTRL_EGRESS_VLAN_MODE_SHIFT |
+		    AR8X16_PORT_CTRL_DOUBLE_TAG, 0)) {
+			ARSWITCH_UNLOCK(sc);
+			return;
+		}
+	}
+
+	if (arswitch_flush_dot1q_vlan(sc)) {
+		ARSWITCH_UNLOCK(sc);
+		return;
+	}
+
+	if (sc->vlan_mode == ETHERSWITCH_VLAN_DOT1Q) {
+		/*
+		 * Reset the port based vlan settings and turn on the
+		 * ingress filter for all ports.
+		 */
+		ports = 0;
+		for (i = 0; i <= sc->numphys; i++)
+			arswitch_modifyreg(sc->sc_dev,
+			    AR8X16_REG_PORT_VLAN(i),
+			    AR8X16_PORT_VLAN_MODE_MASK |
+			    AR8X16_VLAN_MEMBER <<
+			    AR8X16_PORT_VLAN_DEST_PORTS_SHIFT,
+			    AR8X16_PORT_VLAN_MODE_SECURE <<
+			    AR8X16_PORT_VLAN_MODE_SHIFT);
+
+		/*
+		 * Setup vlan 1 as PVID for all switch ports.  Add all ports
+		 * as members of vlan 1.
+		 */
+		sc->vid[0] = 1;
+		/* Set PVID for everyone. */
+		for (i = 0; i <= sc->numphys; i++)
+			arswitch_set_pvid(sc, i, sc->vid[0]);
+		ports = 0;
+		for (i = 0; i <= sc->numphys; i++)
+			ports |= (1 << i);
+		arswitch_set_dot1q_vlan(sc, ports, sc->vid[0]);
+		sc->vid[0] |= ETHERSWITCH_VID_VALID;
+	} else if (sc->vlan_mode == ETHERSWITCH_VLAN_PORT) {
+		/* Initialize the port based vlans. */
+		for (i = 0; i <= sc->numphys; i++) {
+			sc->vid[i] = i | ETHERSWITCH_VID_VALID;
+			ports = 0;
+			for (j = 0; j <= sc->numphys; j++)
+				ports |= (1 << j);
+			arswitch_modifyreg(sc->sc_dev,
+			    AR8X16_REG_PORT_VLAN(i),
+			    AR8X16_PORT_VLAN_MODE_MASK |
+			    AR8X16_VLAN_MEMBER <<
+			    AR8X16_PORT_VLAN_DEST_PORTS_SHIFT,
+			    ports << AR8X16_PORT_VLAN_DEST_PORTS_SHIFT |
+			    AR8X16_PORT_VLAN_MODE_SECURE <<
+			    AR8X16_PORT_VLAN_MODE_PORT_ONLY);
+		}
+	} else {
+		/* Disable the ingress filter and get everyone on all vlans. */
+		for (i = 0; i <= sc->numphys; i++)
+			arswitch_modifyreg(sc->sc_dev,
+			    AR8X16_REG_PORT_VLAN(i),
+			    AR8X16_PORT_VLAN_MODE_MASK |
+			    AR8X16_VLAN_MEMBER <<
+			    AR8X16_PORT_VLAN_DEST_PORTS_SHIFT,
+			    AR8X16_VLAN_MEMBER <<
+			    AR8X16_PORT_VLAN_DEST_PORTS_SHIFT |
+			    AR8X16_PORT_VLAN_MODE_SECURE <<
+			    AR8X16_PORT_VLAN_MODE_PORT_ONLY);
+	}
+	ARSWITCH_UNLOCK(sc);
+}
+
+int
+arswitch_getvgroup(device_t dev, etherswitch_vlangroup_t *vg)
+{
+	struct arswitch_softc *sc;
+	int err;
+
+	sc = device_get_softc(dev);
+	ARSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+
+	if (vg->es_vlangroup > sc->info.es_nvlangroups)
+		return (EINVAL);
+
+	/* Reset the members ports. */
+	vg->es_untagged_ports = 0;
+	vg->es_member_ports = 0;
+
+	/* Not supported. */
+	vg->es_fid = 0;
+
+	/* Vlan ID. */
+	ARSWITCH_LOCK(sc);
+	vg->es_vid = sc->vid[vg->es_vlangroup];
+	if ((vg->es_vid & ETHERSWITCH_VID_VALID) == 0) {
+		ARSWITCH_UNLOCK(sc);
+		return (0);
+	}
+
+	/* Member Ports. */
+	switch (sc->vlan_mode) {
+	case ETHERSWITCH_VLAN_DOT1Q:
+		err = arswitch_get_dot1q_vlan(sc, &vg->es_member_ports,
+		    vg->es_vid);
+		break;
+	case ETHERSWITCH_VLAN_PORT:
+		err = arswitch_get_port_vlan(sc, &vg->es_member_ports,
+		    vg->es_vid);
+		break;
+	default:
+		vg->es_member_ports = 0;
+		err = -1;
+	}
+	ARSWITCH_UNLOCK(sc);
+	vg->es_untagged_ports = vg->es_member_ports;
+	return (err);
+}
+
+int
+arswitch_setvgroup(device_t dev, etherswitch_vlangroup_t *vg)
+{
+	struct arswitch_softc *sc;
+	int err, vid;
+
+	sc = device_get_softc(dev);
+	ARSWITCH_LOCK_ASSERT(sc, MA_NOTOWNED);
+
+	/* Check VLAN mode. */
+	if (sc->vlan_mode == 0)
+		return (EINVAL);
+
+	/*
+	 * Check if we are changing the vlanid for an already used vtu entry.
+	 * Then purge the entry first.
+	 */
+	ARSWITCH_LOCK(sc);
+	vid = sc->vid[vg->es_vlangroup];
+	if (sc->vlan_mode == ETHERSWITCH_VLAN_DOT1Q &&
+	    (vid & ETHERSWITCH_VID_VALID) != 0 &&
+	    (vid & ETHERSWITCH_VID_MASK) !=
+	    (vg->es_vid & ETHERSWITCH_VID_MASK)) {
+		err = arswitch_purge_dot1q_vlan(sc, vid);
+		if (err) {
+			ARSWITCH_UNLOCK(sc);
+			return (err);
+		}
+	}
+
+	/* Vlan ID. */
+	if (sc->vlan_mode == ETHERSWITCH_VLAN_DOT1Q) {
+		sc->vid[vg->es_vlangroup] = vg->es_vid & ETHERSWITCH_VID_MASK;
+		/* Setting the vlanid to zero disables the vlangroup. */
+		if (sc->vid[vg->es_vlangroup] == 0) {
+			ARSWITCH_UNLOCK(sc);
+			return (0);
+		}
+		sc->vid[vg->es_vlangroup] |= ETHERSWITCH_VID_VALID;
+		vid = sc->vid[vg->es_vlangroup];
+	}
+
+	/* Member Ports. */
+	switch (sc->vlan_mode) {
+	case ETHERSWITCH_VLAN_DOT1Q:
+		err = arswitch_set_dot1q_vlan(sc, vg->es_member_ports, vid);
+		break;
+	case ETHERSWITCH_VLAN_PORT:
+		err = arswitch_set_port_vlan(sc, vg->es_member_ports, vid);
+		break;
+	default:
+		err = -1;
+	}
+	ARSWITCH_UNLOCK(sc);
+	return (err);
+}
+
+int
+arswitch_get_pvid(struct arswitch_softc *sc, int port, int *pvid)
+{
+	uint32_t reg;
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	reg = arswitch_readreg(sc->sc_dev, AR8X16_REG_PORT_VLAN(port));
+	*pvid = reg & 0xfff;
+	return (0);
+}
+
+int
+arswitch_set_pvid(struct arswitch_softc *sc, int port, int pvid)
+{
+
+	ARSWITCH_LOCK_ASSERT(sc, MA_OWNED);
+	return (arswitch_modifyreg(sc->sc_dev,
+	    AR8X16_REG_PORT_VLAN(port), 0xfff, pvid));
+}
diff --git a/sys/dev/etherswitch/arswitch/arswitch_vlans.h b/sys/dev/etherswitch/arswitch/arswitch_vlans.h
new file mode 100644
index 0000000..e671e22
--- /dev/null
+++ b/sys/dev/etherswitch/arswitch/arswitch_vlans.h
@@ -0,0 +1,38 @@
+/*-
+ * Copyright (c) 2013 Luiz Otavio O Souza.
+ * Copyright (c) 2011-2012 Stefan Bethke.
+ * 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	__ARSWITCH_VLANS_H__
+#define	__ARSWITCH_VLANS_H__
+
+void arswitch_reset_vlans(struct arswitch_softc *);
+int arswitch_getvgroup(device_t, etherswitch_vlangroup_t *);
+int arswitch_setvgroup(device_t, etherswitch_vlangroup_t *);
+int arswitch_get_pvid(struct arswitch_softc *, int, int *);
+int arswitch_set_pvid(struct arswitch_softc *, int, int);
+
+#endif	/* __ARSWITCH_VLANS_H__ */
diff --git a/sys/dev/etherswitch/arswitch/arswitchreg.h b/sys/dev/etherswitch/arswitch/arswitchreg.h
index 499d94c..38be689 100644
--- a/sys/dev/etherswitch/arswitch/arswitchreg.h
+++ b/sys/dev/etherswitch/arswitch/arswitchreg.h
@@ -114,7 +114,7 @@
 #define		AR8X16_VLAN_PRIO_EN		(1 << 31)
 
 #define	AR8X16_REG_VLAN_DATA		0x0044
-#define		AR8X16_VLAN_MEMBER		0x000003ff
+#define		AR8X16_VLAN_MEMBER		0x0000003f
 #define		AR8X16_VLAN_VALID		(1 << 11)
 
 #define	AR8X16_REG_ARL_CTRL0		0x0050
diff --git a/sys/dev/etherswitch/arswitch/arswitchvar.h b/sys/dev/etherswitch/arswitch/arswitchvar.h
index caf1c1f..2db13cc 100644
--- a/sys/dev/etherswitch/arswitch/arswitchvar.h
+++ b/sys/dev/etherswitch/arswitch/arswitchvar.h
@@ -57,6 +57,10 @@ struct arswitch_softc {
 	struct callout	callout_tick;
 	etherswitch_info_t info;
 
+	/* VLANs support */
+	int		vid[AR8X16_MAX_VLANS];
+	uint32_t	vlan_mode;
+
 	struct {
 		int (* arswitch_hw_setup) (struct arswitch_softc *);
 		int (* arswitch_hw_global_setup) (struct arswitch_softc *);