Merge ^/head r306303 through 306411.

59 files changed, 3409 insertions, 927 deletions

diff --git a/sys/amd64/amd64/efirt.c b/sys/amd64/amd64/efirt.c
index 626cfb0232b0..8db768b9d619 100644
--- a/sys/amd64/amd64/efirt.c
+++ b/sys/amd64/amd64/efirt.c
@@ -53,6 +53,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/vmparam.h>
 #include <vm/vm.h>
 #include <vm/pmap.h>
+#include <vm/vm_map.h>
 #include <vm/vm_object.h>
 #include <vm/vm_page.h>
 #include <vm/vm_pager.h>
@@ -301,6 +302,17 @@ efi_enter(void)
 		PMAP_UNLOCK(curpmap);
 		return (error);
 	}
+
+	/*
+	 * IPI TLB shootdown handler invltlb_pcid_handler() reloads
+	 * %cr3 from the curpmap->pm_cr3, which would disable runtime
+	 * segments mappings.  Block the handler's action by setting
+	 * curpmap to impossible value.  See also comment in
+	 * pmap.c:pmap_activate_sw().
+	 */
+	if (pmap_pcid_enabled && !invpcid_works)
+		PCPU_SET(curpmap, NULL);
+
 	load_cr3(VM_PAGE_TO_PHYS(efi_pml4_page) | (pmap_pcid_enabled ?
 	    curpmap->pm_pcids[PCPU_GET(cpuid)].pm_pcid : 0));
 	/*
@@ -317,7 +329,9 @@ efi_leave(void)
 {
 	pmap_t curpmap;
 
-	curpmap = PCPU_GET(curpmap);
+	curpmap = &curproc->p_vmspace->vm_pmap;
+	if (pmap_pcid_enabled && !invpcid_works)
+		PCPU_SET(curpmap, curpmap);
 	load_cr3(curpmap->pm_cr3 | (pmap_pcid_enabled ?
 	    curpmap->pm_pcids[PCPU_GET(cpuid)].pm_pcid : 0));
 	if (!pmap_pcid_enabled)
diff --git a/sys/amd64/amd64/machdep.c b/sys/amd64/amd64/machdep.c
index c814f6c796b6..b2283339405f 100644
--- a/sys/amd64/amd64/machdep.c
+++ b/sys/amd64/amd64/machdep.c
@@ -1506,6 +1506,16 @@ native_parse_preload_data(u_int64_t modulep)
 	return (kmdp);
 }
 
+static void
+amd64_kdb_init(void)
+{
+	kdb_init();
+#ifdef KDB
+	if (boothowto & RB_KDB)
+		kdb_enter(KDB_WHY_BOOTFLAGS, "Boot flags requested debugger");
+#endif
+}
+
 u_int64_t
 hammer_time(u_int64_t modulep, u_int64_t physfree)
 {
@@ -1517,6 +1527,7 @@ hammer_time(u_int64_t modulep, u_int64_t physfree)
 	u_int64_t msr;
 	char *env;
 	size_t kstack0_sz;
+	int late_console;
 
 	/*
  	 * This may be done better later if it gets more high level
@@ -1561,6 +1572,7 @@ hammer_time(u_int64_t modulep, u_int64_t physfree)
 	physfree += DPCPU_SIZE;
 	PCPU_SET(prvspace, pc);
 	PCPU_SET(curthread, &thread0);
+	/* Non-late cninit() and printf() can be moved up to here. */
 	PCPU_SET(tssp, &common_tss[0]);
 	PCPU_SET(commontssp, &common_tss[0]);
 	PCPU_SET(tss, (struct system_segment_descriptor *)&gdt[GPROC0_SEL]);
@@ -1660,12 +1672,26 @@ hammer_time(u_int64_t modulep, u_int64_t physfree)
 	wrmsr(MSR_STAR, msr);
 	wrmsr(MSR_SF_MASK, PSL_NT|PSL_T|PSL_I|PSL_C|PSL_D);
 
+	/*
+	 * The console and kdb should be initialized even earlier than here,
+	 * but some console drivers don't work until after getmemsize().
+	 * Default to late console initialization to support these drivers.
+	 * This loses mainly printf()s in getmemsize() and early debugging.
+	 */
+	late_console = 1;
+	TUNABLE_INT_FETCH("debug.late_console", &late_console);
+	if (!late_console) {
+		cninit();
+		amd64_kdb_init();
+	}
+
 	getmemsize(kmdp, physfree);
 	init_param2(physmem);
 
 	/* now running on new page tables, configured,and u/iom is accessible */
 
-	cninit();
+	if (late_console)
+		cninit();
 
 #ifdef DEV_ISA
 #ifdef DEV_ATPIC
@@ -1686,13 +1712,8 @@ hammer_time(u_int64_t modulep, u_int64_t physfree)
 #error "have you forgotten the isa device?";
 #endif
 
-	kdb_init();
-
-#ifdef KDB
-	if (boothowto & RB_KDB)
-		kdb_enter(KDB_WHY_BOOTFLAGS,
-		    "Boot flags requested debugger");
-#endif
+	if (late_console)
+		amd64_kdb_init();
 
 	msgbufinit(msgbufp, msgbufsize);
 	fpuinit();
diff --git a/sys/amd64/amd64/pmap.c b/sys/amd64/amd64/pmap.c
index 63042e41dd14..59e1b676d00b 100644
--- a/sys/amd64/amd64/pmap.c
+++ b/sys/amd64/amd64/pmap.c
@@ -6842,6 +6842,7 @@ pmap_activate_sw(struct thread *td)
 {
 	pmap_t oldpmap, pmap;
 	uint64_t cached, cr3;
+	register_t rflags;
 	u_int cpuid;
 
 	oldpmap = PCPU_GET(curpmap);
@@ -6865,16 +6866,43 @@ pmap_activate_sw(struct thread *td)
 		    pmap == kernel_pmap,
 		    ("non-kernel pmap thread %p pmap %p cpu %d pcid %#x",
 		    td, pmap, cpuid, pmap->pm_pcids[cpuid].pm_pcid));
+
+		/*
+		 * If the INVPCID instruction is not available,
+		 * invltlb_pcid_handler() is used for handle
+		 * invalidate_all IPI, which checks for curpmap ==
+		 * smp_tlb_pmap.  Below operations sequence has a
+		 * window where %CR3 is loaded with the new pmap's
+		 * PML4 address, but curpmap value is not yet updated.
+		 * This causes invltlb IPI handler, called between the
+		 * updates, to execute as NOP, which leaves stale TLB
+		 * entries.
+		 *
+		 * Note that the most typical use of
+		 * pmap_activate_sw(), from the context switch, is
+		 * immune to this race, because interrupts are
+		 * disabled (while the thread lock is owned), and IPI
+		 * happends after curpmap is updated.  Protect other
+		 * callers in a similar way, by disabling interrupts
+		 * around the %cr3 register reload and curpmap
+		 * assignment.
+		 */
+		if (!invpcid_works)
+			rflags = intr_disable();
+
 		if (!cached || (cr3 & ~CR3_PCID_MASK) != pmap->pm_cr3) {
 			load_cr3(pmap->pm_cr3 | pmap->pm_pcids[cpuid].pm_pcid |
 			    cached);
 			if (cached)
 				PCPU_INC(pm_save_cnt);
 		}
+		PCPU_SET(curpmap, pmap);
+		if (!invpcid_works)
+			intr_restore(rflags);
 	} else if (cr3 != pmap->pm_cr3) {
 		load_cr3(pmap->pm_cr3);
+		PCPU_SET(curpmap, pmap);
 	}
-	PCPU_SET(curpmap, pmap);
 #ifdef SMP
 	CPU_CLR_ATOMIC(cpuid, &oldpmap->pm_active);
 #else
diff --git a/sys/arm/conf/GENERIC b/sys/arm/conf/GENERIC
new file mode 100644
index 000000000000..d30b1d72368c
--- /dev/null
+++ b/sys/arm/conf/GENERIC
@@ -0,0 +1,145 @@
+#
+# GENERICV6 -- Generic(ish) kernel config.
+#
+# For more information on this file, please read the config(5) manual page,
+# and/or the handbook section on Kernel Configuration Files:
+#
+#    http://www.FreeBSD.org/doc/en_US.ISO8859-1/books/handbook/kernelconfig-config.html
+#
+# The handbook is also available locally in /usr/share/doc/handbook
+# if you've installed the doc distribution, otherwise always see the
+# FreeBSD World Wide Web server (http://www.FreeBSD.org/) for the
+# latest information.
+#
+# An exhaustive list of options and more detailed explanations of the
+# device lines is also present in the ../../conf/NOTES and NOTES files.
+# If you are in doubt as to the purpose or necessity of a line, check first
+# in NOTES.
+#
+# $FreeBSD$
+
+ident		GENERIC
+
+cpu		CPU_CORTEXA
+machine 	arm armv6
+makeoptions	CONF_CFLAGS="-march=armv7a"
+
+makeoptions	KERNVIRTADDR=0xc0000000
+options 	KERNVIRTADDR=0xc0000000
+
+include 	"std.armv6"
+files		"../allwinner/files.allwinner"
+files		"../allwinner/a20/files.a20"
+files		"../allwinner/a31/files.a31"
+files		"../allwinner/a83t/files.a83t"
+files		"../allwinner/h3/files.h3"
+files		"../qemu/files.qemu"
+
+options 	SOC_ALLWINNER_A20
+options 	SOC_ALLWINNER_A31
+options 	SOC_ALLWINNER_A31S
+options 	SOC_ALLWINNER_A83T
+options 	SOC_ALLWINNER_H3
+
+options 	SCHED_ULE		# ULE scheduler
+options 	SMP			# Enable multiple cores
+options 	PLATFORM
+options 	PLATFORM_SMP
+options 	MULTIDELAY
+
+# EXT_RESOURCES pseudo devices
+options 	EXT_RESOURCES
+device		clk
+device		phy
+device		hwreset
+device		regulator
+
+# Interrupt controller
+options 	INTRNG
+device		gic
+
+# ARM Generic Timer
+device		generic_timer
+
+# MMC/SD/SDIO Card slot support
+device		mmc			# mmc/sd bus
+device		mmcsd			# mmc/sd flash cards
+
+# ATA controllers
+device		ahci			# AHCI-compatible SATA controllers
+#device		ata			# Legacy ATA/SATA controllers
+
+# VirtIO
+device		virtio
+device		virtio_mmio
+device		virtio_blk
+device		vtnet
+
+# Console and misc
+device		uart
+device		uart_snps
+device		pl011
+device		pty
+device		snp
+device		md
+device		random			# Entropy device
+device		psci
+
+# I2C support
+device		iicbus
+device		iic
+device		twsi
+device		rsb
+device		axp209			# AXP209 Power Management Unit
+device		axp81x			# AXP813/818 Power Management Unit
+
+# GPIO
+device		gpio
+device		gpioled
+
+device		scbus			# SCSI bus (required for ATA/SCSI)
+device		da			# Direct Access (disks)
+device		pass			# Passthrough device (direct ATA/SCSI access)
+
+# USB support
+options 	USB_HOST_ALIGN=64	# Align usb buffers to cache line size.
+device		usb
+#device		uhci
+device		ohci
+device		ehci
+
+device		umass
+
+# Ethernet
+device		loop
+device		ether
+device		mii
+device		bpf
+
+#device		emac			# 10/100 integrated EMAC controller
+device		dwc			# 10/100/1000 integrated GMAC controller
+device		awg			# 10/100/1000 integrated EMAC controller
+
+# USB ethernet support, requires miibus
+device		miibus
+
+# Sound support
+device		sound
+
+# Framebuffer support
+device		vt
+device		kbdmux
+device		ums
+device		ukbd
+device		videomode
+device		hdmi
+
+# Pinmux
+device		fdt_pinctrl
+
+# Extensible Firmware Interface
+options 	EFI
+
+# Flattened Device Tree
+options 	FDT			# Configure using FDT/DTB data
+makeoptions	MODULES_EXTRA=dtb/allwinner
diff --git a/sys/arm/nvidia/tegra_ahci.c b/sys/arm/nvidia/tegra_ahci.c
index fcd6e300f96f..e981e000406a 100644
--- a/sys/arm/nvidia/tegra_ahci.c
+++ b/sys/arm/nvidia/tegra_ahci.c
@@ -255,7 +255,7 @@ get_fdt_resources(struct tegra_ahci_sc *sc, phandle_t node)
 		return (ENXIO);
 	}
 
-	rv = phy_get_by_ofw_name(sc->dev, 0, "sata-phy", &sc->phy);
+	rv = phy_get_by_ofw_name(sc->dev, 0, "sata-0", &sc->phy);
 	if (rv != 0) {
 		device_printf(sc->dev, "Cannot get 'sata' phy\n");
 		return (ENXIO);
diff --git a/sys/arm/nvidia/tegra_pcie.c b/sys/arm/nvidia/tegra_pcie.c
index 47471b82f8bf..154471b3eecc 100644
--- a/sys/arm/nvidia/tegra_pcie.c
+++ b/sys/arm/nvidia/tegra_pcie.c
@@ -266,6 +266,7 @@ struct tegra_pcib_port {
 	int 		port_idx;		/* chip port index */
 	int		num_lanes;		/* number of lanes */
 	bus_size_t	afi_pex_ctrl;		/* offset of afi_pex_ctrl */
+	phy_t		phy;			/* port phy */
 
 	/* Config space properties. */
 	bus_addr_t	rp_base_addr;		/* PA of config window */
@@ -291,7 +292,6 @@ struct tegra_pcib_softc {
 	struct ofw_pci_range	pref_mem_range;
 	struct ofw_pci_range	io_range;
 
-	phy_t			phy;
 	clk_t			clk_pex;
 	clk_t			clk_afi;
 	clk_t			clk_pll_e;
@@ -963,6 +963,15 @@ tegra_pcib_parse_port(struct tegra_pcib_softc *sc, phandle_t node)
 	port->afi_pex_ctrl = tegra_pcib_pex_ctrl(sc, port->port_idx);
 	sc->lanes_cfg |= port->num_lanes << (4 * port->port_idx);
 
+	/* Phy. */
+	rv = phy_get_by_ofw_name(sc->dev, node, "pcie-0", &port->phy);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot get 'pcie-0' phy for port %d\n",
+		    port->port_idx);
+		goto fail;
+	}
+
 	return (port);
 fail:
 	free(port, M_DEVBUF);
@@ -1067,13 +1076,6 @@ tegra_pcib_parse_fdt_resources(struct tegra_pcib_softc *sc, phandle_t node)
 		return (ENXIO);
 	}
 
-	/* Phy. */
-	rv = phy_get_by_ofw_name(sc->dev, 0, "pcie", &sc->phy);
-	if (rv != 0) {
-		device_printf(sc->dev, "Cannot get 'pcie' phy\n");
-		return (ENXIO);
-	}
-
 	/* Ports */
 	sc->num_ports = 0;
 	for (child = OF_child(node); child != 0; child = OF_peer(child)) {
@@ -1306,13 +1308,19 @@ tegra_pcib_enable(struct tegra_pcib_softc *sc)
 	reg &= ~AFI_FUSE_PCIE_T0_GEN2_DIS;
 	AFI_WR4(sc, AFI_FUSE, reg);
 
-	/* Enable PCIe phy. */
-	rv = phy_enable(sc->dev, sc->phy);
-	if (rv != 0) {
-		device_printf(sc->dev, "Cannot enable phy\n");
-		return (rv);
+	for (i = 0; i < TEGRA_PCIB_MAX_PORTS; i++) {
+		if (sc->ports[i] != NULL) {
+			rv = phy_enable(sc->dev, sc->ports[i]->phy);
+			if (rv != 0) {
+				device_printf(sc->dev,
+				    "Cannot enable phy for port %d\n",
+				    sc->ports[i]->port_idx);
+				return (rv);
+			}
+		}
 	}
 
+
 	rv = hwreset_deassert(sc->hwreset_pcie_x);
 	if (rv != 0) {
 		device_printf(sc->dev, "Cannot unreset  'pci_x' reset\n");
diff --git a/sys/arm/ti/cpsw/if_cpsw.c b/sys/arm/ti/cpsw/if_cpsw.c
index 27f187c05d68..3d4bc020cb5b 100644
--- a/sys/arm/ti/cpsw/if_cpsw.c
+++ b/sys/arm/ti/cpsw/if_cpsw.c
@@ -583,6 +583,11 @@ cpsw_init(struct cpsw_softc *sc)
 	struct cpsw_slot *slot;
 	uint32_t reg;
 
+	/* Disable the interrupt pacing. */
+	reg = cpsw_read_4(sc, CPSW_WR_INT_CONTROL);
+	reg &= ~(CPSW_WR_INT_PACE_EN | CPSW_WR_INT_PRESCALE_MASK);
+	cpsw_write_4(sc, CPSW_WR_INT_CONTROL, reg);
+
 	/* Clear ALE */
 	cpsw_write_4(sc, CPSW_ALE_CONTROL, CPSW_ALE_CTL_CLEAR_TBL);
 
@@ -1936,7 +1941,7 @@ cpsw_tx_dequeue(struct cpsw_softc *sc)
 
 		/* TearDown complete is only marked on the SOP for the packet. */
 		if ((flags & (CPDMA_BD_SOP | CPDMA_BD_TDOWNCMPLT)) ==
-		    (CPDMA_BD_EOP | CPDMA_BD_TDOWNCMPLT)) {
+		    (CPDMA_BD_SOP | CPDMA_BD_TDOWNCMPLT)) {
 			CPSW_DEBUGF(sc, ("TX teardown in progress"));
 			cpsw_write_cp(sc, &sc->tx, 0xfffffffc);
 			// TODO: Increment a count of dropped TX packets
@@ -2492,6 +2497,51 @@ cpsw_stat_attached(SYSCTL_HANDLER_ARGS)
 }
 
 static int
+cpsw_intr_coalesce(SYSCTL_HANDLER_ARGS)
+{
+	int error;
+	struct cpsw_softc *sc;
+	uint32_t ctrl, intr_per_ms;
+
+	sc = (struct cpsw_softc *)arg1;
+	error = sysctl_handle_int(oidp, &sc->coal_us, 0, req);
+	if (error != 0 || req->newptr == NULL)
+		return (error);
+
+	ctrl = cpsw_read_4(sc, CPSW_WR_INT_CONTROL);
+	ctrl &= ~(CPSW_WR_INT_PACE_EN | CPSW_WR_INT_PRESCALE_MASK);
+	if (sc->coal_us == 0) {
+		/* Disable the interrupt pace hardware. */
+		cpsw_write_4(sc, CPSW_WR_INT_CONTROL, ctrl);
+		cpsw_write_4(sc, CPSW_WR_C_RX_IMAX(0), 0);
+		cpsw_write_4(sc, CPSW_WR_C_TX_IMAX(0), 0);
+		return (0);
+	}
+
+	if (sc->coal_us > CPSW_WR_C_IMAX_US_MAX)
+		sc->coal_us = CPSW_WR_C_IMAX_US_MAX;
+	if (sc->coal_us < CPSW_WR_C_IMAX_US_MIN)
+		sc->coal_us = CPSW_WR_C_IMAX_US_MIN;
+	intr_per_ms = 1000 / sc->coal_us;
+	/* Just to make sure... */
+	if (intr_per_ms > CPSW_WR_C_IMAX_MAX)
+		intr_per_ms = CPSW_WR_C_IMAX_MAX;
+	if (intr_per_ms < CPSW_WR_C_IMAX_MIN)
+		intr_per_ms = CPSW_WR_C_IMAX_MIN;
+
+	/* Set the prescale to produce 4us pulses from the 125 Mhz clock. */
+	ctrl |= (125 * 4) & CPSW_WR_INT_PRESCALE_MASK;
+
+	/* Enable the interrupt pace hardware. */
+	cpsw_write_4(sc, CPSW_WR_C_RX_IMAX(0), intr_per_ms);
+	cpsw_write_4(sc, CPSW_WR_C_TX_IMAX(0), intr_per_ms);
+	ctrl |= CPSW_WR_INT_C0_RX_PULSE | CPSW_WR_INT_C0_TX_PULSE;
+	cpsw_write_4(sc, CPSW_WR_INT_CONTROL, ctrl);
+
+	return (0);
+}
+
+static int
 cpsw_stat_uptime(SYSCTL_HANDLER_ARGS)
 {
 	struct cpsw_softc *swsc;
@@ -2576,6 +2626,10 @@ cpsw_add_sysctls(struct cpsw_softc *sc)
 	    CTLTYPE_UINT | CTLFLAG_RD, sc, 0, cpsw_stat_attached, "IU",
 	    "Time since driver attach");
 
+	SYSCTL_ADD_PROC(ctx, parent, OID_AUTO, "intr_coalesce_us",
+	    CTLTYPE_UINT | CTLFLAG_RW, sc, 0, cpsw_intr_coalesce, "IU",
+	    "minimum time between interrupts");
+
 	node = SYSCTL_ADD_NODE(ctx, parent, OID_AUTO, "ports",
 	    CTLFLAG_RD, NULL, "CPSW Ports Statistics");
 	ports_parent = SYSCTL_CHILDREN(node);
diff --git a/sys/arm/ti/cpsw/if_cpswreg.h b/sys/arm/ti/cpsw/if_cpswreg.h
index bea6f1904e3c..9652e03b3614 100644
--- a/sys/arm/ti/cpsw/if_cpswreg.h
+++ b/sys/arm/ti/cpsw/if_cpswreg.h
@@ -138,6 +138,17 @@
 #define	CPSW_WR_SOFT_RESET		(CPSW_WR_OFFSET + 0x04)
 #define	CPSW_WR_CONTROL			(CPSW_WR_OFFSET + 0x08)
 #define	CPSW_WR_INT_CONTROL		(CPSW_WR_OFFSET + 0x0c)
+#define	 CPSW_WR_INT_C0_RX_PULSE	(1 << 16)
+#define	 CPSW_WR_INT_C0_TX_PULSE	(1 << 17)
+#define	 CPSW_WR_INT_C1_RX_PULSE	(1 << 18)
+#define	 CPSW_WR_INT_C1_TX_PULSE	(1 << 19)
+#define	 CPSW_WR_INT_C2_RX_PULSE	(1 << 20)
+#define	 CPSW_WR_INT_C2_TX_PULSE	(1 << 21)
+#define	 CPSW_WR_INT_PACE_EN						\
+	(CPSW_WR_INT_C0_RX_PULSE | CPSW_WR_INT_C0_TX_PULSE |		\
+	 CPSW_WR_INT_C1_RX_PULSE | CPSW_WR_INT_C1_TX_PULSE |		\
+	 CPSW_WR_INT_C2_RX_PULSE | CPSW_WR_INT_C2_TX_PULSE)
+#define	 CPSW_WR_INT_PRESCALE_MASK	0xfff
 #define	CPSW_WR_C_RX_THRESH_EN(p)	(CPSW_WR_OFFSET + (0x10 * (p)) + 0x10)
 #define	CPSW_WR_C_RX_EN(p)		(CPSW_WR_OFFSET + (0x10 * (p)) + 0x14)
 #define	CPSW_WR_C_TX_EN(p)		(CPSW_WR_OFFSET + (0x10 * (p)) + 0x18)
@@ -151,6 +162,13 @@
 #define	 CPSW_WR_C_MISC_HOST_PEND	(1 << 2)
 #define	 CPSW_WR_C_MISC_MDIOLINK	(1 << 1)
 #define	 CPSW_WR_C_MISC_MDIOUSER	(1 << 0)
+#define	CPSW_WR_C_RX_IMAX(p)		(CPSW_WR_OFFSET + (0x08 * (p)) + 0x70)
+#define	CPSW_WR_C_TX_IMAX(p)		(CPSW_WR_OFFSET + (0x08 * (p)) + 0x74)
+#define	 CPSW_WR_C_IMAX_MASK		0x3f
+#define	 CPSW_WR_C_IMAX_MAX		63
+#define	 CPSW_WR_C_IMAX_MIN		2
+#define	 CPSW_WR_C_IMAX_US_MAX		500
+#define	 CPSW_WR_C_IMAX_US_MIN		16
 
 #define	CPSW_CPPI_RAM_OFFSET		0x2000
 #define	CPSW_CPPI_RAM_SIZE		0x2000
diff --git a/sys/arm/ti/cpsw/if_cpswvar.h b/sys/arm/ti/cpsw/if_cpswvar.h
index 953766be84f1..a750744a3e44 100644
--- a/sys/arm/ti/cpsw/if_cpswvar.h
+++ b/sys/arm/ti/cpsw/if_cpswvar.h
@@ -80,6 +80,7 @@ struct cpsw_softc {
 	phandle_t	node;
 	struct bintime	attach_uptime; /* system uptime when attach happened. */
 	struct cpsw_port port[2];
+	unsigned	coal_us;
 
 	/* RX and TX buffer tracking */
 	struct cpsw_queue rx, tx;
diff --git a/sys/boot/common/interp_forth.c b/sys/boot/common/interp_forth.c
index aedac6af09a2..bd59e0dda096 100644
--- a/sys/boot/common/interp_forth.c
+++ b/sys/boot/common/interp_forth.c
@@ -325,13 +325,15 @@ bf_run(char *line)
 	printf("Parse error!\n");
 	break;
     default:
-        /* Hopefully, all other codes filled this buffer */
-	printf("%s\n", command_errmsg);
+	if (command_errmsg != NULL) {
+	    printf("%s\n", command_errmsg);
+	    command_errmsg = NULL;
+	}
     }
     
     if (result == VM_USEREXIT)
 	panic("interpreter exit");
     setenv("interpret", bf_vm->state ? "" : "OK", 1);
 
-    return result;
+    return (result);
 }
diff --git a/sys/cam/ata/ata_xpt.c b/sys/cam/ata/ata_xpt.c
index d2c97601a8d0..71445e65331b 100644
--- a/sys/cam/ata/ata_xpt.c
+++ b/sys/cam/ata/ata_xpt.c
@@ -40,6 +40,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/interrupt.h>
 #include <sys/sbuf.h>
 
+#include <sys/eventhandler.h>
 #include <sys/lock.h>
 #include <sys/mutex.h>
 #include <sys/sysctl.h>
@@ -872,12 +873,24 @@ noerror:
 	{
 		struct ccb_pathinq cpi;
 		int16_t *ptr;
+		int veto = 0;
 
 		ident_buf = &softc->ident_data;
 		for (ptr = (int16_t *)ident_buf;
 		     ptr < (int16_t *)ident_buf + sizeof(struct ata_params)/2; ptr++) {
 			*ptr = le16toh(*ptr);
 		}
+
+		/*
+		 * Allow others to veto this ATA disk attachment.  This
+		 * is mainly used by VMs, whose disk controllers may
+		 * share the disks with the simulated ATA controllers.
+		 */
+		EVENTHANDLER_INVOKE(ada_probe_veto, path, ident_buf, &veto);
+		if (veto) {
+			goto device_fail;
+		}
+
 		if (strncmp(ident_buf->model, "FX", 2) &&
 		    strncmp(ident_buf->model, "NEC", 3) &&
 		    strncmp(ident_buf->model, "Pioneer", 7) &&
diff --git a/sys/cddl/compat/opensolaris/kern/opensolaris_proc.c b/sys/cddl/compat/opensolaris/kern/opensolaris_proc.c
new file mode 100644
index 000000000000..652fa8928589
--- /dev/null
+++ b/sys/cddl/compat/opensolaris/kern/opensolaris_proc.c
@@ -0,0 +1,57 @@
+/*-
+ * Copyright 2016 Mark Johnston <markj@FreeBSD.org>
+ *
+ * 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 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 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.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+#include <sys/proc.h>
+#include <sys/ptrace.h>
+
+int
+uread(proc_t *p, void *kaddr, size_t len, uintptr_t uaddr)
+{
+	ssize_t n;
+
+	PHOLD(p);
+	n = proc_readmem(curthread, p, uaddr, kaddr, len);
+	PRELE(p);
+	if (n != len)
+		return (ENOMEM);
+	return (0);
+}
+
+int
+uwrite(proc_t *p, void *kaddr, size_t len, uintptr_t uaddr)
+{
+	ssize_t n;
+
+	PHOLD(p);
+	n = proc_writemem(curthread, p, uaddr, kaddr, len);
+	PRELE(p);
+	if (n != len)
+		return (ENOMEM);
+	return (0);
+}
diff --git a/sys/cddl/compat/opensolaris/sys/proc.h b/sys/cddl/compat/opensolaris/sys/proc.h
index 5c9e8dec78c4..ce503cf724ac 100644
--- a/sys/cddl/compat/opensolaris/sys/proc.h
+++ b/sys/cddl/compat/opensolaris/sys/proc.h
@@ -92,6 +92,9 @@ do_thread_create(caddr_t stk, size_t stksize, void (*proc)(void *), void *arg,
 	do_thread_create(stk, stksize, proc, arg, len, pp, state, pri)
 #define	thread_exit()	kthread_exit()
 
+int	uread(proc_t *, void *, size_t, uintptr_t);
+int	uwrite(proc_t *, void *, size_t, uintptr_t);
+
 #endif	/* _KERNEL */
 
 #endif	/* _OPENSOLARIS_SYS_PROC_H_ */
diff --git a/sys/cddl/contrib/opensolaris/uts/intel/dtrace/fasttrap_isa.c b/sys/cddl/contrib/opensolaris/uts/intel/dtrace/fasttrap_isa.c
index 8555e2ff3335..e1f95de8a8d8 100644
--- a/sys/cddl/contrib/opensolaris/uts/intel/dtrace/fasttrap_isa.c
+++ b/sys/cddl/contrib/opensolaris/uts/intel/dtrace/fasttrap_isa.c
@@ -59,34 +59,8 @@
 #include <sys/archsystm.h>
 #else
 #include <sys/ptrace.h>
-
-static int
-uread(proc_t *p, void *kaddr, size_t len, uintptr_t uaddr)
-{
-	ssize_t n;
-
-	PHOLD(p);
-	n = proc_readmem(curthread, p, uaddr, kaddr, len);
-	PRELE(p);
-	if (n != len)
-		return (ENOMEM);
-	return (0);
-}
-
-static int
-uwrite(proc_t *p, void *kaddr, size_t len, uintptr_t uaddr)
-{
-	ssize_t n;
-
-	PHOLD(p);
-	n = proc_writemem(curthread, p, uaddr, kaddr, len);
-	PRELE(p);
-	if (n != len)
-		return (ENOMEM);
-	return (0);
-}
-
 #endif /* illumos */
+
 #ifdef __i386__
 #define	r_rax	r_eax
 #define	r_rbx	r_ebx
diff --git a/sys/cddl/contrib/opensolaris/uts/powerpc/dtrace/fasttrap_isa.c b/sys/cddl/contrib/opensolaris/uts/powerpc/dtrace/fasttrap_isa.c
index e8dd684a4530..b0fc5816c5e3 100644
--- a/sys/cddl/contrib/opensolaris/uts/powerpc/dtrace/fasttrap_isa.c
+++ b/sys/cddl/contrib/opensolaris/uts/powerpc/dtrace/fasttrap_isa.c
@@ -44,32 +44,6 @@
 #define OP_RA(x) (((x) & 0x001F0000) >> 16)
 #define OP_RB(x) (((x) & 0x0000F100) >> 11)
 
-static int
-uread(proc_t *p, void *kaddr, size_t len, uintptr_t uaddr)
-{
-	ssize_t n;
-
-	PHOLD(p);
-	n = proc_readmem(curthread, p, uaddr, kaddr, len);
-	PRELE(p);
-	if (n <= 0 || n < len)
-		return (ENOMEM);
-	return (0);
-}
-
-static int
-uwrite(proc_t *p, void *kaddr, size_t len, uintptr_t uaddr)
-{
-	ssize_t n;
-
-	PHOLD(p);
-	n = proc_writemem(curthread, p, uaddr, kaddr, len);
-	PRELE(p);
-	if (n <= 0 || n < len)
-		return (ENOMEM);
-	return (0);
-}
-
 int
 fasttrap_tracepoint_install(proc_t *p, fasttrap_tracepoint_t *tp)
 {
diff --git a/sys/conf/files b/sys/conf/files
index 37683bd47eed..296549639dfb 100644
--- a/sys/conf/files
+++ b/sys/conf/files
@@ -127,6 +127,7 @@ cddl/compat/opensolaris/kern/opensolaris.c		optional zfs | dtrace compile-with "
 cddl/compat/opensolaris/kern/opensolaris_cmn_err.c	optional zfs | dtrace compile-with "${CDDL_C}"
 cddl/compat/opensolaris/kern/opensolaris_kmem.c		optional zfs | dtrace compile-with "${CDDL_C}"
 cddl/compat/opensolaris/kern/opensolaris_misc.c		optional zfs | dtrace compile-with "${CDDL_C}"
+cddl/compat/opensolaris/kern/opensolaris_proc.c		optional zfs | dtrace compile-with "${CDDL_C}"
 cddl/compat/opensolaris/kern/opensolaris_sunddi.c	optional zfs | dtrace compile-with "${CDDL_C}"
 cddl/compat/opensolaris/kern/opensolaris_taskq.c	optional zfs | dtrace compile-with "${CDDL_C}"
 # zfs specific
diff --git a/sys/conf/files.powerpc b/sys/conf/files.powerpc
index 0a8a05db85a5..aee0a1022b9c 100644
--- a/sys/conf/files.powerpc
+++ b/sys/conf/files.powerpc
@@ -135,6 +135,7 @@ powerpc/mikrotik/platform_rb.c	optional	mikrotik
 powerpc/mpc85xx/atpic.c		optional	mpc85xx isa
 powerpc/mpc85xx/ds1553_bus_fdt.c	optional	ds1553 fdt
 powerpc/mpc85xx/ds1553_core.c	optional	ds1553
+powerpc/mpc85xx/fsl_diu.c	optional	mpc85xx diu
 powerpc/mpc85xx/fsl_sdhc.c	optional	mpc85xx sdhc
 powerpc/mpc85xx/i2c.c		optional	iicbus fdt
 powerpc/mpc85xx/isa.c		optional	mpc85xx isa
diff --git a/sys/ddb/db_examine.c b/sys/ddb/db_examine.c
index de2bbe4e7735..a1e5a2858ab7 100644
--- a/sys/ddb/db_examine.c
+++ b/sys/ddb/db_examine.c
@@ -241,7 +241,7 @@ db_print_loc_and_inst(db_addr_t loc)
 	db_printsym(loc, DB_STGY_PROC);
 	if (db_search_symbol(loc, DB_STGY_PROC, &off) != C_DB_SYM_NULL) {
 		db_printf(":\t");
-		(void)db_disasm(loc, true);
+		(void)db_disasm(loc, false);
 	}
 }
 
diff --git a/sys/dev/ath/ath_hal/ah.h b/sys/dev/ath/ath_hal/ah.h
index 7dd8b9b19857..57a7abd4b773 100644
--- a/sys/dev/ath/ath_hal/ah.h
+++ b/sys/dev/ath/ath_hal/ah.h
@@ -635,7 +635,8 @@ typedef enum {
 	REG_EXT_JAPAN_MIDBAND		= 1,
 	REG_EXT_FCC_DFS_HT40		= 2,
 	REG_EXT_JAPAN_NONDFS_HT40	= 3,
-	REG_EXT_JAPAN_DFS_HT40		= 4
+	REG_EXT_JAPAN_DFS_HT40		= 4,
+	REG_EXT_FCC_CH_144		= 5,
 } REG_EXT_BITMAP;
 
 enum {
diff --git a/sys/dev/ath/ath_hal/ah_internal.h b/sys/dev/ath/ath_hal/ah_internal.h
index 6583c73dd11a..0d650ded7587 100644
--- a/sys/dev/ath/ath_hal/ah_internal.h
+++ b/sys/dev/ath/ath_hal/ah_internal.h
@@ -134,7 +134,7 @@ struct ath_hal_rf *ath_hal_rfprobe(struct ath_hal *ah, HAL_STATUS *ecode);
  * right now.
  */
 #ifndef AH_MAXCHAN
-#define	AH_MAXCHAN	96
+#define	AH_MAXCHAN	128
 #endif
 
 #define	HAL_NF_CAL_HIST_LEN_FULL	5
diff --git a/sys/dev/ath/ath_hal/ah_regdomain.c b/sys/dev/ath/ath_hal/ah_regdomain.c
index d388e12c75ef..d3770356d7ea 100644
--- a/sys/dev/ath/ath_hal/ah_regdomain.c
+++ b/sys/dev/ath/ath_hal/ah_regdomain.c
@@ -548,6 +548,14 @@ add_chanlist_mode(struct ath_hal *ah, struct ieee80211_channel chans[],
 			continue;
 		}
 #endif
+
+		/*
+		 * XXX TODO: handle REG_EXT_FCC_CH_144.
+		 *
+		 * Figure out which instances/uses cause us to not
+		 * be allowed to use channel 144 (pri or sec overlap.)
+		 */
+
 		bfreq_lo = MAX(fband->lowChannel + low_adj, freq_lo);
 		bfreq_hi = MIN(fband->highChannel + hi_adj, freq_hi);
 		if (fband->channelSep >= channelSep)
diff --git a/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_domains.h b/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_domains.h
index 04a6ccdebd84..0ebb6d174472 100644
--- a/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_domains.h
+++ b/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_domains.h
@@ -242,6 +242,17 @@ static REG_DOMAIN regDomains[] = {
 	 .chan11a_quarter	= BM3(F8_5180_5240, F8_5260_5320,F10_5745_5825),
 	},
 
+	{.regDmnEnum		= FCC6,
+	 .conformanceTestLimit	= FCC,
+	 .chan11a		= BM5(F8_5180_5240, F5_5260_5320, F1_5500_5580, F2_5660_5720, F6_5745_5825),
+	 .chan11a_turbo		= BM3(T7_5210_5210, T3_5250_5290, T2_5760_5800),
+	 .chan11a_dyn_turbo	= BM4(T7_5200_5200, T1_5240_5240, T2_5280_5280, T1_5765_5805),
+#if 0
+	 .chan11a_half		= BM3(F7_5180_5240, F7_5260_5320, F9_5745_5825),
+	 .chan11a_quarter	= BM3(F8_5180_5240, F8_5260_5320,F10_5745_5825),
+#endif
+	},
+
 	{.regDmnEnum		= MKK1,
 	 .conformanceTestLimit	= MKK,
 	 .pscan			= PSCAN_MKK1,
diff --git a/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_freqbands.h b/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_freqbands.h
index 79d418f3c728..84e7af7f5afd 100644
--- a/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_freqbands.h
+++ b/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_freqbands.h
@@ -125,8 +125,11 @@ static REG_DMN_FREQ_BAND regDmn5GhzFreq[] = {
 	{ 5280, 5320, 17, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC },
 #define	F1_5280_5320	AFTER(F3_5260_5700)
 
+	{ 5500, 5580, 23, 6, 20, 20, DFS_FCC3, PSCAN_FCC },
+#define	F1_5500_5580	AFTER(F1_5280_5320)
+
 	{ 5500, 5620, 30, 6, 20, 20, DFS_ETSI, PSCAN_ETSI },
-#define	F1_5500_5620	AFTER(F1_5280_5320)
+#define	F1_5500_5620	AFTER(F1_5500_5580)
 
 	{ 5500, 5700, 20, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC },
 #define	F1_5500_5700	AFTER(F1_5500_5620)
@@ -136,9 +139,11 @@ static REG_DMN_FREQ_BAND regDmn5GhzFreq[] = {
 #define	F3_5500_5700	AFTER(F2_5500_5700)
 	{ 5500, 5700, 23, 0, 20, 20, DFS_FCC3 | DFS_ETSI | DFS_MKK4, PSCAN_MKK3 | PSCAN_FCC },
 #define	F4_5500_5700	AFTER(F3_5500_5700)
+	{ 5660, 5720, 23, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI },
+#define	F2_5660_5720	AFTER(F4_5500_5700)
 
 	{ 5745, 5805, 23, 0, 20, 20, NO_DFS, NO_PSCAN },
-#define	F1_5745_5805	AFTER(F4_5500_5700)
+#define	F1_5745_5805	AFTER(F2_5660_5720)
 	{ 5745, 5805, 30, 6, 20, 20, NO_DFS, NO_PSCAN },
 #define	F2_5745_5805	AFTER(F1_5745_5805)
 	{ 5745, 5805, 30, 6, 20, 20, DFS_ETSI, PSCAN_ETSI },
@@ -191,6 +196,7 @@ static REG_DMN_FREQ_BAND regDmn5GhzFreq[] = {
 #define	W2_5825_5825	AFTER(W2_5180_5240)
 };
 
+
 /*
  * 5GHz Turbo (dynamic & static) tags
  */
@@ -203,13 +209,14 @@ static REG_DMN_FREQ_BAND regDmn5GhzTurboFreq[] = {
 #define	T1_5370_5490	AFTER(T1_5250_5330)
 	{ 5530, 5650, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN },
 #define	T1_5530_5650	AFTER(T1_5370_5490)
-
+	{ 5200, 5200, 23, 6, 40, 40, NO_DFS, NO_PSCAN },
+#define	T7_5200_5200	AFTER(T1_5530_5650)
 	{ 5150, 5190, 5,  6, 40, 40, NO_DFS, NO_PSCAN },
-#define	T1_5150_5190	AFTER(T1_5530_5650)
-	{ 5230, 5310, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN },
-#define	T1_5230_5310	AFTER(T1_5150_5190)
+#define	T1_5230_5310	AFTER(T7_5200_5200)
 	{ 5350, 5470, 5,  6, 40, 40, NO_DFS, NO_PSCAN },
-#define	T1_5350_5470	AFTER(T1_5230_5310)
+#define	T1_5150_5190	AFTER(T1_5230_5310)
+	{ 5230, 5310, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN },
+#define	T1_5350_5470	AFTER(T1_5150_5190)
 	{ 5510, 5670, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN },
 #define	T1_5510_5670	AFTER(T1_5350_5470)
 
@@ -221,9 +228,13 @@ static REG_DMN_FREQ_BAND regDmn5GhzTurboFreq[] = {
 #define	T1_5210_5210	AFTER(T2_5200_5240)
 	{ 5210, 5210, 23, 0, 40, 40, NO_DFS, NO_PSCAN },
 #define	T2_5210_5210	AFTER(T1_5210_5210)
+	{ 5210, 5210, 23, 6, 40, 40, NO_DFS, NO_PSCAN },
+#define	T7_5210_5210	AFTER(T2_5210_5210)
 
+	{ 5240, 5240, 23, 6, 40, 40, NO_DFS, PSCAN_FCC_T },
+#define	T1_5240_5240	AFTER(T7_5210_5210)
 	{ 5280, 5280, 23, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T },
-#define	T1_5280_5280	AFTER(T2_5210_5210)
+#define	T1_5280_5280	AFTER(T1_5240_5240)
 	{ 5280, 5280, 20, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T },
 #define	T2_5280_5280	AFTER(T1_5280_5280)
 	{ 5250, 5250, 17, 0, 40, 40, DFS_FCC3, PSCAN_FCC_T },
@@ -234,9 +245,11 @@ static REG_DMN_FREQ_BAND regDmn5GhzTurboFreq[] = {
 #define	T1_5250_5290	AFTER(T1_5290_5290)
 	{ 5250, 5290, 23, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T },
 #define	T2_5250_5290	AFTER(T1_5250_5290)
+	{ 5250, 5290, 23, 6, 40, 40, NO_DFS, PSCAN_FCC_T },
+#define	T3_5250_5290	AFTER(T2_5250_5290)
 
 	{ 5540, 5660, 20, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T },
-#define	T1_5540_5660	AFTER(T2_5250_5290)
+#define	T1_5540_5660	AFTER(T3_5250_5290)
 	{ 5760, 5800, 20, 0, 40, 40, NO_DFS, NO_PSCAN },
 #define	T1_5760_5800	AFTER(T1_5540_5660)
 	{ 5760, 5800, 30, 6, 40, 40, NO_DFS, NO_PSCAN },
diff --git a/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_regmap.h b/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_regmap.h
index f77be2268ba7..33a6a7c531f6 100644
--- a/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_regmap.h
+++ b/sys/dev/ath/ath_hal/ah_regdomain/ah_rd_regmap.h
@@ -41,6 +41,7 @@ static REG_DMN_PAIR_MAPPING regDomainPairs[] = {
 	{FCC3_WORLD,	FCC3,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, CTRY_DEFAULT },
 	{FCC4_FCCA,	FCC4,		FCCA,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, CTRY_DEFAULT },
 	{FCC5_FCCB,	FCC5,		FCCB,		NO_REQ, NO_REQ, PSCAN_DEFER, CTRY_DEFAULT },
+	{FCC6_FCCA,	FCC6,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, CTRY_DEFAULT },
 
 	{ETSI1_WORLD,	ETSI1,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, CTRY_DEFAULT },
 	{ETSI2_WORLD,	ETSI2,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, CTRY_DEFAULT },
diff --git a/sys/dev/atkbdc/psm.c b/sys/dev/atkbdc/psm.c
index 56a5c62bae6d..d1c525430184 100644
--- a/sys/dev/atkbdc/psm.c
+++ b/sys/dev/atkbdc/psm.c
@@ -81,6 +81,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/sysctl.h>
 #include <sys/time.h>
 #include <sys/uio.h>
+#include <sys/libkern.h>
 
 #include <sys/limits.h>
 #include <sys/mouse.h>
@@ -161,40 +162,6 @@ typedef struct packetbuf {
 #define	PSM_PACKETQUEUE	128
 #endif
 
-enum {
-	SYNAPTICS_SYSCTL_MIN_PRESSURE,
-	SYNAPTICS_SYSCTL_MAX_PRESSURE,
-	SYNAPTICS_SYSCTL_MAX_WIDTH,
-	SYNAPTICS_SYSCTL_MARGIN_TOP,
-	SYNAPTICS_SYSCTL_MARGIN_RIGHT,
-	SYNAPTICS_SYSCTL_MARGIN_BOTTOM,
-	SYNAPTICS_SYSCTL_MARGIN_LEFT,
-	SYNAPTICS_SYSCTL_NA_TOP,
-	SYNAPTICS_SYSCTL_NA_RIGHT,
-	SYNAPTICS_SYSCTL_NA_BOTTOM,
-	SYNAPTICS_SYSCTL_NA_LEFT,
-	SYNAPTICS_SYSCTL_WINDOW_MIN,
-	SYNAPTICS_SYSCTL_WINDOW_MAX,
-	SYNAPTICS_SYSCTL_MULTIPLICATOR,
-	SYNAPTICS_SYSCTL_WEIGHT_CURRENT,
-	SYNAPTICS_SYSCTL_WEIGHT_PREVIOUS,
-	SYNAPTICS_SYSCTL_WEIGHT_PREVIOUS_NA,
-	SYNAPTICS_SYSCTL_WEIGHT_LEN_SQUARED,
-	SYNAPTICS_SYSCTL_DIV_MIN,
-	SYNAPTICS_SYSCTL_DIV_MAX,
-	SYNAPTICS_SYSCTL_DIV_MAX_NA,
-	SYNAPTICS_SYSCTL_DIV_LEN,
-	SYNAPTICS_SYSCTL_TAP_MAX_DELTA,
-	SYNAPTICS_SYSCTL_TAP_MIN_QUEUE,
-	SYNAPTICS_SYSCTL_TAPHOLD_TIMEOUT,
-	SYNAPTICS_SYSCTL_VSCROLL_HOR_AREA,
-	SYNAPTICS_SYSCTL_VSCROLL_VER_AREA,
-	SYNAPTICS_SYSCTL_VSCROLL_MIN_DELTA,
-	SYNAPTICS_SYSCTL_VSCROLL_DIV_MIN,
-	SYNAPTICS_SYSCTL_VSCROLL_DIV_MAX,
-	SYNAPTICS_SYSCTL_TOUCHPAD_OFF
-};
-
 typedef struct synapticsinfo {
 	struct sysctl_ctx_list	 sysctl_ctx;
 	struct sysctl_oid	*sysctl_tree;
@@ -231,6 +198,11 @@ typedef struct synapticsinfo {
 	int			 vscroll_div_min;
 	int			 vscroll_div_max;
 	int			 touchpad_off;
+	int			 softbuttons_y;
+	int			 softbutton2_x;
+	int			 softbutton3_x;
+	int			 max_x;
+	int			 max_y;
 } synapticsinfo_t;
 
 typedef struct synapticspacket {
@@ -246,22 +218,29 @@ typedef struct synapticspacket {
     ((synhw).infoMajor > (major) ||					\
      ((synhw).infoMajor == (major) && (synhw).infoMinor >= (minor)))
 
-typedef struct synapticsaction {
+typedef struct smoother {
 	synapticspacket_t	queue[SYNAPTICS_PACKETQUEUE];
 	int			queue_len;
 	int			queue_cursor;
-	int			window_min;
 	int			start_x;
 	int			start_y;
 	int			avg_dx;
 	int			avg_dy;
 	int			squelch_x;
 	int			squelch_y;
+	int			is_fuzzy;
+	int			active;
+} smoother_t;
+
+typedef struct gesture {
+	int			window_min;
 	int			fingers_nb;
 	int			tap_button;
 	int			in_taphold;
 	int			in_vscroll;
-} synapticsaction_t;
+	int			zmax;		/* maximum pressure value */
+	struct timeval		taptimeout;	/* tap timeout for touchpads */
+} gesture_t;
 
 enum {
 	TRACKPOINT_SYSCTL_SENSITIVITY,
@@ -295,6 +274,100 @@ typedef struct trackpointinfo {
 	int	skipback;
 } trackpointinfo_t;
 
+typedef struct finger {
+	int			x;
+	int			y;
+	int			p;
+	int			w;
+	int			flags;
+} finger_t;
+#define	PSM_FINGERS		2	/* # of processed fingers */
+#define	PSM_FINGER_IS_PEN	(1<<0)
+#define	PSM_FINGER_FUZZY	(1<<1)
+#define	PSM_FINGER_DEFAULT_P	tap_threshold
+#define	PSM_FINGER_DEFAULT_W	1
+#define	PSM_FINGER_IS_SET(f) ((f).x != -1 && (f).y != -1 && (f).p != 0)
+#define	PSM_FINGER_RESET(f) do { \
+	(f) = (finger_t) { .x = -1, .y = -1, .p = 0, .w = 0, .flags = 0 }; \
+} while (0)
+
+typedef struct elantechhw {
+	int			hwversion;
+	int			fwversion;
+	int			sizex;
+	int			sizey;
+	int			dpmmx;
+	int			dpmmy;
+	int			ntracesx;
+	int			ntracesy;
+	int			issemimt;
+	int			isclickpad;
+	int			hascrc;
+	int			hastrackpoint;
+	int			haspressure;
+} elantechhw_t;
+
+/* minimum versions supported by this driver */
+#define	ELANTECH_HW_IS_V1(fwver) ((fwver) < 0x020030 || (fwver) == 0x020600)
+
+#define	ELANTECH_MAGIC(magic)				\
+	((magic)[0] == 0x3c && (magic)[1] == 0x03 &&	\
+	((magic)[2] == 0xc8 || (magic)[2] == 0x00))
+
+#define	ELANTECH_FW_ID		0x00
+#define	ELANTECH_FW_VERSION	0x01
+#define	ELANTECH_CAPABILITIES	0x02
+#define	ELANTECH_SAMPLE		0x03
+#define	ELANTECH_RESOLUTION	0x04
+#define	ELANTECH_REG_READ	0x10
+#define	ELANTECH_REG_WRITE	0x11
+#define	ELANTECH_REG_RDWR	0x00
+#define	ELANTECH_CUSTOM_CMD	0xf8
+
+#define	ELANTECH_MAX_FINGERS	PSM_FINGERS
+
+#define	ELANTECH_FINGER_SET_XYP(pb) (finger_t) {			\
+    .x = (((pb)->ipacket[1] & 0x0f) << 8) | (pb)->ipacket[2],		\
+    .y = (((pb)->ipacket[4] & 0x0f) << 8) | (pb)->ipacket[5],		\
+    .p = ((pb)->ipacket[1] & 0xf0) | (((pb)->ipacket[4] >> 4) & 0x0f),	\
+    .w = PSM_FINGER_DEFAULT_W,						\
+    .flags = 0								\
+}
+
+enum {
+	ELANTECH_PKT_NOP,
+	ELANTECH_PKT_TRACKPOINT,
+	ELANTECH_PKT_V2_COMMON,
+	ELANTECH_PKT_V2_2FINGER,
+	ELANTECH_PKT_V3,
+	ELANTECH_PKT_V4_STATUS,
+	ELANTECH_PKT_V4_HEAD,
+	ELANTECH_PKT_V4_MOTION
+};
+
+#define	ELANTECH_PKT_IS_TRACKPOINT(pb) (((pb)->ipacket[3] & 0x0f) == 0x06)
+#define	ELANTECH_PKT_IS_DEBOUNCE(pb, hwversion) ((hwversion) == 4 ? 0 :	\
+    (pb)->ipacket[0] == ((hwversion) == 2 ? 0x84 : 0xc4) &&		\
+    (pb)->ipacket[1] == 0xff && (pb)->ipacket[2] == 0xff &&		\
+    (pb)->ipacket[3] == 0x02 && (pb)->ipacket[4] == 0xff &&		\
+    (pb)->ipacket[5] == 0xff)
+#define	ELANTECH_PKT_IS_V2(pb) 						\
+    (((pb)->ipacket[0] & 0x0c) == 0x04 && ((pb)->ipacket[3] & 0x0f) == 0x02)
+#define	ELANTECH_PKT_IS_V3_HEAD(pb, hascrc) ((hascrc) ? 		\
+    ((pb)->ipacket[3] & 0x09) == 0x08 : 				\
+    ((pb)->ipacket[0] & 0x0c) == 0x04 && ((pb)->ipacket[3] & 0xcf) == 0x02)
+#define	ELANTECH_PKT_IS_V3_TAIL(pb, hascrc) ((hascrc) ? 		\
+    ((pb)->ipacket[3] & 0x09) == 0x09 : 				\
+    ((pb)->ipacket[0] & 0x0c) == 0x0c && ((pb)->ipacket[3] & 0xce) == 0x0c)
+#define	ELANTECH_PKT_IS_V4(pb, hascrc) ((hascrc) ? 			\
+    ((pb)->ipacket[3] & 0x08) == 0x00 :					\
+    ((pb)->ipacket[0] & 0x0c) == 0x04 && ((pb)->ipacket[3] & 0x1c) == 0x10)
+
+typedef struct elantechaction {
+	finger_t		fingers[ELANTECH_MAX_FINGERS];
+	int			mask;
+} elantechaction_t;
+
 /* driver control block */
 struct psm_softc {		/* Driver status information */
 	int		unit;
@@ -308,7 +381,10 @@ struct psm_softc {		/* Driver status information */
 	mousehw_t	hw;		/* hardware information */
 	synapticshw_t	synhw;		/* Synaptics hardware information */
 	synapticsinfo_t	syninfo;	/* Synaptics configuration */
-	synapticsaction_t synaction;	/* Synaptics action context */
+	smoother_t	smoother[PSM_FINGERS];	/* Motion smoothing */
+	gesture_t	gesture;	/* Gesture context */
+	elantechhw_t	elanhw;		/* Elantech hardware information */
+	elantechaction_t elanaction;	/* Elantech action context */
 	int		tphw;		/* TrackPoint hardware information */
 	trackpointinfo_t tpinfo;	/* TrackPoint configuration */
 	mousemode_t	mode;		/* operation mode */
@@ -324,13 +400,13 @@ struct psm_softc {		/* Driver status information */
 	int		xaverage;	/* average X position */
 	int		yaverage;	/* average Y position */
 	int		squelch; /* level to filter movement at low speed */
-	int		zmax;	/* maximum pressure value for touchpads */
 	int		syncerrors; /* # of bytes discarded to synchronize */
 	int		pkterrors;  /* # of packets failed during quaranteen. */
 	struct timeval	inputtimeout;
 	struct timeval	lastsoftintr;	/* time of last soft interrupt */
 	struct timeval	lastinputerr;	/* time last sync error happened */
-	struct timeval	taptimeout;	/* tap timeout for touchpads */
+	struct timeval	idletimeout;
+	packetbuf_t	idlepacket;	/* packet to send after idle timeout */
 	int		watchdog;	/* watchdog timer flag */
 	struct callout	callout;	/* watchdog timer call out */
 	struct callout	softcallout; /* buffer timer call out */
@@ -389,6 +465,9 @@ TUNABLE_INT("hw.psm.synaptics_support", &synaptics_support);
 static int trackpoint_support = 0;
 TUNABLE_INT("hw.psm.trackpoint_support", &trackpoint_support);
 
+static int elantech_support = 0;
+TUNABLE_INT("hw.psm.elantech_support", &elantech_support);
+
 static int verbose = PSM_DEBUG;
 TUNABLE_INT("debug.psm.loglevel", &verbose);
 
@@ -411,6 +490,44 @@ typedef struct old_mousemode {
 	int	accelfactor;
 } old_mousemode_t;
 
+#define SYN_OFFSET(field) offsetof(struct psm_softc, syninfo.field)
+enum {
+	SYNAPTICS_SYSCTL_MIN_PRESSURE =		SYN_OFFSET(min_pressure),
+	SYNAPTICS_SYSCTL_MAX_PRESSURE =		SYN_OFFSET(max_pressure),
+	SYNAPTICS_SYSCTL_MAX_WIDTH =		SYN_OFFSET(max_width),
+	SYNAPTICS_SYSCTL_MARGIN_TOP =		SYN_OFFSET(margin_top),
+	SYNAPTICS_SYSCTL_MARGIN_RIGHT =		SYN_OFFSET(margin_right),
+	SYNAPTICS_SYSCTL_MARGIN_BOTTOM =	SYN_OFFSET(margin_bottom),
+	SYNAPTICS_SYSCTL_MARGIN_LEFT =		SYN_OFFSET(margin_left),
+	SYNAPTICS_SYSCTL_NA_TOP =		SYN_OFFSET(na_top),
+	SYNAPTICS_SYSCTL_NA_RIGHT =		SYN_OFFSET(na_right),
+	SYNAPTICS_SYSCTL_NA_BOTTOM =		SYN_OFFSET(na_bottom),
+	SYNAPTICS_SYSCTL_NA_LEFT = 		SYN_OFFSET(na_left),
+	SYNAPTICS_SYSCTL_WINDOW_MIN =		SYN_OFFSET(window_min),
+	SYNAPTICS_SYSCTL_WINDOW_MAX =		SYN_OFFSET(window_max),
+	SYNAPTICS_SYSCTL_MULTIPLICATOR =	SYN_OFFSET(multiplicator),
+	SYNAPTICS_SYSCTL_WEIGHT_CURRENT =	SYN_OFFSET(weight_current),
+	SYNAPTICS_SYSCTL_WEIGHT_PREVIOUS =	SYN_OFFSET(weight_previous),
+	SYNAPTICS_SYSCTL_WEIGHT_PREVIOUS_NA =	SYN_OFFSET(weight_previous_na),
+	SYNAPTICS_SYSCTL_WEIGHT_LEN_SQUARED =	SYN_OFFSET(weight_len_squared),
+	SYNAPTICS_SYSCTL_DIV_MIN =		SYN_OFFSET(div_min),
+	SYNAPTICS_SYSCTL_DIV_MAX =		SYN_OFFSET(div_max),
+	SYNAPTICS_SYSCTL_DIV_MAX_NA =		SYN_OFFSET(div_max_na),
+	SYNAPTICS_SYSCTL_DIV_LEN =		SYN_OFFSET(div_len),
+	SYNAPTICS_SYSCTL_TAP_MAX_DELTA =	SYN_OFFSET(tap_max_delta),
+	SYNAPTICS_SYSCTL_TAP_MIN_QUEUE =	SYN_OFFSET(tap_min_queue),
+	SYNAPTICS_SYSCTL_TAPHOLD_TIMEOUT =	SYN_OFFSET(taphold_timeout),
+	SYNAPTICS_SYSCTL_VSCROLL_HOR_AREA =	SYN_OFFSET(vscroll_hor_area),
+	SYNAPTICS_SYSCTL_VSCROLL_VER_AREA =	SYN_OFFSET(vscroll_ver_area),
+	SYNAPTICS_SYSCTL_VSCROLL_MIN_DELTA =	SYN_OFFSET(vscroll_min_delta),
+	SYNAPTICS_SYSCTL_VSCROLL_DIV_MIN =	SYN_OFFSET(vscroll_div_min),
+	SYNAPTICS_SYSCTL_VSCROLL_DIV_MAX =	SYN_OFFSET(vscroll_div_max),
+	SYNAPTICS_SYSCTL_TOUCHPAD_OFF =		SYN_OFFSET(touchpad_off),
+	SYNAPTICS_SYSCTL_SOFTBUTTONS_Y =	SYN_OFFSET(softbuttons_y),
+	SYNAPTICS_SYSCTL_SOFTBUTTON2_X =	SYN_OFFSET(softbutton2_x),
+	SYNAPTICS_SYSCTL_SOFTBUTTON3_X =	SYN_OFFSET(softbutton3_x),
+};
+
 /* packet formatting function */
 typedef int	packetfunc_t(struct psm_softc *, u_char *, int *, int,
     mousestatus_t *);
@@ -446,6 +563,7 @@ static int	doopen(struct psm_softc *, int);
 static int	reinitialize(struct psm_softc *, int);
 static char	*model_name(int);
 static void	psmsoftintr(void *);
+static void	psmsoftintridle(void *);
 static void	psmintr(void *);
 static void	psmtimeout(void *);
 static int	timeelapsed(const struct timeval *, int, int,
@@ -458,6 +576,13 @@ static int	proc_synaptics(struct psm_softc *, packetbuf_t *,
 		    mousestatus_t *, int *, int *, int *);
 static void	proc_versapad(struct psm_softc *, packetbuf_t *,
 		    mousestatus_t *, int *, int *, int *);
+static int	proc_elantech(struct psm_softc *, packetbuf_t *,
+		    mousestatus_t *, int *, int *, int *);
+static int	psmpalmdetect(struct psm_softc *, finger_t *, int);
+static void	psmgestures(struct psm_softc *, finger_t *, int,
+		    mousestatus_t *);
+static void	psmsmoother(struct psm_softc *, finger_t *, int,
+		    mousestatus_t *, int *, int *);
 static int	tame_mouse(struct psm_softc *, packetbuf_t *, mousestatus_t *,
 		    u_char *);
 
@@ -480,6 +605,7 @@ static probefunc_t	enable_mmanplus;
 static probefunc_t	enable_synaptics;
 static probefunc_t	enable_trackpoint;
 static probefunc_t	enable_versapad;
+static probefunc_t	enable_elantech;
 
 static void set_trackpoint_parameters(struct psm_softc *sc);
 static void synaptics_passthrough_on(struct psm_softc *sc);
@@ -511,6 +637,8 @@ static struct {
 	  0xc8, MOUSE_4DPLUS_PACKETSIZE, enable_4dplus },
 	{ MOUSE_MODEL_SYNAPTICS,	/* Synaptics Touchpad */
 	  0xc0, MOUSE_SYNAPTICS_PACKETSIZE, enable_synaptics },
+	{ MOUSE_MODEL_ELANTECH,		/* Elantech Touchpad */
+	  0x04, MOUSE_ELANTECH_PACKETSIZE, enable_elantech },
 	{ MOUSE_MODEL_INTELLI,		/* Microsoft IntelliMouse */
 	  0x08, MOUSE_PS2INTELLI_PACKETSIZE, enable_msintelli },
 	{ MOUSE_MODEL_GLIDEPOINT,	/* ALPS GlidePoint */
@@ -762,6 +890,7 @@ model_name(int model)
 		{ MOUSE_MODEL_4DPLUS,		"4D+ Mouse" },
 		{ MOUSE_MODEL_SYNAPTICS,	"Synaptics Touchpad" },
 		{ MOUSE_MODEL_TRACKPOINT,	"IBM/Lenovo TrackPoint" },
+		{ MOUSE_MODEL_ELANTECH,		"Elantech Touchpad" },
 		{ MOUSE_MODEL_GENERIC,		"Generic PS/2 mouse" },
 		{ MOUSE_MODEL_UNKNOWN,		"Unknown" },
 	};
@@ -1504,6 +1633,7 @@ psmattach(device_t dev)
 	case MOUSE_MODEL_SYNAPTICS:
 	case MOUSE_MODEL_GLIDEPOINT:
 	case MOUSE_MODEL_VERSAPAD:
+	case MOUSE_MODEL_ELANTECH:
 		sc->config |= PSM_CONFIG_INITAFTERSUSPEND;
 		break;
 	default:
@@ -1512,6 +1642,11 @@ psmattach(device_t dev)
 		break;
 	}
 
+	/* Elantech trackpad`s sync bit differs from touchpad`s one */
+	if (sc->hw.model == MOUSE_MODEL_ELANTECH &&
+	    (sc->elanhw.hascrc || sc->elanhw.hastrackpoint))
+		sc->config |= PSM_CONFIG_NOCHECKSYNC;
+
 	if (!verbose)
 		printf("psm%d: model %s, device ID %d\n",
 		    unit, model_name(sc->hw.model), sc->hw.hwid & 0x00ff);
@@ -2165,20 +2300,6 @@ psmioctl(struct cdev *dev, u_long cmd, caddr_t addr, int flag,
 		    (*(int *)addr > PSM_LEVEL_MAX))
 			return (EINVAL);
 		sc->mode.level = *(int *)addr;
-
-		if (sc->hw.model == MOUSE_MODEL_SYNAPTICS) {
-			/*
-			 * If we are entering PSM_LEVEL_NATIVE, we want to
-			 * enable sending of "extended W mode" packets to
-			 * userland. Reset the mode of the touchpad so that the
-			 * change in the level is picked up.
-			 */
-			error = block_mouse_data(sc, &command_byte);
-			if (error)
-				return (error);
-			synaptics_set_mode(sc, synaptics_preferred_mode(sc));
-			unblock_mouse_data(sc, command_byte);
-		}
 		break;
 
 	case MOUSE_GETSTATUS:
@@ -2627,7 +2748,8 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 {
 	static int touchpad_buttons;
 	static int guest_buttons;
-	int w, x0, y0;
+	static finger_t f[PSM_FINGERS];
+	int w, id, nfingers, ewcode;
 
 	/* TouchPad PS/2 absolute mode message format with capFourButtons:
 	 *
@@ -2683,6 +2805,7 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 		return (-1);
 
 	*x = *y = 0;
+	ms->button = ms->obutton;
 
 	/*
 	 * Pressure value.
@@ -2718,35 +2841,80 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 		w = 4;
 	}
 
-	/*
-	 * Handle packets from the guest device. See:
-	 * Synaptics PS/2 TouchPad Interfacing Guide, Section 5.1
-	 */
-	if (w == 3 && sc->synhw.capPassthrough) {
-		*x = ((pb->ipacket[1] & 0x10) ?
-		    pb->ipacket[4] - 256 : pb->ipacket[4]);
-		*y = ((pb->ipacket[1] & 0x20) ?
-		    pb->ipacket[5] - 256 : pb->ipacket[5]);
-		*z = 0;
+	switch (w) {
+	case 3:
+		/*
+		 * Handle packets from the guest device. See:
+		 * Synaptics PS/2 TouchPad Interfacing Guide, Section 5.1
+		 */
+		if (sc->synhw.capPassthrough) {
+			*x = ((pb->ipacket[1] & 0x10) ?
+			    pb->ipacket[4] - 256 : pb->ipacket[4]);
+			*y = ((pb->ipacket[1] & 0x20) ?
+			    pb->ipacket[5] - 256 : pb->ipacket[5]);
+			*z = 0;
 
-		guest_buttons = 0;
-		if (pb->ipacket[1] & 0x01)
-			guest_buttons |= MOUSE_BUTTON1DOWN;
-		if (pb->ipacket[1] & 0x04)
-			guest_buttons |= MOUSE_BUTTON2DOWN;
-		if (pb->ipacket[1] & 0x02)
-			guest_buttons |= MOUSE_BUTTON3DOWN;
+			guest_buttons = 0;
+			if (pb->ipacket[1] & 0x01)
+				guest_buttons |= MOUSE_BUTTON1DOWN;
+			if (pb->ipacket[1] & 0x04)
+				guest_buttons |= MOUSE_BUTTON2DOWN;
+				if (pb->ipacket[1] & 0x02)
+				guest_buttons |= MOUSE_BUTTON3DOWN;
 
-		ms->button = touchpad_buttons | guest_buttons;
+			ms->button = touchpad_buttons | guest_buttons;
+		}
 		goto SYNAPTICS_END;
-	}
 
-	if (sc->syninfo.touchpad_off) {
-		*x = *y = *z = 0;
-		ms->button = ms->obutton;
+	case 2:
+		/* Handle Extended W mode packets */
+		ewcode = (pb->ipacket[5] & 0xf0) >> 4;
+#if PSM_FINGERS > 1
+		switch (ewcode) {
+		case 1:
+			/* Secondary finger */
+			if (sc->synhw.capAdvancedGestures)
+				f[1] = (finger_t) {
+					.x = (((pb->ipacket[4] & 0x0f) << 8) |
+					    pb->ipacket[1]) << 1,
+					.y = (((pb->ipacket[4] & 0xf0) << 4) |
+					    pb->ipacket[2]) << 1,
+					.p = ((pb->ipacket[3] & 0x30) |
+					    (pb->ipacket[5] & 0x0f)) << 1,
+					.w = PSM_FINGER_DEFAULT_W,
+					.flags = PSM_FINGER_FUZZY,
+				};
+			else if (sc->synhw.capReportsV)
+				f[1] = (finger_t) {
+					.x = (((pb->ipacket[4] & 0x0f) << 8) |
+					    (pb->ipacket[1] & 0xfe)) << 1,
+					.y = (((pb->ipacket[4] & 0xf0) << 4) |
+					    (pb->ipacket[2] & 0xfe)) << 1,
+					.p = ((pb->ipacket[3] & 0x30) |
+					    (pb->ipacket[5] & 0x0e)) << 1,
+					.w = (((pb->ipacket[5] & 0x01) << 2) |
+					    ((pb->ipacket[2] & 0x01) << 1) |
+					    (pb->ipacket[1] & 0x01)) + 8,
+					.flags = PSM_FINGER_FUZZY,
+				};
+		default:
+			break;
+		}
+#endif
 		goto SYNAPTICS_END;
+
+	case 1:
+	case 0:
+		nfingers = w + 2;
+		break;
+
+	default:
+		nfingers = 1;
 	}
 
+	if (sc->syninfo.touchpad_off)
+		goto SYNAPTICS_END;
+
 	/* Button presses */
 	touchpad_buttons = 0;
 	if (pb->ipacket[0] & 0x01)
@@ -2764,6 +2932,10 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 		/* Middle Button */
 		if ((pb->ipacket[0] ^ pb->ipacket[3]) & 0x01)
 			touchpad_buttons |= MOUSE_BUTTON2DOWN;
+	} else if (sc->synhw.capExtended && sc->synhw.capClickPad) {
+		/* ClickPad Button */
+		if ((pb->ipacket[0] ^ pb->ipacket[3]) & 0x01)
+			touchpad_buttons = MOUSE_BUTTON1DOWN;
 	} else if (sc->synhw.capExtended && (sc->synhw.nExtendedButtons > 0)) {
 		/* Extended Buttons */
 		if ((pb->ipacket[0] ^ pb->ipacket[3]) & 0x02) {
@@ -2806,7 +2978,7 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 			pb->ipacket[4] &= ~(mask);
 			pb->ipacket[5] &= ~(mask);
 		} else	if (!sc->syninfo.directional_scrolls &&
-		    !sc->synaction.in_vscroll) {
+		    !sc->gesture.in_vscroll) {
 			/*
 			 * Keep reporting MOUSE DOWN until we get a new packet
 			 * indicating otherwise.
@@ -2814,93 +2986,179 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 			touchpad_buttons |= sc->extended_buttons;
 		}
 	}
-	/* Handle ClickPad. */
-	if (sc->synhw.capClickPad &&
-	    ((pb->ipacket[0] ^ pb->ipacket[3]) & 0x01))
-		touchpad_buttons |= MOUSE_BUTTON1DOWN;
+
+	if (sc->synhw.capReportsV && nfingers > 1)
+		f[0] = (finger_t) {
+			.x = ((pb->ipacket[3] & 0x10) << 8) |
+			    ((pb->ipacket[1] & 0x0f) << 8) |
+			    (pb->ipacket[4] & 0xfd),
+			.y = ((pb->ipacket[3] & 0x20) << 7) |
+			    ((pb->ipacket[1] & 0xf0) << 4) |
+			    (pb->ipacket[5] & 0xfd),
+			.p = *z & 0xfe,
+			.w = (((pb->ipacket[2] & 0x01) << 2) |
+			    (pb->ipacket[5] & 0x02) |
+			    ((pb->ipacket[4] & 0x02) >> 1)) + 8,
+			.flags = PSM_FINGER_FUZZY,
+		};
+	else
+		f[0] = (finger_t) {
+			.x = ((pb->ipacket[3] & 0x10) << 8) |
+			    ((pb->ipacket[1] & 0x0f) << 8) |
+			    pb->ipacket[4],
+			.y = ((pb->ipacket[3] & 0x20) << 7) |
+			    ((pb->ipacket[1] & 0xf0) << 4) |
+			    pb->ipacket[5],
+			.p = *z,
+			.w = w,
+			.flags = nfingers > 1 ? PSM_FINGER_FUZZY : 0,
+		};
+
+	/* Ignore hovering and unmeasurable touches */
+	if (f[0].p < sc->syninfo.min_pressure || f[0].x < 2)
+		nfingers = 0;
+
+	for (id = 0; id < PSM_FINGERS; id++)
+		if (id >= nfingers)
+			PSM_FINGER_RESET(f[id]);
 
 	ms->button = touchpad_buttons | guest_buttons;
 
+	/* Palm detection doesn't terminate the current action. */
+	if (!psmpalmdetect(sc, &f[0], nfingers)) {
+		psmgestures(sc, &f[0], nfingers, ms);
+		for (id = 0; id < PSM_FINGERS; id++)
+			psmsmoother(sc, &f[id], id, ms, x, y);
+	} else {
+		VLOG(2, (LOG_DEBUG, "synaptics: palm detected! (%d)\n", f[0].w));
+	}
+
+SYNAPTICS_END:
+	/*
+	 * Use the extra buttons as a scrollwheel
+	 *
+	 * XXX X.Org uses the Z axis for vertical wheel only,
+	 * whereas moused(8) understands special values to differ
+	 * vertical and horizontal wheels.
+	 *
+	 * xf86-input-mouse needs therefore a small patch to
+	 * understand these special values. Without it, the
+	 * horizontal wheel acts as a vertical wheel in X.Org.
+	 *
+	 * That's why the horizontal wheel is disabled by
+	 * default for now.
+	 */
+	if (ms->button & MOUSE_BUTTON4DOWN)
+		*z = -1;
+	else if (ms->button & MOUSE_BUTTON5DOWN)
+		*z = 1;
+	else if (ms->button & MOUSE_BUTTON6DOWN)
+		*z = -2;
+	else if (ms->button & MOUSE_BUTTON7DOWN)
+		*z = 2;
+	else
+		*z = 0;
+	ms->button &= ~(MOUSE_BUTTON4DOWN | MOUSE_BUTTON5DOWN |
+	    MOUSE_BUTTON6DOWN | MOUSE_BUTTON7DOWN);
+
+	return (0);
+}
+
+static int
+psmpalmdetect(struct psm_softc *sc, finger_t *f, int nfingers)
+{
+	if (!(
+	    ((sc->synhw.capMultiFinger ||
+	      sc->synhw.capAdvancedGestures) && nfingers > 1) ||
+	    (sc->synhw.capPalmDetect && f->w <= sc->syninfo.max_width) ||
+	    (!sc->synhw.capPalmDetect && f->p <= sc->syninfo.max_pressure) ||
+	    (sc->synhw.capPen && f->flags & PSM_FINGER_IS_PEN))) {
+		/*
+		 * We consider the packet irrelevant for the current
+		 * action when:
+		 *  - the width isn't comprised in:
+		 *    [1; max_width]
+		 *  - the pressure isn't comprised in:
+		 *    [min_pressure; max_pressure]
+		 *  - pen aren't supported but PSM_FINGER_IS_PEN is set
+		 */
+		return (1);
+	}
+	return (0);
+}
+
+static void
+psmgestures(struct psm_softc *sc, finger_t *fingers, int nfingers,
+    mousestatus_t *ms)
+{
+	smoother_t *smoother;
+	gesture_t *gest;
+	finger_t *f;
+	int y_ok, center_button, center_x, right_button, right_x, i;
+
+	f = &fingers[0];
+	smoother = &sc->smoother[0];
+	gest = &sc->gesture;
+
+	/* Find first active finger. */
+	if (nfingers > 0) {
+		for (i = 0; i < PSM_FINGERS; i++) {
+			if (PSM_FINGER_IS_SET(fingers[i])) {
+				f = &fingers[i];
+				smoother = &sc->smoother[i];
+				break;
+			}
+		}
+	}
+
 	/*
 	 * Check pressure to detect a real wanted action on the
 	 * touchpad.
 	 */
-	if (*z >= sc->syninfo.min_pressure) {
-		synapticsaction_t *synaction;
-		int cursor, peer, window;
-		int dx, dy, dxp, dyp;
-		int max_width, max_pressure;
+	if (f->p >= sc->syninfo.min_pressure) {
+		int x0, y0;
+		int dxp, dyp;
+		int start_x, start_y;
+		int queue_len;
 		int margin_top, margin_right, margin_bottom, margin_left;
-		int na_top, na_right, na_bottom, na_left;
 		int window_min, window_max;
-		int multiplicator;
-		int weight_current, weight_previous, weight_len_squared;
-		int div_min, div_max, div_len;
 		int vscroll_hor_area, vscroll_ver_area;
 		int two_finger_scroll;
-		int len, weight_prev_x, weight_prev_y;
-		int div_max_x, div_max_y, div_x, div_y;
-		int exiting_scroll;
+		int max_x, max_y;
 
 		/* Read sysctl. */
 		/* XXX Verify values? */
-		max_width = sc->syninfo.max_width;
-		max_pressure = sc->syninfo.max_pressure;
 		margin_top = sc->syninfo.margin_top;
 		margin_right = sc->syninfo.margin_right;
 		margin_bottom = sc->syninfo.margin_bottom;
 		margin_left = sc->syninfo.margin_left;
-		na_top = sc->syninfo.na_top;
-		na_right = sc->syninfo.na_right;
-		na_bottom = sc->syninfo.na_bottom;
-		na_left = sc->syninfo.na_left;
 		window_min = sc->syninfo.window_min;
 		window_max = sc->syninfo.window_max;
-		multiplicator = sc->syninfo.multiplicator;
-		weight_current = sc->syninfo.weight_current;
-		weight_previous = sc->syninfo.weight_previous;
-		weight_len_squared = sc->syninfo.weight_len_squared;
-		div_min = sc->syninfo.div_min;
-		div_max = sc->syninfo.div_max;
-		div_len = sc->syninfo.div_len;
 		vscroll_hor_area = sc->syninfo.vscroll_hor_area;
 		vscroll_ver_area = sc->syninfo.vscroll_ver_area;
 		two_finger_scroll = sc->syninfo.two_finger_scroll;
-
-		exiting_scroll = 0;
-
-		/* Palm detection. */
-		if (!(
-		    ((sc->synhw.capMultiFinger ||
-		      sc->synhw.capAdvancedGestures) && (w == 0 || w == 1)) ||
-		    (sc->synhw.capPalmDetect && w >= 4 && w <= max_width) ||
-		    (!sc->synhw.capPalmDetect && *z <= max_pressure) ||
-		    (sc->synhw.capPen && w == 2))) {
-			/*
-			 * We consider the packet irrelevant for the current
-			 * action when:
-			 *  - the width isn't comprised in:
-			 *    [4; max_width]
-			 *  - the pressure isn't comprised in:
-			 *    [min_pressure; max_pressure]
-			 *  - pen aren't supported but w is 2
-			 *
-			 *  Note that this doesn't terminate the current action.
-			 */
-			VLOG(2, (LOG_DEBUG,
-			    "synaptics: palm detected! (%d)\n", w));
-			goto SYNAPTICS_END;
-		}
+		max_x = sc->syninfo.max_x;
+		max_y = sc->syninfo.max_y;
 
 		/* Read current absolute position. */
-		x0 = ((pb->ipacket[3] & 0x10) << 8) |
-		    ((pb->ipacket[1] & 0x0f) << 8) |
-		    pb->ipacket[4];
-		y0 = ((pb->ipacket[3] & 0x20) << 7) |
-		    ((pb->ipacket[1] & 0xf0) << 4) |
-		    pb->ipacket[5];
+		x0 = f->x;
+		y0 = f->y;
+
+		/*
+		 * Limit the coordinates to the specified margins because
+		 * this area isn't very reliable.
+		 */
+		if (x0 <= margin_left)
+			x0 = margin_left;
+		else if (x0 >= max_x - margin_right)
+			x0 = max_x - margin_right;
+		if (y0 <= margin_bottom)
+			y0 = margin_bottom;
+		else if (y0 >= max_y - margin_top)
+			y0 = max_y - margin_top;
 
-		synaction = &(sc->synaction);
+		VLOG(3, (LOG_DEBUG, "synaptics: ipacket: [%d, %d], %d, %d\n",
+		    x0, y0, f->p, f->w));
 
 		/*
 		 * If the action is just beginning, init the structure and
@@ -2909,107 +3167,82 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 		if (!(sc->flags & PSM_FLAGS_FINGERDOWN)) {
 			VLOG(3, (LOG_DEBUG, "synaptics: ----\n"));
 
-			/* Store the first point of this action. */
-			synaction->start_x = x0;
-			synaction->start_y = y0;
-			dx = dy = 0;
-
 			/* Initialize queue. */
-			synaction->queue_cursor = SYNAPTICS_PACKETQUEUE;
-			synaction->queue_len = 0;
-			synaction->window_min = window_min;
-
-			/* Reset average. */
-			synaction->avg_dx = 0;
-			synaction->avg_dy = 0;
-
-			/* Reset squelch. */
-			synaction->squelch_x = 0;
-			synaction->squelch_y = 0;
+			gest->window_min = window_min;
 
 			/* Reset pressure peak. */
-			sc->zmax = 0;
+			gest->zmax = 0;
 
 			/* Reset fingers count. */
-			synaction->fingers_nb = 0;
+			gest->fingers_nb = 0;
 
 			/* Reset virtual scrolling state. */
-			synaction->in_vscroll = 0;
+			gest->in_vscroll = 0;
 
 			/* Compute tap timeout. */
-			sc->taptimeout.tv_sec  = tap_timeout / 1000000;
-			sc->taptimeout.tv_usec = tap_timeout % 1000000;
-			timevaladd(&sc->taptimeout, &sc->lastsoftintr);
+			gest->taptimeout.tv_sec  = tap_timeout / 1000000;
+			gest->taptimeout.tv_usec = tap_timeout % 1000000;
+			timevaladd(&gest->taptimeout, &sc->lastsoftintr);
 
 			sc->flags |= PSM_FLAGS_FINGERDOWN;
+
+			/* Smoother has not been reset yet */
+			queue_len = 1;
+			start_x = x0;
+			start_y = y0;
 		} else {
-			/* Calculate the current delta. */
-			cursor = synaction->queue_cursor;
-			dx = x0 - synaction->queue[cursor].x;
-			dy = y0 - synaction->queue[cursor].y;
+			queue_len = smoother->queue_len + 1;
+			start_x = smoother->start_x;
+			start_y = smoother->start_y;
 		}
 
-		/* If in tap-hold, add the recorded button. */
-		if (synaction->in_taphold)
-			ms->button |= synaction->tap_button;
+		/* Process ClickPad softbuttons */
+		if (sc->synhw.capClickPad && ms->button & MOUSE_BUTTON1DOWN) {
+			y_ok = sc->syninfo.softbuttons_y >= 0 ?
+			    start_y < sc->syninfo.softbuttons_y :
+			    start_y > max_y - sc->syninfo.softbuttons_y;
 
-		/*
-		 * From now on, we can use the SYNAPTICS_END label to skip
-		 * the current packet.
-		 */
+			center_button = MOUSE_BUTTON2DOWN;
+			center_x = sc->syninfo.softbutton2_x;
+			right_button = MOUSE_BUTTON3DOWN;
+			right_x = sc->syninfo.softbutton3_x;
 
-		/*
-		 * Limit the coordinates to the specified margins because
-		 * this area isn't very reliable.
-		 */
-		if (x0 <= margin_left)
-			x0 = margin_left;
-		else if (x0 >= 6143 - margin_right)
-			x0 = 6143 - margin_right;
-		if (y0 <= margin_bottom)
-			y0 = margin_bottom;
-		else if (y0 >= 6143 - margin_top)
-			y0 = 6143 - margin_top;
+			if (center_x > 0 && right_x > 0 && center_x > right_x) {
+				center_button = MOUSE_BUTTON3DOWN;
+				center_x = sc->syninfo.softbutton3_x;
+				right_button = MOUSE_BUTTON2DOWN;
+				right_x = sc->syninfo.softbutton2_x;
+			}
 
-		VLOG(3, (LOG_DEBUG, "synaptics: ipacket: [%d, %d], %d, %d\n",
-		    x0, y0, *z, w));
+			if (right_x > 0 && start_x > right_x && y_ok)
+				ms->button = (ms->button &
+				    ~MOUSE_BUTTON1DOWN) | right_button;
+			else if (center_x > 0 && start_x > center_x && y_ok)
+				ms->button = (ms->button &
+				    ~MOUSE_BUTTON1DOWN) | center_button;
+		}
 
-		/* Queue this new packet. */
-		cursor = SYNAPTICS_QUEUE_CURSOR(synaction->queue_cursor - 1);
-		synaction->queue[cursor].x = x0;
-		synaction->queue[cursor].y = y0;
-		synaction->queue_cursor = cursor;
-		if (synaction->queue_len < SYNAPTICS_PACKETQUEUE)
-			synaction->queue_len++;
-		VLOG(5, (LOG_DEBUG,
-		    "synaptics: cursor[%d]: x=%d, y=%d, dx=%d, dy=%d\n",
-		    cursor, x0, y0, dx, dy));
+		/* If in tap-hold, add the recorded button. */
+		if (gest->in_taphold)
+			ms->button |= gest->tap_button;
 
 		/*
 		 * For tap, we keep the maximum number of fingers and the
 		 * pressure peak. Also with multiple fingers, we increase
 		 * the minimum window.
 		 */
-		switch (w) {
-		case 1: /* Three or more fingers. */
-			synaction->fingers_nb = imax(3, synaction->fingers_nb);
-			synaction->window_min = window_max;
-			break;
-		case 0: /* Two fingers. */
-			synaction->fingers_nb = imax(2, synaction->fingers_nb);
-			synaction->window_min = window_max;
-			break;
-		default: /* One finger or undetectable. */
-			synaction->fingers_nb = imax(1, synaction->fingers_nb);
-		}
-		sc->zmax = imax(*z, sc->zmax);
+		if (nfingers > 1)
+			gest->window_min = window_max;
+		gest->fingers_nb = imax(nfingers, gest->fingers_nb);
+		gest->zmax = imax(f->p, gest->zmax);
 
-		/* Do we have enough packets to consider this a movement? */
-		if (synaction->queue_len < synaction->window_min)
-			goto SYNAPTICS_END;
+		/* Do we have enough packets to consider this a gesture? */
+		if (queue_len < gest->window_min)
+			return;
 
 		/* Is a scrolling action occurring? */
-		if (!synaction->in_taphold && !synaction->in_vscroll) {
+		if (!gest->in_taphold && !ms->button &&
+		    (!gest->in_vscroll || two_finger_scroll)) {
 			/*
 			 * A scrolling action must not conflict with a tap
 			 * action. Here are the conditions to consider a
@@ -3020,12 +3253,10 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 			 *       first should be above a configurable minimum
 			 *     . tap timed out
 			 */
-			dxp = abs(synaction->queue[synaction->queue_cursor].x -
-			    synaction->start_x);
-			dyp = abs(synaction->queue[synaction->queue_cursor].y -
-			    synaction->start_y);
+			dxp = abs(x0 - start_x);
+			dyp = abs(y0 - start_y);
 
-			if (timevalcmp(&sc->lastsoftintr, &sc->taptimeout, >) ||
+			if (timevalcmp(&sc->lastsoftintr, &gest->taptimeout, >) ||
 			    dxp >= sc->syninfo.vscroll_min_delta ||
 			    dyp >= sc->syninfo.vscroll_min_delta) {
 				/*
@@ -3034,176 +3265,65 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 				 * as that keeps the maximum number of fingers.
 				 */
 				if (two_finger_scroll) {
-					if (w == 0) {
-						synaction->in_vscroll +=
+					if (nfingers == 2) {
+						gest->in_vscroll +=
 						    dyp ? 2 : 0;
-						synaction->in_vscroll +=
+						gest->in_vscroll +=
 						    dxp ? 1 : 0;
 					}
 				} else {
 					/* Check for horizontal scrolling. */
 					if ((vscroll_hor_area > 0 &&
-					    synaction->start_y <=
-					        vscroll_hor_area) ||
+					    start_y <= vscroll_hor_area) ||
 					    (vscroll_hor_area < 0 &&
-					     synaction->start_y >=
-					     6143 + vscroll_hor_area))
-						synaction->in_vscroll += 2;
+					     start_y >=
+					     max_y + vscroll_hor_area))
+						gest->in_vscroll += 2;
 
 					/* Check for vertical scrolling. */
 					if ((vscroll_ver_area > 0 &&
-					    synaction->start_x <=
-						vscroll_ver_area) ||
+					    start_x <= vscroll_ver_area) ||
 					    (vscroll_ver_area < 0 &&
-					     synaction->start_x >=
-					     6143 + vscroll_ver_area))
-						synaction->in_vscroll += 1;
+					     start_x >=
+					     max_x + vscroll_ver_area))
+						gest->in_vscroll += 1;
 				}
 
 				/* Avoid conflicts if area overlaps. */
-				if (synaction->in_vscroll >= 3)
-					synaction->in_vscroll =
+				if (gest->in_vscroll >= 3)
+					gest->in_vscroll =
 					    (dxp > dyp) ? 2 : 1;
 			}
 		}
 		/*
 		 * Reset two finger scrolling when the number of fingers
-		 * is different from two.
+		 * is different from two or any button is pressed.
 		 */
-		if (two_finger_scroll && w != 0 && synaction->in_vscroll != 0) {
-			synaction->in_vscroll = 0;
-			exiting_scroll = 1;
-		}
+		if (two_finger_scroll && gest->in_vscroll != 0 &&
+		    (nfingers != 2 || ms->button))
+			gest->in_vscroll = 0;
 
 		VLOG(5, (LOG_DEBUG,
 			"synaptics: virtual scrolling: %s "
 			"(direction=%d, dxp=%d, dyp=%d, fingers=%d)\n",
-			synaction->in_vscroll ? "YES" : "NO",
-			synaction->in_vscroll, dxp, dyp,
-			synaction->fingers_nb));
+			gest->in_vscroll ? "YES" : "NO",
+			gest->in_vscroll, dxp, dyp,
+			gest->fingers_nb));
 
-		weight_prev_x = weight_prev_y = weight_previous;
-		div_max_x = div_max_y = div_max;
-
-		if (synaction->in_vscroll) {
-			/* Dividers are different with virtual scrolling. */
-			div_min = sc->syninfo.vscroll_div_min;
-			div_max_x = div_max_y = sc->syninfo.vscroll_div_max;
-		} else {
-			/*
-			 * There's a lot of noise in coordinates when
-			 * the finger is on the touchpad's borders. When
-			 * using this area, we apply a special weight and
-			 * div.
-			 */
-			if (x0 <= na_left || x0 >= 6143 - na_right) {
-				weight_prev_x = sc->syninfo.weight_previous_na;
-				div_max_x = sc->syninfo.div_max_na;
-			}
-
-			if (y0 <= na_bottom || y0 >= 6143 - na_top) {
-				weight_prev_y = sc->syninfo.weight_previous_na;
-				div_max_y = sc->syninfo.div_max_na;
-			}
-		}
-
-		/*
-		 * Calculate weights for the average operands and
-		 * the divisor. Both depend on the distance between
-		 * the current packet and a previous one (based on the
-		 * window width).
-		 */
-		window = imin(synaction->queue_len, window_max);
-		peer = SYNAPTICS_QUEUE_CURSOR(cursor + window - 1);
-		dxp = abs(x0 - synaction->queue[peer].x) + 1;
-		dyp = abs(y0 - synaction->queue[peer].y) + 1;
-		len = (dxp * dxp) + (dyp * dyp);
-		weight_prev_x = imin(weight_prev_x,
-		    weight_len_squared * weight_prev_x / len);
-		weight_prev_y = imin(weight_prev_y,
-		    weight_len_squared * weight_prev_y / len);
-
-		len = (dxp + dyp) / 2;
-		div_x = div_len * div_max_x / len;
-		div_x = imin(div_max_x, div_x);
-		div_x = imax(div_min, div_x);
-		div_y = div_len * div_max_y / len;
-		div_y = imin(div_max_y, div_y);
-		div_y = imax(div_min, div_y);
-
-		VLOG(3, (LOG_DEBUG,
-		    "synaptics: peer=%d, len=%d, weight=%d/%d, div=%d/%d\n",
-		    peer, len, weight_prev_x, weight_prev_y, div_x, div_y));
-
-		/* Compute averages. */
-		synaction->avg_dx =
-		    (weight_current * dx * multiplicator +
-		     weight_prev_x * synaction->avg_dx) /
-		    (weight_current + weight_prev_x);
-
-		synaction->avg_dy =
-		    (weight_current * dy * multiplicator +
-		     weight_prev_y * synaction->avg_dy) /
-		    (weight_current + weight_prev_y);
-
-		VLOG(5, (LOG_DEBUG,
-		    "synaptics: avg_dx~=%d, avg_dy~=%d\n",
-		    synaction->avg_dx / multiplicator,
-		    synaction->avg_dy / multiplicator));
-
-		/* Use these averages to calculate x & y. */
-		synaction->squelch_x += synaction->avg_dx;
-		*x = synaction->squelch_x / (div_x * multiplicator);
-		synaction->squelch_x = synaction->squelch_x %
-		    (div_x * multiplicator);
-
-		synaction->squelch_y += synaction->avg_dy;
-		*y = synaction->squelch_y / (div_y * multiplicator);
-		synaction->squelch_y = synaction->squelch_y %
-		    (div_y * multiplicator);
-
-		if (synaction->in_vscroll) {
-			switch(synaction->in_vscroll) {
-			case 1: /* Vertical scrolling. */
-				if (*y != 0)
-					ms->button |= (*y > 0) ?
-					    MOUSE_BUTTON4DOWN :
-					    MOUSE_BUTTON5DOWN;
-				break;
-			case 2: /* Horizontal scrolling. */
-				if (*x != 0)
-					ms->button |= (*x > 0) ?
-					    MOUSE_BUTTON7DOWN :
-					    MOUSE_BUTTON6DOWN;
-				break;
-			}
-
-			/* The pointer is not moved. */
-			*x = *y = 0;
-		} else {
-			/* On exit the x/y pos may jump, ignore this */
-			if (exiting_scroll)
-				*x = *y = 0;
-
-			VLOG(3, (LOG_DEBUG, "synaptics: [%d, %d] -> [%d, %d]\n",
-			    dx, dy, *x, *y));
-		}
 	} else if (sc->flags & PSM_FLAGS_FINGERDOWN) {
 		/*
 		 * An action is currently taking place but the pressure
 		 * dropped under the minimum, putting an end to it.
 		 */
-		synapticsaction_t *synaction;
 		int taphold_timeout, dx, dy, tap_max_delta;
 
-		synaction = &(sc->synaction);
-		dx = abs(synaction->queue[synaction->queue_cursor].x -
-		    synaction->start_x);
-		dy = abs(synaction->queue[synaction->queue_cursor].y -
-		    synaction->start_y);
+		dx = abs(smoother->queue[smoother->queue_cursor].x -
+		    smoother->start_x);
+		dy = abs(smoother->queue[smoother->queue_cursor].y -
+		    smoother->start_y);
 
 		/* Max delta is disabled for multi-fingers tap. */
-		if (synaction->fingers_nb > 1)
+		if (gest->fingers_nb > 1)
 			tap_max_delta = imax(dx, dy);
 		else
 			tap_max_delta = sc->syninfo.tap_max_delta;
@@ -3214,12 +3334,12 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 		VLOG(3, (LOG_DEBUG,
 		    "synaptics: zmax=%d, dx=%d, dy=%d, "
 		    "delta=%d, fingers=%d, queue=%d\n",
-		    sc->zmax, dx, dy, tap_max_delta, synaction->fingers_nb,
-		    synaction->queue_len));
-		if (!synaction->in_vscroll && sc->zmax >= tap_threshold &&
-		    timevalcmp(&sc->lastsoftintr, &sc->taptimeout, <=) &&
+		    gest->zmax, dx, dy, tap_max_delta, gest->fingers_nb,
+		    smoother->queue_len));
+		if (!gest->in_vscroll && gest->zmax >= tap_threshold &&
+		    timevalcmp(&sc->lastsoftintr, &gest->taptimeout, <=) &&
 		    dx <= tap_max_delta && dy <= tap_max_delta &&
-		    synaction->queue_len >= sc->syninfo.tap_min_queue) {
+		    smoother->queue_len >= sc->syninfo.tap_min_queue) {
 			/*
 			 * We have a tap if:
 			 *   - the maximum pressure went over tap_threshold
@@ -3228,12 +3348,12 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 			 * To handle tap-hold, we must delay any button push to
 			 * the next action.
 			 */
-			if (synaction->in_taphold) {
+			if (gest->in_taphold) {
 				/*
 				 * This is the second and last tap of a
 				 * double tap action, not a tap-hold.
 				 */
-				synaction->in_taphold = 0;
+				gest->in_taphold = 0;
 
 				/*
 				 * For double-tap to work:
@@ -3245,102 +3365,690 @@ proc_synaptics(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 				 */
 				VLOG(2, (LOG_DEBUG,
 				    "synaptics: button RELEASE: %d\n",
-				    synaction->tap_button));
+				    gest->tap_button));
 				sc->flags |= PSM_FLAGS_FINGERDOWN;
+
+				/* Schedule button press on next interrupt */
+				sc->idletimeout.tv_sec  = psmhz > 1 ?
+				    0 : 1;
+				sc->idletimeout.tv_usec = psmhz > 1 ?
+				    1000000 / psmhz : 0;
 			} else {
 				/*
 				 * This is the first tap: we set the
 				 * tap-hold state and notify the button
 				 * down event.
 				 */
-				synaction->in_taphold = 1;
+				gest->in_taphold = 1;
 				taphold_timeout = sc->syninfo.taphold_timeout;
-				sc->taptimeout.tv_sec  = taphold_timeout /
+				gest->taptimeout.tv_sec  = taphold_timeout /
 				    1000000;
-				sc->taptimeout.tv_usec = taphold_timeout %
+				gest->taptimeout.tv_usec = taphold_timeout %
 				    1000000;
-				timevaladd(&sc->taptimeout, &sc->lastsoftintr);
+				sc->idletimeout = gest->taptimeout;
+				timevaladd(&gest->taptimeout,
+				    &sc->lastsoftintr);
 
-				switch (synaction->fingers_nb) {
+				switch (gest->fingers_nb) {
 				case 3:
-					synaction->tap_button =
+					gest->tap_button =
 					    MOUSE_BUTTON2DOWN;
 					break;
 				case 2:
-					synaction->tap_button =
+					gest->tap_button =
 					    MOUSE_BUTTON3DOWN;
 					break;
 				default:
-					synaction->tap_button =
+					gest->tap_button =
 					    MOUSE_BUTTON1DOWN;
 				}
 				VLOG(2, (LOG_DEBUG,
 				    "synaptics: button PRESS: %d\n",
-				    synaction->tap_button));
-				ms->button |= synaction->tap_button;
+				    gest->tap_button));
+				ms->button |= gest->tap_button;
 			}
 		} else {
 			/*
 			 * Not enough pressure or timeout: reset
 			 * tap-hold state.
 			 */
-			if (synaction->in_taphold) {
+			if (gest->in_taphold) {
 				VLOG(2, (LOG_DEBUG,
 				    "synaptics: button RELEASE: %d\n",
-				    synaction->tap_button));
-				synaction->in_taphold = 0;
+				    gest->tap_button));
+				gest->in_taphold = 0;
 			} else {
 				VLOG(2, (LOG_DEBUG,
 				    "synaptics: not a tap-hold\n"));
 			}
 		}
-	} else if (!(sc->flags & PSM_FLAGS_FINGERDOWN) &&
-	    sc->synaction.in_taphold) {
+	} else if (!(sc->flags & PSM_FLAGS_FINGERDOWN) && gest->in_taphold) {
 		/*
 		 * For a tap-hold to work, the button must remain down at
 		 * least until timeout (where the in_taphold flags will be
 		 * cleared) or during the next action.
 		 */
-		if (timevalcmp(&sc->lastsoftintr, &sc->taptimeout, <=)) {
-			ms->button |= sc->synaction.tap_button;
+		if (timevalcmp(&sc->lastsoftintr, &gest->taptimeout, <=)) {
+			ms->button |= gest->tap_button;
 		} else {
-			VLOG(2, (LOG_DEBUG,
-			    "synaptics: button RELEASE: %d\n",
-			    sc->synaction.tap_button));
-			sc->synaction.in_taphold = 0;
+			VLOG(2, (LOG_DEBUG, "synaptics: button RELEASE: %d\n",
+			    gest->tap_button));
+			gest->in_taphold = 0;
 		}
 	}
 
-SYNAPTICS_END:
+	return;
+}
+
+static void
+psmsmoother(struct psm_softc *sc, finger_t *f, int smoother_id,
+    mousestatus_t *ms, int *x, int *y)
+{
+	smoother_t *smoother = &sc->smoother[smoother_id];
+	gesture_t *gest = &(sc->gesture);
+
 	/*
-	 * Use the extra buttons as a scrollwheel
-	 *
-	 * XXX X.Org uses the Z axis for vertical wheel only,
-	 * whereas moused(8) understands special values to differ
-	 * vertical and horizontal wheels.
-	 *
-	 * xf86-input-mouse needs therefore a small patch to
-	 * understand these special values. Without it, the
-	 * horizontal wheel acts as a vertical wheel in X.Org.
-	 *
-	 * That's why the horizontal wheel is disabled by
-	 * default for now.
+	 * Check pressure to detect a real wanted action on the
+	 * touchpad.
+	 */
+	if (f->p >= sc->syninfo.min_pressure) {
+		int x0, y0;
+		int cursor, peer, window;
+		int dx, dy, dxp, dyp;
+		int max_width, max_pressure;
+		int margin_top, margin_right, margin_bottom, margin_left;
+		int na_top, na_right, na_bottom, na_left;
+		int window_min, window_max;
+		int multiplicator;
+		int weight_current, weight_previous, weight_len_squared;
+		int div_min, div_max, div_len;
+		int vscroll_hor_area, vscroll_ver_area;
+		int two_finger_scroll;
+		int max_x, max_y;
+		int len, weight_prev_x, weight_prev_y;
+		int div_max_x, div_max_y, div_x, div_y;
+		int is_fuzzy;
+
+		/* Read sysctl. */
+		/* XXX Verify values? */
+		max_width = sc->syninfo.max_width;
+		max_pressure = sc->syninfo.max_pressure;
+		margin_top = sc->syninfo.margin_top;
+		margin_right = sc->syninfo.margin_right;
+		margin_bottom = sc->syninfo.margin_bottom;
+		margin_left = sc->syninfo.margin_left;
+		na_top = sc->syninfo.na_top;
+		na_right = sc->syninfo.na_right;
+		na_bottom = sc->syninfo.na_bottom;
+		na_left = sc->syninfo.na_left;
+		window_min = sc->syninfo.window_min;
+		window_max = sc->syninfo.window_max;
+		multiplicator = sc->syninfo.multiplicator;
+		weight_current = sc->syninfo.weight_current;
+		weight_previous = sc->syninfo.weight_previous;
+		weight_len_squared = sc->syninfo.weight_len_squared;
+		div_min = sc->syninfo.div_min;
+		div_max = sc->syninfo.div_max;
+		div_len = sc->syninfo.div_len;
+		vscroll_hor_area = sc->syninfo.vscroll_hor_area;
+		vscroll_ver_area = sc->syninfo.vscroll_ver_area;
+		two_finger_scroll = sc->syninfo.two_finger_scroll;
+		max_x = sc->syninfo.max_x;
+		max_y = sc->syninfo.max_y;
+
+		is_fuzzy = (f->flags & PSM_FINGER_FUZZY) != 0;
+
+		/* Read current absolute position. */
+		x0 = f->x;
+		y0 = f->y;
+
+		/*
+		 * Limit the coordinates to the specified margins because
+		 * this area isn't very reliable.
+		 */
+		if (x0 <= margin_left)
+			x0 = margin_left;
+		else if (x0 >= max_x - margin_right)
+			x0 = max_x - margin_right;
+		if (y0 <= margin_bottom)
+			y0 = margin_bottom;
+		else if (y0 >= max_y - margin_top)
+			y0 = max_y - margin_top;
+
+		/* If the action is just beginning, init the structure. */
+		if (smoother->active == 0) {
+			VLOG(3, (LOG_DEBUG, "smoother%d: ---\n", smoother_id));
+
+			/* Store the first point of this action. */
+			smoother->start_x = x0;
+			smoother->start_y = y0;
+			dx = dy = 0;
+
+			/* Initialize queue. */
+			smoother->queue_cursor = SYNAPTICS_PACKETQUEUE;
+			smoother->queue_len = 0;
+
+			/* Reset average. */
+			smoother->avg_dx = 0;
+			smoother->avg_dy = 0;
+
+			/* Reset squelch. */
+			smoother->squelch_x = 0;
+			smoother->squelch_y = 0;
+
+			/* Activate queue */
+			smoother->active = 1;
+		} else {
+			/* Calculate the current delta. */
+			cursor = smoother->queue_cursor;
+			dx = x0 - smoother->queue[cursor].x;
+			dy = y0 - smoother->queue[cursor].y;
+		}
+
+		VLOG(3, (LOG_DEBUG, "smoother%d: ipacket: [%d, %d], %d, %d\n",
+		    smoother_id, x0, y0, f->p, f->w));
+
+		/* Queue this new packet. */
+		cursor = SYNAPTICS_QUEUE_CURSOR(smoother->queue_cursor - 1);
+		smoother->queue[cursor].x = x0;
+		smoother->queue[cursor].y = y0;
+		smoother->queue_cursor = cursor;
+		if (smoother->queue_len < SYNAPTICS_PACKETQUEUE)
+			smoother->queue_len++;
+		VLOG(5, (LOG_DEBUG,
+		    "smoother%d: cursor[%d]: x=%d, y=%d, dx=%d, dy=%d\n",
+		    smoother_id, cursor, x0, y0, dx, dy));
+
+		/* Do we have enough packets to consider this a movement? */
+		if (smoother->queue_len < gest->window_min)
+			return;
+
+		weight_prev_x = weight_prev_y = weight_previous;
+		div_max_x = div_max_y = div_max;
+
+		if (gest->in_vscroll) {
+			/* Dividers are different with virtual scrolling. */
+			div_min = sc->syninfo.vscroll_div_min;
+			div_max_x = div_max_y = sc->syninfo.vscroll_div_max;
+		} else {
+			/*
+			 * There's a lot of noise in coordinates when
+			 * the finger is on the touchpad's borders. When
+			 * using this area, we apply a special weight and
+			 * div.
+			 */
+			if (x0 <= na_left || x0 >= max_x - na_right) {
+				weight_prev_x = sc->syninfo.weight_previous_na;
+				div_max_x = sc->syninfo.div_max_na;
+			}
+
+			if (y0 <= na_bottom || y0 >= max_y - na_top) {
+				weight_prev_y = sc->syninfo.weight_previous_na;
+				div_max_y = sc->syninfo.div_max_na;
+			}
+		}
+
+		/*
+		 * Calculate weights for the average operands and
+		 * the divisor. Both depend on the distance between
+		 * the current packet and a previous one (based on the
+		 * window width).
+		 */
+		window = imin(smoother->queue_len, window_max);
+		peer = SYNAPTICS_QUEUE_CURSOR(cursor + window - 1);
+		dxp = abs(x0 - smoother->queue[peer].x) + 1;
+		dyp = abs(y0 - smoother->queue[peer].y) + 1;
+		len = (dxp * dxp) + (dyp * dyp);
+		weight_prev_x = imin(weight_prev_x,
+		    weight_len_squared * weight_prev_x / len);
+		weight_prev_y = imin(weight_prev_y,
+		    weight_len_squared * weight_prev_y / len);
+
+		len = (dxp + dyp) / 2;
+		div_x = div_len * div_max_x / len;
+		div_x = imin(div_max_x, div_x);
+		div_x = imax(div_min, div_x);
+		div_y = div_len * div_max_y / len;
+		div_y = imin(div_max_y, div_y);
+		div_y = imax(div_min, div_y);
+
+		VLOG(3, (LOG_DEBUG,
+		    "smoother%d: peer=%d, len=%d, weight=%d/%d, div=%d/%d\n",
+		    smoother_id, peer, len, weight_prev_x, weight_prev_y,
+		    div_x, div_y));
+
+		/* Compute averages. */
+		smoother->avg_dx =
+		    (weight_current * dx * multiplicator +
+		     weight_prev_x * smoother->avg_dx) /
+		    (weight_current + weight_prev_x);
+
+		smoother->avg_dy =
+		    (weight_current * dy * multiplicator +
+		     weight_prev_y * smoother->avg_dy) /
+		    (weight_current + weight_prev_y);
+
+		VLOG(5, (LOG_DEBUG,
+		    "smoother%d: avg_dx~=%d, avg_dy~=%d\n", smoother_id,
+		    smoother->avg_dx / multiplicator,
+		    smoother->avg_dy / multiplicator));
+
+		/* Use these averages to calculate x & y. */
+		smoother->squelch_x += smoother->avg_dx;
+		dxp = smoother->squelch_x / (div_x * multiplicator);
+		smoother->squelch_x = smoother->squelch_x %
+		    (div_x * multiplicator);
+
+		smoother->squelch_y += smoother->avg_dy;
+		dyp = smoother->squelch_y / (div_y * multiplicator);
+		smoother->squelch_y = smoother->squelch_y %
+		    (div_y * multiplicator);
+
+		switch(gest->in_vscroll) {
+		case 0: /* Pointer movement. */
+			/* On real<->fuzzy finger switch the x/y pos jumps */
+			if (is_fuzzy == smoother->is_fuzzy) {
+				*x += dxp;
+				*y += dyp;
+			}
+
+			VLOG(3, (LOG_DEBUG, "smoother%d: [%d, %d] -> [%d, %d]\n",
+			    smoother_id, dx, dy, dxp, dyp));
+			break;
+		case 1: /* Vertical scrolling. */
+			if (dyp != 0)
+				ms->button |= (dyp > 0) ?
+				    MOUSE_BUTTON4DOWN : MOUSE_BUTTON5DOWN;
+			break;
+		case 2: /* Horizontal scrolling. */
+			if (dxp != 0)
+				ms->button |= (dxp > 0) ?
+				    MOUSE_BUTTON7DOWN : MOUSE_BUTTON6DOWN;
+			break;
+		}
+
+		smoother->is_fuzzy = is_fuzzy;
+
+	} else {
+		/*
+		 * Deactivate queue. Note: We can not just reset queue here
+		 * as these values are still used by gesture processor.
+		 * So postpone reset till next touch.
+		 */
+		smoother->active = 0;
+	}
+}
+
+static int
+proc_elantech(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
+    int *x, int *y, int *z)
+{
+	static int touchpad_button, trackpoint_button;
+	finger_t fn, f[ELANTECH_MAX_FINGERS];
+	int pkt, id, scale, i, nfingers, mask;
+
+	if (!elantech_support)
+		return (0);
+
+	/* Determine packet format and do a sanity check for out of sync packets. */
+	if (ELANTECH_PKT_IS_DEBOUNCE(pb, sc->elanhw.hwversion))
+		pkt = ELANTECH_PKT_NOP;
+	else if (ELANTECH_PKT_IS_TRACKPOINT(pb))
+		pkt = ELANTECH_PKT_TRACKPOINT;
+	else
+	switch (sc->elanhw.hwversion) {
+	case 2:
+		if (!ELANTECH_PKT_IS_V2(pb))
+			return (-1);
+
+		pkt = (pb->ipacket[0] & 0xc0) == 0x80 ?
+		    ELANTECH_PKT_V2_2FINGER : ELANTECH_PKT_V2_COMMON;
+		break;
+	case 3:
+		if (!ELANTECH_PKT_IS_V3_HEAD(pb, sc->elanhw.hascrc) &&
+		    !ELANTECH_PKT_IS_V3_TAIL(pb, sc->elanhw.hascrc))
+			return (-1);
+
+		pkt = ELANTECH_PKT_V3;
+		break;
+	case 4:
+		if (!ELANTECH_PKT_IS_V4(pb, sc->elanhw.hascrc))
+			return (-1);
+
+		switch (pb->ipacket[3] & 0x03) {
+		case 0x00:
+			pkt = ELANTECH_PKT_V4_STATUS;
+			break;
+		case 0x01:
+			pkt = ELANTECH_PKT_V4_HEAD;
+			break;
+		case 0x02:
+			pkt = ELANTECH_PKT_V4_MOTION;
+			break;
+		default:
+			return (-1);
+		}
+		break;
+	default:
+		return (-1);
+	}
+
+	VLOG(5, (LOG_DEBUG, "elantech: ipacket format: %d\n", pkt));
+
+	for (id = 0; id < ELANTECH_MAX_FINGERS; id++)
+		PSM_FINGER_RESET(f[id]);
+
+	*x = *y = *z = 0;
+	ms->button = ms->obutton;
+
+	if (sc->syninfo.touchpad_off)
+		return (0);
+
+	/* Common legend
+	 * L: Left mouse button pressed
+	 * R: Right mouse button pressed
+	 * N: number of fingers on touchpad
+	 * X: absolute x value (horizontal)
+	 * Y: absolute y value (vertical)
+	 * W; width of the finger touch
+	 * P: pressure
 	 */
+	switch (pkt) {
+	case ELANTECH_PKT_V2_COMMON:	/* HW V2. One/Three finger touch */
+		/*               7   6   5   4   3   2   1   0 (LSB)
+		 * -------------------------------------------
+		 * ipacket[0]:  N1  N0  W3  W2   .   .   R   L
+		 * ipacket[1]:  P7  P6  P5  P4 X11 X10  X9  X8
+		 * ipacket[2]:  X7  X6  X5  X4  X3  X2  X1  X0
+		 * ipacket[3]:  N4  VF  W1  W0   .   .   .  B2
+		 * ipacket[4]:  P3  P1  P2  P0 Y11 Y10  Y9  Y8
+		 * ipacket[5]:  Y7  Y6  Y5  Y4  Y3  Y2  Y1  Y0
+		 * -------------------------------------------
+		 * N4: set if more than 3 fingers (only in 3 fingers mode)
+		 * VF: a kind of flag? (only on EF123, 0 when finger
+		 *     is over one of the buttons, 1 otherwise)
+		 * B2: (on EF113 only, 0 otherwise), one button pressed
+		 * P & W is not reported on EF113 touchpads
+		 */
+		nfingers = (pb->ipacket[0] & 0xc0) >> 6;
+		if (nfingers == 3 && (pb->ipacket[3] & 0x80))
+			nfingers = 4;
+		mask = (1 << nfingers) - 1;
 
-	if (ms->button & MOUSE_BUTTON4DOWN) {
+		fn = ELANTECH_FINGER_SET_XYP(pb);
+		if (sc->elanhw.haspressure) {
+			fn.w = ((pb->ipacket[0] & 0x30) >> 2) |
+			    ((pb->ipacket[3] & 0x30) >> 4);
+		} else {
+			fn.p = PSM_FINGER_DEFAULT_P;
+			fn.w = PSM_FINGER_DEFAULT_W;
+		}
+
+		/*
+		 * HW v2 dont report exact finger positions when 3 or more
+		 * fingers are on touchpad. Use reported value as fingers
+		 * position as it is required for tap detection
+		 */
+		if (nfingers > 2)
+			fn.flags = PSM_FINGER_FUZZY;
+
+		for (id = 0; id < imin(nfingers, ELANTECH_MAX_FINGERS); id++)
+			f[id] = fn;
+		break;
+
+	case ELANTECH_PKT_V2_2FINGER:	/*HW V2. Two finger touch */
+		/*               7   6   5   4   3   2   1   0 (LSB)
+		 * -------------------------------------------
+		 * ipacket[0]:  N1  N0 AY8 AX8   .   .   R   L
+		 * ipacket[1]: AX7 AX6 AX5 AX4 AX3 AX2 AX1 AX0
+		 * ipacket[2]: AY7 AY6 AY5 AY4 AY3 AY2 AY1 AY0
+		 * ipacket[3]:   .   . BY8 BX8   .   .   .   .
+		 * ipacket[4]: BX7 BX6 BX5 BX4 BX3 BX2 BX1 BX0
+		 * ipacket[5]: BY7 BY6 BY5 BY4 BY3 BY2 BY1 BY0
+		 * -------------------------------------------
+		 * AX: lower-left finger absolute x value
+		 * AY: lower-left finger absolute y value
+		 * BX: upper-right finger absolute x value
+		 * BY: upper-right finger absolute y value
+		 */
+		nfingers = 2;
+		mask = (1 << nfingers) - 1;
+
+		for (id = 0; id < imin(2, ELANTECH_MAX_FINGERS); id ++)
+			f[id] = (finger_t) {
+				.x = (((pb->ipacket[id * 3] & 0x10) << 4) |
+				    pb->ipacket[id * 3 + 1]) << 2,
+				.y = (((pb->ipacket[id * 3] & 0x20) << 3) |
+				    pb->ipacket[id * 3 + 2]) << 2,
+				.p = PSM_FINGER_DEFAULT_P,
+				.w = PSM_FINGER_DEFAULT_W,
+				/* HW ver.2 sends bounding box */
+				.flags = PSM_FINGER_FUZZY
+			};
+		break;
+
+	case ELANTECH_PKT_V3:	/* HW Version 3 */
+		/*               7   6   5   4   3   2   1   0 (LSB)
+		 * -------------------------------------------
+		 * ipacket[0]:  N1  N0  W3  W2   0   1   R   L
+		 * ipacket[1]:  P7  P6  P5  P4 X11 X10  X9  X8
+		 * ipacket[2]:  X7  X6  X5  X4  X3  X2  X1  X0
+		 * ipacket[3]:   0   0  W1  W0   0   0   1   0
+		 * ipacket[4]:  P3  P1  P2  P0 Y11 Y10  Y9  Y8
+		 * ipacket[5]:  Y7  Y6  Y5  Y4  Y3  Y2  Y1  Y0
+		 * -------------------------------------------
+		 */
+		nfingers = (pb->ipacket[0] & 0xc0) >> 6;
+		mask = (1 << nfingers) - 1;
+		id = nfingers - 1;
+
+		fn = ELANTECH_FINGER_SET_XYP(pb);
+		fn.w = ((pb->ipacket[0] & 0x30) >> 2) |
+		    ((pb->ipacket[3] & 0x30) >> 4);
+
+		/*
+		 * HW v3 dont report exact finger positions when 3 or more
+		 * fingers are on touchpad. Use reported value as fingers
+		 * position as it is required for tap detection
+		 */
+		if (nfingers > 1)
+			fn.flags = PSM_FINGER_FUZZY;
+
+		for (id = 0; id < imin(nfingers, ELANTECH_MAX_FINGERS); id++)
+			f[id] = fn;
+
+		if (nfingers == 2) {
+			if (ELANTECH_PKT_IS_V3_HEAD(pb, sc->elanhw.hascrc)) {
+				sc->elanaction.fingers[0] = fn;
+				return (0);
+			} else
+				f[0] = sc->elanaction.fingers[0];
+		}
+		break;
+
+	case ELANTECH_PKT_V4_STATUS:	/* HW Version 4. Status packet */
+		/*               7   6   5   4   3   2   1   0 (LSB)
+		 * -------------------------------------------
+		 * ipacket[0]:   .   .   .   .   0   1   R   L
+		 * ipacket[1]:   .   .   .  F4  F3  F2  F1  F0
+		 * ipacket[2]:   .   .   .   .   .   .   .   .
+		 * ipacket[3]:   .   .   .   1   0   0   0   0
+		 * ipacket[4]:  PL   .   .   .   .   .   .   .
+		 * ipacket[5]:   .   .   .   .   .   .   .   .
+		 * -------------------------------------------
+		 * Fn: finger n is on touchpad
+		 * PL: palm
+		 * HV ver4 sends a status packet to indicate that the numbers
+		 * or identities of the fingers has been changed
+		 */
+
+		mask = pb->ipacket[1] & 0x1f;
+		nfingers = bitcount(mask);
+
+		/* Skip "new finger is on touchpad" packets */
+		if ((sc->elanaction.mask & mask) == sc->elanaction.mask &&
+		    (mask & ~sc->elanaction.mask)) {
+			sc->elanaction.mask = mask;
+			return (0);
+		}
+
+		break;
+
+	case ELANTECH_PKT_V4_HEAD:	/* HW Version 4. Head packet */
+		/*               7   6   5   4   3   2   1   0 (LSB)
+		 * -------------------------------------------
+		 * ipacket[0]:  W3  W2  W1  W0   0   1   R   L
+		 * ipacket[1]:  P7  P6  P5  P4 X11 X10  X9  X8
+		 * ipacket[2]:  X7  X6  X5  X4  X3  X2  X1  X0
+		 * ipacket[3]: ID2 ID1 ID0   1   0   0   0   1
+		 * ipacket[4]:  P3  P1  P2  P0 Y11 Y10  Y9  Y8
+		 * ipacket[5]:  Y7  Y6  Y5  Y4  Y3  Y2  Y1  Y0
+		 * -------------------------------------------
+		 * ID: finger id
+		 * HW ver 4 sends head packets in two cases:
+		 * 1. One finger touch and movement.
+		 * 2. Next after status packet to tell new finger positions.
+		 */
+		mask = sc->elanaction.mask;
+		nfingers = bitcount(mask);
+		id = ((pb->ipacket[3] & 0xe0) >> 5) - 1;
+
+		if (id >= 0 && id < ELANTECH_MAX_FINGERS) {
+			f[id] = ELANTECH_FINGER_SET_XYP(pb);
+			f[id].w = (pb->ipacket[0] & 0xf0) >> 4;
+		}
+		break;
+
+	case ELANTECH_PKT_V4_MOTION:	/* HW Version 4. Motion packet */
+		/*               7   6   5   4   3   2   1   0 (LSB)
+		 * -------------------------------------------
+		 * ipacket[0]: ID2 ID1 ID0  OF   0   1   R   L
+		 * ipacket[1]: DX7 DX6 DX5 DX4 DX3 DX2 DX1 DX0
+		 * ipacket[2]: DY7 DY6 DY5 DY4 DY3 DY2 DY1 DY0
+		 * ipacket[3]: ID2 ID1 ID0   1   0   0   1   0
+		 * ipacket[4]: DX7 DX6 DX5 DX4 DX3 DX2 DX1 DX0
+		 * ipacket[5]: DY7 DY6 DY5 DY4 DY3 DY2 DY1 DY0
+		 * -------------------------------------------
+		 * OF: delta overflows (> 127 or < -128), in this case
+		 *     firmware sends us (delta x / 5) and (delta y / 5)
+		 * ID: finger id
+		 * DX: delta x (two's complement)
+		 * XY: delta y (two's complement)
+		 * byte 0 ~ 2 for one finger
+		 * byte 3 ~ 5 for another finger
+		 */
+		mask = sc->elanaction.mask;
+		nfingers = bitcount(mask);
+
+		scale = (pb->ipacket[0] & 0x10) ? 5 : 1;
+		for (i = 0; i <= 3; i += 3) {
+			id = ((pb->ipacket[i] & 0xe0) >> 5) - 1;
+			if (id < 0 || id >= ELANTECH_MAX_FINGERS)
+				continue;
+
+			if (PSM_FINGER_IS_SET(sc->elanaction.fingers[id])) {
+				f[id] = sc->elanaction.fingers[id];
+				f[id].x += imax(-f[id].x,
+				    (signed char)pb->ipacket[i+1] * scale);
+				f[id].y += imax(-f[id].y,
+				    (signed char)pb->ipacket[i+2] * scale);
+			} else {
+				VLOG(3, (LOG_DEBUG, "elantech: "
+				    "HW v4 motion packet skipped\n"));
+			}
+		}
+
+		break;
+
+	case ELANTECH_PKT_TRACKPOINT:
+		/*               7   6   5   4   3   2   1   0 (LSB)
+		 * -------------------------------------------
+		 * ipacket[0]:   0   0  SX  SY   0   M   R   L
+		 * ipacket[1]: ~SX   0   0   0   0   0   0   0
+		 * ipacket[2]: ~SY   0   0   0   0   0   0   0
+		 * ipacket[3]:   0   0 ~SY ~SX   0   1   1   0
+		 * ipacket[4]:  X7  X6  X5  X4  X3  X2  X1  X0
+		 * ipacket[5]:  Y7  Y6  Y5  Y4  Y3  Y2  Y1  Y0
+		 * -------------------------------------------
+		 * X and Y are written in two's complement spread
+		 * over 9 bits with SX/SY the relative top bit and
+		 * X7..X0 and Y7..Y0 the lower bits.
+		 */
+		*x = (pb->ipacket[0] & 0x20) ?
+		    pb->ipacket[4] - 256 : pb->ipacket[4];
+		*y = (pb->ipacket[0] & 0x10) ?
+		    pb->ipacket[5] - 256 : pb->ipacket[5];
+
+		trackpoint_button =
+		    ((pb->ipacket[0] & 0x01) ? MOUSE_BUTTON1DOWN : 0) |
+		    ((pb->ipacket[0] & 0x02) ? MOUSE_BUTTON3DOWN : 0) |
+		    ((pb->ipacket[0] & 0x04) ? MOUSE_BUTTON2DOWN : 0);
+
+		ms->button = touchpad_button | trackpoint_button;
+		return (0);
+
+	case ELANTECH_PKT_NOP:
+		return (0);
+
+	default:
+		return (-1);
+	}
+
+	for (id = 0; id < ELANTECH_MAX_FINGERS; id++)
+		if (PSM_FINGER_IS_SET(f[id]))
+			VLOG(2, (LOG_DEBUG, "elantech: "
+			    "finger %d: down [%d, %d], %d, %d, %d\n", id + 1,
+			    f[id].x, f[id].y, f[id].p, f[id].w, f[id].flags));
+
+	/* Touchpad button presses */
+	if (sc->elanhw.isclickpad) {
+		touchpad_button =
+		    ((pb->ipacket[0] & 0x03) ? MOUSE_BUTTON1DOWN : 0);
+	} else {
+		touchpad_button =
+		    ((pb->ipacket[0] & 0x01) ? MOUSE_BUTTON1DOWN : 0) |
+		    ((pb->ipacket[0] & 0x02) ? MOUSE_BUTTON3DOWN : 0);
+	}
+
+	ms->button = touchpad_button | trackpoint_button;
+
+	/* Palm detection doesn't terminate the current action. */
+	if (!psmpalmdetect(sc, &f[0], nfingers)) {
+		/* Send finger 1 position to gesture processor */
+		if (PSM_FINGER_IS_SET(f[0]) || PSM_FINGER_IS_SET(f[1]) ||
+		    nfingers == 0)
+			psmgestures(sc, &f[0], imin(nfingers, 3), ms);
+		/* Send fingers positions to movement smoothers */
+		for (id = 0; id < PSM_FINGERS; id++)
+			if (PSM_FINGER_IS_SET(f[id]) || !(mask & (1 << id)))
+				psmsmoother(sc, &f[id], id, ms, x, y);
+	} else {
+		VLOG(2, (LOG_DEBUG, "elantech: palm detected! (%d)\n",
+		    f[0].w));
+	}
+
+	/* Store current finger positions in action context */
+	for (id = 0; id < ELANTECH_MAX_FINGERS; id++) {
+		if (PSM_FINGER_IS_SET(f[id]))
+			sc->elanaction.fingers[id] = f[id];
+		if ((sc->elanaction.mask & (1 << id)) && !(mask & (1 << id)))
+			PSM_FINGER_RESET(sc->elanaction.fingers[id]);
+	}
+	sc->elanaction.mask = mask;
+
+	/* Use the extra buttons as a scrollwheel */
+	if (ms->button & MOUSE_BUTTON4DOWN)
 		*z = -1;
-		ms->button &= ~MOUSE_BUTTON4DOWN;
-	} else if (ms->button & MOUSE_BUTTON5DOWN) {
+	else if (ms->button & MOUSE_BUTTON5DOWN)
 		*z = 1;
-		ms->button &= ~MOUSE_BUTTON5DOWN;
-	} else if (ms->button & MOUSE_BUTTON6DOWN) {
+	else if (ms->button & MOUSE_BUTTON6DOWN)
 		*z = -2;
-		ms->button &= ~MOUSE_BUTTON6DOWN;
-	} else if (ms->button & MOUSE_BUTTON7DOWN) {
+	else if (ms->button & MOUSE_BUTTON7DOWN)
 		*z = 2;
-		ms->button &= ~MOUSE_BUTTON7DOWN;
-	} else
+	else
 		*z = 0;
+	ms->button &= ~(MOUSE_BUTTON4DOWN | MOUSE_BUTTON5DOWN |
+	    MOUSE_BUTTON6DOWN | MOUSE_BUTTON7DOWN);
 
 	return (0);
 }
@@ -3413,6 +4121,31 @@ proc_versapad(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *ms,
 }
 
 static void
+psmsoftintridle(void *arg)
+{
+	struct psm_softc *sc = arg;
+	packetbuf_t *pb;
+
+	/* Invoke soft handler only when pqueue is empty. Otherwise it will be
+	 * invoked from psmintr soon with pqueue filled with real data */
+	if (sc->pqueue_start == sc->pqueue_end &&
+	    sc->idlepacket.inputbytes > 0) {
+		/* Grow circular queue backwards to avoid race with psmintr */
+		if (--sc->pqueue_start < 0)
+			sc->pqueue_start = PSM_PACKETQUEUE - 1;
+
+		pb = &sc->pqueue[sc->pqueue_start];
+		memcpy(pb, &sc->idlepacket, sizeof(packetbuf_t));
+		VLOG(4, (LOG_DEBUG,
+		    "psmsoftintridle: %02x %02x %02x %02x %02x %02x\n",
+		    pb->ipacket[0], pb->ipacket[1], pb->ipacket[2],
+		    pb->ipacket[3], pb->ipacket[4], pb->ipacket[5]));
+
+		psmsoftintr(arg);
+	}
+}
+
+static void
 psmsoftintr(void *arg)
 {
 	/*
@@ -3465,6 +4198,8 @@ psmsoftintr(void *arg)
 		if (sc->config & PSM_CONFIG_FORCETAP)
 			ms.button |= ((c & MOUSE_PS2_TAP)) ?
 			    0 : MOUSE_BUTTON4DOWN;
+		timevalclear(&sc->idletimeout);
+		sc->idlepacket.inputbytes = 0;
 
 		switch (sc->hw.model) {
 
@@ -3603,6 +4338,11 @@ psmsoftintr(void *arg)
 				goto next;
 			break;
 
+		case MOUSE_MODEL_ELANTECH:
+			if (proc_elantech(sc, pb, &ms, &x, &y, &z) != 0)
+				goto next;
+			break;
+
 		case MOUSE_MODEL_TRACKPOINT:
 		case MOUSE_MODEL_GENERIC:
 		default:
@@ -3627,6 +4367,10 @@ psmsoftintr(void *arg)
 		}
 	}
 
+	/* Store last packet for reinjection if it has not been set already */
+	if (timevalisset(&sc->idletimeout) && sc->idlepacket.inputbytes == 0)
+		sc->idlepacket = *pb;
+
 	ms.dx = x;
 	ms.dy = y;
 	ms.dz = z;
@@ -3673,6 +4417,16 @@ next:
 		pgsigio(&sc->async, SIGIO, 0);
 	}
 	sc->state &= ~PSM_SOFTARMED;
+
+	/* schedule injection of predefined packet after idletimeout
+	 * if no data packets have been received from psmintr */
+	if (timevalisset(&sc->idletimeout)) {
+		sc->state |= PSM_SOFTARMED;
+		callout_reset(&sc->softcallout, tvtohz(&sc->idletimeout),
+		    psmsoftintridle, sc);
+		VLOG(2, (LOG_DEBUG, "softintr: callout set: %d ticks\n",
+		    tvtohz(&sc->idletimeout)));
+	}
 	splx(s);
 }
 
@@ -4106,10 +4860,17 @@ enable_4dplus(struct psm_softc *sc, enum probearg arg)
 static int
 synaptics_sysctl(SYSCTL_HANDLER_ARGS)
 {
+	struct psm_softc *sc;
 	int error, arg;
 
+	if (oidp->oid_arg1 == NULL || oidp->oid_arg2 < 0 ||
+	    oidp->oid_arg2 > SYNAPTICS_SYSCTL_SOFTBUTTON3_X)
+		return (EINVAL);
+
+	sc = oidp->oid_arg1;
+
 	/* Read the current value. */
-	arg = *(int *)oidp->oid_arg1;
+	arg = *(int *)((char *)sc + oidp->oid_arg2);
 	error = sysctl_handle_int(oidp, &arg, 0, req);
 
 	/* Sanity check. */
@@ -4131,14 +4892,23 @@ synaptics_sysctl(SYSCTL_HANDLER_ARGS)
 			return (EINVAL);
 		break;
 	case SYNAPTICS_SYSCTL_MARGIN_TOP:
-	case SYNAPTICS_SYSCTL_MARGIN_RIGHT:
 	case SYNAPTICS_SYSCTL_MARGIN_BOTTOM:
-	case SYNAPTICS_SYSCTL_MARGIN_LEFT:
 	case SYNAPTICS_SYSCTL_NA_TOP:
-	case SYNAPTICS_SYSCTL_NA_RIGHT:
 	case SYNAPTICS_SYSCTL_NA_BOTTOM:
+		if (arg < 0 || arg > sc->synhw.maximumYCoord)
+			return (EINVAL);
+		break;
+	case SYNAPTICS_SYSCTL_SOFTBUTTON2_X:
+	case SYNAPTICS_SYSCTL_SOFTBUTTON3_X:
+		/* Softbuttons is clickpad only feature */
+		if (!sc->synhw.capClickPad && arg != 0)
+			return (EINVAL);
+		/* FALLTHROUGH */
+	case SYNAPTICS_SYSCTL_MARGIN_RIGHT:
+	case SYNAPTICS_SYSCTL_MARGIN_LEFT:
+	case SYNAPTICS_SYSCTL_NA_RIGHT:
 	case SYNAPTICS_SYSCTL_NA_LEFT:
-		if (arg < 0 || arg > 6143)
+		if (arg < 0 || arg > sc->synhw.maximumXCoord)
 			return (EINVAL);
 		break;
 	case SYNAPTICS_SYSCTL_WINDOW_MIN:
@@ -4168,8 +4938,18 @@ synaptics_sysctl(SYSCTL_HANDLER_ARGS)
 			return (EINVAL);
 		break;
 	case SYNAPTICS_SYSCTL_VSCROLL_HOR_AREA:
+		if (arg < -sc->synhw.maximumXCoord ||
+		    arg > sc->synhw.maximumXCoord)
+			return (EINVAL);
+		break;
+	case SYNAPTICS_SYSCTL_SOFTBUTTONS_Y:
+		/* Softbuttons is clickpad only feature */
+		if (!sc->synhw.capClickPad && arg != 0)
+			return (EINVAL);
+		/* FALLTHROUGH */
 	case SYNAPTICS_SYSCTL_VSCROLL_VER_AREA:
-		if (arg < -6143 || arg > 6143)
+		if (arg < -sc->synhw.maximumYCoord ||
+		    arg > sc->synhw.maximumYCoord)
 			return (EINVAL);
 		break;
         case SYNAPTICS_SYSCTL_TOUCHPAD_OFF:
@@ -4181,13 +4961,51 @@ synaptics_sysctl(SYSCTL_HANDLER_ARGS)
 	}
 
 	/* Update. */
-	*(int *)oidp->oid_arg1 = arg;
+	*(int *)((char *)sc + oidp->oid_arg2) = arg;
 
 	return (error);
 }
 
 static void
-synaptics_sysctl_create_tree(struct psm_softc *sc)
+synaptics_sysctl_create_softbuttons_tree(struct psm_softc *sc)
+{
+	/*
+	 * Set predefined sizes for softbuttons.
+	 * Values are taken to match HP Pavilion dv6 clickpad drawings
+	 * with thin middle softbutton placed on separator
+	 */
+
+	/* hw.psm.synaptics.softbuttons_y */
+	sc->syninfo.softbuttons_y = 1700;
+	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
+	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
+	    "softbuttons_y", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
+	    sc, SYNAPTICS_SYSCTL_SOFTBUTTONS_Y,
+	    synaptics_sysctl, "I",
+	    "Vertical size of softbuttons area");
+
+	/* hw.psm.synaptics.softbutton2_x */
+	sc->syninfo.softbutton2_x = 3100;
+	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
+	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
+	    "softbutton2_x", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
+	    sc, SYNAPTICS_SYSCTL_SOFTBUTTON2_X,
+	    synaptics_sysctl, "I",
+	    "Horisontal position of 2-nd softbutton left edge (0-disable)");
+
+	/* hw.psm.synaptics.softbutton3_x */
+	sc->syninfo.softbutton3_x = 3900;
+	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
+	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
+	    "softbutton3_x", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
+	    sc, SYNAPTICS_SYSCTL_SOFTBUTTON3_X,
+	    synaptics_sysctl, "I",
+	    "Horisontal position of 3-rd softbutton left edge (0-disable)");
+}
+
+static void
+synaptics_sysctl_create_tree(struct psm_softc *sc, const char *name,
+    const char *descr)
 {
 
 	if (sc->syninfo.sysctl_tree != NULL)
@@ -4196,8 +5014,8 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	/* Attach extra synaptics sysctl nodes under hw.psm.synaptics */
 	sysctl_ctx_init(&sc->syninfo.sysctl_ctx);
 	sc->syninfo.sysctl_tree = SYSCTL_ADD_NODE(&sc->syninfo.sysctl_ctx,
-	    SYSCTL_STATIC_CHILDREN(_hw_psm), OID_AUTO, "synaptics", CTLFLAG_RD,
-	    0, "Synaptics TouchPad");
+	    SYSCTL_STATIC_CHILDREN(_hw_psm), OID_AUTO, name, CTLFLAG_RD,
+	    0, descr);
 
 	/* hw.psm.synaptics.directional_scrolls. */
 	sc->syninfo.directional_scrolls = 0;
@@ -4208,6 +5026,22 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	    "Enable hardware scrolling pad (if non-zero) or register it as "
 	    "extended buttons (if 0)");
 
+	/* hw.psm.synaptics.max_x. */
+	sc->syninfo.max_x = 6143;
+	SYSCTL_ADD_INT(&sc->syninfo.sysctl_ctx,
+	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
+	    "max_x", CTLFLAG_RD|CTLFLAG_ANYBODY,
+	    &sc->syninfo.max_x, 0,
+	    "Horizontal reporting range");
+
+	/* hw.psm.synaptics.max_y. */
+	sc->syninfo.max_y = 6143;
+	SYSCTL_ADD_INT(&sc->syninfo.sysctl_ctx,
+	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
+	    "max_y", CTLFLAG_RD|CTLFLAG_ANYBODY,
+	    &sc->syninfo.max_y, 0,
+	    "Vertical reporting range");
+
 	/*
 	 * Turn off two finger scroll if we have a
 	 * physical area reserved for scrolling or when
@@ -4230,7 +5064,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "min_pressure", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.min_pressure, SYNAPTICS_SYSCTL_MIN_PRESSURE,
+	    sc, SYNAPTICS_SYSCTL_MIN_PRESSURE,
 	    synaptics_sysctl, "I",
 	    "Minimum pressure required to start an action");
 
@@ -4239,7 +5073,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "max_pressure", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.max_pressure, SYNAPTICS_SYSCTL_MAX_PRESSURE,
+	    sc, SYNAPTICS_SYSCTL_MAX_PRESSURE,
 	    synaptics_sysctl, "I",
 	    "Maximum pressure to detect palm");
 
@@ -4248,7 +5082,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "max_width", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.max_width, SYNAPTICS_SYSCTL_MAX_WIDTH,
+	    sc, SYNAPTICS_SYSCTL_MAX_WIDTH,
 	    synaptics_sysctl, "I",
 	    "Maximum finger width to detect palm");
 
@@ -4257,7 +5091,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "margin_top", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.margin_top, SYNAPTICS_SYSCTL_MARGIN_TOP,
+	    sc, SYNAPTICS_SYSCTL_MARGIN_TOP,
 	    synaptics_sysctl, "I",
 	    "Top margin");
 
@@ -4266,7 +5100,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "margin_right", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.margin_right, SYNAPTICS_SYSCTL_MARGIN_RIGHT,
+	    sc, SYNAPTICS_SYSCTL_MARGIN_RIGHT,
 	    synaptics_sysctl, "I",
 	    "Right margin");
 
@@ -4275,7 +5109,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "margin_bottom", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.margin_bottom, SYNAPTICS_SYSCTL_MARGIN_BOTTOM,
+	    sc, SYNAPTICS_SYSCTL_MARGIN_BOTTOM,
 	    synaptics_sysctl, "I",
 	    "Bottom margin");
 
@@ -4284,7 +5118,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "margin_left", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.margin_left, SYNAPTICS_SYSCTL_MARGIN_LEFT,
+	    sc, SYNAPTICS_SYSCTL_MARGIN_LEFT,
 	    synaptics_sysctl, "I",
 	    "Left margin");
 
@@ -4293,7 +5127,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "na_top", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.na_top, SYNAPTICS_SYSCTL_NA_TOP,
+	    sc, SYNAPTICS_SYSCTL_NA_TOP,
 	    synaptics_sysctl, "I",
 	    "Top noisy area, where weight_previous_na is used instead "
 	    "of weight_previous");
@@ -4303,7 +5137,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "na_right", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.na_right, SYNAPTICS_SYSCTL_NA_RIGHT,
+	    sc, SYNAPTICS_SYSCTL_NA_RIGHT,
 	    synaptics_sysctl, "I",
 	    "Right noisy area, where weight_previous_na is used instead "
 	    "of weight_previous");
@@ -4313,7 +5147,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "na_bottom", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.na_bottom, SYNAPTICS_SYSCTL_NA_BOTTOM,
+	    sc, SYNAPTICS_SYSCTL_NA_BOTTOM,
 	    synaptics_sysctl, "I",
 	    "Bottom noisy area, where weight_previous_na is used instead "
 	    "of weight_previous");
@@ -4323,7 +5157,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "na_left", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.na_left, SYNAPTICS_SYSCTL_NA_LEFT,
+	    sc, SYNAPTICS_SYSCTL_NA_LEFT,
 	    synaptics_sysctl, "I",
 	    "Left noisy area, where weight_previous_na is used instead "
 	    "of weight_previous");
@@ -4333,7 +5167,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "window_min", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.window_min, SYNAPTICS_SYSCTL_WINDOW_MIN,
+	    sc, SYNAPTICS_SYSCTL_WINDOW_MIN,
 	    synaptics_sysctl, "I",
 	    "Minimum window size to start an action");
 
@@ -4342,7 +5176,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "window_max", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.window_max, SYNAPTICS_SYSCTL_WINDOW_MAX,
+	    sc, SYNAPTICS_SYSCTL_WINDOW_MAX,
 	    synaptics_sysctl, "I",
 	    "Maximum window size");
 
@@ -4351,7 +5185,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "multiplicator", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.multiplicator, SYNAPTICS_SYSCTL_MULTIPLICATOR,
+	    sc, SYNAPTICS_SYSCTL_MULTIPLICATOR,
 	    synaptics_sysctl, "I",
 	    "Multiplicator to increase precision in averages and divisions");
 
@@ -4360,7 +5194,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "weight_current", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.weight_current, SYNAPTICS_SYSCTL_WEIGHT_CURRENT,
+	    sc, SYNAPTICS_SYSCTL_WEIGHT_CURRENT,
 	    synaptics_sysctl, "I",
 	    "Weight of the current movement in the new average");
 
@@ -4369,7 +5203,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "weight_previous", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.weight_previous, SYNAPTICS_SYSCTL_WEIGHT_PREVIOUS,
+	    sc, SYNAPTICS_SYSCTL_WEIGHT_PREVIOUS,
 	    synaptics_sysctl, "I",
 	    "Weight of the previous average");
 
@@ -4378,8 +5212,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "weight_previous_na", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.weight_previous_na,
-	    SYNAPTICS_SYSCTL_WEIGHT_PREVIOUS_NA,
+	    sc, SYNAPTICS_SYSCTL_WEIGHT_PREVIOUS_NA,
 	    synaptics_sysctl, "I",
 	    "Weight of the previous average (inside the noisy area)");
 
@@ -4388,8 +5221,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "weight_len_squared", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.weight_len_squared,
-	    SYNAPTICS_SYSCTL_WEIGHT_LEN_SQUARED,
+	    sc, SYNAPTICS_SYSCTL_WEIGHT_LEN_SQUARED,
 	    synaptics_sysctl, "I",
 	    "Length (squared) of segments where weight_previous "
 	    "starts to decrease");
@@ -4399,7 +5231,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "div_min", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.div_min, SYNAPTICS_SYSCTL_DIV_MIN,
+	    sc, SYNAPTICS_SYSCTL_DIV_MIN,
 	    synaptics_sysctl, "I",
 	    "Divisor for fast movements");
 
@@ -4408,7 +5240,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "div_max", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.div_max, SYNAPTICS_SYSCTL_DIV_MAX,
+	    sc, SYNAPTICS_SYSCTL_DIV_MAX,
 	    synaptics_sysctl, "I",
 	    "Divisor for slow movements");
 
@@ -4417,7 +5249,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "div_max_na", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.div_max_na, SYNAPTICS_SYSCTL_DIV_MAX_NA,
+	    sc, SYNAPTICS_SYSCTL_DIV_MAX_NA,
 	    synaptics_sysctl, "I",
 	    "Divisor with slow movements (inside the noisy area)");
 
@@ -4426,7 +5258,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "div_len", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.div_len, SYNAPTICS_SYSCTL_DIV_LEN,
+	    sc, SYNAPTICS_SYSCTL_DIV_LEN,
 	    synaptics_sysctl, "I",
 	    "Length of segments where div_max starts to decrease");
 
@@ -4435,7 +5267,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "tap_max_delta", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.tap_max_delta, SYNAPTICS_SYSCTL_TAP_MAX_DELTA,
+	    sc, SYNAPTICS_SYSCTL_TAP_MAX_DELTA,
 	    synaptics_sysctl, "I",
 	    "Length of segments above which a tap is ignored");
 
@@ -4444,17 +5276,17 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "tap_min_queue", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.tap_min_queue, SYNAPTICS_SYSCTL_TAP_MIN_QUEUE,
+	    sc, SYNAPTICS_SYSCTL_TAP_MIN_QUEUE,
 	    synaptics_sysctl, "I",
 	    "Number of packets required to consider a tap");
 
 	/* hw.psm.synaptics.taphold_timeout. */
-	sc->synaction.in_taphold = 0;
+	sc->gesture.in_taphold = 0;
 	sc->syninfo.taphold_timeout = tap_timeout;
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "taphold_timeout", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.taphold_timeout, SYNAPTICS_SYSCTL_TAPHOLD_TIMEOUT,
+	    sc, SYNAPTICS_SYSCTL_TAPHOLD_TIMEOUT,
 	    synaptics_sysctl, "I",
 	    "Maximum elapsed time between two taps to consider a tap-hold "
 	    "action");
@@ -4464,16 +5296,16 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "vscroll_hor_area", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.vscroll_hor_area, SYNAPTICS_SYSCTL_VSCROLL_HOR_AREA,
+	    sc, SYNAPTICS_SYSCTL_VSCROLL_HOR_AREA,
 	    synaptics_sysctl, "I",
 	    "Area reserved for horizontal virtual scrolling");
 
 	/* hw.psm.synaptics.vscroll_ver_area. */
-	sc->syninfo.vscroll_ver_area = -600;
+	sc->syninfo.vscroll_ver_area = -400 - sc->syninfo.margin_right;
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "vscroll_ver_area", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.vscroll_ver_area, SYNAPTICS_SYSCTL_VSCROLL_VER_AREA,
+	    sc, SYNAPTICS_SYSCTL_VSCROLL_VER_AREA,
 	    synaptics_sysctl, "I",
 	    "Area reserved for vertical virtual scrolling");
 
@@ -4482,8 +5314,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "vscroll_min_delta", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.vscroll_min_delta,
-	    SYNAPTICS_SYSCTL_VSCROLL_MIN_DELTA,
+	    sc, SYNAPTICS_SYSCTL_VSCROLL_MIN_DELTA,
 	    synaptics_sysctl, "I",
 	    "Minimum movement to consider virtual scrolling");
 
@@ -4492,7 +5323,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "vscroll_div_min", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.vscroll_div_min, SYNAPTICS_SYSCTL_VSCROLL_DIV_MIN,
+	    sc, SYNAPTICS_SYSCTL_VSCROLL_DIV_MIN,
 	    synaptics_sysctl, "I",
 	    "Divisor for fast scrolling");
 
@@ -4501,7 +5332,7 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "vscroll_div_max", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.vscroll_div_max, SYNAPTICS_SYSCTL_VSCROLL_DIV_MAX,
+	    sc, SYNAPTICS_SYSCTL_VSCROLL_DIV_MAX,
 	    synaptics_sysctl, "I",
 	    "Divisor for slow scrolling");
 
@@ -4510,28 +5341,29 @@ synaptics_sysctl_create_tree(struct psm_softc *sc)
 	SYSCTL_ADD_PROC(&sc->syninfo.sysctl_ctx,
 	    SYSCTL_CHILDREN(sc->syninfo.sysctl_tree), OID_AUTO,
 	    "touchpad_off", CTLTYPE_INT|CTLFLAG_RW|CTLFLAG_ANYBODY,
-	    &sc->syninfo.touchpad_off, SYNAPTICS_SYSCTL_TOUCHPAD_OFF,
+	    sc, SYNAPTICS_SYSCTL_TOUCHPAD_OFF,
 	    synaptics_sysctl, "I",
 	    "Turn off touchpad");
+
+	sc->syninfo.softbuttons_y = 0;
+	sc->syninfo.softbutton2_x = 0;
+	sc->syninfo.softbutton3_x = 0;
+
+	/* skip softbuttons sysctl on not clickpads */
+	if (sc->synhw.capClickPad)
+		synaptics_sysctl_create_softbuttons_tree(sc);
 }
 
 static int
 synaptics_preferred_mode(struct psm_softc *sc) {
 	int mode_byte;
 
-	mode_byte = 0xc0;
+	mode_byte = 0xc4;
 
 	/* request wmode where available */
 	if (sc->synhw.capExtended)
 		mode_byte |= 1;
 
-	/*
-	 * Disable gesture processing when native packets are requested. This
-	 * enables sending of encapsulated "extended W mode" packets.
-	 */
-	if (sc->mode.level == PSM_LEVEL_NATIVE)
-		mode_byte |= (1 << 2);
-
 	return mode_byte;
 }
 
@@ -4546,7 +5378,8 @@ synaptics_set_mode(struct psm_softc *sc, int mode_byte) {
 	 * Enable advanced gestures mode if supported and we are not entering
 	 * passthrough mode.
 	 */
-	if (sc->synhw.capAdvancedGestures && !(mode_byte & (1 << 5))) {
+	if ((sc->synhw.capAdvancedGestures || sc->synhw.capReportsV) &&
+	    !(mode_byte & (1 << 5))) {
 		mouse_ext_command(sc->kbdc, 3);
 		set_mouse_sampling_rate(sc->kbdc, 0xc8);
 	}
@@ -4754,7 +5587,15 @@ enable_synaptics(struct psm_softc *sc, enum probearg arg)
 						     ((status[1] & 0x0f) << 1);
 				synhw.maximumYCoord = (status[2] << 5) |
 						     ((status[1] & 0xf0) >> 3);
+			} else {
+				/*
+				 * Typical bezel limits. Taken from 'Synaptics
+				 * PS/2 * TouchPad Interfacing Guide' p.3.2.3.
+				 */
+				synhw.maximumXCoord = 5472;
+				synhw.maximumYCoord = 4448;
 			}
+
 			if (synhw.capReportsMin) {
 				if (!set_mouse_scaling(kbdc, 1))
 					return (FALSE);
@@ -4767,6 +5608,13 @@ enable_synaptics(struct psm_softc *sc, enum probearg arg)
 						     ((status[1] & 0x0f) << 1);
 				synhw.minimumYCoord = (status[2] << 5) |
 						     ((status[1] & 0xf0) >> 3);
+			} else {
+				/*
+				 * Typical bezel limits. Taken from 'Synaptics
+				 * PS/2 * TouchPad Interfacing Guide' p.3.2.3.
+				 */
+				synhw.minimumXCoord = 1472;
+				synhw.minimumYCoord = 1408;
 			}
 
 			if (verbose >= 2) {
@@ -4854,7 +5702,8 @@ enable_synaptics(struct psm_softc *sc, enum probearg arg)
 
 	if (arg == PROBE) {
 		/* Create sysctl tree. */
-		synaptics_sysctl_create_tree(sc);
+		synaptics_sysctl_create_tree(sc, "synaptics",
+		    "Synaptics TouchPad");
 		sc->hw.buttons = buttons;
 	}
 
@@ -5171,6 +6020,340 @@ enable_versapad(struct psm_softc *sc, enum probearg arg)
 	return (TRUE);				/* PS/2 absolute mode */
 }
 
+/* Elantech Touchpad */
+static int
+elantech_read_1(KBDC kbdc, int hwversion, int reg, int *val)
+{
+	int res, readcmd, retidx;
+	int resp[3];
+
+	readcmd = hwversion == 2 ? ELANTECH_REG_READ : ELANTECH_REG_RDWR;
+	retidx = hwversion == 4 ? 1 : 0;
+
+	res = send_aux_command(kbdc, ELANTECH_CUSTOM_CMD) != PSM_ACK;
+	res |= send_aux_command(kbdc, readcmd) != PSM_ACK;
+	res |= send_aux_command(kbdc, ELANTECH_CUSTOM_CMD) != PSM_ACK;
+	res |= send_aux_command(kbdc, reg) != PSM_ACK;
+	res |= get_mouse_status(kbdc, resp, 0, 3) != 3;
+
+	if (res == 0)
+		*val = resp[retidx];
+
+	return (res);
+}
+
+static int
+elantech_write_1(KBDC kbdc, int hwversion, int reg, int val)
+{
+	int res, writecmd;
+
+	writecmd = hwversion == 2 ? ELANTECH_REG_WRITE : ELANTECH_REG_RDWR;
+
+	res = send_aux_command(kbdc, ELANTECH_CUSTOM_CMD) != PSM_ACK;
+	res |= send_aux_command(kbdc, writecmd) != PSM_ACK;
+	res |= send_aux_command(kbdc, ELANTECH_CUSTOM_CMD) != PSM_ACK;
+	res |= send_aux_command(kbdc, reg) != PSM_ACK;
+	if (hwversion == 4) {
+		res |= send_aux_command(kbdc, ELANTECH_CUSTOM_CMD) != PSM_ACK;
+		res |= send_aux_command(kbdc, writecmd) != PSM_ACK;
+	}
+	res |= send_aux_command(kbdc, ELANTECH_CUSTOM_CMD) != PSM_ACK;
+	res |= send_aux_command(kbdc, val) != PSM_ACK;
+	res |= set_mouse_scaling(kbdc, 1) == 0;
+
+	return (res);
+}
+
+static int
+elantech_cmd(KBDC kbdc, int hwversion, int cmd, int *resp)
+{
+	int res;
+
+	if (hwversion == 2) {
+		res = set_mouse_scaling(kbdc, 1) == 0;
+		res |= mouse_ext_command(kbdc, cmd) == 0;
+	} else {
+		res = send_aux_command(kbdc, ELANTECH_CUSTOM_CMD) != PSM_ACK;
+		res |= send_aux_command(kbdc, cmd) != PSM_ACK;
+	}
+	res |= get_mouse_status(kbdc, resp, 0, 3) != 3;
+
+	return (res);
+}
+
+static int
+elantech_init(KBDC kbdc, elantechhw_t *elanhw)
+{
+	int i, val, res, hwversion, reg10;
+
+	/* set absolute mode */
+	hwversion = elanhw->hwversion;
+	reg10 = -1;
+	switch (hwversion) {
+	case 2:
+		reg10 = elanhw->fwversion == 0x020030 ? 0x54 : 0xc4;
+		res = elantech_write_1(kbdc, hwversion, 0x10, reg10);
+		if (res)
+			break;
+		res = elantech_write_1(kbdc, hwversion, 0x11, 0x8A);
+		break;
+	case 3:
+		reg10 = 0x0b;
+		res = elantech_write_1(kbdc, hwversion, 0x10, reg10);
+		break;
+	case 4:
+		res = elantech_write_1(kbdc, hwversion, 0x07, 0x01);
+		break;
+	default:
+		res = 1;
+	}
+
+	/* Read back reg 0x10 to ensure hardware is ready. */
+	if (res == 0 && reg10 >= 0) {
+		for (i = 0; i < 5; i++) {
+			if (elantech_read_1(kbdc, hwversion, 0x10, &val) == 0)
+				break;
+			DELAY(2000);
+		}
+		if (i == 5)
+			res = 1;
+	}
+
+	if (res)
+		printf("couldn't set absolute mode\n");
+
+	return (res);
+}
+
+static void
+elantech_init_synaptics(struct psm_softc *sc)
+{
+
+	/* Set capabilites required by movement smother */
+	sc->synhw.infoMajor = sc->elanhw.hwversion;
+	sc->synhw.infoMinor = sc->elanhw.fwversion;
+	sc->synhw.infoXupmm = sc->elanhw.dpmmx;
+	sc->synhw.infoYupmm = sc->elanhw.dpmmy;
+	sc->synhw.verticalScroll = 0;
+	sc->synhw.nExtendedQueries = 4;
+	sc->synhw.capExtended = 1;
+	sc->synhw.capPassthrough = sc->elanhw.hastrackpoint;
+	sc->synhw.capClickPad = sc->elanhw.isclickpad;
+	sc->synhw.capMultiFinger = 1;
+	sc->synhw.capAdvancedGestures = 1;
+	sc->synhw.capPalmDetect = 1;
+	sc->synhw.capPen = 0;
+	sc->synhw.capReportsMax = 1;
+	sc->synhw.maximumXCoord = sc->elanhw.sizex;
+	sc->synhw.maximumYCoord = sc->elanhw.sizey;
+	sc->synhw.capReportsMin = 1;
+	sc->synhw.minimumXCoord = 0;
+	sc->synhw.minimumYCoord = 0;
+
+	if (sc->syninfo.sysctl_tree == NULL) {
+		synaptics_sysctl_create_tree(sc, "elantech",
+		    "Elantech Touchpad");
+
+		/*
+		 * Adjust synaptic smoother tunables
+		 * 1. Disable finger detection pressure threshold. Unlike
+		 *    synaptics we assume the finger is acting when packet with
+		 *    its X&Y arrives not when pressure exceedes some threshold
+		 * 2. Disable unrelated features like margins and noisy areas
+		 * 3. Disable virtual scroll areas as 2nd finger is preferable
+		 * 4. For clickpads set bottom quarter as 42% - 16% - 42% sized
+		 *    softbuttons
+		 * 5. Scale down divisors and movement lengths by a factor of 3
+		 *    where 3 is Synaptics to Elantech (~2200/800) dpi ratio
+		 */
+
+		/* Set reporting range to be equal touchpad size */
+		sc->syninfo.max_x = sc->elanhw.sizex;
+		sc->syninfo.max_y = sc->elanhw.sizey;
+
+		/* Disable finger detection pressure threshold */
+		sc->syninfo.min_pressure = 1;
+
+		/* Use full area of touchpad */
+		sc->syninfo.margin_top = 0;
+		sc->syninfo.margin_right = 0;
+		sc->syninfo.margin_bottom = 0;
+		sc->syninfo.margin_left = 0;
+
+		/* Disable noisy area */
+		sc->syninfo.na_top = 0;
+		sc->syninfo.na_right = 0;
+		sc->syninfo.na_bottom = 0;
+		sc->syninfo.na_left = 0;
+
+		/* Tune divisors and movement lengths */
+		sc->syninfo.weight_len_squared = 200;
+		sc->syninfo.div_min = 3;
+		sc->syninfo.div_max = 6;
+		sc->syninfo.div_max_na = 10;
+		sc->syninfo.div_len = 30;
+		sc->syninfo.tap_max_delta = 25;
+
+		/* Disable virtual scrolling areas and tune its divisors */
+		sc->syninfo.vscroll_hor_area = 0;
+		sc->syninfo.vscroll_ver_area = 0;
+		sc->syninfo.vscroll_min_delta = 15;
+		sc->syninfo.vscroll_div_min = 30;
+		sc->syninfo.vscroll_div_max = 50;
+
+		/* Set bottom quarter as 42% - 16% - 42% sized softbuttons */
+		if (sc->elanhw.isclickpad) {
+			sc->syninfo.softbuttons_y = sc->elanhw.sizey / 4;
+			sc->syninfo.softbutton2_x = sc->elanhw.sizex * 11 / 25;
+			sc->syninfo.softbutton3_x = sc->elanhw.sizex * 14 / 25;
+		}
+	}
+
+	return;
+}
+
+static int
+enable_elantech(struct psm_softc *sc, enum probearg arg)
+{
+	static const int ic2hw[] =
+	/*IC: 0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f */
+	    { 0, 0, 2, 0, 2, 3, 4, 4, 4, 4, 4, 4, 4, 4, 4, 0 };
+	elantechhw_t elanhw;
+	int icversion, hwversion, dptracex, dptracey, id, resp[3], dpix, dpiy;
+	KBDC kbdc = sc->kbdc;
+
+	VLOG(3, (LOG_DEBUG, "elantech: BEGIN init\n"));
+
+	set_mouse_scaling(kbdc, 1);
+	set_mouse_scaling(kbdc, 1);
+	set_mouse_scaling(kbdc, 1);
+	if (get_mouse_status(kbdc, resp, 0, 3) != 3)
+		return (FALSE);
+
+	if (!ELANTECH_MAGIC(resp))
+		return (FALSE);
+
+	/* Identify the Touchpad version. */
+	if (elantech_cmd(kbdc, 2, ELANTECH_FW_VERSION, resp))
+		return (FALSE);
+
+	bzero(&elanhw, sizeof(elanhw));
+
+	elanhw.fwversion = (resp[0] << 16) | (resp[1] << 8) | resp[2];
+	icversion = resp[0] & 0x0f;
+	hwversion = ic2hw[icversion];
+
+	if (verbose >= 2)
+		printf("Elantech touchpad hardware v.%d firmware v.0x%06x\n",
+		    hwversion, elanhw.fwversion);
+
+	if (ELANTECH_HW_IS_V1(elanhw.fwversion)) {
+		printf ("  Unsupported touchpad hardware (v1)\n");
+		return (FALSE);
+	}
+	if (hwversion == 0) {
+		printf ("  Unknown touchpad hardware (firmware v.0x%06x)\n",
+		    elanhw.fwversion);
+		return (FALSE);
+	}
+
+	/* Get the Touchpad model information. */
+	elanhw.hwversion = hwversion;
+	elanhw.issemimt = hwversion == 2;
+	elanhw.isclickpad = (resp[1] & 0x10) != 0;
+	elanhw.hascrc = (resp[1] & 0x40) != 0;
+	elanhw.haspressure = elanhw.fwversion >= 0x020800;
+
+	/* Read the capability bits. */
+	if (elantech_cmd(kbdc, hwversion, ELANTECH_CAPABILITIES, resp) != 0) {
+		printf("  Failed to read capability bits\n");
+		return (FALSE);
+	}
+
+	elanhw.ntracesx = resp[1] - 1;
+	elanhw.ntracesy = resp[2] - 1;
+	elanhw.hastrackpoint = (resp[0] & 0x80) != 0;
+
+	/* Get the touchpad resolution */
+	switch (hwversion) {
+	case 4:
+		if (elantech_cmd(kbdc, hwversion, ELANTECH_RESOLUTION, resp)
+		    == 0) {
+			dpix = (resp[1] & 0x0f) * 10 + 790;
+			dpiy = ((resp[1] & 0xf0) >> 4) * 10 + 790;
+			elanhw.dpmmx = (dpix * 10 + 5) / 254;
+			elanhw.dpmmy = (dpiy * 10 + 5) / 254;
+			break;
+		}
+		/* FALLTHROUGH */
+	case 2:
+	case 3:
+		elanhw.dpmmx = elanhw.dpmmy = 32; /* 800 dpi */
+		break;
+	}
+
+	if (!elantech_support)
+		return (FALSE);
+
+	if (elantech_init(kbdc, &elanhw)) {
+		printf("couldn't initialize elantech touchpad\n");
+		return (FALSE);
+	}
+
+	/*
+	 * Get the touchpad reporting range.
+	 * On HW v.3 touchpads it should be done after switching hardware
+	 * to real resolution mode (by setting bit 3 of reg10)
+	 */
+	if (elantech_cmd(kbdc, hwversion, ELANTECH_FW_ID, resp) != 0) {
+		printf("  Failed to read touchpad size\n");
+		elanhw.sizex = 10000; /* Arbitrary high values to     */
+		elanhw.sizey = 10000; /* prevent clipping in smoother */
+	} else if (hwversion == 2) {
+		dptracex = dptracey = 64;
+		if ((elanhw.fwversion >> 16) == 0x14 && (resp[1] & 0x10) &&
+		    !elantech_cmd(kbdc, hwversion, ELANTECH_SAMPLE, resp)) {
+			dptracex = resp[1] / 2;
+			dptracey = resp[2] / 2;
+		}
+		elanhw.sizex = (elanhw.ntracesx - 1) * dptracex;
+		elanhw.sizey = (elanhw.ntracesy - 1) * dptracey;
+	} else {
+		elanhw.sizex = (resp[0] & 0x0f) << 8 | resp[1];
+		elanhw.sizey = (resp[0] & 0xf0) << 4 | resp[2];
+	}
+
+	if (verbose >= 2) {
+		printf("  Model information:\n");
+		printf("   MaxX:       %d\n", elanhw.sizex);
+		printf("   MaxY:       %d\n", elanhw.sizey);
+		printf("   DpmmX:      %d\n", elanhw.dpmmx);
+		printf("   DpmmY:      %d\n", elanhw.dpmmy);
+		printf("   TracesX:    %d\n", elanhw.ntracesx);
+		printf("   TracesY:    %d\n", elanhw.ntracesy);
+		printf("   SemiMT:     %d\n", elanhw.issemimt);
+		printf("   Clickpad:   %d\n", elanhw.isclickpad);
+		printf("   Trackpoint: %d\n", elanhw.hastrackpoint);
+		printf("   CRC:        %d\n", elanhw.hascrc);
+		printf("   Pressure:   %d\n", elanhw.haspressure);
+	}
+
+	VLOG(3, (LOG_DEBUG, "elantech: END init\n"));
+
+	if (arg == PROBE) {
+		sc->elanhw = elanhw;
+		sc->hw.buttons = 3;
+
+		/* Initialize synaptics movement smoother */
+		elantech_init_synaptics(sc);
+
+		for (id = 0; id < ELANTECH_MAX_FINGERS; id++)
+			PSM_FINGER_RESET(sc->elanaction.fingers[id]);
+	}
+
+	return (TRUE);
+}
+
 /*
  * Return true if 'now' is earlier than (start + (secs.usecs)).
  * Now may be NULL and the function will fetch the current time from
diff --git a/sys/dev/hyperv/include/vmbus.h b/sys/dev/hyperv/include/vmbus.h
index 090f9b1fcd93..43831eb4b955 100644
--- a/sys/dev/hyperv/include/vmbus.h
+++ b/sys/dev/hyperv/include/vmbus.h
@@ -133,6 +133,7 @@ int		vmbus_chan_open_br(struct vmbus_channel *chan,
 		    const struct vmbus_chan_br *cbr, const void *udata,
 		    int udlen, vmbus_chan_callback_t cb, void *cbarg);
 void		vmbus_chan_close(struct vmbus_channel *chan);
+void		vmbus_chan_intr_drain(struct vmbus_channel *chan);
 
 int		vmbus_chan_gpadl_connect(struct vmbus_channel *chan,
 		    bus_addr_t paddr, int size, uint32_t *gpadl);
@@ -173,5 +174,7 @@ const struct hyperv_guid *
 		vmbus_chan_guid_inst(const struct vmbus_channel *chan);
 int		vmbus_chan_prplist_nelem(int br_size, int prpcnt_max,
 		    int dlen_max);
+bool		vmbus_chan_rx_empty(const struct vmbus_channel *chan);
+bool		vmbus_chan_tx_empty(const struct vmbus_channel *chan);
 
 #endif	/* !_VMBUS_H_ */
diff --git a/sys/dev/hyperv/netvsc/hv_net_vsc.c b/sys/dev/hyperv/netvsc/hv_net_vsc.c
index 575f0808bf9b..21504c3584c6 100644
--- a/sys/dev/hyperv/netvsc/hv_net_vsc.c
+++ b/sys/dev/hyperv/netvsc/hv_net_vsc.c
@@ -348,6 +348,16 @@ hn_nvs_disconn_rxbuf(struct hn_softc *sc)
 			return (error);
 		}
 		sc->hn_flags &= ~HN_FLAG_RXBUF_CONNECTED;
+
+		/*
+		 * Wait for the hypervisor to receive this NVS request.
+		 */
+		while (!vmbus_chan_tx_empty(sc->hn_prichan))
+			pause("waittx", 1);
+		/*
+		 * Linger long enough for NVS to disconnect RXBUF.
+		 */
+		pause("lingtx", (200 * hz) / 1000);
 	}
 
 	if (sc->hn_rxbuf_gpadl != 0) {
@@ -389,6 +399,17 @@ hn_nvs_disconn_chim(struct hn_softc *sc)
 			return (error);
 		}
 		sc->hn_flags &= ~HN_FLAG_CHIM_CONNECTED;
+
+		/*
+		 * Wait for the hypervisor to receive this NVS request.
+		 */
+		while (!vmbus_chan_tx_empty(sc->hn_prichan))
+			pause("waittx", 1);
+		/*
+		 * Linger long enough for NVS to disconnect chimney
+		 * sending buffer.
+		 */
+		pause("lingtx", (200 * hz) / 1000);
 	}
 
 	if (sc->hn_chim_gpadl != 0) {
@@ -597,27 +618,15 @@ hn_nvs_attach(struct hn_softc *sc, int mtu)
 	return (0);
 }
 
-/*
- * Net VSC disconnect from VSP
- */
-static void
-hv_nv_disconnect_from_vsp(struct hn_softc *sc)
+void
+hn_nvs_detach(struct hn_softc *sc)
 {
+
+	/* NOTE: there are no requests to stop the NVS. */
 	hn_nvs_disconn_rxbuf(sc);
 	hn_nvs_disconn_chim(sc);
 }
 
-/*
- * Net VSC on device remove
- */
-int
-hv_nv_on_device_remove(struct hn_softc *sc)
-{
-	
-	hv_nv_disconnect_from_vsp(sc);
-	return (0);
-}
-
 void
 hn_nvs_sent_xact(struct hn_send_ctx *sndc,
     struct hn_softc *sc __unused, struct vmbus_channel *chan __unused,
diff --git a/sys/dev/hyperv/netvsc/hv_net_vsc.h b/sys/dev/hyperv/netvsc/hv_net_vsc.h
index 6f54446e84ee..ce94f4e0ee6f 100644
--- a/sys/dev/hyperv/netvsc/hv_net_vsc.h
+++ b/sys/dev/hyperv/netvsc/hv_net_vsc.h
@@ -178,6 +178,7 @@ struct hn_tx_ring {
 	bus_dma_tag_t	hn_tx_data_dtag;
 	uint64_t	hn_csum_assist;
 
+	int		hn_suspended;
 	int		hn_gpa_cnt;
 	struct vmbus_gpa hn_gpa[NETVSC_PACKET_MAXPAGE];
 
@@ -264,12 +265,9 @@ struct hn_softc {
 /*
  * Externs
  */
-extern int hv_promisc_mode;
 struct hn_send_ctx;
 
 void netvsc_linkstatus_callback(struct hn_softc *sc, uint32_t status);
-int hn_nvs_attach(struct hn_softc *sc, int mtu);
-int hv_nv_on_device_remove(struct hn_softc *sc);
 int hv_nv_on_send(struct vmbus_channel *chan, uint32_t rndis_mtype,
 	struct hn_send_ctx *sndc, struct vmbus_gpa *gpa, int gpa_cnt);
 
diff --git a/sys/dev/hyperv/netvsc/hv_netvsc_drv_freebsd.c b/sys/dev/hyperv/netvsc/hv_netvsc_drv_freebsd.c
index 2c08a2d22158..f869fe52751e 100644
--- a/sys/dev/hyperv/netvsc/hv_netvsc_drv_freebsd.c
+++ b/sys/dev/hyperv/netvsc/hv_netvsc_drv_freebsd.c
@@ -205,8 +205,6 @@ struct hn_txdesc {
  * Globals
  */
 
-int hv_promisc_mode = 0;    /* normal mode by default */
-
 SYSCTL_NODE(_hw, OID_AUTO, hn, CTLFLAG_RD | CTLFLAG_MPSAFE, NULL,
     "Hyper-V network interface");
 
@@ -348,6 +346,11 @@ static void hn_detach_allchans(struct hn_softc *);
 static void hn_chan_callback(struct vmbus_channel *chan, void *xrxr);
 static void hn_set_ring_inuse(struct hn_softc *, int);
 static int hn_synth_attach(struct hn_softc *, int);
+static void hn_synth_detach(struct hn_softc *);
+static bool hn_tx_ring_pending(struct hn_tx_ring *);
+static void hn_suspend(struct hn_softc *);
+static void hn_resume(struct hn_softc *);
+static void hn_tx_ring_qflush(struct hn_tx_ring *);
 
 static void hn_nvs_handle_notify(struct hn_softc *sc,
 		const struct vmbus_chanpkt_hdr *pkt);
@@ -740,29 +743,19 @@ failed:
 }
 
 /*
- * Standard detach entry point
+ * TODO: Use this for error handling on attach path.
  */
 static int
 netvsc_detach(device_t dev)
 {
 	struct hn_softc *sc = device_get_softc(dev);
 
-	if (bootverbose)
-		printf("netvsc_detach\n");
-
-	/*
-	 * XXXKYS:  Need to clean up all our
-	 * driver state; this is the driver
-	 * unloading.
-	 */
-
-	/*
-	 * XXXKYS:  Need to stop outgoing traffic and unregister
-	 * the netdevice.
-	 */
+	/* TODO: ether_ifdetach */
 
-	hv_rf_on_device_remove(sc);
-	hn_detach_allchans(sc);
+	HN_LOCK(sc);
+	/* TODO: hn_stop */
+	hn_synth_detach(sc);
+	HN_UNLOCK(sc);
 
 	hn_stop_tx_tasks(sc);
 
@@ -775,6 +768,8 @@ netvsc_detach(device_t dev)
 
 	vmbus_xact_ctx_destroy(sc->hn_xact);
 	HN_LOCK_DESTROY(sc);
+
+	/* TODO: if_free */
 	return (0);
 }
 
@@ -905,6 +900,23 @@ hn_txdesc_hold(struct hn_txdesc *txd)
 	atomic_add_int(&txd->refs, 1);
 }
 
+static bool
+hn_tx_ring_pending(struct hn_tx_ring *txr)
+{
+	bool pending = false;
+
+#ifndef HN_USE_TXDESC_BUFRING
+	mtx_lock_spin(&txr->hn_txlist_spin);
+	if (txr->hn_txdesc_avail != txr->hn_txdesc_cnt)
+		pending = true;
+	mtx_unlock_spin(&txr->hn_txlist_spin);
+#else
+	if (!buf_ring_full(txr->hn_txdesc_br))
+		pending = true;
+#endif
+	return (pending);
+}
+
 static __inline void
 hn_txeof(struct hn_tx_ring *txr)
 {
@@ -1241,6 +1253,9 @@ hn_start_locked(struct hn_tx_ring *txr, int len)
 	KASSERT(txr == &sc->hn_tx_ring[0], ("not the first TX ring"));
 	mtx_assert(&txr->hn_tx_lock, MA_OWNED);
 
+	if (__predict_false(txr->hn_suspended))
+		return 0;
+
 	if ((ifp->if_drv_flags & (IFF_DRV_RUNNING | IFF_DRV_OACTIVE)) !=
 	    IFF_DRV_RUNNING)
 		return 0;
@@ -1627,23 +1642,17 @@ hn_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 			hn_set_lro_lenlim(sc, HN_LRO_LENLIM_MIN(ifp));
 #endif
 
-		/* We must remove and add back the device to cause the new
-		 * MTU to take effect.  This includes tearing down, but not
-		 * deleting the channel, then bringing it back up.
-		 */
-		error = hv_rf_on_device_remove(sc);
-		if (error) {
-			HN_UNLOCK(sc);
-			break;
-		}
+		if (ifp->if_drv_flags & IFF_DRV_RUNNING)
+			hn_suspend(sc);
 
 		/*
-		 * Detach all of the channels.
+		 * Detach the synthetics parts, i.e. NVS and RNDIS.
 		 */
-		hn_detach_allchans(sc);
+		hn_synth_detach(sc);
 
 		/*
-		 * Attach the synthetic parts, i.e. NVS and RNDIS.
+		 * Reattach the synthetic parts, i.e. NVS and RNDIS,
+		 * with the new MTU setting.
 		 * XXX check error.
 		 */
 		hn_synth_attach(sc, ifr->ifr_mtu);
@@ -1651,7 +1660,9 @@ hn_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		if (sc->hn_tx_ring[0].hn_chim_size > sc->hn_chim_szmax)
 			hn_set_chim_size(sc, sc->hn_chim_szmax);
 
-		hn_init_locked(sc);
+		/* All done!  Resume now. */
+		if (ifp->if_drv_flags & IFF_DRV_RUNNING)
+			hn_resume(sc);
 
 		HN_UNLOCK(sc);
 		break;
@@ -1766,7 +1777,7 @@ static void
 hn_stop(struct hn_softc *sc)
 {
 	struct ifnet *ifp;
-	int ret, i;
+	int i;
 
 	HN_LOCK_ASSERT(sc);
 
@@ -1782,7 +1793,8 @@ hn_stop(struct hn_softc *sc)
 
 	if_link_state_change(ifp, LINK_STATE_DOWN);
 
-	ret = hv_rf_on_close(sc);
+	/* Disable RX by clearing RX filter. */
+	hn_rndis_set_rxfilter(sc, 0);
 }
 
 /*
@@ -1857,9 +1869,8 @@ hn_init_locked(struct hn_softc *sc)
 		return;
 	}
 
-	hv_promisc_mode = 1;
-
-	ret = hv_rf_on_open(sc);
+	/* TODO: add hn_rx_filter */
+	ret = hn_rndis_set_rxfilter(sc, NDIS_PACKET_TYPE_PROMISCUOUS);
 	if (ret != 0)
 		return;
 
@@ -2984,6 +2995,9 @@ hn_xmit(struct hn_tx_ring *txr, int len)
 	KASSERT(hn_use_if_start == 0,
 	    ("hn_xmit is called, when if_start is enabled"));
 
+	if (__predict_false(txr->hn_suspended))
+		return 0;
+
 	if ((ifp->if_drv_flags & IFF_DRV_RUNNING) == 0 || txr->hn_oactive)
 		return 0;
 
@@ -3070,20 +3084,24 @@ do_sched:
 }
 
 static void
+hn_tx_ring_qflush(struct hn_tx_ring *txr)
+{
+	struct mbuf *m;
+
+	mtx_lock(&txr->hn_tx_lock);
+	while ((m = buf_ring_dequeue_sc(txr->hn_mbuf_br)) != NULL)
+		m_freem(m);
+	mtx_unlock(&txr->hn_tx_lock);
+}
+
+static void
 hn_xmit_qflush(struct ifnet *ifp)
 {
 	struct hn_softc *sc = ifp->if_softc;
 	int i;
 
-	for (i = 0; i < sc->hn_tx_ring_inuse; ++i) {
-		struct hn_tx_ring *txr = &sc->hn_tx_ring[i];
-		struct mbuf *m;
-
-		mtx_lock(&txr->hn_tx_lock);
-		while ((m = buf_ring_dequeue_sc(txr->hn_mbuf_br)) != NULL)
-			m_freem(m);
-		mtx_unlock(&txr->hn_tx_lock);
-	}
+	for (i = 0; i < sc->hn_tx_ring_inuse; ++i)
+		hn_tx_ring_qflush(&sc->hn_tx_ring[i]);
 	if_qflush(ifp);
 }
 
@@ -3484,6 +3502,26 @@ back:
 	return (0);
 }
 
+/*
+ * NOTE:
+ * The interface must have been suspended though hn_suspend(), before
+ * this function get called.
+ */
+static void
+hn_synth_detach(struct hn_softc *sc)
+{
+	HN_LOCK_ASSERT(sc);
+
+	/* Detach the RNDIS first. */
+	hn_rndis_detach(sc);
+
+	/* Detach NVS. */
+	hn_nvs_detach(sc);
+
+	/* Detach all of the channels. */
+	hn_detach_allchans(sc);
+}
+
 static void
 hn_set_ring_inuse(struct hn_softc *sc, int ring_cnt)
 {
@@ -3503,6 +3541,114 @@ hn_set_ring_inuse(struct hn_softc *sc, int ring_cnt)
 }
 
 static void
+hn_rx_drain(struct vmbus_channel *chan)
+{
+
+	while (!vmbus_chan_rx_empty(chan) || !vmbus_chan_tx_empty(chan))
+		pause("waitch", 1);
+	vmbus_chan_intr_drain(chan);
+}
+
+static void
+hn_suspend(struct hn_softc *sc)
+{
+	struct vmbus_channel **subch = NULL;
+	int i, nsubch;
+
+	HN_LOCK_ASSERT(sc);
+
+	/*
+	 * Suspend TX.
+	 */
+	for (i = 0; i < sc->hn_tx_ring_inuse; ++i) {
+		struct hn_tx_ring *txr = &sc->hn_tx_ring[i];
+
+		mtx_lock(&txr->hn_tx_lock);
+		txr->hn_suspended = 1;
+		mtx_unlock(&txr->hn_tx_lock);
+		/* No one is able send more packets now. */
+
+		/* Wait for all pending sends to finish. */
+		while (hn_tx_ring_pending(txr))
+			pause("hnwtx", 1 /* 1 tick */);
+	}
+
+	/*
+	 * Disable RX by clearing RX filter.
+	 */
+	hn_rndis_set_rxfilter(sc, 0);
+
+	/*
+	 * Give RNDIS enough time to flush all pending data packets.
+	 */
+	pause("waitrx", (200 * hz) / 1000);
+
+	/*
+	 * Drain RX/TX bufrings and interrupts.
+	 */
+	nsubch = sc->hn_rx_ring_inuse - 1;
+	if (nsubch > 0)
+		subch = vmbus_subchan_get(sc->hn_prichan, nsubch);
+
+	if (subch != NULL) {
+		for (i = 0; i < nsubch; ++i)
+			hn_rx_drain(subch[i]);
+	}
+	hn_rx_drain(sc->hn_prichan);
+
+	if (subch != NULL)
+		vmbus_subchan_rel(subch, nsubch);
+}
+
+static void
+hn_resume(struct hn_softc *sc)
+{
+	struct hn_tx_ring *txr;
+	int i;
+
+	HN_LOCK_ASSERT(sc);
+
+	/*
+	 * Re-enable RX.
+	 * TODO: add hn_rx_filter.
+	 */
+	hn_rndis_set_rxfilter(sc, NDIS_PACKET_TYPE_PROMISCUOUS);
+
+	/*
+	 * Make sure to clear suspend status on "all" TX rings,
+	 * since hn_tx_ring_inuse can be changed after hn_suspend().
+	 */
+	for (i = 0; i < sc->hn_tx_ring_cnt; ++i) {
+		txr = &sc->hn_tx_ring[i];
+
+		mtx_lock(&txr->hn_tx_lock);
+		txr->hn_suspended = 0;
+		mtx_unlock(&txr->hn_tx_lock);
+	}
+
+	if (!hn_use_if_start) {
+		/*
+		 * Flush unused drbrs, since hn_tx_ring_inuse may be
+		 * reduced.
+		 */
+		for (i = sc->hn_tx_ring_inuse; i < sc->hn_tx_ring_cnt; ++i)
+			hn_tx_ring_qflush(&sc->hn_tx_ring[i]);
+	}
+
+	/*
+	 * Kick start TX.
+	 */
+	for (i = 0; i < sc->hn_tx_ring_inuse; ++i) {
+		txr = &sc->hn_tx_ring[i];
+		/*
+		 * Use txeof task, so that any pending oactive can be
+		 * cleared properly.
+		 */
+		taskqueue_enqueue(txr->hn_tx_taskq, &txr->hn_txeof_task);
+	}
+}
+
+static void
 hn_nvs_handle_notify(struct hn_softc *sc, const struct vmbus_chanpkt_hdr *pkt)
 {
 	const struct hn_nvs_hdr *hdr;
diff --git a/sys/dev/hyperv/netvsc/hv_rndis_filter.c b/sys/dev/hyperv/netvsc/hv_rndis_filter.c
index 451e7d7150d4..ddd07d551616 100644
--- a/sys/dev/hyperv/netvsc/hv_rndis_filter.c
+++ b/sys/dev/hyperv/netvsc/hv_rndis_filter.c
@@ -887,7 +887,7 @@ hn_rndis_conf_rss(struct hn_softc *sc, uint16_t flags)
 	return (error);
 }
 
-static int
+int
 hn_rndis_set_rxfilter(struct hn_softc *sc, uint32_t filter)
 {
 	int error;
@@ -957,11 +957,8 @@ done:
 	return (error);
 }
 
-/*
- * RNDIS filter halt device
- */
 static int
-hv_rf_halt_device(struct hn_softc *sc)
+hn_rndis_halt(struct hn_softc *sc)
 {
 	struct vmbus_xact *xact;
 	struct rndis_halt_req *halt;
@@ -1008,50 +1005,12 @@ hn_rndis_attach(struct hn_softc *sc)
 	return (0);
 }
 
-/*
- * RNDIS filter on device remove
- */
-int
-hv_rf_on_device_remove(struct hn_softc *sc)
-{
-	int ret;
-
-	/* Halt and release the rndis device */
-	ret = hv_rf_halt_device(sc);
-
-	/* Pass control to inner driver to remove the device */
-	ret |= hv_nv_on_device_remove(sc);
-
-	return (ret);
-}
-
-/*
- * RNDIS filter on open
- */
-int
-hv_rf_on_open(struct hn_softc *sc)
-{
-	uint32_t filter;
-
-	/* XXX */
-	if (hv_promisc_mode != 1) {
-		filter = NDIS_PACKET_TYPE_BROADCAST |
-		    NDIS_PACKET_TYPE_ALL_MULTICAST |
-		    NDIS_PACKET_TYPE_DIRECTED;
-	} else {
-		filter = NDIS_PACKET_TYPE_PROMISCUOUS;
-	}
-	return (hn_rndis_set_rxfilter(sc, filter));
-}
-
-/*
- * RNDIS filter on close
- */
-int 
-hv_rf_on_close(struct hn_softc *sc)
+void
+hn_rndis_detach(struct hn_softc *sc)
 {
 
-	return (hn_rndis_set_rxfilter(sc, 0));
+	/* Halt the RNDIS. */
+	hn_rndis_halt(sc);
 }
 
 void
diff --git a/sys/dev/hyperv/netvsc/hv_rndis_filter.h b/sys/dev/hyperv/netvsc/hv_rndis_filter.h
index f389cedf283e..3ecda3b80d03 100644
--- a/sys/dev/hyperv/netvsc/hv_rndis_filter.h
+++ b/sys/dev/hyperv/netvsc/hv_rndis_filter.h
@@ -43,9 +43,6 @@ struct hn_rx_ring;
 void hv_rf_on_receive(struct hn_softc *sc, struct hn_rx_ring *rxr,
     const void *data, int dlen);
 void hv_rf_channel_rollup(struct hn_rx_ring *rxr, struct hn_tx_ring *txr);
-int hv_rf_on_device_remove(struct hn_softc *sc);
-int hv_rf_on_open(struct hn_softc *sc);
-int hv_rf_on_close(struct hn_softc *sc);
 
 #endif  /* __HV_RNDIS_FILTER_H__ */
 
diff --git a/sys/dev/hyperv/netvsc/if_hnvar.h b/sys/dev/hyperv/netvsc/if_hnvar.h
index 38ff16ee72c9..e48033c35ab3 100644
--- a/sys/dev/hyperv/netvsc/if_hnvar.h
+++ b/sys/dev/hyperv/netvsc/if_hnvar.h
@@ -118,6 +118,7 @@ uint32_t	hn_chim_alloc(struct hn_softc *sc);
 void		hn_chim_free(struct hn_softc *sc, uint32_t chim_idx);
 
 int		hn_rndis_attach(struct hn_softc *sc);
+void		hn_rndis_detach(struct hn_softc *sc);
 int		hn_rndis_conf_rss(struct hn_softc *sc, uint16_t flags);
 void		*hn_rndis_pktinfo_append(struct rndis_packet_msg *,
 		    size_t pktsize, size_t pi_dlen, uint32_t pi_type);
@@ -125,8 +126,11 @@ int		hn_rndis_get_rsscaps(struct hn_softc *sc, int *rxr_cnt);
 int		hn_rndis_get_eaddr(struct hn_softc *sc, uint8_t *eaddr);
 int		hn_rndis_get_linkstatus(struct hn_softc *sc,
 		    uint32_t *link_status);
+/* filter: NDIS_PACKET_TYPE_ or 0. */
+int		hn_rndis_set_rxfilter(struct hn_softc *sc, uint32_t filter);
 
 int		hn_nvs_attach(struct hn_softc *sc, int mtu);
+void		hn_nvs_detach(struct hn_softc *sc);
 int		hn_nvs_alloc_subchans(struct hn_softc *sc, int *nsubch);
 void		hn_nvs_sent_xact(struct hn_send_ctx *sndc, struct hn_softc *sc,
 		    struct vmbus_channel *chan, const void *data, int dlen);
diff --git a/sys/dev/hyperv/vmbus/vmbus.c b/sys/dev/hyperv/vmbus/vmbus.c
index 6753c9e9b524..d9cb81ba3f66 100644
--- a/sys/dev/hyperv/vmbus/vmbus.c
+++ b/sys/dev/hyperv/vmbus/vmbus.c
@@ -83,9 +83,7 @@ static int			vmbus_connect(struct vmbus_softc *, uint32_t);
 static int			vmbus_req_channels(struct vmbus_softc *sc);
 static void			vmbus_disconnect(struct vmbus_softc *);
 static int			vmbus_scan(struct vmbus_softc *);
-static void			vmbus_scan_wait(struct vmbus_softc *);
-static void			vmbus_scan_newchan(struct vmbus_softc *);
-static void			vmbus_scan_newdev(struct vmbus_softc *);
+static void			vmbus_scan_teardown(struct vmbus_softc *);
 static void			vmbus_scan_done(struct vmbus_softc *,
 				    const struct vmbus_message *);
 static void			vmbus_chanmsg_handle(struct vmbus_softc *,
@@ -393,50 +391,22 @@ vmbus_req_channels(struct vmbus_softc *sc)
 }
 
 static void
-vmbus_scan_newchan(struct vmbus_softc *sc)
+vmbus_scan_done_task(void *xsc, int pending __unused)
 {
-	mtx_lock(&sc->vmbus_scan_lock);
-	if ((sc->vmbus_scan_chcnt & VMBUS_SCAN_CHCNT_DONE) == 0)
-		sc->vmbus_scan_chcnt++;
-	mtx_unlock(&sc->vmbus_scan_lock);
+	struct vmbus_softc *sc = xsc;
+
+	mtx_lock(&Giant);
+	sc->vmbus_scandone = true;
+	mtx_unlock(&Giant);
+	wakeup(&sc->vmbus_scandone);
 }
 
 static void
 vmbus_scan_done(struct vmbus_softc *sc,
     const struct vmbus_message *msg __unused)
 {
-	mtx_lock(&sc->vmbus_scan_lock);
-	sc->vmbus_scan_chcnt |= VMBUS_SCAN_CHCNT_DONE;
-	mtx_unlock(&sc->vmbus_scan_lock);
-	wakeup(&sc->vmbus_scan_chcnt);
-}
 
-static void
-vmbus_scan_newdev(struct vmbus_softc *sc)
-{
-	mtx_lock(&sc->vmbus_scan_lock);
-	sc->vmbus_scan_devcnt++;
-	mtx_unlock(&sc->vmbus_scan_lock);
-	wakeup(&sc->vmbus_scan_devcnt);
-}
-
-static void
-vmbus_scan_wait(struct vmbus_softc *sc)
-{
-	uint32_t chancnt;
-
-	mtx_lock(&sc->vmbus_scan_lock);
-	while ((sc->vmbus_scan_chcnt & VMBUS_SCAN_CHCNT_DONE) == 0) {
-		mtx_sleep(&sc->vmbus_scan_chcnt, &sc->vmbus_scan_lock, 0,
-		    "waitch", 0);
-	}
-	chancnt = sc->vmbus_scan_chcnt & ~VMBUS_SCAN_CHCNT_DONE;
-
-	while (sc->vmbus_scan_devcnt != chancnt) {
-		mtx_sleep(&sc->vmbus_scan_devcnt, &sc->vmbus_scan_lock, 0,
-		    "waitdev", 0);
-	}
-	mtx_unlock(&sc->vmbus_scan_lock);
+	taskqueue_enqueue(sc->vmbus_devtq, &sc->vmbus_scandone_task);
 }
 
 static int
@@ -445,31 +415,71 @@ vmbus_scan(struct vmbus_softc *sc)
 	int error;
 
 	/*
+	 * Identify, probe and attach for non-channel devices.
+	 */
+	bus_generic_probe(sc->vmbus_dev);
+	bus_generic_attach(sc->vmbus_dev);
+
+	/*
+	 * This taskqueue serializes vmbus devices' attach and detach
+	 * for channel offer and rescind messages.
+	 */
+	sc->vmbus_devtq = taskqueue_create("vmbus dev", M_WAITOK,
+	    taskqueue_thread_enqueue, &sc->vmbus_devtq);
+	taskqueue_start_threads(&sc->vmbus_devtq, 1, PI_NET, "vmbusdev");
+	TASK_INIT(&sc->vmbus_scandone_task, 0, vmbus_scan_done_task, sc);
+
+	/*
+	 * This taskqueue handles sub-channel detach, so that vmbus
+	 * device's detach running in vmbus_devtq can drain its sub-
+	 * channels.
+	 */
+	sc->vmbus_subchtq = taskqueue_create("vmbus subch", M_WAITOK,
+	    taskqueue_thread_enqueue, &sc->vmbus_subchtq);
+	taskqueue_start_threads(&sc->vmbus_subchtq, 1, PI_NET, "vmbussch");
+
+	/*
 	 * Start vmbus scanning.
 	 */
 	error = vmbus_req_channels(sc);
 	if (error) {
 		device_printf(sc->vmbus_dev, "channel request failed: %d\n",
 		    error);
-		return error;
+		return (error);
 	}
 
 	/*
-	 * Wait for all devices are added to vmbus.
+	 * Wait for all vmbus devices from the initial channel offers to be
+	 * attached.
 	 */
-	vmbus_scan_wait(sc);
-
-	/*
-	 * Identify, probe and attach.
-	 */
-	bus_generic_probe(sc->vmbus_dev);
-	bus_generic_attach(sc->vmbus_dev);
+	GIANT_REQUIRED;
+	while (!sc->vmbus_scandone)
+		mtx_sleep(&sc->vmbus_scandone, &Giant, 0, "vmbusdev", 0);
 
 	if (bootverbose) {
 		device_printf(sc->vmbus_dev, "device scan, probe and attach "
 		    "done\n");
 	}
-	return 0;
+	return (0);
+}
+
+static void
+vmbus_scan_teardown(struct vmbus_softc *sc)
+{
+
+	GIANT_REQUIRED;
+	if (sc->vmbus_devtq != NULL) {
+		mtx_unlock(&Giant);
+		taskqueue_free(sc->vmbus_devtq);
+		mtx_lock(&Giant);
+		sc->vmbus_devtq = NULL;
+	}
+	if (sc->vmbus_subchtq != NULL) {
+		mtx_unlock(&Giant);
+		taskqueue_free(sc->vmbus_subchtq);
+		mtx_lock(&Giant);
+		sc->vmbus_subchtq = NULL;
+	}
 }
 
 static void
@@ -918,45 +928,35 @@ vmbus_add_child(struct vmbus_channel *chan)
 {
 	struct vmbus_softc *sc = chan->ch_vmbus;
 	device_t parent = sc->vmbus_dev;
-	int error = 0;
 
-	/* New channel has been offered */
-	vmbus_scan_newchan(sc);
+	mtx_lock(&Giant);
 
 	chan->ch_dev = device_add_child(parent, NULL, -1);
 	if (chan->ch_dev == NULL) {
+		mtx_unlock(&Giant);
 		device_printf(parent, "device_add_child for chan%u failed\n",
 		    chan->ch_id);
-		error = ENXIO;
-		goto done;
+		return (ENXIO);
 	}
 	device_set_ivars(chan->ch_dev, chan);
+	device_probe_and_attach(chan->ch_dev);
 
-done:
-	/* New device has been/should be added to vmbus. */
-	vmbus_scan_newdev(sc);
-	return error;
+	mtx_unlock(&Giant);
+	return (0);
 }
 
 int
 vmbus_delete_child(struct vmbus_channel *chan)
 {
-	int error;
-
-	if (chan->ch_dev == NULL) {
-		/* Failed to add a device. */
-		return 0;
-	}
+	int error = 0;
 
-	/*
-	 * XXXKYS: Ensure that this is the opposite of
-	 * device_add_child()
-	 */
 	mtx_lock(&Giant);
-	error = device_delete_child(chan->ch_vmbus->vmbus_dev, chan->ch_dev);
+	if (chan->ch_dev != NULL) {
+		error = device_delete_child(chan->ch_vmbus->vmbus_dev,
+		    chan->ch_dev);
+	}
 	mtx_unlock(&Giant);
-
-	return error;
+	return (error);
 }
 
 static int
@@ -1028,10 +1028,11 @@ vmbus_doattach(struct vmbus_softc *sc)
 		return (0);
 	sc->vmbus_flags |= VMBUS_FLAG_ATTACHED;
 
-	mtx_init(&sc->vmbus_scan_lock, "vmbus scan", NULL, MTX_DEF);
 	sc->vmbus_gpadl = VMBUS_GPADL_START;
 	mtx_init(&sc->vmbus_prichan_lock, "vmbus prichan", NULL, MTX_DEF);
 	TAILQ_INIT(&sc->vmbus_prichans);
+	mtx_init(&sc->vmbus_chan_lock, "vmbus channel", NULL, MTX_DEF);
+	TAILQ_INIT(&sc->vmbus_chans);
 	sc->vmbus_chmap = malloc(
 	    sizeof(struct vmbus_channel *) * VMBUS_CHAN_MAX, M_DEVBUF,
 	    M_WAITOK | M_ZERO);
@@ -1095,6 +1096,7 @@ vmbus_doattach(struct vmbus_softc *sc)
 	return (ret);
 
 cleanup:
+	vmbus_scan_teardown(sc);
 	vmbus_intr_teardown(sc);
 	vmbus_dma_free(sc);
 	if (sc->vmbus_xc != NULL) {
@@ -1102,8 +1104,8 @@ cleanup:
 		sc->vmbus_xc = NULL;
 	}
 	free(sc->vmbus_chmap, M_DEVBUF);
-	mtx_destroy(&sc->vmbus_scan_lock);
 	mtx_destroy(&sc->vmbus_prichan_lock);
+	mtx_destroy(&sc->vmbus_chan_lock);
 
 	return (ret);
 }
@@ -1146,8 +1148,11 @@ vmbus_detach(device_t dev)
 {
 	struct vmbus_softc *sc = device_get_softc(dev);
 
+	bus_generic_detach(dev);
 	vmbus_chan_destroy_all(sc);
 
+	vmbus_scan_teardown(sc);
+
 	vmbus_disconnect(sc);
 
 	if (sc->vmbus_flags & VMBUS_FLAG_SYNIC) {
@@ -1164,8 +1169,8 @@ vmbus_detach(device_t dev)
 	}
 
 	free(sc->vmbus_chmap, M_DEVBUF);
-	mtx_destroy(&sc->vmbus_scan_lock);
 	mtx_destroy(&sc->vmbus_prichan_lock);
+	mtx_destroy(&sc->vmbus_chan_lock);
 
 	return (0);
 }
diff --git a/sys/dev/hyperv/vmbus/vmbus_brvar.h b/sys/dev/hyperv/vmbus/vmbus_brvar.h
index bd967f6e869b..cbec3ebdd501 100644
--- a/sys/dev/hyperv/vmbus/vmbus_brvar.h
+++ b/sys/dev/hyperv/vmbus/vmbus_brvar.h
@@ -83,6 +83,20 @@ vmbus_txbr_maxpktsz(const struct vmbus_txbr *tbr)
 	return (tbr->txbr_dsize - sizeof(uint64_t) - 1);
 }
 
+static __inline bool
+vmbus_txbr_empty(const struct vmbus_txbr *tbr)
+{
+
+	return (tbr->txbr_windex == tbr->txbr_rindex ? true : false);
+}
+
+static __inline bool
+vmbus_rxbr_empty(const struct vmbus_rxbr *rbr)
+{
+
+	return (rbr->rxbr_windex == rbr->rxbr_rindex ? true : false);
+}
+
 static __inline int
 vmbus_br_nelem(int br_size, int elem_size)
 {
diff --git a/sys/dev/hyperv/vmbus/vmbus_chan.c b/sys/dev/hyperv/vmbus/vmbus_chan.c
index cfc71211782d..e7e194715273 100644
--- a/sys/dev/hyperv/vmbus/vmbus_chan.c
+++ b/sys/dev/hyperv/vmbus/vmbus_chan.c
@@ -59,10 +59,30 @@ static struct vmbus_channel	*vmbus_chan_alloc(struct vmbus_softc *);
 static void			vmbus_chan_free(struct vmbus_channel *);
 static int			vmbus_chan_add(struct vmbus_channel *);
 static void			vmbus_chan_cpu_default(struct vmbus_channel *);
+static int			vmbus_chan_release(struct vmbus_channel *);
+static void			vmbus_chan_set_chmap(struct vmbus_channel *);
+static void			vmbus_chan_clear_chmap(struct vmbus_channel *);
+
+static void			vmbus_chan_ins_prilist(struct vmbus_softc *,
+				    struct vmbus_channel *);
+static void			vmbus_chan_rem_prilist(struct vmbus_softc *,
+				    struct vmbus_channel *);
+static void			vmbus_chan_ins_list(struct vmbus_softc *,
+				    struct vmbus_channel *);
+static void			vmbus_chan_rem_list(struct vmbus_softc *,
+				    struct vmbus_channel *);
+static void			vmbus_chan_ins_sublist(struct vmbus_channel *,
+				    struct vmbus_channel *);
+static void			vmbus_chan_rem_sublist(struct vmbus_channel *,
+				    struct vmbus_channel *);
 
 static void			vmbus_chan_task(void *, int);
 static void			vmbus_chan_task_nobatch(void *, int);
-static void			vmbus_chan_detach_task(void *, int);
+static void			vmbus_chan_clrchmap_task(void *, int);
+static void			vmbus_prichan_attach_task(void *, int);
+static void			vmbus_subchan_attach_task(void *, int);
+static void			vmbus_prichan_detach_task(void *, int);
+static void			vmbus_subchan_detach_task(void *, int);
 
 static void			vmbus_chan_msgproc_choffer(struct vmbus_softc *,
 				    const struct vmbus_message *);
@@ -96,6 +116,83 @@ vmbus_chan_signal_tx(const struct vmbus_channel *chan)
 		hypercall_signal_event(chan->ch_monprm_dma.hv_paddr);
 }
 
+static void
+vmbus_chan_ins_prilist(struct vmbus_softc *sc, struct vmbus_channel *chan)
+{
+
+	mtx_assert(&sc->vmbus_prichan_lock, MA_OWNED);
+	if (atomic_testandset_int(&chan->ch_stflags,
+	    VMBUS_CHAN_ST_ONPRIL_SHIFT))
+		panic("channel is already on the prilist");
+	TAILQ_INSERT_TAIL(&sc->vmbus_prichans, chan, ch_prilink);
+}
+
+static void
+vmbus_chan_rem_prilist(struct vmbus_softc *sc, struct vmbus_channel *chan)
+{
+
+	mtx_assert(&sc->vmbus_prichan_lock, MA_OWNED);
+	if (atomic_testandclear_int(&chan->ch_stflags,
+	    VMBUS_CHAN_ST_ONPRIL_SHIFT) == 0)
+		panic("channel is not on the prilist");
+	TAILQ_REMOVE(&sc->vmbus_prichans, chan, ch_prilink);
+}
+
+static void
+vmbus_chan_ins_sublist(struct vmbus_channel *prichan,
+    struct vmbus_channel *chan)
+{
+
+	mtx_assert(&prichan->ch_subchan_lock, MA_OWNED);
+
+	if (atomic_testandset_int(&chan->ch_stflags,
+	    VMBUS_CHAN_ST_ONSUBL_SHIFT))
+		panic("channel is already on the sublist");
+	TAILQ_INSERT_TAIL(&prichan->ch_subchans, chan, ch_sublink);
+
+	/* Bump sub-channel count. */
+	prichan->ch_subchan_cnt++;
+}
+
+static void
+vmbus_chan_rem_sublist(struct vmbus_channel *prichan,
+    struct vmbus_channel *chan)
+{
+
+	mtx_assert(&prichan->ch_subchan_lock, MA_OWNED);
+
+	KASSERT(prichan->ch_subchan_cnt > 0,
+	    ("invalid subchan_cnt %d", prichan->ch_subchan_cnt));
+	prichan->ch_subchan_cnt--;
+
+	if (atomic_testandclear_int(&chan->ch_stflags,
+	    VMBUS_CHAN_ST_ONSUBL_SHIFT) == 0)
+		panic("channel is not on the sublist");
+	TAILQ_REMOVE(&prichan->ch_subchans, chan, ch_sublink);
+}
+
+static void
+vmbus_chan_ins_list(struct vmbus_softc *sc, struct vmbus_channel *chan)
+{
+
+	mtx_assert(&sc->vmbus_chan_lock, MA_OWNED);
+	if (atomic_testandset_int(&chan->ch_stflags,
+	    VMBUS_CHAN_ST_ONLIST_SHIFT))
+		panic("channel is already on the list");
+	TAILQ_INSERT_TAIL(&sc->vmbus_chans, chan, ch_link);
+}
+
+static void
+vmbus_chan_rem_list(struct vmbus_softc *sc, struct vmbus_channel *chan)
+{
+
+	mtx_assert(&sc->vmbus_chan_lock, MA_OWNED);
+	if (atomic_testandclear_int(&chan->ch_stflags,
+	    VMBUS_CHAN_ST_ONLIST_SHIFT) == 0)
+		panic("channel is not on the list");
+	TAILQ_REMOVE(&sc->vmbus_chans, chan, ch_link);
+}
+
 static int
 vmbus_chan_sysctl_mnf(SYSCTL_HANDLER_ARGS)
 {
@@ -235,6 +332,7 @@ vmbus_chan_open_br(struct vmbus_channel *chan, const struct vmbus_chan_br *cbr,
 	struct vmbus_msghc *mh;
 	uint32_t status;
 	int error, txbr_size, rxbr_size;
+	task_fn_t *task_fn;
 	uint8_t *br;
 
 	if (udlen > VMBUS_CHANMSG_CHOPEN_UDATA_SIZE) {
@@ -269,9 +367,10 @@ vmbus_chan_open_br(struct vmbus_channel *chan, const struct vmbus_chan_br *cbr,
 
 	chan->ch_tq = VMBUS_PCPU_GET(chan->ch_vmbus, event_tq, chan->ch_cpuid);
 	if (chan->ch_flags & VMBUS_CHAN_FLAG_BATCHREAD)
-		TASK_INIT(&chan->ch_task, 0, vmbus_chan_task, chan);
+		task_fn = vmbus_chan_task;
 	else
-		TASK_INIT(&chan->ch_task, 0, vmbus_chan_task_nobatch, chan);
+		task_fn = vmbus_chan_task_nobatch;
+	TASK_INIT(&chan->ch_task, 0, task_fn, chan);
 
 	/* TX bufring comes first */
 	vmbus_txbr_setup(&chan->ch_txbr, br, txbr_size);
@@ -293,6 +392,12 @@ vmbus_chan_open_br(struct vmbus_channel *chan, const struct vmbus_chan_br *cbr,
 	}
 
 	/*
+	 * Install this channel, before it is opened, but after everything
+	 * else has been setup.
+	 */
+	vmbus_chan_set_chmap(chan);
+
+	/*
 	 * Open channel w/ the bufring GPADL on the target CPU.
 	 */
 	mh = vmbus_msghc_get(sc, sizeof(*req));
@@ -341,6 +446,7 @@ vmbus_chan_open_br(struct vmbus_channel *chan, const struct vmbus_chan_br *cbr,
 	error = ENXIO;
 
 failed:
+	vmbus_chan_clear_chmap(chan);
 	if (chan->ch_bufring_gpadl) {
 		vmbus_chan_gpadl_disconnect(chan, chan->ch_bufring_gpadl);
 		chan->ch_bufring_gpadl = 0;
@@ -517,12 +623,38 @@ vmbus_chan_gpadl_disconnect(struct vmbus_channel *chan, uint32_t gpadl)
 }
 
 static void
+vmbus_chan_clrchmap_task(void *xchan, int pending __unused)
+{
+	struct vmbus_channel *chan = xchan;
+
+	critical_enter();
+	chan->ch_vmbus->vmbus_chmap[chan->ch_id] = NULL;
+	critical_exit();
+}
+
+static void
+vmbus_chan_clear_chmap(struct vmbus_channel *chan)
+{
+	struct task chmap_task;
+
+	TASK_INIT(&chmap_task, 0, vmbus_chan_clrchmap_task, chan);
+	taskqueue_enqueue(chan->ch_tq, &chmap_task);
+	taskqueue_drain(chan->ch_tq, &chmap_task);
+}
+
+static void
+vmbus_chan_set_chmap(struct vmbus_channel *chan)
+{
+	__compiler_membar();
+	chan->ch_vmbus->vmbus_chmap[chan->ch_id] = chan;
+}
+
+static void
 vmbus_chan_close_internal(struct vmbus_channel *chan)
 {
 	struct vmbus_softc *sc = chan->ch_vmbus;
 	struct vmbus_msghc *mh;
 	struct vmbus_chanmsg_chclose *req;
-	struct taskqueue *tq = chan->ch_tq;
 	int error;
 
 	/* TODO: stringent check */
@@ -535,12 +667,14 @@ vmbus_chan_close_internal(struct vmbus_channel *chan)
 	sysctl_ctx_free(&chan->ch_sysctl_ctx);
 
 	/*
-	 * Set ch_tq to NULL to avoid more requests be scheduled.
-	 * XXX pretty broken; need rework.
+	 * NOTE:
+	 * Order is critical.  This channel _must_ be uninstalled first,
+	 * else the channel task may be enqueued by the IDT after it has
+	 * been drained.
 	 */
+	vmbus_chan_clear_chmap(chan);
+	taskqueue_drain(chan->ch_tq, &chan->ch_task);
 	chan->ch_tq = NULL;
-	taskqueue_drain(tq, &chan->ch_task);
-	chan->ch_cb = NULL;
 
 	/*
 	 * Close this channel.
@@ -622,6 +756,13 @@ vmbus_chan_close(struct vmbus_channel *chan)
 	vmbus_chan_close_internal(chan);
 }
 
+void
+vmbus_chan_intr_drain(struct vmbus_channel *chan)
+{
+
+	taskqueue_drain(chan->ch_tq, &chan->ch_task);
+}
+
 int
 vmbus_chan_send(struct vmbus_channel *chan, uint16_t type, uint16_t flags,
     void *data, int dlen, uint64_t xactid)
@@ -884,10 +1025,11 @@ vmbus_event_flags_proc(struct vmbus_softc *sc, volatile u_long *event_flags,
 			flags &= ~(1UL << chid_ofs);
 
 			chan = sc->vmbus_chmap[chid_base + chid_ofs];
-
-			/* if channel is closed or closing */
-			if (chan == NULL || chan->ch_tq == NULL)
+			if (__predict_false(chan == NULL)) {
+				/* Channel is closed. */
 				continue;
+			}
+			__compiler_membar();
 
 			if (chan->ch_flags & VMBUS_CHAN_FLAG_BATCHREAD)
 				vmbus_rxbr_intr_mask(&chan->ch_rxbr);
@@ -968,7 +1110,6 @@ vmbus_chan_alloc(struct vmbus_softc *sc)
 	chan->ch_vmbus = sc;
 	mtx_init(&chan->ch_subchan_lock, "vmbus subchan", NULL, MTX_DEF);
 	TAILQ_INIT(&chan->ch_subchans);
-	TASK_INIT(&chan->ch_detach_task, 0, vmbus_chan_detach_task, chan);
 	vmbus_rxbr_init(&chan->ch_rxbr);
 	vmbus_txbr_init(&chan->ch_txbr);
 
@@ -978,9 +1119,14 @@ vmbus_chan_alloc(struct vmbus_softc *sc)
 static void
 vmbus_chan_free(struct vmbus_channel *chan)
 {
-	/* TODO: assert sub-channel list is empty */
-	/* TODO: asset no longer on the primary channel's sub-channel list */
-	/* TODO: asset no longer on the vmbus channel list */
+
+	KASSERT(TAILQ_EMPTY(&chan->ch_subchans) && chan->ch_subchan_cnt == 0,
+	    ("still owns sub-channels"));
+	KASSERT((chan->ch_stflags &
+	    (VMBUS_CHAN_ST_OPENED |
+	     VMBUS_CHAN_ST_ONPRIL |
+	     VMBUS_CHAN_ST_ONSUBL |
+	     VMBUS_CHAN_ST_ONLIST)) == 0, ("free busy channel"));
 	hyperv_dmamem_free(&chan->ch_monprm_dma, chan->ch_monprm);
 	mtx_destroy(&chan->ch_subchan_lock);
 	vmbus_rxbr_deinit(&chan->ch_rxbr);
@@ -1007,7 +1153,6 @@ vmbus_chan_add(struct vmbus_channel *newchan)
 		    newchan->ch_id);
 		return EINVAL;
 	}
-	sc->vmbus_chmap[newchan->ch_id] = newchan;
 
 	if (bootverbose) {
 		device_printf(sc->vmbus_dev, "chan%u subidx%u offer\n",
@@ -1029,10 +1174,9 @@ vmbus_chan_add(struct vmbus_channel *newchan)
 	if (VMBUS_CHAN_ISPRIMARY(newchan)) {
 		if (prichan == NULL) {
 			/* Install the new primary channel */
-			TAILQ_INSERT_TAIL(&sc->vmbus_prichans, newchan,
-			    ch_prilink);
+			vmbus_chan_ins_prilist(sc, newchan);
 			mtx_unlock(&sc->vmbus_prichan_lock);
-			return 0;
+			goto done;
 		} else {
 			mtx_unlock(&sc->vmbus_prichan_lock);
 			device_printf(sc->vmbus_dev, "duplicated primary "
@@ -1066,16 +1210,20 @@ vmbus_chan_add(struct vmbus_channel *newchan)
 	newchan->ch_dev = prichan->ch_dev;
 
 	mtx_lock(&prichan->ch_subchan_lock);
-	TAILQ_INSERT_TAIL(&prichan->ch_subchans, newchan, ch_sublink);
+	vmbus_chan_ins_sublist(prichan, newchan);
+	mtx_unlock(&prichan->ch_subchan_lock);
 	/*
-	 * Bump up sub-channel count and notify anyone that is
-	 * interested in this sub-channel, after this sub-channel
-	 * is setup.
+	 * Notify anyone that is interested in this sub-channel,
+	 * after this sub-channel is setup.
 	 */
-	prichan->ch_subchan_cnt++;
-	mtx_unlock(&prichan->ch_subchan_lock);
 	wakeup(prichan);
-
+done:
+	/*
+	 * Hook this channel up for later rescind.
+	 */
+	mtx_lock(&sc->vmbus_chan_lock);
+	vmbus_chan_ins_list(sc, newchan);
+	mtx_unlock(&sc->vmbus_chan_lock);
 	return 0;
 }
 
@@ -1126,6 +1274,7 @@ vmbus_chan_msgproc_choffer(struct vmbus_softc *sc,
 {
 	const struct vmbus_chanmsg_choffer *offer;
 	struct vmbus_channel *chan;
+	task_fn_t *detach_fn, *attach_fn;
 	int error;
 
 	offer = (const struct vmbus_chanmsg_choffer *)msg->msg_data;
@@ -1174,6 +1323,21 @@ vmbus_chan_msgproc_choffer(struct vmbus_softc *sc,
 	    &sc->vmbus_tx_evtflags[chan->ch_id >> VMBUS_EVTFLAG_SHIFT];
 	chan->ch_evtflag_mask = 1UL << (chan->ch_id & VMBUS_EVTFLAG_MASK);
 
+	/*
+	 * Setup attach and detach tasks.
+	 */
+	if (VMBUS_CHAN_ISPRIMARY(chan)) {
+		chan->ch_mgmt_tq = sc->vmbus_devtq;
+		attach_fn = vmbus_prichan_attach_task;
+		detach_fn = vmbus_prichan_detach_task;
+	} else {
+		chan->ch_mgmt_tq = sc->vmbus_subchtq;
+		attach_fn = vmbus_subchan_attach_task;
+		detach_fn = vmbus_subchan_detach_task;
+	}
+	TASK_INIT(&chan->ch_attach_task, 0, attach_fn, chan);
+	TASK_INIT(&chan->ch_detach_task, 0, detach_fn, chan);
+
 	/* Select default cpu for this channel. */
 	vmbus_chan_cpu_default(chan);
 
@@ -1184,22 +1348,9 @@ vmbus_chan_msgproc_choffer(struct vmbus_softc *sc,
 		vmbus_chan_free(chan);
 		return;
 	}
-
-	if (VMBUS_CHAN_ISPRIMARY(chan)) {
-		/*
-		 * Add device for this primary channel.
-		 *
-		 * NOTE:
-		 * Error is ignored here; don't have much to do if error
-		 * really happens.
-		 */
-		vmbus_add_child(chan);
-	}
+	taskqueue_enqueue(chan->ch_mgmt_tq, &chan->ch_attach_task);
 }
 
-/*
- * XXX pretty broken; need rework.
- */
 static void
 vmbus_chan_msgproc_chrescind(struct vmbus_softc *sc,
     const struct vmbus_message *msg)
@@ -1219,91 +1370,162 @@ vmbus_chan_msgproc_chrescind(struct vmbus_softc *sc,
 		    note->chm_chanid);
 	}
 
-	chan = sc->vmbus_chmap[note->chm_chanid];
-	if (chan == NULL)
+	/*
+	 * Find and remove the target channel from the channel list.
+	 */
+	mtx_lock(&sc->vmbus_chan_lock);
+	TAILQ_FOREACH(chan, &sc->vmbus_chans, ch_link) {
+		if (chan->ch_id == note->chm_chanid)
+			break;
+	}
+	if (chan == NULL) {
+		mtx_unlock(&sc->vmbus_chan_lock);
+		device_printf(sc->vmbus_dev, "chan%u is not offered\n",
+		    note->chm_chanid);
 		return;
-	sc->vmbus_chmap[note->chm_chanid] = NULL;
+	}
+	vmbus_chan_rem_list(sc, chan);
+	mtx_unlock(&sc->vmbus_chan_lock);
+
+	if (VMBUS_CHAN_ISPRIMARY(chan)) {
+		/*
+		 * The target channel is a primary channel; remove the
+		 * target channel from the primary channel list now,
+		 * instead of later, so that it will not be found by
+		 * other sub-channel offers, which are processed in
+		 * this thread.
+		 */
+		mtx_lock(&sc->vmbus_prichan_lock);
+		vmbus_chan_rem_prilist(sc, chan);
+		mtx_unlock(&sc->vmbus_prichan_lock);
+	}
 
-	taskqueue_enqueue(taskqueue_thread, &chan->ch_detach_task);
+	/* Detach the target channel. */
+	taskqueue_enqueue(chan->ch_mgmt_tq, &chan->ch_detach_task);
 }
 
-static void
-vmbus_chan_detach_task(void *xchan, int pending __unused)
+static int
+vmbus_chan_release(struct vmbus_channel *chan)
 {
-	struct vmbus_channel *chan = xchan;
+	struct vmbus_softc *sc = chan->ch_vmbus;
+	struct vmbus_chanmsg_chfree *req;
+	struct vmbus_msghc *mh;
+	int error;
 
-	if (VMBUS_CHAN_ISPRIMARY(chan)) {
-		/* Only primary channel owns the device */
-		vmbus_delete_child(chan);
-		/* NOTE: DO NOT free primary channel for now */
-	} else {
-		struct vmbus_softc *sc = chan->ch_vmbus;
-		struct vmbus_channel *pri_chan = chan->ch_prichan;
-		struct vmbus_chanmsg_chfree *req;
-		struct vmbus_msghc *mh;
-		int error;
+	mh = vmbus_msghc_get(sc, sizeof(*req));
+	if (mh == NULL) {
+		device_printf(sc->vmbus_dev, "can not get msg hypercall for "
+		    "chfree(chan%u)\n", chan->ch_id);
+		return (ENXIO);
+	}
+
+	req = vmbus_msghc_dataptr(mh);
+	req->chm_hdr.chm_type = VMBUS_CHANMSG_TYPE_CHFREE;
+	req->chm_chanid = chan->ch_id;
+
+	error = vmbus_msghc_exec_noresult(mh);
+	vmbus_msghc_put(sc, mh);
 
-		mh = vmbus_msghc_get(sc, sizeof(*req));
-		if (mh == NULL) {
-			device_printf(sc->vmbus_dev,
-			    "can not get msg hypercall for chfree(chan%u)\n",
+	if (error) {
+		device_printf(sc->vmbus_dev, "chfree(chan%u) failed: %d",
+		    chan->ch_id, error);
+	} else {
+		if (bootverbose) {
+			device_printf(sc->vmbus_dev, "chan%u freed\n",
 			    chan->ch_id);
-			goto remove;
 		}
+	}
+	return (error);
+}
 
-		req = vmbus_msghc_dataptr(mh);
-		req->chm_hdr.chm_type = VMBUS_CHANMSG_TYPE_CHFREE;
-		req->chm_chanid = chan->ch_id;
+static void
+vmbus_prichan_detach_task(void *xchan, int pending __unused)
+{
+	struct vmbus_channel *chan = xchan;
 
-		error = vmbus_msghc_exec_noresult(mh);
-		vmbus_msghc_put(sc, mh);
+	KASSERT(VMBUS_CHAN_ISPRIMARY(chan),
+	    ("chan%u is not primary channel", chan->ch_id));
 
-		if (error) {
-			device_printf(sc->vmbus_dev,
-			    "chfree(chan%u) failed: %d",
-			    chan->ch_id, error);
-			/* NOTE: Move on! */
-		} else {
-			if (bootverbose) {
-				device_printf(sc->vmbus_dev, "chan%u freed\n",
-				    chan->ch_id);
-			}
-		}
-remove:
-		mtx_lock(&pri_chan->ch_subchan_lock);
-		TAILQ_REMOVE(&pri_chan->ch_subchans, chan, ch_sublink);
-		KASSERT(pri_chan->ch_subchan_cnt > 0,
-		    ("invalid subchan_cnt %d", pri_chan->ch_subchan_cnt));
-		pri_chan->ch_subchan_cnt--;
-		mtx_unlock(&pri_chan->ch_subchan_lock);
-		wakeup(pri_chan);
+	/* Delete and detach the device associated with this channel. */
+	vmbus_delete_child(chan);
 
-		vmbus_chan_free(chan);
-	}
+	/* Release this channel (back to vmbus). */
+	vmbus_chan_release(chan);
+
+	/* Free this channel's resource. */
+	vmbus_chan_free(chan);
+}
+
+static void
+vmbus_subchan_detach_task(void *xchan, int pending __unused)
+{
+	struct vmbus_channel *chan = xchan;
+	struct vmbus_channel *pri_chan = chan->ch_prichan;
+
+	KASSERT(!VMBUS_CHAN_ISPRIMARY(chan),
+	    ("chan%u is primary channel", chan->ch_id));
+
+	/* Release this channel (back to vmbus). */
+	vmbus_chan_release(chan);
+
+	/* Unlink from its primary channel's sub-channel list. */
+	mtx_lock(&pri_chan->ch_subchan_lock);
+	vmbus_chan_rem_sublist(pri_chan, chan);
+	mtx_unlock(&pri_chan->ch_subchan_lock);
+	/* Notify anyone that is waiting for this sub-channel to vanish. */
+	wakeup(pri_chan);
+
+	/* Free this channel's resource. */
+	vmbus_chan_free(chan);
+}
+
+static void
+vmbus_prichan_attach_task(void *xchan, int pending __unused)
+{
+
+	/*
+	 * Add device for this primary channel.
+	 */
+	vmbus_add_child(xchan);
+}
+
+static void
+vmbus_subchan_attach_task(void *xchan __unused, int pending __unused)
+{
+
+	/* Nothing */
 }
 
-/*
- * Detach all devices and destroy the corresponding primary channels.
- */
 void
 vmbus_chan_destroy_all(struct vmbus_softc *sc)
 {
-	struct vmbus_channel *chan;
 
-	mtx_lock(&sc->vmbus_prichan_lock);
-	while ((chan = TAILQ_FIRST(&sc->vmbus_prichans)) != NULL) {
-		KASSERT(VMBUS_CHAN_ISPRIMARY(chan), ("not primary channel"));
-		TAILQ_REMOVE(&sc->vmbus_prichans, chan, ch_prilink);
-		mtx_unlock(&sc->vmbus_prichan_lock);
+	/*
+	 * Detach all devices and destroy the corresponding primary
+	 * channels.
+	 */
+	for (;;) {
+		struct vmbus_channel *chan;
 
-		vmbus_delete_child(chan);
-		vmbus_chan_free(chan);
+		mtx_lock(&sc->vmbus_chan_lock);
+		TAILQ_FOREACH(chan, &sc->vmbus_chans, ch_link) {
+			if (VMBUS_CHAN_ISPRIMARY(chan))
+				break;
+		}
+		if (chan == NULL) {
+			/* No more primary channels; done. */
+			mtx_unlock(&sc->vmbus_chan_lock);
+			break;
+		}
+		vmbus_chan_rem_list(sc, chan);
+		mtx_unlock(&sc->vmbus_chan_lock);
 
 		mtx_lock(&sc->vmbus_prichan_lock);
+		vmbus_chan_rem_prilist(sc, chan);
+		mtx_unlock(&sc->vmbus_prichan_lock);
+
+		taskqueue_enqueue(chan->ch_mgmt_tq, &chan->ch_detach_task);
 	}
-	bzero(sc->vmbus_chmap,
-	    sizeof(struct vmbus_channel *) * VMBUS_CHAN_MAX);
-	mtx_unlock(&sc->vmbus_prichan_lock);
 }
 
 /*
@@ -1477,3 +1699,17 @@ vmbus_chan_prplist_nelem(int br_size, int prpcnt_max, int dlen_max)
 
 	return (vmbus_br_nelem(br_size, elem_size));
 }
+
+bool
+vmbus_chan_tx_empty(const struct vmbus_channel *chan)
+{
+
+	return (vmbus_txbr_empty(&chan->ch_txbr));
+}
+
+bool
+vmbus_chan_rx_empty(const struct vmbus_channel *chan)
+{
+
+	return (vmbus_rxbr_empty(&chan->ch_rxbr));
+}
diff --git a/sys/dev/hyperv/vmbus/vmbus_chanvar.h b/sys/dev/hyperv/vmbus/vmbus_chanvar.h
index 68a134d3f2fb..b415d0079eb3 100644
--- a/sys/dev/hyperv/vmbus/vmbus_chanvar.h
+++ b/sys/dev/hyperv/vmbus/vmbus_chanvar.h
@@ -124,8 +124,14 @@ struct vmbus_channel {
 	struct hyperv_dma		ch_bufring_dma;
 	uint32_t			ch_bufring_gpadl;
 
-	struct task			ch_detach_task;
+	struct task			ch_attach_task;	/* run in ch_mgmt_tq */
+	struct task			ch_detach_task;	/* run in ch_mgmt_tq */
+	struct taskqueue		*ch_mgmt_tq;
+
+	/* If this is a primary channel */
 	TAILQ_ENTRY(vmbus_channel)	ch_prilink;	/* primary chan link */
+
+	TAILQ_ENTRY(vmbus_channel)	ch_link;	/* channel link */
 	uint32_t			ch_subidx;	/* subchan index */
 	volatile uint32_t		ch_stflags;	/* atomic-op */
 							/* VMBUS_CHAN_ST_ */
@@ -150,7 +156,13 @@ struct vmbus_channel {
 #define VMBUS_CHAN_TXF_HASMNF		0x0001
 
 #define VMBUS_CHAN_ST_OPENED_SHIFT	0
+#define VMBUS_CHAN_ST_ONPRIL_SHIFT	1
+#define VMBUS_CHAN_ST_ONSUBL_SHIFT	2
+#define VMBUS_CHAN_ST_ONLIST_SHIFT	3
 #define VMBUS_CHAN_ST_OPENED		(1 << VMBUS_CHAN_ST_OPENED_SHIFT)
+#define VMBUS_CHAN_ST_ONPRIL		(1 << VMBUS_CHAN_ST_ONPRIL_SHIFT)
+#define VMBUS_CHAN_ST_ONSUBL		(1 << VMBUS_CHAN_ST_ONSUBL_SHIFT)
+#define VMBUS_CHAN_ST_ONLIST		(1 << VMBUS_CHAN_ST_ONLIST_SHIFT)
 
 struct vmbus_softc;
 struct vmbus_message;
diff --git a/sys/dev/hyperv/vmbus/vmbus_var.h b/sys/dev/hyperv/vmbus/vmbus_var.h
index 47d9004e5719..df4553a9e401 100644
--- a/sys/dev/hyperv/vmbus/vmbus_var.h
+++ b/sys/dev/hyperv/vmbus/vmbus_var.h
@@ -107,14 +107,19 @@ struct vmbus_softc {
 	struct hyperv_dma	vmbus_mnf1_dma;
 	struct hyperv_dma	vmbus_mnf2_dma;
 
-	struct mtx		vmbus_scan_lock;
-	uint32_t		vmbus_scan_chcnt;
-#define VMBUS_SCAN_CHCNT_DONE	0x80000000
-	uint32_t		vmbus_scan_devcnt;
+	bool			vmbus_scandone;
+	struct task		vmbus_scandone_task;
+
+	struct taskqueue	*vmbus_devtq;	/* for dev attach/detach */
+	struct taskqueue	*vmbus_subchtq;	/* for sub-chan attach/detach */
 
 	/* Primary channels */
 	struct mtx		vmbus_prichan_lock;
 	TAILQ_HEAD(, vmbus_channel) vmbus_prichans;
+
+	/* Complete channel list */
+	struct mtx		vmbus_chan_lock;
+	TAILQ_HEAD(, vmbus_channel) vmbus_chans;
 };
 
 #define VMBUS_FLAG_ATTACHED	0x0001	/* vmbus was attached */
diff --git a/sys/dev/pci/pci.c b/sys/dev/pci/pci.c
index 665f62e0911e..8de548f0e2cc 100644
--- a/sys/dev/pci/pci.c
+++ b/sys/dev/pci/pci.c
@@ -5000,6 +5000,7 @@ pci_reserve_map(device_t dev, device_t child, int type, int *rid,
 	struct resource_list *rl = &dinfo->resources;
 	struct resource *res;
 	struct pci_map *pm;
+	uint16_t cmd;
 	pci_addr_t map, testval;
 	int mapsize;
 
@@ -5089,8 +5090,17 @@ pci_reserve_map(device_t dev, device_t child, int type, int *rid,
 		device_printf(child,
 		    "Lazy allocation of %#jx bytes rid %#x type %d at %#jx\n",
 		    count, *rid, type, rman_get_start(res));
+
+	/* Disable decoding via the CMD register before updating the BAR */
+	cmd = pci_read_config(child, PCIR_COMMAND, 2);
+	pci_write_config(child, PCIR_COMMAND,
+	    cmd & ~(PCI_BAR_MEM(map) ? PCIM_CMD_MEMEN : PCIM_CMD_PORTEN), 2);
+
 	map = rman_get_start(res);
 	pci_write_bar(child, pm, map);
+
+	/* Restore the original value of the CMD register */
+	pci_write_config(child, PCIR_COMMAND, cmd, 2);
 out:
 	return (res);
 }
diff --git a/sys/dev/usb/wlan/if_rsu.c b/sys/dev/usb/wlan/if_rsu.c
index c08b4af5dc74..4ee9decf4e2b 100644
--- a/sys/dev/usb/wlan/if_rsu.c
+++ b/sys/dev/usb/wlan/if_rsu.c
@@ -1270,9 +1270,12 @@ rsu_newstate(struct ieee80211vap *vap, enum ieee80211_state nstate, int arg)
 	default:
 		break;
 	}
-	sc->sc_calibrating = 1;
-	/* Start periodic calibration. */
-	taskqueue_enqueue_timeout(taskqueue_thread, &sc->calib_task, hz);
+	if (startcal != 0) {
+		sc->sc_calibrating = 1;
+		/* Start periodic calibration. */
+		taskqueue_enqueue_timeout(taskqueue_thread, &sc->calib_task,
+		    hz);
+	}
 	RSU_UNLOCK(sc);
 	IEEE80211_LOCK(ic);
 	return (uvp->newstate(vap, nstate, arg));
diff --git a/sys/i386/i386/db_disasm.c b/sys/i386/i386/db_disasm.c
index 2b398da8e96c..b3a67636dd08 100644
--- a/sys/i386/i386/db_disasm.c
+++ b/sys/i386/i386/db_disasm.c
@@ -31,6 +31,7 @@ __FBSDID("$FreeBSD$");
  * Instruction disassembler.
  */
 #include <sys/param.h>
+#include <sys/kdb.h>
 
 #include <ddb/ddb.h>
 #include <ddb/db_access.h>
@@ -883,6 +884,7 @@ struct i_addr {
 	const char *	base;
 	const char *	index;
 	int		ss;
+	bool		defss;	/* set if %ss is the default segment */
 };
 
 static const char * const db_index_reg_16[8] = {
@@ -954,10 +956,12 @@ db_read_address(loc, short_addr, regmodrm, addrp)
 	}
 	addrp->is_reg = FALSE;
 	addrp->index = NULL;
+	addrp->ss = 0;
+	addrp->defss = FALSE;
 
 	if (short_addr) {
-	    addrp->index = NULL;
-	    addrp->ss = 0;
+	    if (rm == 2 || rm == 3 || (rm == 6 && mod != 0))
+		addrp->defss = TRUE;
 	    switch (mod) {
 		case 0:
 		    if (rm == 6) {
@@ -984,7 +988,7 @@ db_read_address(loc, short_addr, regmodrm, addrp)
 	    }
 	}
 	else {
-	    if (mod != 3 && rm == 4) {
+	    if (rm == 4) {
 		get_value_inc(sib, loc, 1, FALSE);
 		rm = sib_base(sib);
 		index = sib_index(sib);
@@ -1035,6 +1039,9 @@ db_print_address(seg, size, addrp)
 	if (seg) {
 	    db_printf("%s:", seg);
 	}
+	else if (addrp->defss) {
+	    db_printf("%%ss:");
+	}
 
 	db_printsym((db_addr_t)addrp->disp, DB_STGY_ANY);
 	if (addrp->base != NULL || addrp->index != NULL) {
@@ -1168,9 +1175,17 @@ db_disasm(db_addr_t loc, bool altfmt)
 	int	len;
 	struct i_addr	address;
 
+	if (db_segsize(kdb_frame) == 16)
+	   altfmt = !altfmt;
 	get_value_inc(inst, loc, 1, FALSE);
-	short_addr = FALSE;
-	size = LONG;
+	if (altfmt) {
+	    short_addr = TRUE;
+	    size = WORD;
+	}
+	else {
+	    short_addr = FALSE;
+	    size = LONG;
+	}
 	seg = NULL;
 
 	/*
@@ -1180,11 +1195,11 @@ db_disasm(db_addr_t loc, bool altfmt)
 	prefix = TRUE;
 	do {
 	    switch (inst) {
-		case 0x66:		/* data16 */
-		    size = WORD;
+		case 0x66:
+		    size = (altfmt ? LONG : WORD);
 		    break;
 		case 0x67:
-		    short_addr = TRUE;
+		    short_addr = !altfmt;
 		    break;
 		case 0x26:
 		    seg = "%es";
diff --git a/sys/i386/i386/db_interface.c b/sys/i386/i386/db_interface.c
index 7079e5688b85..3c0ac813ff65 100644
--- a/sys/i386/i386/db_interface.c
+++ b/sys/i386/i386/db_interface.c
@@ -135,6 +135,30 @@ db_write_bytes(vm_offset_t addr, size_t size, char *data)
 	return (ret);
 }
 
+int
+db_segsize(struct trapframe *tfp)
+{
+	struct proc_ldt *plp;
+	struct segment_descriptor *sdp;
+	int sel;
+
+	if (tfp == NULL)
+	    return (32);
+	if (tfp->tf_eflags & PSL_VM)
+	    return (16);
+	sel = tfp->tf_cs & 0xffff;
+	if (sel == GSEL(GCODE_SEL, SEL_KPL))
+	    return (32);
+	/* Rare cases follow.  User mode cases are currently unreachable. */
+	if (ISLDT(sel)) {
+	    plp = curthread->td_proc->p_md.md_ldt;
+	    sdp = (plp != NULL) ? &plp->ldt_sd : &ldt[0].sd;
+	} else {
+	    sdp = &gdt[PCPU_GET(cpuid) * NGDT].sd;
+	}
+	return (sdp[IDXSEL(sel)].sd_def32 == 0 ? 16 : 32);
+}
+
 void
 db_show_mdpcpu(struct pcpu *pc)
 {
diff --git a/sys/i386/i386/db_trace.c b/sys/i386/i386/db_trace.c
index 4918ed232dd3..4f5f4130f2a4 100644
--- a/sys/i386/i386/db_trace.c
+++ b/sys/i386/i386/db_trace.c
@@ -421,6 +421,17 @@ db_backtrace(struct thread *td, struct trapframe *tf, struct i386_frame *frame,
 	int instr, narg;
 	boolean_t first;
 
+	if (db_segsize(tf) == 16) {
+		db_printf(
+"--- 16-bit%s, cs:eip = %#x:%#x, ss:esp = %#x:%#x, ebp = %#x, tf = %p ---\n",
+		    (tf->tf_eflags & PSL_VM) ? " (vm86)" : "",
+		    tf->tf_cs, tf->tf_eip,
+		    TF_HAS_STACKREGS(tf) ? tf->tf_ss : rss(),
+		    TF_HAS_STACKREGS(tf) ? tf->tf_esp : (intptr_t)&tf->tf_esp,
+		    tf->tf_ebp, tf);
+		return (0);
+	}
+
 	/*
 	 * If an indirect call via an invalid pointer caused a trap,
 	 * %pc contains the invalid address while the return address
diff --git a/sys/i386/i386/machdep.c b/sys/i386/i386/machdep.c
index c88c6140a41a..9200d00401f5 100644
--- a/sys/i386/i386/machdep.c
+++ b/sys/i386/i386/machdep.c
@@ -2089,7 +2089,6 @@ getmemsize(int first)
 	 * use that and do not make any VM86 calls.
 	 */
 	physmap_idx = 0;
-	smapbase = NULL;
 	kmdp = preload_search_by_type("elf kernel");
 	if (kmdp == NULL)
 		kmdp = preload_search_by_type("elf32 kernel");
@@ -2223,6 +2222,9 @@ physmap_done:
 	 * highest page of the physical address space.  It should be
 	 * called something like "Maxphyspage".  We may adjust this 
 	 * based on ``hw.physmem'' and the results of the memory test.
+	 *
+	 * This is especially confusing when it is much larger than the
+	 * memory size and is displayed as "realmem".
 	 */
 	Maxmem = atop(physmap[physmap_idx + 1]);
 
@@ -2428,6 +2430,19 @@ do_next:
 }
 #endif /* PC98 */
 
+static void
+i386_kdb_init(void)
+{
+#ifdef DDB
+	db_fetch_ksymtab(bootinfo.bi_symtab, bootinfo.bi_esymtab);
+#endif
+	kdb_init();
+#ifdef KDB
+	if (boothowto & RB_KDB)
+		kdb_enter(KDB_WHY_BOOTFLAGS, "Boot flags requested debugger");
+#endif
+}
+
 register_t
 init386(first)
 	int first;
@@ -2438,6 +2453,7 @@ init386(first)
 #ifdef CPU_ENABLE_SSE
 	struct xstate_hdr *xhdr;
 #endif
+	int late_console;
 
 	thread0.td_kstack = proc0kstack;
 	thread0.td_kstack_pages = TD0_KSTACK_PAGES;
@@ -2502,6 +2518,7 @@ init386(first)
 	first += DPCPU_SIZE;
 	PCPU_SET(prvspace, pc);
 	PCPU_SET(curthread, &thread0);
+	/* Non-late cninit() and printf() can be moved up to here. */
 
 	/*
 	 * Initialize mutexes.
@@ -2668,30 +2685,33 @@ init386(first)
 #endif
 #endif
 
+	/*
+	 * The console and kdb should be initialized even earlier than here,
+	 * but some console drivers don't work until after getmemsize().
+	 * Default to late console initialization to support these drivers.
+	 * This loses mainly printf()s in getmemsize() and early debugging.
+	 */
+	late_console = 1;
+	TUNABLE_INT_FETCH("debug.late_console", &late_console);
+	if (!late_console) {
+		cninit();
+		i386_kdb_init();
+	}
+
 	vm86_initialize();
 	getmemsize(first);
 	init_param2(physmem);
 
 	/* now running on new page tables, configured,and u/iom is accessible */
 
-	/*
-	 * Initialize the console before we print anything out.
-	 */
-	cninit();
+	if (late_console)
+		cninit();
 
 	if (metadata_missing)
 		printf("WARNING: loader(8) metadata is missing!\n");
 
-#ifdef DDB
-	db_fetch_ksymtab(bootinfo.bi_symtab, bootinfo.bi_esymtab);
-#endif
-
-	kdb_init();
-
-#ifdef KDB
-	if (boothowto & RB_KDB)
-		kdb_enter(KDB_WHY_BOOTFLAGS, "Boot flags requested debugger");
-#endif
+	if (late_console)
+		i386_kdb_init();
 
 	msgbufinit(msgbufp, msgbufsize);
 #ifdef DEV_NPX
diff --git a/sys/i386/i386/sys_machdep.c b/sys/i386/i386/sys_machdep.c
index 9c8d94b44b81..bbf631781d1d 100644
--- a/sys/i386/i386/sys_machdep.c
+++ b/sys/i386/i386/sys_machdep.c
@@ -438,7 +438,7 @@ user_ldt_alloc(struct mdproc *mdp, int len)
 
 	new_ldt->ldt_len = len = NEW_MAX_LD(len);
 	new_ldt->ldt_base = (caddr_t)kmem_malloc(kernel_arena,
-	    len * sizeof(union descriptor), M_WAITOK);
+	    len * sizeof(union descriptor), M_WAITOK | M_ZERO);
 	new_ldt->ldt_refcnt = 1;
 	new_ldt->ldt_active = 0;
 
diff --git a/sys/i386/include/db_machdep.h b/sys/i386/include/db_machdep.h
index 3ab06486a3fb..e8d1d8eab95e 100644
--- a/sys/i386/include/db_machdep.h
+++ b/sys/i386/include/db_machdep.h
@@ -98,4 +98,6 @@ do {						\
 #define	DB_SMALL_VALUE_MAX	0x7fffffff
 #define	DB_SMALL_VALUE_MIN	(-0x400001)
 
+int	db_segsize(struct trapframe *tfp);
+
 #endif /* !_MACHINE_DB_MACHDEP_H_ */
diff --git a/sys/kern/kern_descrip.c b/sys/kern/kern_descrip.c
index 55d5e441f483..3312b8876df0 100644
--- a/sys/kern/kern_descrip.c
+++ b/sys/kern/kern_descrip.c
@@ -1761,8 +1761,8 @@ falloc_noinstall(struct thread *td, struct file **resultfp)
 	    priv_check(td, PRIV_MAXFILES) != 0) ||
 	    openfiles >= maxfiles) {
 		if (ppsratecheck(&lastfail, &curfail, 1)) {
-			printf("kern.maxfiles limit exceeded by uid %i, "
-			    "please see tuning(7).\n", td->td_ucred->cr_ruid);
+			printf("kern.maxfiles limit exceeded by uid %i, (%s) "
+			    "please see tuning(7).\n", td->td_ucred->cr_ruid, td->td_proc->p_comm);
 		}
 		return (ENFILE);
 	}
diff --git a/sys/kern/kern_mutex.c b/sys/kern/kern_mutex.c
index 69f8a0c766b9..bd053c7bb7be 100644
--- a/sys/kern/kern_mutex.c
+++ b/sys/kern/kern_mutex.c
@@ -924,7 +924,7 @@ __mtx_assert(const volatile uintptr_t *c, int what, const char *file, int line)
 {
 	const struct mtx *m;
 
-	if (panicstr != NULL || dumping)
+	if (panicstr != NULL || dumping || SCHEDULER_STOPPED())
 		return;
 
 	m = mtxlock2mtx(c);
diff --git a/sys/mips/mips/pm_machdep.c b/sys/mips/mips/pm_machdep.c
index 02f915eb512c..577d1104690a 100644
--- a/sys/mips/mips/pm_machdep.c
+++ b/sys/mips/mips/pm_machdep.c
@@ -143,6 +143,7 @@ sendsig(sig_t catcher, ksiginfo_t *ksi, sigset_t *mask)
 		/* sf.sf_ahu.sf_action = (__siginfohandler_t *)catcher; */
 
 		/* fill siginfo structure */
+		sf.sf_si = ksi->ksi_info;
 		sf.sf_si.si_signo = sig;
 		sf.sf_si.si_code = ksi->ksi_code;
 		sf.sf_si.si_addr = (void*)(intptr_t)regs->badvaddr;
diff --git a/sys/mips/mips/trap.c b/sys/mips/mips/trap.c
index e071e34bef6f..e14b12be2139 100644
--- a/sys/mips/mips/trap.c
+++ b/sys/mips/mips/trap.c
@@ -741,8 +741,11 @@ dofault:
 				}
 				goto err;
 			}
-			ucode = ftype;
-			i = ((rv == KERN_PROTECTION_FAILURE) ? SIGBUS : SIGSEGV);
+			i = SIGSEGV;
+			if (rv == KERN_PROTECTION_FAILURE)
+				ucode = SEGV_ACCERR;
+			else
+				ucode = SEGV_MAPERR;
 			addr = trapframe->pc;
 
 			msg = "BAD_PAGE_FAULT";
diff --git a/sys/modules/opensolaris/Makefile b/sys/modules/opensolaris/Makefile
index 1620ab971989..c22c64c6367b 100644
--- a/sys/modules/opensolaris/Makefile
+++ b/sys/modules/opensolaris/Makefile
@@ -9,6 +9,7 @@ SRCS=		opensolaris.c		\
 		opensolaris_cmn_err.c	\
 		opensolaris_kmem.c	\
 		opensolaris_misc.c	\
+		opensolaris_proc.c	\
 		opensolaris_sunddi.c
 
 _A=${SYSDIR}/cddl/contrib/opensolaris/common/atomic
diff --git a/sys/net/if_var.h b/sys/net/if_var.h
index 2298de5a2816..191fc87f38a7 100644
--- a/sys/net/if_var.h
+++ b/sys/net/if_var.h
@@ -178,9 +178,6 @@ struct if_encap_req {
 
 /*
  * Structure defining a network interface.
- *
- * Size ILP32:  592 (approx)
- *	 LP64: 1048 (approx)
  */
 struct ifnet {
 	/* General book keeping of interface lists. */
@@ -451,9 +448,6 @@ struct ifaddr {
 	counter_u64_t	ifa_obytes;
 };
 
-/* For compatibility with other BSDs. SCTP uses it. */
-#define	ifa_list	ifa_link
-
 struct ifaddr *	ifa_alloc(size_t size, int flags);
 void	ifa_free(struct ifaddr *ifa);
 void	ifa_ref(struct ifaddr *ifa);
diff --git a/sys/netinet/sctp_bsd_addr.c b/sys/netinet/sctp_bsd_addr.c
index deb73e7f4d6b..2cd0416606c4 100644
--- a/sys/netinet/sctp_bsd_addr.c
+++ b/sys/netinet/sctp_bsd_addr.c
@@ -214,7 +214,7 @@ sctp_init_ifns_for_vrf(int vrfid)
 			continue;
 		}
 		IF_ADDR_RLOCK(ifn);
-		TAILQ_FOREACH(ifa, &ifn->if_addrlist, ifa_list) {
+		TAILQ_FOREACH(ifa, &ifn->if_addrhead, ifa_link) {
 			if (ifa->ifa_addr == NULL) {
 				continue;
 			}
@@ -365,7 +365,7 @@ void
 		if (!(*pred) (ifn)) {
 			continue;
 		}
-		TAILQ_FOREACH(ifa, &ifn->if_addrlist, ifa_list) {
+		TAILQ_FOREACH(ifa, &ifn->if_addrhead, ifa_link) {
 			sctp_addr_change(ifa, add ? RTM_ADD : RTM_DELETE);
 		}
 	}
diff --git a/sys/netinet6/nd6_nbr.c b/sys/netinet6/nd6_nbr.c
index 2ac1d7a10a72..6df012ba4dcc 100644
--- a/sys/netinet6/nd6_nbr.c
+++ b/sys/netinet6/nd6_nbr.c
@@ -1217,40 +1217,26 @@ nd6_dad_start(struct ifaddr *ifa, int delay)
 	struct dadq *dp;
 	char ip6buf[INET6_ADDRSTRLEN];
 
+	KASSERT((ia->ia6_flags & IN6_IFF_TENTATIVE) != 0,
+	    ("starting DAD on non-tentative address %p", ifa));
+
 	/*
 	 * If we don't need DAD, don't do it.
 	 * There are several cases:
-	 * - DAD is disabled (ip6_dad_count == 0)
+	 * - DAD is disabled globally or on the interface
 	 * - the interface address is anycast
 	 */
-	if (!(ia->ia6_flags & IN6_IFF_TENTATIVE)) {
-		log(LOG_DEBUG,
-			"nd6_dad_start: called with non-tentative address "
-			"%s(%s)\n",
-			ip6_sprintf(ip6buf, &ia->ia_addr.sin6_addr),
-			ifa->ifa_ifp ? if_name(ifa->ifa_ifp) : "???");
-		return;
-	}
-	if (ia->ia6_flags & IN6_IFF_ANYCAST) {
-		ia->ia6_flags &= ~IN6_IFF_TENTATIVE;
-		return;
-	}
-	if (!V_ip6_dad_count) {
-		ia->ia6_flags &= ~IN6_IFF_TENTATIVE;
-		return;
-	}
-	if (ifa->ifa_ifp == NULL)
-		panic("nd6_dad_start: ifa->ifa_ifp == NULL");
-	if (ND_IFINFO(ifa->ifa_ifp)->flags & ND6_IFF_NO_DAD) {
+	if ((ia->ia6_flags & IN6_IFF_ANYCAST) != 0 ||
+	    V_ip6_dad_count == 0 ||
+	    (ND_IFINFO(ifa->ifa_ifp)->flags & ND6_IFF_NO_DAD) != 0) {
 		ia->ia6_flags &= ~IN6_IFF_TENTATIVE;
 		return;
 	}
-	if (!(ifa->ifa_ifp->if_flags & IFF_UP) ||
-	    !(ifa->ifa_ifp->if_drv_flags & IFF_DRV_RUNNING) ||
-	    (ND_IFINFO(ifa->ifa_ifp)->flags & ND6_IFF_IFDISABLED)) {
-		ia->ia6_flags |= IN6_IFF_TENTATIVE;
+	if ((ifa->ifa_ifp->if_flags & IFF_UP) == 0 ||
+	    (ifa->ifa_ifp->if_drv_flags & IFF_DRV_RUNNING) == 0 ||
+	    (ND_IFINFO(ifa->ifa_ifp)->flags & ND6_IFF_IFDISABLED) != 0)
 		return;
-	}
+
 	if ((dp = nd6_dad_find(ifa, NULL)) != NULL) {
 		/*
 		 * DAD is already in progress.  Let the existing entry
@@ -1329,11 +1315,10 @@ nd6_dad_timer(struct dadq *dp)
 	struct in6_ifaddr *ia = (struct in6_ifaddr *)ifa;
 	char ip6buf[INET6_ADDRSTRLEN];
 
-	/* Sanity check */
-	if (ia == NULL) {
-		log(LOG_ERR, "nd6_dad_timer: called with null parameter\n");
-		goto err;
-	}
+	KASSERT(ia != NULL, ("DAD entry %p with no address", dp));
+	KASSERT((ia->ia6_flags & IN6_IFF_TENTATIVE) != 0,
+	    ("DAD entry %p for non-tentative address", dp));
+
 	if (ND_IFINFO(ifp)->flags & ND6_IFF_IFDISABLED) {
 		/* Do not need DAD for ifdisabled interface. */
 		log(LOG_ERR, "nd6_dad_timer: cancel DAD on %s because of "
@@ -1347,13 +1332,6 @@ nd6_dad_timer(struct dadq *dp)
 			ifa->ifa_ifp ? if_name(ifa->ifa_ifp) : "???");
 		goto err;
 	}
-	if ((ia->ia6_flags & IN6_IFF_TENTATIVE) == 0) {
-		log(LOG_ERR, "nd6_dad_timer: called with non-tentative address "
-			"%s(%s)\n",
-			ip6_sprintf(ip6buf, &ia->ia_addr.sin6_addr),
-			ifa->ifa_ifp ? if_name(ifa->ifa_ifp) : "???");
-		goto err;
-	}
 
 	/* Stop DAD if the interface is down even after dad_maxtry attempts. */
 	if ((dp->dad_ns_tcount > V_dad_maxtry) &&
diff --git a/sys/netpfil/pf/pf_if.c b/sys/netpfil/pf/pf_if.c
index 4c05cd2f2f8e..c5c8b5a876f7 100644
--- a/sys/netpfil/pf/pf_if.c
+++ b/sys/netpfil/pf/pf_if.c
@@ -523,7 +523,7 @@ pfi_instance_add(struct ifnet *ifp, int net, int flags)
 	int		 net2, af;
 
 	IF_ADDR_RLOCK(ifp);
-	TAILQ_FOREACH(ia, &ifp->if_addrhead, ifa_list) {
+	TAILQ_FOREACH(ia, &ifp->if_addrhead, ifa_link) {
 		if (ia->ifa_addr == NULL)
 			continue;
 		af = ia->ifa_addr->sa_family;
diff --git a/sys/powerpc/conf/MPC85XX b/sys/powerpc/conf/MPC85XX
index 6cd84ef86ad5..97464e84949c 100644
--- a/sys/powerpc/conf/MPC85XX
+++ b/sys/powerpc/conf/MPC85XX
@@ -93,3 +93,7 @@ device		ehci
 device		umass
 device		usb
 device		vlan
+
+# P1022 DIU
+device  	diu
+device  	videomode
diff --git a/sys/powerpc/mpc85xx/fsl_diu.c b/sys/powerpc/mpc85xx/fsl_diu.c
new file mode 100644
index 000000000000..da9fe4251ede
--- /dev/null
+++ b/sys/powerpc/mpc85xx/fsl_diu.c
@@ -0,0 +1,479 @@
+/*-
+ * Copyright (c) 2015 Justin Hibbits
+ * 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.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/endian.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/timeet.h>
+#include <sys/timetc.h>
+#include <sys/watchdog.h>
+#include <sys/fbio.h>
+#include <sys/consio.h>
+#include <sys/eventhandler.h>
+#include <sys/gpio.h>
+
+#include <vm/vm.h>
+#include <vm/pmap.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <dev/videomode/videomode.h>
+#include <dev/videomode/edidvar.h>
+
+#include <dev/vt/vt.h>
+#include <dev/vt/colors/vt_termcolors.h>
+
+#include <powerpc/mpc85xx/mpc85xx.h>
+
+#include "gpio_if.h"
+
+#include <machine/bus.h>
+#include <machine/cpu.h>
+
+#include "fb_if.h"
+
+#define	DIU_DESC_1		0x000	/* Plane1 Area Descriptor Pointer Register */
+#define	DIU_DESC_2		0x004	/* Plane2 Area Descriptor Pointer Register */
+#define	DIU_DESC_3		0x008	/* Plane3 Area Descriptor Pointer Register */
+#define	DIU_GAMMA		0x00C	/* Gamma Register */
+#define	DIU_PALETTE		0x010	/* Palette Register */
+#define	DIU_CURSOR		0x014	/* Cursor Register */
+#define	DIU_CURS_POS		0x018	/* Cursor Position Register */
+#define	 CURSOR_Y_SHIFT		 16
+#define	 CURSOR_X_SHIFT		 0
+#define	DIU_DIU_MODE		0x01C	/* DIU4 Mode */
+#define	 DIU_MODE_M		0x7
+#define	 DIU_MODE_S		0
+#define	 DIU_MODE_NORMAL	0x1
+#define	 DIU_MODE_2		0x2
+#define	 DIU_MODE_3		0x3
+#define	 DIU_MODE_COLBAR	0x4
+#define	DIU_BGND		0x020	/* Background */
+#define	DIU_BGND_WB		0x024	/* Background Color in write back Mode Register */
+#define	DIU_DISP_SIZE		0x028	/* Display Size */
+#define	 DELTA_Y_S		16
+#define	 DELTA_X_S		0
+#define	DIU_WB_SIZE		0x02C	/* Write back Plane Size Register */
+#define	 DELTA_Y_WB_S		16
+#define	 DELTA_X_WB_S		0
+#define	DIU_WB_MEM_ADDR		0x030	/* Address to Store the write back Plane Register */
+#define	DIU_HSYN_PARA		0x034	/* Horizontal Sync Parameter */
+#define	 BP_H_SHIFT		22
+#define	 PW_H_SHIFT		11
+#define	 FP_H_SHIFT		0
+#define	DIU_VSYN_PARA		0x038	/* Vertical Sync Parameter */
+#define	 BP_V_SHIFT		22
+#define	 PW_V_SHIFT		11
+#define	 FP_V_SHIFT		0
+#define	DIU_SYNPOL		0x03C	/* Synchronize Polarity */
+#define	 BP_VS			(1 << 4)
+#define	 BP_HS			(1 << 3)
+#define	 INV_CS			(1 << 2)
+#define	 INV_VS			(1 << 1)
+#define	 INV_HS			(1 << 0)
+#define	 INV_PDI_VS		(1 << 8) /* Polarity of PDI input VSYNC. */
+#define	 INV_PDI_HS		(1 << 9) /* Polarity of PDI input HSYNC. */
+#define	 INV_PDI_DE		(1 << 10) /* Polarity of PDI input DE. */
+#define	DIU_THRESHOLD		0x040	/* Threshold */
+#define	 LS_BF_VS_SHIFT		16
+#define	 OUT_BUF_LOW_SHIFT	0
+#define	DIU_INT_STATUS		0x044	/* Interrupt Status */
+#define	DIU_INT_MASK		0x048	/* Interrupt Mask */
+#define	DIU_COLBAR_1		0x04C	/* COLBAR_1 */
+#define	 DIU_COLORBARn_R(x)	 ((x & 0xff) << 16)
+#define	 DIU_COLORBARn_G(x)	 ((x & 0xff) << 8)
+#define	 DIU_COLORBARn_B(x)	 ((x & 0xff) << 0)
+#define	DIU_COLBAR_2		0x050	/* COLBAR_2 */
+#define	DIU_COLBAR_3		0x054	/* COLBAR_3 */
+#define	DIU_COLBAR_4		0x058	/* COLBAR_4 */
+#define	DIU_COLBAR_5		0x05c	/* COLBAR_5 */
+#define	DIU_COLBAR_6		0x060	/* COLBAR_6 */
+#define	DIU_COLBAR_7		0x064	/* COLBAR_7 */
+#define	DIU_COLBAR_8		0x068	/* COLBAR_8 */
+#define	DIU_FILLING		0x06C	/* Filling Register */
+#define	DIU_PLUT		0x070	/* Priority Look Up Table Register */
+
+/* Control Descriptor */
+#define DIU_CTRLDESCL(n, m)	0x200 + (0x40 * n) + 0x4 * (m - 1)
+#define DIU_CTRLDESCLn_1(n)	DIU_CTRLDESCL(n, 1)
+#define DIU_CTRLDESCLn_2(n)	DIU_CTRLDESCL(n, 2)
+#define DIU_CTRLDESCLn_3(n)	DIU_CTRLDESCL(n, 3)
+#define	 TRANS_SHIFT		20
+#define DIU_CTRLDESCLn_4(n)	DIU_CTRLDESCL(n, 4)
+#define	 BPP_MASK		0xf		/* Bit per pixel Mask */
+#define	 BPP_SHIFT		16		/* Bit per pixel Shift */
+#define	 BPP24			0x5
+#define	 EN_LAYER		(1 << 31)	/* Enable the layer */
+#define DIU_CTRLDESCLn_5(n)	DIU_CTRLDESCL(n, 5)
+#define DIU_CTRLDESCLn_6(n)	DIU_CTRLDESCL(n, 6)
+#define DIU_CTRLDESCLn_7(n)	DIU_CTRLDESCL(n, 7)
+#define DIU_CTRLDESCLn_8(n)	DIU_CTRLDESCL(n, 8)
+#define DIU_CTRLDESCLn_9(n)	DIU_CTRLDESCL(n, 9)
+
+#define	NUM_LAYERS	1
+
+struct panel_info {
+	uint32_t	panel_width;
+	uint32_t	panel_height;
+	uint32_t	panel_hbp;
+	uint32_t	panel_hpw;
+	uint32_t	panel_hfp;
+	uint32_t	panel_vbp;
+	uint32_t	panel_vpw;
+	uint32_t	panel_vfp;
+	uint32_t	panel_freq;
+	uint32_t	clk_div;
+};
+
+struct diu_area_descriptor {
+	uint32_t	pixel_format;
+	uint32_t	bitmap_address;
+	uint32_t	source_size;
+	uint32_t	aoi_size;
+	uint32_t	aoi_offset;
+	uint32_t	display_offset;
+	uint32_t	chroma_key_max;
+	uint32_t	chroma_key_min;
+	uint32_t	next_ad_addr;
+} __aligned(32);
+
+struct diu_softc {
+	struct resource		*res[2];
+	void			*ih;
+	device_t		sc_dev;
+	device_t		sc_fbd;		/* fbd child */
+	struct fb_info		sc_info;
+	struct panel_info	sc_panel;
+	struct diu_area_descriptor *sc_planes[3];
+	uint8_t			*sc_gamma;
+	uint8_t			*sc_cursor;
+};
+
+static struct resource_spec diu_spec[] = {
+	{ SYS_RES_MEMORY,	0,	RF_ACTIVE },
+	{ SYS_RES_IRQ,		0,	RF_ACTIVE },
+	{ -1, 0 }
+};
+
+static int
+diu_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_is_compatible(dev, "fsl,diu"))
+		return (ENXIO);
+
+	device_set_desc(dev, "Freescale Display Interface Unit");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static void
+diu_intr(void *arg)
+{
+	struct diu_softc *sc;
+	int reg;
+
+	sc = arg;
+
+	/* Ack interrupts */
+	reg = bus_read_4(sc->res[0], DIU_INT_STATUS);
+	bus_write_4(sc->res[0], DIU_INT_STATUS, reg);
+
+	/* TODO interrupt handler */
+}
+
+static int
+diu_set_pxclk(device_t dev, unsigned int freq)
+{
+	phandle_t node;
+	unsigned long bus_freq;
+	uint32_t pxclk_set;
+	uint32_t clkdvd;
+	int res;
+
+	node = ofw_bus_get_node(device_get_parent(dev));
+	if ((res = OF_getencprop(node, "bus-frequency",
+	    (pcell_t *)&bus_freq, sizeof(bus_freq)) <= 0)) {
+		device_printf(dev, "Unable to get bus frequency\n");
+		return (ENXIO);
+	}
+
+	/* freq is in kHz */
+	freq *= 1000;
+	/* adding freq/2 to round-to-closest */
+	pxclk_set = min(max((bus_freq + freq/2) / freq, 2), 255) << 16;
+	pxclk_set |= OCP85XX_CLKDVDR_PXCKEN;
+	clkdvd = ccsr_read4(OCP85XX_CLKDVDR);
+	clkdvd &= ~(OCP85XX_CLKDVDR_PXCKEN | OCP85XX_CLKDVDR_PXCKINV |
+		OCP85XX_CLKDVDR_PXCLK_MASK);
+	ccsr_write4(OCP85XX_CLKDVDR, clkdvd);
+	ccsr_write4(OCP85XX_CLKDVDR, clkdvd | pxclk_set);
+
+	return (0);
+}
+
+static int
+diu_init(struct diu_softc *sc)
+{
+	struct panel_info *panel;
+	int reg;
+
+	panel = &sc->sc_panel;
+
+	/* Temporarily disable the DIU while configuring */
+	reg = bus_read_4(sc->res[0], DIU_DIU_MODE);
+	reg &= ~(DIU_MODE_M << DIU_MODE_S);
+	bus_write_4(sc->res[0], DIU_DIU_MODE, reg);
+
+	if (diu_set_pxclk(sc->sc_dev, panel->panel_freq) < 0) {
+		return (ENXIO);
+	}
+
+	/* Configure DIU */
+	/* Need to set these somehow later... */
+	bus_write_4(sc->res[0], DIU_GAMMA, vtophys(sc->sc_gamma));
+	bus_write_4(sc->res[0], DIU_CURSOR, vtophys(sc->sc_cursor));
+	bus_write_4(sc->res[0], DIU_CURS_POS, 0);
+
+	reg = ((sc->sc_info.fb_height) << DELTA_Y_S);
+	reg |= sc->sc_info.fb_width;
+	bus_write_4(sc->res[0], DIU_DISP_SIZE, reg);
+
+	reg = (panel->panel_hbp << BP_H_SHIFT);
+	reg |= (panel->panel_hpw << PW_H_SHIFT);
+	reg |= (panel->panel_hfp << FP_H_SHIFT);
+	bus_write_4(sc->res[0], DIU_HSYN_PARA, reg);
+
+	reg = (panel->panel_vbp << BP_V_SHIFT);
+	reg |= (panel->panel_vpw << PW_V_SHIFT);
+	reg |= (panel->panel_vfp << FP_V_SHIFT);
+	bus_write_4(sc->res[0], DIU_VSYN_PARA, reg);
+
+	bus_write_4(sc->res[0], DIU_BGND, 0);
+
+	/* Mask all the interrupts */
+	bus_write_4(sc->res[0], DIU_INT_MASK, 0x3f);
+
+	/* Reset all layers */
+	sc->sc_planes[0] = contigmalloc(sizeof(struct diu_area_descriptor),
+		M_DEVBUF, M_ZERO, 0, BUS_SPACE_MAXADDR_32BIT, 32, 0);
+	bus_write_4(sc->res[0], DIU_DESC_1, vtophys(sc->sc_planes[0]));
+	bus_write_4(sc->res[0], DIU_DESC_2, 0);
+	bus_write_4(sc->res[0], DIU_DESC_3, 0);
+
+	/* Setup first plane */
+	/* Area descriptor fields are little endian, so byte swap. */
+	/* Word 0: Pixel format */
+	/* Set to 8:8:8:8 ARGB, 4 bytes per pixel, no flip. */
+#define MAKE_PXLFMT(as,rs,gs,bs,a,r,g,b,f,s)	\
+		htole32((as << (4 * a)) | (rs << 4 * r) | \
+		    (gs << 4 * g) | (bs << 4 * b) | \
+		    (f << 28) | (s << 16) | \
+		    (a << 25) | (r << 19) | \
+		    (g << 21) | (b << 24))
+	reg = MAKE_PXLFMT(8, 8, 8, 8, 3, 2, 1, 0, 1, 3);
+	sc->sc_planes[0]->pixel_format = reg;
+	/* Word 1: Bitmap address */
+	sc->sc_planes[0]->bitmap_address = htole32(sc->sc_info.fb_pbase);
+	/* Word 2: Source size/global alpha */
+	reg = (sc->sc_info.fb_width | (sc->sc_info.fb_height << 12));
+	sc->sc_planes[0]->source_size = htole32(reg);
+	/* Word 3: AOI Size */
+	reg = (sc->sc_info.fb_width | (sc->sc_info.fb_height << 16));
+	sc->sc_planes[0]->aoi_size = htole32(reg);
+	/* Word 4: AOI Offset */
+	sc->sc_planes[0]->aoi_offset = 0;
+	/* Word 5: Display offset */
+	sc->sc_planes[0]->display_offset = 0;
+	/* Word 6: Chroma key max */
+	sc->sc_planes[0]->chroma_key_max = 0;
+	/* Word 7: Chroma key min */
+	reg = 255 << 16 | 255 << 8 | 255;
+	sc->sc_planes[0]->chroma_key_min = htole32(reg);
+	/* Word 8: Next AD */
+	sc->sc_planes[0]->next_ad_addr = 0;
+
+	/* TODO: derive this from the panel size */
+	bus_write_4(sc->res[0], DIU_PLUT, 0x1f5f666);
+
+	/* Enable DIU in normal mode */
+	reg = bus_read_4(sc->res[0], DIU_DIU_MODE);
+	reg &= ~(DIU_MODE_M << DIU_MODE_S);
+	reg |= (DIU_MODE_NORMAL << DIU_MODE_S);
+	bus_write_4(sc->res[0], DIU_DIU_MODE, reg);
+
+	return (0);
+}
+
+static int
+diu_attach(device_t dev)
+{
+	struct edid_info *edid;
+	struct diu_softc *sc;
+	const struct videomode *videomode;
+	void *edid_cells;
+	const char *vm_name;
+	phandle_t node;
+	int h, r, w;
+	int err, i;
+
+	sc = device_get_softc(dev);
+	sc->sc_dev = dev;
+
+	if (bus_alloc_resources(dev, diu_spec, sc->res)) {
+		device_printf(dev, "could not allocate resources\n");
+		return (ENXIO);
+	}
+
+	node = ofw_bus_get_node(dev);
+	/* Setup interrupt handler */
+	err = bus_setup_intr(dev, sc->res[1], INTR_TYPE_BIO | INTR_MPSAFE,
+	    NULL, diu_intr, sc, &sc->ih);
+	if (err) {
+		device_printf(dev, "Unable to alloc interrupt resource.\n");
+		return (ENXIO);
+	}
+
+	/* TODO: Eventually, allow EDID to be dynamically provided. */
+	if (OF_getprop_alloc(node, "edid", 1, &edid_cells) <= 0) {
+		/*
+		 * u-boot uses the environment variable name 'video-mode', so
+		 * just use the same name here.  Should allow another variable
+		 * that better fits our design model, but this is fine.
+		 */
+		if ((vm_name = kern_getenv("video-mode")) == NULL) {
+			device_printf(dev,
+			    "No EDID data and no video-mode env set\n");
+			return (ENXIO);
+		}
+	}
+	if (edid_cells != NULL) {
+		if (edid_parse(edid_cells, edid) != 0) {
+			device_printf(dev, "Error parsing EDID\n");
+			OF_prop_free(edid_cells);
+			return (ENXIO);
+		}
+		videomode = edid->edid_preferred_mode;
+	} else {
+		/* Parse video-mode kenv variable. */
+		if ((err = sscanf(vm_name, "fslfb:%dx%d@%d", &w, &h, &r)) != 3) {
+			device_printf(dev,
+			    "Cannot parse video mode: %s\n", vm_name);
+			return (ENXIO);
+		}
+		videomode = pick_mode_by_ref(w, h, r);
+		if (videomode == NULL) {
+			device_printf(dev,
+			    "Cannot find mode for %dx%d@%d", w, h, r);
+			return (ENXIO);
+		}
+	}
+
+	sc->sc_panel.panel_width = videomode->hdisplay;
+	sc->sc_panel.panel_height = videomode->vdisplay;
+	sc->sc_panel.panel_hbp = videomode->hsync_start - videomode->hdisplay;
+	sc->sc_panel.panel_hfp = videomode->htotal - videomode->hsync_end;
+	sc->sc_panel.panel_hpw = videomode->hsync_end - videomode->hsync_start;
+	sc->sc_panel.panel_vbp = videomode->vsync_start - videomode->vdisplay;
+	sc->sc_panel.panel_vfp = videomode->vtotal - videomode->vsync_end;
+	sc->sc_panel.panel_vpw = videomode->vsync_end - videomode->vsync_start;
+	sc->sc_panel.panel_freq = videomode->dot_clock;
+
+	sc->sc_info.fb_width = sc->sc_panel.panel_width;
+	sc->sc_info.fb_height = sc->sc_panel.panel_height;
+	sc->sc_info.fb_stride = sc->sc_info.fb_width * 4;
+	sc->sc_info.fb_bpp = sc->sc_info.fb_depth = 32;
+	sc->sc_info.fb_size = sc->sc_info.fb_height * sc->sc_info.fb_stride;
+	sc->sc_info.fb_vbase = (intptr_t)contigmalloc(sc->sc_info.fb_size,
+	    M_DEVBUF, M_ZERO, 0, BUS_SPACE_MAXADDR_32BIT, PAGE_SIZE, 0);
+	sc->sc_info.fb_pbase = (intptr_t)vtophys(sc->sc_info.fb_vbase);
+	
+	/* Gamma table is 3 consecutive segments of 256 bytes. */
+	sc->sc_gamma = contigmalloc(3 * 256, M_DEVBUF, 0, 0,
+	    BUS_SPACE_MAXADDR_32BIT, PAGE_SIZE, 0);
+	/* Initialize gamma to default */
+	for (i = 0; i < 3 * 256; i++)
+		sc->sc_gamma[i] = (i % 256);
+
+	/* Cursor format is 32x32x16bpp */
+	sc->sc_cursor = contigmalloc(32 * 32 * 2, M_DEVBUF, M_ZERO, 0,
+	    BUS_SPACE_MAXADDR_32BIT, PAGE_SIZE, 0);
+
+	diu_init(sc);
+
+	sc->sc_info.fb_name = device_get_nameunit(dev);
+
+	/* Ask newbus to attach framebuffer device to me. */
+	sc->sc_fbd = device_add_child(dev, "fbd", device_get_unit(dev));
+	if (sc->sc_fbd == NULL)
+		device_printf(dev, "Can't attach fbd device\n");
+
+	if ((err = device_probe_and_attach(sc->sc_fbd)) != 0) {
+		device_printf(dev, "Failed to attach fbd device: %d\n", err);
+	}
+
+	return (0);
+}
+
+static struct fb_info *
+diu_fb_getinfo(device_t dev)
+{
+	struct diu_softc *sc = device_get_softc(dev);
+
+	return (&sc->sc_info);
+}
+
+static device_method_t diu_methods[] = {
+	DEVMETHOD(device_probe,		diu_probe),
+	DEVMETHOD(device_attach,	diu_attach),
+
+	/* Framebuffer service methods */
+	DEVMETHOD(fb_getinfo,		diu_fb_getinfo),
+	{ 0, 0 }
+};
+
+static driver_t diu_driver = {
+	"fb",
+	diu_methods,
+	sizeof(struct diu_softc),
+};
+
+static devclass_t diu_devclass;
+
+DRIVER_MODULE(fb, simplebus, diu_driver, diu_devclass, 0, 0);
diff --git a/sys/powerpc/mpc85xx/mpc85xx.h b/sys/powerpc/mpc85xx/mpc85xx.h
index e81e12546933..697533b40af7 100644
--- a/sys/powerpc/mpc85xx/mpc85xx.h
+++ b/sys/powerpc/mpc85xx/mpc85xx.h
@@ -139,6 +139,13 @@ extern vm_offset_t		ccsrbar_va;
  */
 #define	OCP85XX_RSTCR		(CCSRBAR_VA + 0xe00b0)
 
+#define	OCP85XX_CLKDVDR		(CCSRBAR_VA + 0xe0800)
+#define	  OCP85XX_CLKDVDR_PXCKEN	  0x80000000
+#define	  OCP85XX_CLKDVDR_SSICKEN	  0x20000000
+#define	  OCP85XX_CLKDVDR_PXCKINV	  0x10000000
+#define	  OCP85XX_CLKDVDR_PXCLK_MASK	  0x00FF0000
+#define	  OCP85XX_CLKDVDR_SSICLK_MASK	  0x000000FF
+
 /*
  * Run Control/Power Management Registers.
  */
diff --git a/sys/sys/eventhandler.h b/sys/sys/eventhandler.h
index d82ece71eda9..164b0f07293d 100644
--- a/sys/sys/eventhandler.h
+++ b/sys/sys/eventhandler.h
@@ -270,4 +270,11 @@ typedef void (*unregister_framebuffer_fn)(void *, struct fb_info *);
 EVENTHANDLER_DECLARE(register_framebuffer, register_framebuffer_fn);
 EVENTHANDLER_DECLARE(unregister_framebuffer, unregister_framebuffer_fn);
 
+/* Veto ada attachment */
+struct cam_path;
+struct ata_params;
+typedef void (*ada_probe_veto_fn)(void *, struct cam_path *,
+    struct ata_params *, int *);
+EVENTHANDLER_DECLARE(ada_probe_veto, ada_probe_veto_fn);
+
 #endif /* _SYS_EVENTHANDLER_H_ */
diff --git a/sys/sys/mouse.h b/sys/sys/mouse.h
index 24d0ffc7a561..9fd1d6d8c085 100644
--- a/sys/sys/mouse.h
+++ b/sys/sys/mouse.h
@@ -170,6 +170,7 @@ typedef struct synapticshw {
 #define MOUSE_MODEL_4DPLUS		12
 #define MOUSE_MODEL_SYNAPTICS		13
 #define	MOUSE_MODEL_TRACKPOINT		14
+#define	MOUSE_MODEL_ELANTECH		15
 
 typedef struct mousemode {
 	int protocol;		/* MOUSE_PROTO_XXX */
@@ -240,6 +241,9 @@ typedef struct mousevar {
 /* Synaptics Touchpad */
 #define MOUSE_SYNAPTICS_PACKETSIZE	6	/* '3' works better */
 
+/* Elantech Touchpad */
+#define MOUSE_ELANTECH_PACKETSIZE	6
+
 /* Microsoft Serial mouse data packet */
 #define MOUSE_MSS_PACKETSIZE	3
 #define MOUSE_MSS_SYNCMASK	0x40