MFC r262534, r262548, r262549, r262552, r262568, r262581, r262583, r262584,

    r262585, r262587, r262696, r262712

  Replace many pasted identical definitions of cpu_initclocks() with a common
  implementation in arm/machdep.c.

  aicasm: Don't complain about missing prototypes to ease bootstrap issues.

  Vybrid: Add driver for Inter-Integrated Circuit (I2C).

  imx6: Initialize the Low Power Mode bits to keep the ARM cores running
  during WFI.

  All our current ARM multi-core systems have all cores in one package with
  a shared L2 cache, reflect that in the common cpu_topo() routine.

  mpcore timer: Supply a DELAY() implementation via weak linkage, so that
  SoC-specific code can supply a better implementation.

  imx6: Add some rudimentary voltage control.

  Add an armv7 implementation of cpu_sleep().

  Add __used attribute so that the DELAY implementation doesn't get
  optimized away as unreferenced, causing linker errors when trying to
  resolve the weak reference to the missing function.

22 files changed, 636 insertions, 91 deletions

diff --git a/sys/arm/allwinner/timer.c b/sys/arm/allwinner/timer.c
index 4668026..c87f947 100644
--- a/sys/arm/allwinner/timer.c
+++ b/sys/arm/allwinner/timer.c
@@ -295,12 +295,6 @@ a10_timer_get_timerfreq(struct a10_timer_softc *sc)
 	return (sc->timer0_freq);
 }
 
-void
-cpu_initclocks(void)
-{
-	cpu_initclocks_bsp();
-}
-
 static int
 a10_timer_hardclock(void *arg)
 {
diff --git a/sys/arm/arm/cpufunc.c b/sys/arm/arm/cpufunc.c
index b8ad4e5..44e81d8 100644
--- a/sys/arm/arm/cpufunc.c
+++ b/sys/arm/arm/cpufunc.c
@@ -1107,7 +1107,7 @@ struct cpu_functions cortexa_cpufuncs = {
 	cpufunc_nullop,                 /* flush_brnchtgt_C     */
 	(void *)cpufunc_nullop,         /* flush_brnchtgt_E     */
 	
-	arm11_sleep,                    /* sleep                */
+	armv7_sleep,                    /* sleep                */
 	
 	/* Soft functions */
 	
diff --git a/sys/arm/arm/cpufunc_asm_armv7.S b/sys/arm/arm/cpufunc_asm_armv7.S
index 74933eb..2a4bb98 100644
--- a/sys/arm/arm/cpufunc_asm_armv7.S
+++ b/sys/arm/arm/cpufunc_asm_armv7.S
@@ -343,3 +343,9 @@ ENTRY(armv7_idcache_inv_all)
 	bx      lr                      @ return
 END(armv7_l1cache_inv_all)
 
+ENTRY_NP(armv7_sleep)
+	dsb
+	wfi
+	bx	lr
+END(armv7_sleep)
+
diff --git a/sys/arm/arm/generic_timer.c b/sys/arm/arm/generic_timer.c
index 8de31bf..1621e24 100644
--- a/sys/arm/arm/generic_timer.c
+++ b/sys/arm/arm/generic_timer.c
@@ -332,16 +332,6 @@ static devclass_t arm_tmr_devclass;
 DRIVER_MODULE(timer, simplebus, arm_tmr_driver, arm_tmr_devclass, 0, 0);
 
 void
-cpu_initclocks(void)
-{
-
-	if (PCPU_GET(cpuid) == 0)
-		cpu_initclocks_bsp();
-	else
-		cpu_initclocks_ap();
-}
-
-void
 DELAY(int usec)
 {
 	int32_t counts, counts_per_usec;
diff --git a/sys/arm/arm/machdep.c b/sys/arm/arm/machdep.c
index 1386210..68f4318 100644
--- a/sys/arm/arm/machdep.c
+++ b/sys/arm/arm/machdep.c
@@ -456,6 +456,30 @@ cpu_idle_wakeup(int cpu)
 	return (0);
 }
 
+/*
+ * Most ARM platforms don't need to do anything special to init their clocks
+ * (they get intialized during normal device attachment), and by not defining a
+ * cpu_initclocks() function they get this generic one.  Any platform that needs
+ * to do something special can just provide their own implementation, which will
+ * override this one due to the weak linkage.
+ */
+void
+arm_generic_initclocks(void)
+{
+
+#ifndef NO_EVENTTIMERS
+#ifdef SMP
+	if (PCPU_GET(cpuid) == 0)
+		cpu_initclocks_bsp();
+	else
+		cpu_initclocks_ap();
+#else
+	cpu_initclocks_bsp();
+#endif
+#endif
+}
+__weak_reference(arm_generic_initclocks, cpu_initclocks);
+
 int
 fill_regs(struct thread *td, struct reg *regs)
 {
diff --git a/sys/arm/arm/mp_machdep.c b/sys/arm/arm/mp_machdep.c
index 584eda5..fd275fe 100644
--- a/sys/arm/arm/mp_machdep.c
+++ b/sys/arm/arm/mp_machdep.c
@@ -371,7 +371,7 @@ struct cpu_group *
 cpu_topo(void)
 {
 
-	return (smp_topo_1level(CG_SHARE_L2, 1, 0));
+	return (smp_topo_1level(CG_SHARE_L2, mp_ncpus, 0));
 }
 
 void
diff --git a/sys/arm/arm/mpcore_timer.c b/sys/arm/arm/mpcore_timer.c
index 5024efe..4a0f23b 100644
--- a/sys/arm/arm/mpcore_timer.c
+++ b/sys/arm/arm/mpcore_timer.c
@@ -129,12 +129,12 @@ uint32_t platform_arm_tmr_freq = 0;
 static timecounter_get_t arm_tmr_get_timecount;
 
 static struct timecounter arm_tmr_timecount = {
-	.tc_name           = "ARM MPCore Timecounter",
+	.tc_name           = "MPCore",
 	.tc_get_timecount  = arm_tmr_get_timecount,
 	.tc_poll_pps       = NULL,
 	.tc_counter_mask   = ~0u,
 	.tc_frequency      = 0,
-	.tc_quality        = 1000,
+	.tc_quality        = 800,
 };
 
 /**
@@ -254,7 +254,7 @@ arm_tmr_probe(device_t dev)
 	if (!ofw_bus_is_compatible(dev, "arm,mpcore-timers"))
 		return (ENXIO);
 
-	device_set_desc(dev, "ARM Generic MPCore Timers");
+	device_set_desc(dev, "ARM MPCore Timers");
 	return (BUS_PROBE_DEFAULT);
 }
 
@@ -327,7 +327,7 @@ arm_tmr_attach(device_t dev)
 		return (ENXIO);
 	}
 
-	sc->et.et_name = "ARM MPCore Eventtimer";
+	sc->et.et_name = "MPCore";
 	sc->et.et_flags = ET_FLAGS_PERIODIC | ET_FLAGS_ONESHOT | ET_FLAGS_PERCPU;
 	sc->et.et_quality = 1000;
 
@@ -359,25 +359,6 @@ static devclass_t arm_tmr_devclass;
 DRIVER_MODULE(mp_tmr, simplebus, arm_tmr_driver, arm_tmr_devclass, 0, 0);
 
 /**
- *	cpu_initclocks - called by system to initialise the cpu clocks
- *
- *	This is a boilerplat function, most of the setup has already been done
- *	when the driver was attached.  Therefore this function must only be called
- *	after the driver is attached.
- *
- *	RETURNS
- *	nothing
- */
-void
-cpu_initclocks(void)
-{
-	if (PCPU_GET(cpuid) == 0)
-		cpu_initclocks_bsp();
-	else
-		cpu_initclocks_ap();
-}
-
-/**
  *	DELAY - Delay for at least usec microseconds.
  *	@usec: number of microseconds to delay by
  *
@@ -388,8 +369,8 @@ cpu_initclocks(void)
  *	RETURNS:
  *	nothing
  */
-void
-DELAY(int usec)
+static void __used /* Must emit function code for the weak ref below. */
+arm_tmr_DELAY(int usec)
 {
 	int32_t counts_per_usec;
 	int32_t counts;
@@ -427,3 +408,11 @@ DELAY(int usec)
 		first = last;
 	}
 }
+
+/*
+ * Supply a DELAY() implementation via weak linkage.  A platform may want to use
+ * the mpcore per-cpu eventtimers but provide its own DELAY() routine,
+ * especially when the core frequency can change on the fly.
+ */
+__weak_reference(arm_tmr_DELAY, DELAY);
+
diff --git a/sys/arm/at91/uart_dev_at91usart.c b/sys/arm/at91/uart_dev_at91usart.c
index 3f02a4e..049fff5 100644
--- a/sys/arm/at91/uart_dev_at91usart.c
+++ b/sys/arm/at91/uart_dev_at91usart.c
@@ -279,7 +279,7 @@ at91_usart_init(struct uart_bas *bas, int baudrate, int databits, int stopbits,
 	 * we don't want to hang here forever if the hardware is in a bad state.
 	 */
 	if (!(RD4(bas, USART_CSR) & USART_CSR_TXRDY))
-	    DELAY(1000);
+		DELAY(10000);
 
 	at91_usart_param(bas, baudrate, databits, stopbits, parity);
 
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_systimer.c b/sys/arm/broadcom/bcm2835/bcm2835_systimer.c
index 1dc8d5d..5e73747 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_systimer.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_systimer.c
@@ -278,12 +278,6 @@ static devclass_t bcm_systimer_devclass;
 DRIVER_MODULE(bcm_systimer, simplebus, bcm_systimer_driver, bcm_systimer_devclass, 0, 0);
 
 void
-cpu_initclocks(void)
-{
-	cpu_initclocks_bsp();
-}
-
-void
 DELAY(int usec)
 {
 	int32_t counts;
diff --git a/sys/arm/conf/VYBRID.common b/sys/arm/conf/VYBRID.common
index fde330b..c49620f 100644
--- a/sys/arm/conf/VYBRID.common
+++ b/sys/arm/conf/VYBRID.common
@@ -124,8 +124,8 @@ device		nand
 device		uart
 
 # I2C (TWSI)
-#device		iic
-#device		iicbus
+device		iic
+device		iicbus
 
 # Ethernet
 device		ether
diff --git a/sys/arm/freescale/imx/imx6_anatop.c b/sys/arm/freescale/imx/imx6_anatop.c
index c393656..c713d8a 100644
--- a/sys/arm/freescale/imx/imx6_anatop.c
+++ b/sys/arm/freescale/imx/imx6_anatop.c
@@ -66,6 +66,7 @@ __FBSDID("$FreeBSD$");
 #include <dev/ofw/ofw_bus_subr.h>
 
 #include <machine/bus.h>
+#include <machine/fdt.h>
 
 #include <arm/freescale/fsl_ocotpreg.h>
 #include <arm/freescale/fsl_ocotpvar.h>
@@ -85,8 +86,11 @@ struct imx6_anatop_softc {
 	struct resource	*res[2];
 	uint32_t	cpu_curhz;
 	uint32_t	cpu_curmhz;
+	uint32_t	cpu_curmv;
 	uint32_t	cpu_minhz;
+	uint32_t	cpu_minmv;
 	uint32_t	cpu_maxhz;
+	uint32_t	cpu_maxmv;
 	uint32_t	refosc_hz;
 	void		*temp_intrhand;
 	uint32_t	temp_high_val;
@@ -103,12 +107,15 @@ struct imx6_anatop_softc {
 static struct imx6_anatop_softc *imx6_anatop_sc;
 
 /*
- * Table of CPU max frequencies.  This is indexed by the max frequency value
- * (0-3) from the ocotp CFG3 register.
+ * Tables of CPU max frequencies and corresponding voltages.  This is indexed by
+ * the max frequency value (0-3) from the ocotp CFG3 register.
  */
 static uint32_t imx6_cpu_maxhz_tab[] = {
 	 792000000, 852000000, 996000000, 1200000000
 };
+static uint32_t imx6_cpu_millivolt_tab[] = {
+	1150, 1225, 1225, 1275
+};
 
 #define	TZ_ZEROC	2732	/* deci-Kelvin <-> deci-Celcius offset. */
 
@@ -130,6 +137,64 @@ imx6_anatop_write_4(bus_size_t offset, uint32_t value)
 	bus_write_4(imx6_anatop_sc->res[MEMRES], offset, value);
 }
 
+static void
+vdd_set(struct imx6_anatop_softc *sc, int mv)
+{
+	int newtarg, oldtarg;
+	uint32_t delay, pmureg;
+	static boolean_t init_done = false;
+
+	/*
+	 * The datasheet says VDD_PU and VDD_SOC must be equal, and VDD_ARM
+	 * can't be more than 50mV above or 200mV below them.  For now to keep
+	 * things simple we set all three to the same value.
+	 */
+
+	pmureg = imx6_anatop_read_4(IMX6_ANALOG_PMU_REG_CORE);
+	oldtarg = pmureg & IMX6_ANALOG_PMU_REG0_TARG_MASK;
+
+	/* Convert mV to target value.  Clamp target to valid range. */
+	if (mv < 725)
+		newtarg = 0x00;
+	else if (mv > 1450)
+		newtarg = 0x1F;
+	else
+		newtarg = (mv - 700) / 25;
+
+	/*
+	 * The first time through the 3 voltages might not be equal so use a
+	 * long conservative delay.  After that we need to delay 3uS for every
+	 * 25mV step upward.  No need to delay at all when lowering.
+	 */
+	if (init_done) {
+		if (newtarg == oldtarg)
+			return;
+		else if (newtarg > oldtarg)
+			delay = (newtarg - oldtarg) * 3;
+		else
+			delay = 0;
+	} else {
+		delay = 700 / 25 * 3;
+		init_done = true;
+	}
+
+	/*
+	 * Make the change and wait for it to take effect.
+	 */
+	pmureg &= ~(IMX6_ANALOG_PMU_REG0_TARG_MASK |
+	    IMX6_ANALOG_PMU_REG1_TARG_MASK |
+	    IMX6_ANALOG_PMU_REG2_TARG_MASK);
+
+	pmureg |= newtarg << IMX6_ANALOG_PMU_REG0_TARG_SHIFT;
+	pmureg |= newtarg << IMX6_ANALOG_PMU_REG1_TARG_SHIFT;
+	pmureg |= newtarg << IMX6_ANALOG_PMU_REG2_TARG_SHIFT;
+
+	imx6_anatop_write_4(IMX6_ANALOG_PMU_REG_CORE, pmureg);
+	DELAY(delay);
+	sc->cpu_curmv = newtarg * 25 + 700;
+	device_printf(sc->dev, "voltage set to %u\n", sc->cpu_curmv);
+}
+
 static inline uint32_t
 cpufreq_hz_from_div(struct imx6_anatop_softc *sc, uint32_t div)
 {
@@ -231,7 +296,9 @@ cpufreq_initialize(struct imx6_anatop_softc *sc)
 	    FSL_OCOTP_CFG3_SPEED_MASK) >> FSL_OCOTP_CFG3_SPEED_SHIFT;
 
 	sc->cpu_minhz = cpufreq_actual_hz(sc, imx6_cpu_maxhz_tab[0]);
+	sc->cpu_minmv = imx6_cpu_millivolt_tab[0];
 	sc->cpu_maxhz = cpufreq_actual_hz(sc, imx6_cpu_maxhz_tab[cfg3speed]);
+	sc->cpu_maxmv = imx6_cpu_millivolt_tab[cfg3speed];
 
 	/*
 	 * Set the CPU to maximum speed.
@@ -241,6 +308,7 @@ cpufreq_initialize(struct imx6_anatop_softc *sc)
 	 * basically assumes that a single core can't overheat before interrupts
 	 * are enabled; empirical testing shows that to be a safe assumption.
 	 */
+	vdd_set(sc, sc->cpu_maxmv);
 	cpufreq_set_clock(sc, sc->cpu_maxhz);
 	device_printf(sc->dev, "CPU frequency %uMHz\n", sc->cpu_curmhz);
 }
@@ -321,6 +389,7 @@ tempmon_gofast(struct imx6_anatop_softc *sc)
 {
 
 	if (sc->cpu_curhz < sc->cpu_maxhz) {
+		vdd_set(sc, sc->cpu_maxmv);
 		cpufreq_set_clock(sc, sc->cpu_maxhz);
 	}
 }
@@ -331,6 +400,7 @@ tempmon_goslow(struct imx6_anatop_softc *sc)
 
 	if (sc->cpu_curhz > sc->cpu_minhz) {
 		cpufreq_set_clock(sc, sc->cpu_minhz);
+		vdd_set(sc, sc->cpu_minmv);
 	}
 }
 
@@ -451,6 +521,11 @@ imx6_anatop_attach(device_t dev)
 	if (err != 0)
 		goto out;
 
+	SYSCTL_ADD_UINT(device_get_sysctl_ctx(sc->dev),
+	    SYSCTL_CHILDREN(device_get_sysctl_tree(sc->dev)),
+	    OID_AUTO, "cpu_voltage", CTLFLAG_RD,
+	    &sc->cpu_curmv, 0, "Current CPU voltage in millivolts");
+
 	imx6_anatop_sc = sc;
 
 	/*
diff --git a/sys/arm/freescale/imx/imx6_ccm.c b/sys/arm/freescale/imx/imx6_ccm.c
index e42335a..2ed301f 100644
--- a/sys/arm/freescale/imx/imx6_ccm.c
+++ b/sys/arm/freescale/imx/imx6_ccm.c
@@ -92,6 +92,7 @@ ccm_attach(device_t dev)
 {
 	struct ccm_softc *sc;
 	int err, rid;
+	uint32_t reg;
 
 	sc = device_get_softc(dev);
 	err = 0;
@@ -107,6 +108,26 @@ ccm_attach(device_t dev)
 	}
 
 	ccm_sc = sc;
+
+	/*
+	 * Configure the Low Power Mode setting to leave the ARM core power on
+	 * when a WFI instruction is executed.  This lets the MPCore timers and
+	 * GIC continue to run, which is helpful when the only thing that can
+	 * wake you up is an MPCore Private Timer interrupt delivered via GIC.
+	 *
+	 * XXX Based on the docs, setting CCM_CGPR_INT_MEM_CLK_LPM shouldn't be
+	 * required when the LPM bits are set to LPM_RUN.  But experimentally
+	 * I've experienced a fairly rare lockup when not setting it.  I was
+	 * unable to prove conclusively that the lockup was related to power
+	 * management or that this definitively fixes it.  Revisit this.
+	 */
+	reg = RD4(sc, CCM_CGPR);
+	reg |= CCM_CGPR_INT_MEM_CLK_LPM;
+	WR4(sc, CCM_CGPR, reg);
+	reg = RD4(sc, CCM_CLPCR);
+	reg = (reg & ~CCM_CLPCR_LPM_MASK) | CCM_CLPCR_LPM_RUN;
+	WR4(sc, CCM_CLPCR, reg);
+
 	err = 0;
 
 out:
diff --git a/sys/arm/freescale/imx/imx6_ccmreg.h b/sys/arm/freescale/imx/imx6_ccmreg.h
index 3c651dc..9cb7acc 100644
--- a/sys/arm/freescale/imx/imx6_ccmreg.h
+++ b/sys/arm/freescale/imx/imx6_ccmreg.h
@@ -29,13 +29,20 @@
 #ifndef	IMX6_CCMREG_H
 #define	IMX6_CCMREG_H
 
-#define	CCM_CCGR1		0x06C
-#define	CCM_CCGR2		0x070
-#define	CCM_CCGR3		0x074
-#define	CCM_CCGR4		0x078
-#define	CCM_CCGR5		0x07C
-#define	CCM_CCGR6		0x080
-#define	CCM_CMEOR		0x088
+#define	CCM_CLPCR			0x054
+#define	  CCM_CLPCR_LPM_MASK		  0x03
+#define	  CCM_CLPCR_LPM_RUN		  0x00
+#define	  CCM_CLPCR_LPM_WAIT		  0x01
+#define	  CCM_CLPCR_LPM_STOP		  0x02
+#define	CCM_CGPR			0x064
+#define	  CCM_CGPR_INT_MEM_CLK_LPM	  (1 << 17)
+#define	CCM_CCGR1			0x06C
+#define	CCM_CCGR2			0x070
+#define	CCM_CCGR3			0x074
+#define	CCM_CCGR4			0x078
+#define	CCM_CCGR5			0x07C
+#define	CCM_CCGR6			0x080
+#define	CCM_CMEOR			0x088
                   
 
 #endif
diff --git a/sys/arm/freescale/imx/imx_gpt.c b/sys/arm/freescale/imx/imx_gpt.c
index f792a97..73bc8ec 100644
--- a/sys/arm/freescale/imx/imx_gpt.c
+++ b/sys/arm/freescale/imx/imx_gpt.c
@@ -319,17 +319,6 @@ imx_gpt_get_timerfreq(struct imx_gpt_softc *sc)
 	return (sc->clkfreq);
 }
 
-void
-cpu_initclocks(void)
-{
-
-	if (imx_gpt_sc == NULL) {
-		panic("%s: i.MX GPT driver has not been initialized!", __func__);
-	}
-
-	cpu_initclocks_bsp();
-}
-
 static int
 imx_gpt_intr(void *arg)
 {
diff --git a/sys/arm/freescale/vybrid/files.vybrid b/sys/arm/freescale/vybrid/files.vybrid
index 890e23e..1103f13 100644
--- a/sys/arm/freescale/vybrid/files.vybrid
+++ b/sys/arm/freescale/vybrid/files.vybrid
@@ -23,6 +23,7 @@ arm/freescale/vybrid/vf_mscm.c			standard
 arm/freescale/vybrid/vf_src.c			standard
 arm/freescale/vybrid/vf_edma.c			standard
 arm/freescale/vybrid/vf_dmamux.c		standard
+arm/freescale/vybrid/vf_i2c.c			optional	iicbus
 arm/freescale/vybrid/vf_tcon.c			optional	vt
 arm/freescale/vybrid/vf_dcu4.c			optional	vt
 arm/freescale/vybrid/vf_nfc.c			optional	nand
diff --git a/sys/arm/freescale/vybrid/vf_i2c.c b/sys/arm/freescale/vybrid/vf_i2c.c
new file mode 100644
index 0000000..67254b4
--- /dev/null
+++ b/sys/arm/freescale/vybrid/vf_i2c.c
@@ -0,0 +1,471 @@
+/*-
+ * Copyright (c) 2014 Ruslan Bukin <br@bsdpad.com>
+ * 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.
+ */
+
+/*
+ * Vybrid Family Inter-Integrated Circuit (I2C)
+ * Chapter 48, Vybrid Reference Manual, Rev. 5, 07/2013
+ */
+
+/*
+ * This driver is based on the I2C driver for IMX (imx/i2c.c).
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.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 <dev/iicbus/iiconf.h>
+#include <dev/iicbus/iicbus.h>
+
+#include "iicbus_if.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 <machine/bus.h>
+#include <machine/fdt.h>
+#include <machine/cpu.h>
+#include <machine/intr.h>
+
+#include <arm/freescale/vybrid/vf_common.h>
+
+#define	I2C_IBAD	0x0	/* I2C Bus Address Register */
+#define	I2C_IBFD	0x1	/* I2C Bus Frequency Divider Register */
+#define	I2C_IBCR	0x2	/* I2C Bus Control Register */
+#define	 IBCR_MDIS		(1 << 7) /* Module disable. */
+#define	 IBCR_IBIE		(1 << 6) /* I-Bus Interrupt Enable. */
+#define	 IBCR_MSSL		(1 << 5) /* Master/Slave mode select. */
+#define	 IBCR_TXRX		(1 << 4) /* Transmit/Receive mode select. */
+#define	 IBCR_NOACK		(1 << 3) /* Data Acknowledge disable. */
+#define	 IBCR_RSTA		(1 << 2) /* Repeat Start. */
+#define	 IBCR_DMAEN		(1 << 1) /* DMA Enable. */
+#define	I2C_IBSR	0x3	/* I2C Bus Status Register */
+#define	 IBSR_TCF		(1 << 7) /* Transfer complete. */
+#define	 IBSR_IAAS		(1 << 6) /* Addressed as a slave. */
+#define	 IBSR_IBB		(1 << 5) /* Bus busy. */
+#define	 IBSR_IBAL		(1 << 4) /* Arbitration Lost. */
+#define	 IBSR_SRW		(1 << 2) /* Slave Read/Write. */
+#define	 IBSR_IBIF		(1 << 1) /* I-Bus Interrupt Flag. */
+#define	 IBSR_RXAK		(1 << 0) /* Received Acknowledge. */
+#define	I2C_IBDR	0x4	/* I2C Bus Data I/O Register */
+#define	I2C_IBIC	0x5	/* I2C Bus Interrupt Config Register */
+#define	 IBIC_BIIE		(1 << 7) /* Bus Idle Interrupt Enable bit. */
+#define	I2C_IBDBG	0x6	/* I2C Bus Debug Register */
+
+#ifdef DEBUG
+#define vf_i2c_dbg(_sc, fmt, args...) \
+	device_printf((_sc)->dev, fmt, ##args)
+#else
+#define vf_i2c_dbg(_sc, fmt, args...)
+#endif
+
+static int i2c_repeated_start(device_t, u_char, int);
+static int i2c_start(device_t, u_char, int);
+static int i2c_stop(device_t);
+static int i2c_reset(device_t, u_char, u_char, u_char *);
+static int i2c_read(device_t, char *, int, int *, int, int);
+static int i2c_write(device_t, const char *, int, int *, int);
+
+struct i2c_softc {
+	struct resource		*res[2];
+	bus_space_tag_t		bst;
+	bus_space_handle_t	bsh;
+	device_t		dev;
+	device_t		iicbus;
+	struct mtx		mutex;
+};
+
+static struct resource_spec i2c_spec[] = {
+	{ SYS_RES_MEMORY,	0,	RF_ACTIVE },
+	{ SYS_RES_IRQ,		0,	RF_ACTIVE },
+	{ -1, 0 }
+};
+
+static int
+i2c_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_is_compatible(dev, "fsl,mvf600-i2c"))
+		return (ENXIO);
+
+	device_set_desc(dev, "Vybrid Family Inter-Integrated Circuit (I2C)");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+i2c_attach(device_t dev)
+{
+	struct i2c_softc *sc;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	mtx_init(&sc->mutex, device_get_nameunit(dev), "I2C", MTX_DEF);
+
+	if (bus_alloc_resources(dev, i2c_spec, sc->res)) {
+		device_printf(dev, "could not allocate resources\n");
+		return (ENXIO);
+	}
+
+	/* Memory interface */
+	sc->bst = rman_get_bustag(sc->res[0]);
+	sc->bsh = rman_get_bushandle(sc->res[0]);
+
+	WRITE1(sc, I2C_IBIC, IBIC_BIIE);
+
+	sc->iicbus = device_add_child(dev, "iicbus", -1);
+	if (sc->iicbus == NULL) {
+		device_printf(dev, "could not add iicbus child");
+		mtx_destroy(&sc->mutex);
+		return (ENXIO);
+	}
+
+	bus_generic_attach(dev);
+
+	return (0);
+}
+
+/* Wait for transfer interrupt flag */
+static int
+wait_for_iif(struct i2c_softc *sc)
+{
+	int retry;
+
+	retry = 1000;
+	while (retry --) {
+		if (READ1(sc, I2C_IBSR) & IBSR_IBIF) {
+			WRITE1(sc, I2C_IBSR, IBSR_IBIF);
+			return (IIC_NOERR);
+		}
+		DELAY(10);
+	}
+
+	return (IIC_ETIMEOUT);
+}
+
+/* Wait for free bus */
+static int
+wait_for_nibb(struct i2c_softc *sc)
+{
+	int retry;
+
+	retry = 1000;
+	while (retry --) {
+		if ((READ1(sc, I2C_IBSR) & IBSR_IBB) == 0)
+			return (IIC_NOERR);
+		DELAY(10);
+	}
+
+	return (IIC_ETIMEOUT);
+}
+
+/* Wait for transfer complete+interrupt flag */
+static int
+wait_for_icf(struct i2c_softc *sc)
+{
+	int retry;
+
+	retry = 1000;
+	while (retry --) {
+		if (READ1(sc, I2C_IBSR) & IBSR_TCF) {
+			if (READ1(sc, I2C_IBSR) & IBSR_IBIF) {
+				WRITE1(sc, I2C_IBSR, IBSR_IBIF);
+				return (IIC_NOERR);
+			}
+		}
+		DELAY(10);
+	}
+
+	return (IIC_ETIMEOUT);
+}
+
+static int
+i2c_repeated_start(device_t dev, u_char slave, int timeout)
+{
+	struct i2c_softc *sc;
+	int error;
+	int reg;
+
+	sc = device_get_softc(dev);
+
+	vf_i2c_dbg(sc, "i2c repeated start\n");
+
+	mtx_lock(&sc->mutex);
+
+	WRITE1(sc, I2C_IBAD, slave);
+
+	if ((READ1(sc, I2C_IBSR) & IBSR_IBB) == 0) {
+		mtx_unlock(&sc->mutex);
+		return (IIC_EBUSBSY);
+	}
+
+	/* Set repeated start condition */
+	DELAY(10);
+
+	reg = READ1(sc, I2C_IBCR);
+	reg |= (IBCR_RSTA | IBCR_IBIE);
+	WRITE1(sc, I2C_IBCR, reg);
+
+	DELAY(10);
+
+	/* Write target address - LSB is R/W bit */
+	WRITE1(sc, I2C_IBDR, slave);
+
+	error = wait_for_iif(sc);
+
+	mtx_unlock(&sc->mutex);
+
+	if (error)
+		return (error);
+
+	return (IIC_NOERR);
+}
+
+static int
+i2c_start(device_t dev, u_char slave, int timeout)
+{
+	struct i2c_softc *sc;
+	int error;
+	int reg;
+
+	sc = device_get_softc(dev);
+
+	vf_i2c_dbg(sc, "i2c start\n");
+
+	mtx_lock(&sc->mutex);
+
+	WRITE1(sc, I2C_IBAD, slave);
+
+	if (READ1(sc, I2C_IBSR) & IBSR_IBB) {
+		mtx_unlock(&sc->mutex);
+		vf_i2c_dbg(sc, "cant i2c start: IIC_EBUSBSY\n");
+		return (IIC_EBUSBSY);
+	}
+
+	/* Set start condition */
+	reg = (IBCR_MSSL | IBCR_NOACK | IBCR_IBIE);
+	WRITE1(sc, I2C_IBCR, reg);
+
+	DELAY(100);
+
+	reg |= (IBCR_TXRX);
+	WRITE1(sc, I2C_IBCR, reg);
+
+	/* Write target address - LSB is R/W bit */
+	WRITE1(sc, I2C_IBDR, slave);
+
+	error = wait_for_iif(sc);
+
+	mtx_unlock(&sc->mutex);
+	if (error) {
+		vf_i2c_dbg(sc, "cant i2c start: iif error\n");
+		return (error);
+	}
+
+	return (IIC_NOERR);
+}
+
+static int
+i2c_stop(device_t dev)
+{
+	struct i2c_softc *sc;
+
+	sc = device_get_softc(dev);
+
+	vf_i2c_dbg(sc, "i2c stop\n");
+
+	mtx_lock(&sc->mutex);
+
+	WRITE1(sc, I2C_IBCR, IBCR_NOACK | IBCR_IBIE);
+
+	DELAY(100);
+
+	/* Reset controller if bus still busy after STOP */
+	if (wait_for_nibb(sc) == IIC_ETIMEOUT) {
+		WRITE1(sc, I2C_IBCR, IBCR_MDIS);
+		DELAY(1000);
+		WRITE1(sc, I2C_IBCR, IBCR_NOACK);
+	}
+	mtx_unlock(&sc->mutex);
+
+	return (IIC_NOERR);
+}
+
+static int
+i2c_reset(device_t dev, u_char speed, u_char addr, u_char *oldadr)
+{
+	struct i2c_softc *sc;
+
+	sc = device_get_softc(dev);
+
+	vf_i2c_dbg(sc, "i2c reset\n");
+
+	switch (speed) {
+	case IIC_FAST:
+	case IIC_SLOW:
+	case IIC_UNKNOWN:
+	case IIC_FASTEST:
+	default:
+		break;
+	}
+
+	mtx_lock(&sc->mutex);
+	WRITE1(sc, I2C_IBCR, IBCR_MDIS);
+
+	DELAY(1000);
+
+	WRITE1(sc, I2C_IBFD, 20);
+	WRITE1(sc, I2C_IBCR, 0x0); /* Enable i2c */
+
+	DELAY(1000);
+
+	mtx_unlock(&sc->mutex);
+
+	return (IIC_NOERR);
+}
+
+static int
+i2c_read(device_t dev, char *buf, int len, int *read, int last, int delay)
+{
+	struct i2c_softc *sc;
+	int error;
+
+	sc = device_get_softc(dev);
+
+	vf_i2c_dbg(sc, "i2c read\n");
+
+	*read = 0;
+
+	mtx_lock(&sc->mutex);
+
+	if (len) {
+		if (len == 1)
+			WRITE1(sc, I2C_IBCR, IBCR_IBIE | IBCR_MSSL |	\
+			    IBCR_NOACK);
+		else
+			WRITE1(sc, I2C_IBCR, IBCR_IBIE | IBCR_MSSL);
+
+		/* dummy read */
+		READ1(sc, I2C_IBDR);
+		DELAY(1000);
+	}
+
+	while (*read < len) {
+		error = wait_for_icf(sc);
+		if (error) {
+			mtx_unlock(&sc->mutex);
+			return (error);
+		}
+
+		if ((*read == len - 2) && last) {
+			/* NO ACK on last byte */
+			WRITE1(sc, I2C_IBCR, IBCR_IBIE | IBCR_MSSL |	\
+			    IBCR_NOACK);
+		}
+
+		if ((*read == len - 1) && last) {
+			/* Transfer done, remove master bit */
+			WRITE1(sc, I2C_IBCR, IBCR_IBIE | IBCR_NOACK);
+		}
+
+		*buf++ = READ1(sc, I2C_IBDR);
+		(*read)++;
+	}
+	mtx_unlock(&sc->mutex);
+
+	return (IIC_NOERR);
+}
+
+static int
+i2c_write(device_t dev, const char *buf, int len, int *sent, int timeout)
+{
+	struct i2c_softc *sc;
+	int error;
+
+	sc = device_get_softc(dev);
+
+	vf_i2c_dbg(sc, "i2c write\n");
+
+	*sent = 0;
+
+	mtx_lock(&sc->mutex);
+	while (*sent < len) {
+
+		WRITE1(sc, I2C_IBDR, *buf++);
+
+		error = wait_for_iif(sc);
+		if (error) {
+			mtx_unlock(&sc->mutex);
+			return (error);
+		}
+
+		(*sent)++;
+	}
+	mtx_unlock(&sc->mutex);
+
+	return (IIC_NOERR);
+}
+
+static device_method_t i2c_methods[] = {
+	DEVMETHOD(device_probe,		i2c_probe),
+	DEVMETHOD(device_attach,	i2c_attach),
+
+	DEVMETHOD(iicbus_callback,		iicbus_null_callback),
+	DEVMETHOD(iicbus_repeated_start,	i2c_repeated_start),
+	DEVMETHOD(iicbus_start,			i2c_start),
+	DEVMETHOD(iicbus_stop,			i2c_stop),
+	DEVMETHOD(iicbus_reset,			i2c_reset),
+	DEVMETHOD(iicbus_read,			i2c_read),
+	DEVMETHOD(iicbus_write,			i2c_write),
+	DEVMETHOD(iicbus_transfer,		iicbus_transfer_gen),
+
+	{ 0, 0 }
+};
+
+static driver_t i2c_driver = {
+	"i2c",
+	i2c_methods,
+	sizeof(struct i2c_softc),
+};
+
+static devclass_t i2c_devclass;
+
+DRIVER_MODULE(i2c, simplebus, i2c_driver, i2c_devclass, 0, 0);
+DRIVER_MODULE(iicbus, i2c, iicbus_driver, iicbus_devclass, 0, 0);
diff --git a/sys/arm/include/cpufunc.h b/sys/arm/include/cpufunc.h
index f38f9c1..68b15f5 100644
--- a/sys/arm/include/cpufunc.h
+++ b/sys/arm/include/cpufunc.h
@@ -523,6 +523,7 @@ void	armv7_setup			(char *string);
 void	armv7_context_switch		(void);
 void	armv7_drain_writebuf		(void);
 void	armv7_sev			(void);
+void	armv7_sleep			(int unused);
 u_int	armv7_auxctrl			(u_int, u_int);
 void	pj4bv7_setup			(char *string);
 void	pj4b_config			(void);
diff --git a/sys/arm/include/machdep.h b/sys/arm/include/machdep.h
index 101adf7..67cc9bf 100644
--- a/sys/arm/include/machdep.h
+++ b/sys/arm/include/machdep.h
@@ -32,6 +32,7 @@ vm_offset_t freebsd_parse_boot_param(struct arm_boot_params *abp);
 vm_offset_t linux_parse_boot_param(struct arm_boot_params *abp);
 vm_offset_t fake_preload_metadata(struct arm_boot_params *abp);
 vm_offset_t parse_boot_param(struct arm_boot_params *abp);
+void arm_generic_initclocks(void);
 
 /*
  * Initialization functions called by the common initarm() function in
diff --git a/sys/arm/lpc/lpc_timer.c b/sys/arm/lpc/lpc_timer.c
index 5769435..ded53fa 100644
--- a/sys/arm/lpc/lpc_timer.c
+++ b/sys/arm/lpc/lpc_timer.c
@@ -280,12 +280,6 @@ lpc_get_timecount(struct timecounter *tc)
 }
 
 void
-cpu_initclocks(void)
-{
-	cpu_initclocks_bsp();
-}
-
-void
 DELAY(int usec)
 {
 	uint32_t counter;
diff --git a/sys/arm/mv/timer.c b/sys/arm/mv/timer.c
index 3c6f149..ef8ce5f 100644
--- a/sys/arm/mv/timer.c
+++ b/sys/arm/mv/timer.c
@@ -224,13 +224,6 @@ mv_timer_get_timecount(struct timecounter *tc)
 }
 
 void
-cpu_initclocks(void)
-{
-
-	cpu_initclocks_bsp();
-}
-
-void
 DELAY(int usec)
 {
 	uint32_t	val, val_temp;
diff --git a/sys/arm/ti/am335x/am335x_dmtimer.c b/sys/arm/ti/am335x/am335x_dmtimer.c
index a01cf6a..240a7a7 100644
--- a/sys/arm/ti/am335x/am335x_dmtimer.c
+++ b/sys/arm/ti/am335x/am335x_dmtimer.c
@@ -662,12 +662,6 @@ DRIVER_MODULE(am335x_dmtimer, simplebus, am335x_dmtimer_driver, am335x_dmtimer_d
 MODULE_DEPEND(am335x_dmtimer, am335x_prcm, 1, 1, 1);
 
 void
-cpu_initclocks(void)
-{
-	cpu_initclocks_bsp();
-}
-
-void
 DELAY(int usec)
 {
 	struct am335x_dmtimer_softc *sc;
diff --git a/sys/dev/aic7xxx/aicasm/Makefile b/sys/dev/aic7xxx/aicasm/Makefile
index 97b93d0..e160807 100644
--- a/sys/dev/aic7xxx/aicasm/Makefile
+++ b/sys/dev/aic7xxx/aicasm/Makefile
@@ -39,3 +39,4 @@ LFLAGS+= -d
 .endif
 
 .include <bsd.prog.mk>
+CFLAGS+= -Wno-missing-prototypes