Import basic support for Nvidia Jetson TK1 board and tegra124 SoC.

The following pheripherals are supported: UART, MMC, AHCI, EHCI, PCIe, I2C,
PMIC, GPIO, CPU temperature and clock.

Note: The PCIe driver is pure mash at this moment. It will be reworked
immediately when both D5237 and D2579 enter the current tree.

36 files changed, 16088 insertions, 0 deletions

diff --git a/sys/arm/nvidia/as3722.c b/sys/arm/nvidia/as3722.c
new file mode 100644
index 0000000..3a03322
--- /dev/null
+++ b/sys/arm/nvidia/as3722.c
@@ -0,0 +1,411 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * AS3722 PMIC driver
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/gpio.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/sx.h>
+
+#include <machine/bus.h>
+
+#include <dev/extres/regulator/regulator.h>
+#include <dev/fdt/fdt_pinctrl.h>
+#include <dev/gpio/gpiobusvar.h>
+#include <dev/iicbus/iiconf.h>
+#include <dev/iicbus/iicbus.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <gnu/dts/include/dt-bindings/mfd/as3722.h>
+
+#include "clock_if.h"
+#include "regdev_if.h"
+
+#include "as3722.h"
+
+static struct ofw_compat_data compat_data[] = {
+	{"ams,as3722",		1},
+	{NULL,			0},
+};
+
+#define	LOCK(_sc)		sx_xlock(&(_sc)->lock)
+#define	UNLOCK(_sc)		sx_xunlock(&(_sc)->lock)
+#define	LOCK_INIT(_sc)		sx_init(&(_sc)->lock, "as3722")
+#define	LOCK_DESTROY(_sc)	sx_destroy(&(_sc)->lock);
+#define	ASSERT_LOCKED(_sc)	sx_assert(&(_sc)->lock, SA_XLOCKED);
+#define	ASSERT_UNLOCKED(_sc)	sx_assert(&(_sc)->lock, SA_UNLOCKED);
+
+#define	AS3722_DEVICE_ID	0x0C
+
+/*
+ * Raw register access function.
+ */
+int
+as3722_read(struct as3722_softc *sc, uint8_t reg, uint8_t *val)
+{
+	uint8_t addr;
+	int rv;
+	struct iic_msg msgs[2] = {
+		{0, IIC_M_WR, 1, &addr},
+		{0, IIC_M_RD, 1, val},
+	};
+
+	msgs[0].slave = sc->bus_addr;
+	msgs[1].slave = sc->bus_addr;
+	addr = reg;
+
+	rv = iicbus_transfer(sc->dev, msgs, 2);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Error when reading reg 0x%02X, rv: %d\n", reg,  rv);
+		return (EIO);
+	}
+
+	return (0);
+}
+
+int as3722_read_buf(struct as3722_softc *sc, uint8_t reg, uint8_t *buf,
+    size_t size)
+{
+	uint8_t addr;
+	int rv;
+	struct iic_msg msgs[2] = {
+		{0, IIC_M_WR, 1, &addr},
+		{0, IIC_M_RD, size, buf},
+	};
+
+	msgs[0].slave = sc->bus_addr;
+	msgs[1].slave = sc->bus_addr;
+	addr = reg;
+
+	rv = iicbus_transfer(sc->dev, msgs, 2);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Error when reading reg 0x%02X, rv: %d\n", reg,  rv);
+		return (EIO);
+	}
+
+	return (0);
+}
+
+int
+as3722_write(struct as3722_softc *sc, uint8_t reg, uint8_t val)
+{
+	uint8_t data[2];
+	int rv;
+
+	struct iic_msg msgs[1] = {
+		{0, IIC_M_WR, 2, data},
+	};
+
+	msgs[0].slave = sc->bus_addr;
+	data[0] = reg;
+	data[1] = val;
+
+	rv = iicbus_transfer(sc->dev, msgs, 1);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Error when writing reg 0x%02X, rv: %d\n", reg, rv);
+		return (EIO);
+	}
+	return (0);
+}
+
+int as3722_write_buf(struct as3722_softc *sc, uint8_t reg, uint8_t *buf,
+    size_t size)
+{
+	uint8_t data[1];
+	int rv;
+	struct iic_msg msgs[2] = {
+		{0, IIC_M_WR, 1, data},
+		{0, IIC_M_WR | IIC_M_NOSTART, size, buf},
+	};
+
+	msgs[0].slave = sc->bus_addr;
+	msgs[1].slave = sc->bus_addr;
+	data[0] = reg;
+
+	rv = iicbus_transfer(sc->dev, msgs, 2);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Error when writing reg 0x%02X, rv: %d\n", reg, rv);
+		return (EIO);
+	}
+	return (0);
+}
+
+int
+as3722_modify(struct as3722_softc *sc, uint8_t reg, uint8_t clear, uint8_t set)
+{
+	uint8_t val;
+	int rv;
+
+	rv = as3722_read(sc, reg, &val);
+	if (rv != 0)
+		return (rv);
+
+	val &= ~clear;
+	val |= set;
+
+	rv = as3722_write(sc, reg, val);
+	if (rv != 0)
+		return (rv);
+
+	return (0);
+}
+
+static int
+as3722_get_version(struct as3722_softc *sc)
+{
+	uint8_t reg;
+	int rv;
+
+	/* Verify AS3722 ID and version. */
+	rv = RD1(sc, AS3722_ASIC_ID1, &reg);
+	if (rv != 0)
+		return (ENXIO);
+
+	if (reg != AS3722_DEVICE_ID) {
+		device_printf(sc->dev, "Invalid chip ID is 0x%x\n", reg);
+		return (ENXIO);
+	}
+
+	rv = RD1(sc, AS3722_ASIC_ID2, &sc->chip_rev);
+	if (rv != 0)
+		return (ENXIO);
+
+	if (bootverbose)
+		device_printf(sc->dev, "AS3722 rev: 0x%x\n", sc->chip_rev);
+	return (0);
+}
+
+static int
+as3722_init(struct as3722_softc *sc)
+{
+	uint32_t reg;
+	int rv;
+
+	reg = 0;
+	if (sc->int_pullup)
+		reg |= AS3722_INT_PULL_UP;
+	if (sc->i2c_pullup)
+		reg |= AS3722_I2C_PULL_UP;
+
+	rv = RM1(sc, AS3722_IO_VOLTAGE,
+	    AS3722_INT_PULL_UP | AS3722_I2C_PULL_UP, reg);
+	if (rv != 0)
+		return (ENXIO);
+
+	/* mask interrupts */
+	rv = WR1(sc, AS3722_INTERRUPT_MASK1, 0);
+	if (rv != 0)
+		return (ENXIO);
+	rv = WR1(sc, AS3722_INTERRUPT_MASK2, 0);
+	if (rv != 0)
+		return (ENXIO);
+	rv = WR1(sc, AS3722_INTERRUPT_MASK3, 0);
+	if (rv != 0)
+		return (ENXIO);
+	rv = WR1(sc, AS3722_INTERRUPT_MASK4, 0);
+	if (rv != 0)
+		return (ENXIO);
+	return (0);
+}
+
+static int
+as3722_parse_fdt(struct as3722_softc *sc, phandle_t node)
+{
+
+	sc->int_pullup =
+	    OF_hasprop(node, "ams,enable-internal-int-pullup") ? 1 : 0;
+	sc->i2c_pullup =
+	    OF_hasprop(node, "ams,enable-internal-i2c-pullup") ? 1 : 0;
+	return 0;
+}
+
+static void
+as3722_intr(void *arg)
+{
+	struct as3722_softc *sc;
+
+	sc = (struct as3722_softc *)arg;
+	/* XXX Finish temperature alarms. */
+}
+
+static int
+as3722_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_search_compatible(dev, compat_data)->ocd_data)
+		return (ENXIO);
+
+	device_set_desc(dev, "AS3722 PMIC");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+as3722_attach(device_t dev)
+{
+	struct as3722_softc *sc;
+	const char *dname;
+	int dunit, rv, rid;
+	phandle_t node;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	sc->bus_addr = iicbus_get_addr(dev);
+	node = ofw_bus_get_node(sc->dev);
+	dname = device_get_name(dev);
+	dunit = device_get_unit(dev);
+	rv = 0;
+	LOCK_INIT(sc);
+
+	rid = 0;
+	sc->irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	    RF_ACTIVE);
+	if (sc->irq_res == NULL) {
+		device_printf(dev, "Cannot allocate interrupt.\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	rv = as3722_parse_fdt(sc, node);
+	if (rv != 0)
+		goto fail;
+	rv = as3722_get_version(sc);
+	if (rv != 0)
+		goto fail;
+	rv = as3722_init(sc);
+	if (rv != 0)
+		goto fail;
+	rv = as3722_regulator_attach(sc, node);
+	if (rv != 0)
+		goto fail;
+	rv = as3722_gpio_attach(sc, node);
+	if (rv != 0)
+		goto fail;
+	rv = as3722_rtc_attach(sc, node);
+	if (rv != 0)
+		goto fail;
+
+	fdt_pinctrl_register(dev, NULL);
+	fdt_pinctrl_configure_by_name(dev, "default");
+
+	/* Setup  interrupt. */
+	rv = bus_setup_intr(dev, sc->irq_res, INTR_TYPE_MISC | INTR_MPSAFE,
+	    NULL, as3722_intr, sc, &sc->irq_h);
+	if (rv) {
+		device_printf(dev, "Cannot setup interrupt.\n");
+		goto fail;
+	}
+	return (bus_generic_attach(dev));
+
+fail:
+	if (sc->irq_h != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->irq_h);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	LOCK_DESTROY(sc);
+	return (rv);
+}
+
+static int
+as3722_detach(device_t dev)
+{
+	struct as3722_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (sc->irq_h != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->irq_h);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	LOCK_DESTROY(sc);
+
+	return (bus_generic_detach(dev));
+}
+
+static phandle_t
+as3722_gpio_get_node(device_t bus, device_t dev)
+{
+
+	/* We only have one child, the GPIO bus, which needs our own node. */
+	return (ofw_bus_get_node(bus));
+}
+
+static device_method_t as3722_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		as3722_probe),
+	DEVMETHOD(device_attach,	as3722_attach),
+	DEVMETHOD(device_detach,	as3722_detach),
+
+	/* Regdev interface */
+	DEVMETHOD(regdev_map,		as3722_regulator_map),
+
+	/* RTC interface */
+	DEVMETHOD(clock_gettime,	as3722_rtc_gettime),
+	DEVMETHOD(clock_settime,	as3722_rtc_settime),
+
+	/* GPIO protocol interface */
+	DEVMETHOD(gpio_get_bus,		as3722_gpio_get_bus),
+	DEVMETHOD(gpio_pin_max,		as3722_gpio_pin_max),
+	DEVMETHOD(gpio_pin_getname,	as3722_gpio_pin_getname),
+	DEVMETHOD(gpio_pin_getflags,	as3722_gpio_pin_getflags),
+	DEVMETHOD(gpio_pin_getcaps,	as3722_gpio_pin_getcaps),
+	DEVMETHOD(gpio_pin_setflags,	as3722_gpio_pin_setflags),
+	DEVMETHOD(gpio_pin_get,		as3722_gpio_pin_get),
+	DEVMETHOD(gpio_pin_set,		as3722_gpio_pin_set),
+	DEVMETHOD(gpio_pin_toggle,	as3722_gpio_pin_toggle),
+	DEVMETHOD(gpio_map_gpios,	as3722_gpio_map_gpios),
+
+	/* fdt_pinctrl interface */
+	DEVMETHOD(fdt_pinctrl_configure, as3722_pinmux_configure),
+
+	/* ofw_bus interface */
+	DEVMETHOD(ofw_bus_get_node,	as3722_gpio_get_node),
+
+	DEVMETHOD_END
+};
+
+static devclass_t as3722_devclass;
+DEFINE_CLASS_0(gpio, as3722_driver, as3722_methods,
+    sizeof(struct as3722_softc));
+EARLY_DRIVER_MODULE(as3722, iicbus, as3722_driver, as3722_devclass,
+    0, 0, 74);
diff --git a/sys/arm/nvidia/as3722.h b/sys/arm/nvidia/as3722.h
new file mode 100644
index 0000000..c559a8a
--- /dev/null
+++ b/sys/arm/nvidia/as3722.h
@@ -0,0 +1,323 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _AS3722_H_
+
+#include <sys/clock.h>
+
+#define	AS3722_SD0_VOLTAGE			0x00
+#define	  AS3722_SD_VSEL_MASK				0x7F /* For all SD */
+#define	  AS3722_SD0_VSEL_MIN				0x01
+#define	  AS3722_SD0_VSEL_MAX				0x5A
+#define	  AS3722_SD0_VSEL_LOW_VOL_MAX			0x6E
+
+#define	AS3722_SD1_VOLTAGE			0x01
+#define	AS3722_SD2_VOLTAGE			0x02
+#define	  AS3722_SD2_VSEL_MIN				0x01
+#define	  AS3722_SD2_VSEL_MAX				0x7F
+#define	AS3722_SD3_VOLTAGE			0x03
+#define	AS3722_SD4_VOLTAGE			0x04
+#define	AS3722_SD5_VOLTAGE			0x05
+#define	AS3722_SD6_VOLTAGE			0x06
+#define	AS3722_GPIO0_CONTROL			0x08
+#define	  AS3722_GPIO_INVERT				0x80
+#define	  AS3722_GPIO_IOSF_MASK				0x0F
+#define	  AS3722_GPIO_IOSF_SHIFT			3
+#define	  AS3722_GPIO_MODE_MASK				0x07
+#define	  AS3722_GPIO_MODE_SHIFT			0
+
+#define	AS3722_GPIO1_CONTROL			0x09
+#define	AS3722_GPIO2_CONTROL			0x0A
+#define	AS3722_GPIO3_CONTROL			0x0B
+#define	AS3722_GPIO4_CONTROL			0x0C
+#define	AS3722_GPIO5_CONTROL			0x0D
+#define	AS3722_GPIO6_CONTROL			0x0E
+#define	AS3722_GPIO7_CONTROL			0x0F
+#define	AS3722_LDO0_VOLTAGE			0x10
+#define	  AS3722_LDO0_VSEL_MASK				0x1F
+#define	  AS3722_LDO0_VSEL_MIN				0x01
+#define	  AS3722_LDO0_VSEL_MAX				0x12
+#define	  AS3722_LDO0_NUM_VOLT				0x12
+
+#define	AS3722_LDO1_VOLTAGE			0x11
+#define	  AS3722_LDO_VSEL_MASK				0x7F
+#define	  AS3722_LDO_VSEL_MIN				0x01
+#define	  AS3722_LDO_VSEL_MAX				0x7F
+#define	  AS3722_LDO_VSEL_DNU_MIN			0x25
+#define	  AS3722_LDO_VSEL_DNU_MAX			0x3F
+#define	  AS3722_LDO_NUM_VOLT				0x80
+
+#define	AS3722_LDO2_VOLTAGE			0x12
+#define	AS3722_LDO3_VOLTAGE			0x13
+#define	  AS3722_LDO3_VSEL_MASK				0x3F
+#define	  AS3722_LDO3_VSEL_MIN				0x01
+#define	  AS3722_LDO3_VSEL_MAX				0x2D
+#define	  AS3722_LDO3_NUM_VOLT				0x2D
+#define	  AS3722_LDO3_MODE_MASK				(0x3 << 6)
+#define	  AS3722_LDO3_MODE_GET(x)			(((x) >> 6) & 0x3)
+#define	  AS3722_LDO3_MODE(x)				(((x) & 0x3) << 6)
+#define	  AS3722_LDO3_MODE_PMOS				AS3722_LDO3_MODE(0)
+#define	  AS3722_LDO3_MODE_PMOS_TRACKING		AS3722_LDO3_MODE(1)
+#define	  AS3722_LDO3_MODE_NMOS				AS3722_LDO3_MODE(2)
+#define	  AS3722_LDO3_MODE_SWITCH			AS3722_LDO3_MODE(3)
+
+#define	AS3722_LDO4_VOLTAGE			0x14
+#define	AS3722_LDO5_VOLTAGE			0x15
+#define	AS3722_LDO6_VOLTAGE			0x16
+#define	  AS3722_LDO6_SEL_BYPASS			0x3F
+#define	AS3722_LDO7_VOLTAGE			0x17
+#define	AS3722_LDO9_VOLTAGE			0x19
+#define	AS3722_LDO10_VOLTAGE			0x1A
+#define	AS3722_LDO11_VOLTAGE			0x1B
+#define	AS3722_LDO3_SETTINGS			0x1D
+#define	AS3722_GPIO_DEB1			0x1E
+#define	AS3722_GPIO_DEB2			0x1F
+#define	AS3722_GPIO_SIGNAL_OUT			0x20
+#define	AS3722_GPIO_SIGNAL_IN			0x21
+#define	AS3722_REG_SEQU_MOD1			0x22
+#define	AS3722_REG_SEQU_MOD2			0x23
+#define	AS3722_REG_SEQU_MOD3			0x24
+#define	AS3722_SD_PHSW_CTRL			0x27
+#define	AS3722_SD_PHSW_STATUS			0x28
+
+#define	AS3722_SD0_CONTROL			0x29
+#define	 AS3722_SD0_MODE_FAST				(1 << 4)
+
+#define	AS3722_SD1_CONTROL			0x2A
+#define	 AS3722_SD1_MODE_FAST				(1 << 4)
+
+#define	AS3722_SDMPH_CONTROL			0x2B
+#define	AS3722_SD23_CONTROL			0x2C
+#define	 AS3722_SD3_MODE_FAST				(1 << 6)
+#define	 AS3722_SD2_MODE_FAST				(1 << 2)
+
+#define	AS3722_SD4_CONTROL			0x2D
+#define	 AS3722_SD4_MODE_FAST				(1 << 2)
+
+#define	AS3722_SD5_CONTROL			0x2E
+#define	 AS3722_SD5_MODE_FAST				(1 << 2)
+
+#define	AS3722_SD6_CONTROL			0x2F
+#define	 AS3722_SD6_MODE_FAST				(1 << 4)
+
+#define	AS3722_SD_DVM				0x30
+#define	AS3722_RESET_REASON			0x31
+#define	AS3722_BATTERY_VOLTAGE_MONITOR		0x32
+#define	AS3722_STARTUP_CONTROL			0x33
+#define	AS3722_RESET_TIMER			0x34
+#define	AS3722_REFERENCE_CONTROL		0x35
+#define	AS3722_RESET_CONTROL			0x36
+#define	AS3722_OVERTEMPERATURE_CONTROL		0x37
+#define	AS3722_WATCHDOG_CONTROL			0x38
+#define	AS3722_REG_STANDBY_MOD1			0x39
+#define	AS3722_REG_STANDBY_MOD2			0x3A
+#define	AS3722_REG_STANDBY_MOD3			0x3B
+#define	AS3722_ENABLE_CTRL1			0x3C
+#define	 AS3722_SD3_EXT_ENABLE_MASK			0xC0
+#define	 AS3722_SD2_EXT_ENABLE_MASK			0x30
+#define	 AS3722_SD1_EXT_ENABLE_MASK			0x0C
+#define	 AS3722_SD0_EXT_ENABLE_MASK			0x03
+
+#define	AS3722_ENABLE_CTRL2			0x3D
+#define	 AS3722_SD6_EXT_ENABLE_MASK			0x30
+#define	 AS3722_SD5_EXT_ENABLE_MASK			0x0C
+#define	 AS3722_SD4_EXT_ENABLE_MASK			0x03
+
+#define	AS3722_ENABLE_CTRL3			0x3E
+#define	 AS3722_LDO3_EXT_ENABLE_MASK			0xC0
+#define	 AS3722_LDO2_EXT_ENABLE_MASK			0x30
+#define	 AS3722_LDO1_EXT_ENABLE_MASK			0x0C
+#define	 AS3722_LDO0_EXT_ENABLE_MASK			0x03
+
+#define	AS3722_ENABLE_CTRL4			0x3F
+#define	 AS3722_LDO7_EXT_ENABLE_MASK			0xC0
+#define	 AS3722_LDO6_EXT_ENABLE_MASK			0x30
+#define	 AS3722_LDO5_EXT_ENABLE_MASK			0x0C
+#define	 AS3722_LDO4_EXT_ENABLE_MASK			0x03
+
+#define	AS3722_ENABLE_CTRL5			0x40
+#define	 AS3722_LDO11_EXT_ENABLE_MASK			0xC0
+#define	 AS3722_LDO10_EXT_ENABLE_MASK			0x30
+#define	 AS3722_LDO9_EXT_ENABLE_MASK			0x0C
+
+#define	AS3722_PWM_CONTROL_L			0x41
+#define	AS3722_PWM_CONTROL_H			0x42
+#define	AS3722_WATCHDOG_TIMER			0x46
+#define	AS3722_WATCHDOG_SOFTWARE_SIGNAL		0x48
+#define	AS3722_IO_VOLTAGE			0x49
+#define	 AS3722_I2C_PULL_UP				(1 << 4)
+#define	 AS3722_INT_PULL_UP				(1 << 5)
+
+#define	AS3722_BATTERY_VOLTAGE_MONITOR2		0x4A
+#define	AS3722_SD_CONTROL			0x4D
+#define	  AS3722_SDN_CTRL(x)				(1 << (x))
+
+#define	AS3722_LDO_CONTROL0			0x4E
+#define	 AS3722_LDO7_CTRL				(1 << 7)
+#define	 AS3722_LDO6_CTRL				(1 << 6)
+#define	 AS3722_LDO5_CTRL				(1 << 5)
+#define	 AS3722_LDO4_CTRL				(1 << 4)
+#define	 AS3722_LDO3_CTRL				(1 << 3)
+#define	 AS3722_LDO2_CTRL				(1 << 2)
+#define	 AS3722_LDO1_CTRL				(1 << 1)
+#define	 AS3722_LDO0_CTRL				(1 << 0)
+
+#define	AS3722_LDO_CONTROL1			0x4F
+#define	 AS3722_LDO11_CTRL				(1 << 3)
+#define	 AS3722_LDO10_CTRL				(1 << 2)
+#define	 AS3722_LDO9_CTRL				(1 << 1)
+
+#define	AS3722_SD0_PROTECT			0x50
+#define	AS3722_SD6_PROTECT			0x51
+#define	AS3722_PWM_VCONTROL1			0x52
+#define	AS3722_PWM_VCONTROL2			0x53
+#define	AS3722_PWM_VCONTROL3			0x54
+#define	AS3722_PWM_VCONTROL4			0x55
+#define	AS3722_BB_CHARGER			0x57
+#define	AS3722_CTRL_SEQU1			0x58
+#define	AS3722_CTRL_SEQU2			0x59
+#define	AS3722_OV_CURRENT			0x5A
+#define	AS3722_OV_CURRENT_DEB			0x5B
+#define	AS3722_SDLV_DEB				0x5C
+#define	AS3722_OC_PG_CTRL			0x5D
+#define	AS3722_OC_PG_CTRL2			0x5E
+#define	AS3722_CTRL_STATUS			0x5F
+#define	AS3722_RTC_CONTROL			0x60
+#define	 AS3722_RTC_AM_PM_MODE				(1 << 7)
+#define	 AS3722_RTC_CLK32K_OUT_EN			(1 << 5)
+#define	 AS3722_RTC_IRQ_MODE				(1 << 3)
+#define	 AS3722_RTC_ON					(1 << 2)
+#define	 AS3722_RTC_ALARM_WAKEUP_EN			(1 << 1)
+#define	 AS3722_RTC_REP_WAKEUP_EN			(1 << 0)
+
+#define	AS3722_RTC_SECOND			0x61
+#define	AS3722_RTC_MINUTE			0x62
+#define	AS3722_RTC_HOUR				0x63
+#define	AS3722_RTC_DAY				0x64
+#define	AS3722_RTC_MONTH			0x65
+#define	AS3722_RTC_YEAR				0x66
+#define	AS3722_RTC_ALARM_SECOND			0x67
+#define	AS3722_RTC_ALARM_MINUTE			0x68
+#define	AS3722_RTC_ALARM_HOUR			0x69
+#define	AS3722_RTC_ALARM_DAY			0x6A
+#define	AS3722_RTC_ALARM_MONTH			0x6B
+#define	AS3722_RTC_ALARM_YEAR			0x6C
+#define	AS3722_SRAM				0x6D
+#define	AS3722_RTC_ACCESS			0x6F
+#define	AS3722_REG_STATUS			0x73
+#define	AS3722_INTERRUPT_MASK1			0x74
+#define	AS3722_INTERRUPT_MASK2			0x75
+#define	AS3722_INTERRUPT_MASK3			0x76
+#define	AS3722_INTERRUPT_MASK4			0x77
+#define	AS3722_INTERRUPT_STATUS1		0x78
+#define	AS3722_INTERRUPT_STATUS2		0x79
+#define	AS3722_INTERRUPT_STATUS3		0x7A
+#define	AS3722_INTERRUPT_STATUS4		0x7B
+#define	AS3722_TEMP_STATUS			0x7D
+#define	AS3722_ADC0_CONTROL			0x80
+#define	AS3722_ADC1_CONTROL			0x81
+#define	AS3722_ADC0_MSB_RESULT			0x82
+#define	AS3722_ADC0_LSB_RESULT			0x83
+#define	AS3722_ADC1_MSB_RESULT			0x84
+#define	AS3722_ADC1_LSB_RESULT			0x85
+#define	AS3722_ADC1_THRESHOLD_HI_MSB		0x86
+#define	AS3722_ADC1_THRESHOLD_HI_LSB		0x87
+#define	AS3722_ADC1_THRESHOLD_LO_MSB		0x88
+#define	AS3722_ADC1_THRESHOLD_LO_LSB		0x89
+#define	AS3722_ADC_CONFIGURATION		0x8A
+#define	AS3722_ASIC_ID1				0x90
+#define	AS3722_ASIC_ID2				0x91
+#define	AS3722_LOCK				0x9E
+#define	AS3722_FUSE7				0x9E
+#define	  AS3722_FUSE7_SD0_LOW_VOLTAGE			(1 << 4)
+
+struct as3722_reg_sc;
+struct as3722_gpio_pin;
+
+struct as3722_softc {
+	device_t		dev;
+	struct sx		lock;
+	int			bus_addr;
+	struct resource		*irq_res;
+	void			*irq_h;
+
+	uint8_t			chip_rev;
+	int			int_pullup;
+	int			i2c_pullup;
+
+	/* Regulators. */
+	struct as3722_reg_sc	**regs;
+	int			nregs;
+
+	/* GPIO */
+	device_t		gpio_busdev;
+	struct as3722_gpio_pin	**gpio_pins;
+	int			gpio_npins;
+	struct sx		gpio_lock;
+
+};
+
+#define	RD1(sc, reg, val)	as3722_read(sc, reg, val)
+#define	WR1(sc, reg, val)	as3722_write(sc, reg, val)
+#define	RM1(sc, reg, clr, set)	as3722_modify(sc, reg, clr, set)
+
+int as3722_read(struct as3722_softc *sc, uint8_t reg, uint8_t *val);
+int as3722_write(struct as3722_softc *sc, uint8_t reg, uint8_t val);
+int as3722_modify(struct as3722_softc *sc, uint8_t reg, uint8_t clear,
+    uint8_t set);
+int as3722_read_buf(struct as3722_softc *sc, uint8_t reg, uint8_t *buf,
+    size_t size);
+int as3722_write_buf(struct as3722_softc *sc, uint8_t reg, uint8_t *buf,
+    size_t size);
+
+/* Regulators */
+int as3722_regulator_attach(struct as3722_softc *sc, phandle_t node);
+int as3722_regulator_map(device_t dev, phandle_t xref, int ncells,
+    pcell_t *cells, int *num);
+
+/* RTC */
+int as3722_rtc_attach(struct as3722_softc *sc, phandle_t node);
+int as3722_rtc_gettime(device_t dev, struct timespec *ts);
+int as3722_rtc_settime(device_t dev, struct timespec *ts);
+
+/* GPIO */
+device_t as3722_gpio_get_bus(device_t dev);
+int as3722_gpio_pin_max(device_t dev, int *maxpin);
+int as3722_gpio_pin_getname(device_t dev, uint32_t pin, char *name);
+int as3722_gpio_pin_getflags(device_t dev, uint32_t pin, uint32_t *flags);
+int as3722_gpio_pin_getcaps(device_t dev, uint32_t pin, uint32_t *caps);
+int as3722_gpio_pin_setflags(device_t dev, uint32_t pin, uint32_t flags);
+int as3722_gpio_pin_set(device_t dev, uint32_t pin, unsigned int value);
+int as3722_gpio_pin_get(device_t dev, uint32_t pin, unsigned int *val);
+int as3722_gpio_pin_toggle(device_t dev, uint32_t pin);
+int as3722_gpio_map_gpios(device_t dev, phandle_t pdev, phandle_t gparent,
+    int gcells, pcell_t *gpios, uint32_t *pin, uint32_t *flags);
+int as3722_gpio_attach(struct as3722_softc *sc, phandle_t node);
+int as3722_pinmux_configure(device_t dev, phandle_t cfgxref);
+
+#endif /* _AS3722_H_ */
diff --git a/sys/arm/nvidia/as3722_gpio.c b/sys/arm/nvidia/as3722_gpio.c
new file mode 100644
index 0000000..8e53bce
--- /dev/null
+++ b/sys/arm/nvidia/as3722_gpio.c
@@ -0,0 +1,577 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/gpio.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+#include <sys/sx.h>
+
+#include <machine/bus.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/gpio/gpiobusvar.h>
+
+#include "as3722.h"
+
+MALLOC_DEFINE(M_AS3722_GPIO, "AS3722 gpio", "AS3722 GPIO");
+
+/* AS3722_GPIOx_CONTROL	 MODE and IOSF definition. */
+#define	AS3722_IOSF_GPIO				0x00
+#define	AS3722_IOSF_INTERRUPT_OUT			0x01
+#define	AS3722_IOSF_VSUP_VBAT_LOW_UNDEBOUNCE_OUT	0x02
+#define	AS3722_IOSF_GPIO_IN_INTERRUPT			0x03
+#define	AS3722_IOSF_PWM_IN				0x04
+#define	AS3722_IOSF_VOLTAGE_IN_STANDBY			0x05
+#define	AS3722_IOSF_OC_PG_SD0				0x06
+#define	AS3722_IOSF_POWERGOOD_OUT			0x07
+#define	AS3722_IOSF_CLK32K_OUT				0x08
+#define	AS3722_IOSF_WATCHDOG_IN				0x09
+#define	AS3722_IOSF_SOFT_RESET_IN			0x0b
+#define	AS3722_IOSF_PWM_OUT				0x0c
+#define	AS3722_IOSF_VSUP_VBAT_LOW_DEBOUNCE_OUT		0x0d
+#define	AS3722_IOSF_OC_PG_SD6				0x0e
+
+#define	AS3722_MODE_INPUT				0
+#define	AS3722_MODE_PUSH_PULL				1
+#define	AS3722_MODE_OPEN_DRAIN				2
+#define	AS3722_MODE_TRISTATE				3
+#define	AS3722_MODE_INPUT_PULL_UP_LV			4
+#define	AS3722_MODE_INPUT_PULL_DOWN			5
+#define	AS3722_MODE_OPEN_DRAIN_LV			6
+#define	AS3722_MODE_PUSH_PULL_LV			7
+
+#define	NGPIO		8
+
+#define	GPIO_LOCK(_sc)	sx_slock(&(_sc)->gpio_lock)
+#define	GPIO_UNLOCK(_sc)	sx_unlock(&(_sc)->gpio_lock)
+#define	GPIO_ASSERT(_sc)	sx_assert(&(_sc)->gpio_lock, SA_LOCKED)
+
+#define	AS3722_CFG_BIAS_DISABLE		0x0001
+#define	AS3722_CFG_BIAS_PULL_UP		0x0002
+#define	AS3722_CFG_BIAS_PULL_DOWN	0x0004
+#define	AS3722_CFG_BIAS_HIGH_IMPEDANCE	0x0008
+#define	AS3722_CFG_OPEN_DRAIN		0x0010
+
+static const struct {
+	const char	*name;
+	int  		config;		/* AS3722_CFG_  */
+} as3722_cfg_names[] = {
+	{"bias-disable",	AS3722_CFG_BIAS_DISABLE},
+	{"bias-pull-up",	AS3722_CFG_BIAS_PULL_UP},
+	{"bias-pull-down",	AS3722_CFG_BIAS_PULL_DOWN},
+	{"bias-high-impedance",	AS3722_CFG_BIAS_HIGH_IMPEDANCE},
+	{"drive-open-drain",	AS3722_CFG_OPEN_DRAIN},
+};
+
+static struct {
+	const char *name;
+	int fnc_val;
+} as3722_fnc_table[] = {
+	{"gpio",			AS3722_IOSF_GPIO},
+	{"interrupt-out",		AS3722_IOSF_INTERRUPT_OUT},
+	{"vsup-vbat-low-undebounce-out", AS3722_IOSF_VSUP_VBAT_LOW_UNDEBOUNCE_OUT},
+	{"gpio-in-interrupt",		AS3722_IOSF_GPIO_IN_INTERRUPT},
+	{"pwm-in",			AS3722_IOSF_PWM_IN},
+	{"voltage-in-standby",		AS3722_IOSF_VOLTAGE_IN_STANDBY},
+	{"oc-pg-sd0",			AS3722_IOSF_OC_PG_SD0},
+	{"powergood-out",		AS3722_IOSF_POWERGOOD_OUT},
+	{"clk32k-out",			AS3722_IOSF_CLK32K_OUT},
+	{"watchdog-in",			AS3722_IOSF_WATCHDOG_IN},
+	{"soft-reset-in",		AS3722_IOSF_SOFT_RESET_IN},
+	{"pwm-out",			AS3722_IOSF_PWM_OUT},
+	{"vsup-vbat-low-debounce-out",	AS3722_IOSF_VSUP_VBAT_LOW_DEBOUNCE_OUT},
+	{"oc-pg-sd6",			AS3722_IOSF_OC_PG_SD6},
+};
+
+struct as3722_pincfg {
+	char	*function;
+	int	flags;
+};
+
+struct as3722_gpio_pin {
+	int	pin_caps;
+	uint8_t	pin_ctrl_reg;
+	char	pin_name[GPIOMAXNAME];
+	int	pin_cfg_flags;
+};
+
+
+/* --------------------------------------------------------------------------
+ *
+ *  Pinmux functions.
+ */
+static int
+as3722_pinmux_get_function(struct as3722_softc *sc, char *name)
+{
+	int i;
+
+	for (i = 0; i < nitems(as3722_fnc_table); i++) {
+		if (strcmp(as3722_fnc_table[i].name, name) == 0)
+			 return (as3722_fnc_table[i].fnc_val);
+	}
+	return (-1);
+}
+
+
+
+static int
+as3722_pinmux_config_node(struct as3722_softc *sc, char *pin_name,
+    struct as3722_pincfg *cfg)
+{
+	uint8_t ctrl;
+	int rv, fnc, pin;
+
+	for (pin = 0; pin < sc->gpio_npins; pin++) {
+		if (strcmp(sc->gpio_pins[pin]->pin_name, pin_name) == 0)
+			 break;
+	}
+	if (pin >= sc->gpio_npins) {
+		device_printf(sc->dev, "Unknown pin: %s\n", pin_name);
+		return (ENXIO);
+	}
+
+	ctrl = sc->gpio_pins[pin]->pin_ctrl_reg;
+	sc->gpio_pins[pin]->pin_cfg_flags = cfg->flags;
+	if (cfg->function != NULL) {
+		fnc = as3722_pinmux_get_function(sc, cfg->function);
+		if (fnc == -1) {
+			device_printf(sc->dev,
+			    "Unknown function %s for pin %s\n", cfg->function,
+			    sc->gpio_pins[pin]->pin_name);
+			return (ENXIO);
+		}
+		switch (fnc) {
+		case AS3722_IOSF_INTERRUPT_OUT:
+		case AS3722_IOSF_VSUP_VBAT_LOW_UNDEBOUNCE_OUT:
+		case AS3722_IOSF_OC_PG_SD0:
+		case AS3722_IOSF_POWERGOOD_OUT:
+		case AS3722_IOSF_CLK32K_OUT:
+		case AS3722_IOSF_PWM_OUT:
+		case AS3722_IOSF_OC_PG_SD6:
+			ctrl &= ~(AS3722_GPIO_MODE_MASK <<
+			    AS3722_GPIO_MODE_SHIFT);
+			ctrl |= AS3722_MODE_PUSH_PULL << AS3722_GPIO_MODE_SHIFT;
+			/* XXX Handle flags (OC + pullup) */
+			break;
+		case AS3722_IOSF_GPIO_IN_INTERRUPT:
+		case AS3722_IOSF_PWM_IN:
+		case AS3722_IOSF_VOLTAGE_IN_STANDBY:
+		case AS3722_IOSF_WATCHDOG_IN:
+		case AS3722_IOSF_SOFT_RESET_IN:
+			ctrl &= ~(AS3722_GPIO_MODE_MASK <<
+			    AS3722_GPIO_MODE_SHIFT);
+			ctrl |= AS3722_MODE_INPUT << AS3722_GPIO_MODE_SHIFT;
+			/* XXX Handle flags (pulldown + pullup) */
+
+		default:
+			break;
+		}
+		ctrl &= ~(AS3722_GPIO_IOSF_MASK << AS3722_GPIO_IOSF_SHIFT);
+		ctrl |= fnc << AS3722_GPIO_IOSF_SHIFT;
+	}
+	rv = 0;
+	if (ctrl != sc->gpio_pins[pin]->pin_ctrl_reg) {
+		rv = WR1(sc, AS3722_GPIO0_CONTROL + pin, ctrl);
+		sc->gpio_pins[pin]->pin_ctrl_reg = ctrl;
+	}
+	return (rv);
+}
+
+static int
+as3722_pinmux_read_node(struct as3722_softc *sc, phandle_t node,
+     struct as3722_pincfg *cfg, char **pins, int *lpins)
+{
+	int rv, i;
+
+	*lpins = OF_getprop_alloc(node, "pins", 1, (void **)pins);
+	if (*lpins <= 0)
+		return (ENOENT);
+
+	/* Read function (mux) settings. */
+	rv = OF_getprop_alloc(node, "function", 1, (void **)&cfg->function);
+	if (rv <= 0)
+		cfg->function = NULL;
+
+	/* Read boolean properties. */
+	for (i = 0; i < nitems(as3722_cfg_names); i++) {
+		if (OF_hasprop(node, as3722_cfg_names[i].name))
+			cfg->flags |= as3722_cfg_names[i].config;
+	}
+	return (0);
+}
+
+static int
+as3722_pinmux_process_node(struct as3722_softc *sc, phandle_t node)
+{
+	struct as3722_pincfg cfg;
+	char *pins, *pname;
+	int i, len, lpins, rv;
+
+	rv = as3722_pinmux_read_node(sc, node, &cfg, &pins, &lpins);
+	if (rv != 0)
+		return (rv);
+
+	len = 0;
+	pname = pins;
+	do {
+		i = strlen(pname) + 1;
+		rv = as3722_pinmux_config_node(sc, pname, &cfg);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			    "Cannot configure pin: %s: %d\n", pname, rv);
+		}
+		len += i;
+		pname += i;
+	} while (len < lpins);
+
+	if (pins != NULL)
+		free(pins, M_OFWPROP);
+	if (cfg.function != NULL)
+		free(cfg.function, M_OFWPROP);
+
+	return (rv);
+}
+
+int as3722_pinmux_configure(device_t dev, phandle_t cfgxref)
+{
+	struct as3722_softc *sc;
+	phandle_t node, cfgnode;
+	int rv;
+
+	sc = device_get_softc(dev);
+	cfgnode = OF_node_from_xref(cfgxref);
+
+	for (node = OF_child(cfgnode); node != 0; node = OF_peer(node)) {
+		if (!fdt_is_enabled(node))
+			continue;
+		rv = as3722_pinmux_process_node(sc, node);
+		if (rv != 0)
+			device_printf(dev, "Failed to process pinmux");
+
+	}
+	return (0);
+}
+
+/* --------------------------------------------------------------------------
+ *
+ *  GPIO
+ */
+device_t
+as3722_gpio_get_bus(device_t dev)
+{
+	struct as3722_softc *sc;
+
+	sc = device_get_softc(dev);
+	return (sc->gpio_busdev);
+}
+
+int
+as3722_gpio_pin_max(device_t dev, int *maxpin)
+{
+
+	*maxpin = NGPIO - 1;
+	return (0);
+}
+
+int
+as3722_gpio_pin_getcaps(device_t dev, uint32_t pin, uint32_t *caps)
+{
+	struct as3722_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+	GPIO_LOCK(sc);
+	*caps = sc->gpio_pins[pin]->pin_caps;
+	GPIO_UNLOCK(sc);
+	return (0);
+}
+
+int
+as3722_gpio_pin_getname(device_t dev, uint32_t pin, char *name)
+{
+	struct as3722_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+	GPIO_LOCK(sc);
+	memcpy(name, sc->gpio_pins[pin]->pin_name, GPIOMAXNAME);
+	GPIO_UNLOCK(sc);
+	return (0);
+}
+
+int
+as3722_gpio_pin_getflags(device_t dev, uint32_t pin, uint32_t *out_flags)
+{
+	struct as3722_softc *sc;
+	uint8_t tmp, mode, iosf;
+	uint32_t flags;
+	bool inverted;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	tmp = sc->gpio_pins[pin]->pin_ctrl_reg;
+	GPIO_UNLOCK(sc);
+	iosf = (tmp >> AS3722_GPIO_IOSF_SHIFT) & AS3722_GPIO_IOSF_MASK;
+	mode = (tmp >> AS3722_GPIO_MODE_SHIFT) & AS3722_GPIO_MODE_MASK;
+	inverted = (tmp & AS3722_GPIO_INVERT) != 0;
+	/* Is pin in GPIO mode ? */
+	if (iosf != AS3722_IOSF_GPIO)
+		return (ENXIO);
+
+	flags = 0;
+	switch (mode) {
+	case AS3722_MODE_INPUT:
+		flags = GPIO_PIN_INPUT;
+		break;
+	case AS3722_MODE_PUSH_PULL:
+	case AS3722_MODE_PUSH_PULL_LV:
+		flags = GPIO_PIN_OUTPUT;
+		break;
+	case AS3722_MODE_OPEN_DRAIN:
+	case AS3722_MODE_OPEN_DRAIN_LV:
+		flags = GPIO_PIN_INPUT | GPIO_PIN_OUTPUT | GPIO_PIN_OPENDRAIN;
+		break;
+	case AS3722_MODE_TRISTATE:
+		flags = GPIO_PIN_TRISTATE;
+		break;
+	case AS3722_MODE_INPUT_PULL_UP_LV:
+		flags = GPIO_PIN_INPUT | GPIO_PIN_PULLUP;
+		break;
+
+	case AS3722_MODE_INPUT_PULL_DOWN:
+		flags = GPIO_PIN_OUTPUT | GPIO_PIN_PULLDOWN;
+		break;
+	}
+	if (inverted)
+		flags |= GPIO_PIN_INVIN | GPIO_PIN_INVOUT;
+	*out_flags = flags;
+	return (0);
+}
+
+static int
+as3722_gpio_get_mode(struct as3722_softc *sc, uint32_t pin, uint32_t gpio_flags)
+{
+	uint8_t ctrl;
+	int flags;
+
+	ctrl = sc->gpio_pins[pin]->pin_ctrl_reg;
+	flags =  sc->gpio_pins[pin]->pin_cfg_flags;
+
+	/* Tristate mode. */
+	if (flags & AS3722_CFG_BIAS_HIGH_IMPEDANCE ||
+	    gpio_flags & GPIO_PIN_TRISTATE)
+		return (AS3722_MODE_TRISTATE);
+
+	/* Open drain modes. */
+	if (flags & AS3722_CFG_OPEN_DRAIN || gpio_flags & GPIO_PIN_OPENDRAIN) {
+		/* Only pull up have effect */
+		if (flags & AS3722_CFG_BIAS_PULL_UP ||
+		    gpio_flags & GPIO_PIN_PULLUP)
+			return (AS3722_MODE_OPEN_DRAIN_LV);
+		return (AS3722_MODE_OPEN_DRAIN);
+	}
+	/* Input modes. */
+	if (gpio_flags & GPIO_PIN_INPUT) {
+		/* Accept pull up or pull down. */
+		if (flags & AS3722_CFG_BIAS_PULL_UP ||
+		    gpio_flags & GPIO_PIN_PULLUP)
+			return (AS3722_MODE_INPUT_PULL_UP_LV);
+
+		if (flags & AS3722_CFG_BIAS_PULL_DOWN ||
+		    gpio_flags & GPIO_PIN_PULLDOWN)
+			return (AS3722_MODE_INPUT_PULL_DOWN);
+		return (AS3722_MODE_INPUT);
+	}
+	/*
+	 * Output modes.
+	 * Pull down is used as indicator of low voltage output.
+	 */
+	if (flags & AS3722_CFG_BIAS_PULL_DOWN ||
+		    gpio_flags & GPIO_PIN_PULLDOWN)
+		return (AS3722_MODE_PUSH_PULL_LV);
+	return (AS3722_MODE_PUSH_PULL);
+}
+
+int
+as3722_gpio_pin_setflags(device_t dev, uint32_t pin, uint32_t flags)
+{
+	struct as3722_softc *sc;
+	uint8_t ctrl, mode, iosf;
+	int rv;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	ctrl = sc->gpio_pins[pin]->pin_ctrl_reg;
+	iosf = (ctrl >> AS3722_GPIO_IOSF_SHIFT) & AS3722_GPIO_IOSF_MASK;
+	/* Is pin in GPIO mode ? */
+	if (iosf != AS3722_IOSF_GPIO) {
+		GPIO_UNLOCK(sc);
+		return (ENXIO);
+	}
+	mode = as3722_gpio_get_mode(sc, pin, flags);
+	ctrl &= ~(AS3722_GPIO_MODE_MASK << AS3722_GPIO_MODE_SHIFT);
+	ctrl |= AS3722_MODE_PUSH_PULL << AS3722_GPIO_MODE_SHIFT;
+	rv = 0;
+	if (ctrl != sc->gpio_pins[pin]->pin_ctrl_reg) {
+		rv = WR1(sc, AS3722_GPIO0_CONTROL + pin, ctrl);
+		sc->gpio_pins[pin]->pin_ctrl_reg = ctrl;
+	}
+	GPIO_UNLOCK(sc);
+	return (rv);
+}
+
+int
+as3722_gpio_pin_set(device_t dev, uint32_t pin, uint32_t val)
+{
+	struct as3722_softc *sc;
+	uint8_t tmp;
+	int rv;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	tmp =  (val != 0) ? 1 : 0;
+	if (sc->gpio_pins[pin]->pin_ctrl_reg & AS3722_GPIO_INVERT)
+		tmp ^= 1;
+
+	GPIO_LOCK(sc);
+	rv = RM1(sc, AS3722_GPIO_SIGNAL_OUT, (1 << pin), (tmp << pin));
+	GPIO_UNLOCK(sc);
+	return (rv);
+}
+
+int
+as3722_gpio_pin_get(device_t dev, uint32_t pin, uint32_t *val)
+{
+	struct as3722_softc *sc;
+	uint8_t tmp, mode, ctrl;
+	int rv;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	ctrl = sc->gpio_pins[pin]->pin_ctrl_reg;
+	mode = (ctrl >> AS3722_GPIO_MODE_SHIFT) & AS3722_GPIO_MODE_MASK;
+	if ((mode == AS3722_MODE_PUSH_PULL) ||
+	    (mode == AS3722_MODE_PUSH_PULL_LV))
+		rv = RD1(sc, AS3722_GPIO_SIGNAL_OUT, &tmp);
+	else
+		rv = RD1(sc, AS3722_GPIO_SIGNAL_IN, &tmp);
+	GPIO_UNLOCK(sc);
+	if (rv != 0)
+		return (rv);
+
+	*val = tmp & (1 << pin) ? 1 : 0;
+	if (ctrl & AS3722_GPIO_INVERT)
+		*val ^= 1;
+	return (0);
+}
+
+int
+as3722_gpio_pin_toggle(device_t dev, uint32_t pin)
+{
+	struct as3722_softc *sc;
+	uint8_t tmp;
+	int rv;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	rv = RD1(sc, AS3722_GPIO_SIGNAL_OUT, &tmp);
+	if (rv != 0) {
+		GPIO_UNLOCK(sc);
+		return (rv);
+	}
+	tmp ^= (1 <<pin);
+	rv = RM1(sc, AS3722_GPIO_SIGNAL_OUT, (1 << pin), tmp);
+	GPIO_UNLOCK(sc);
+	return (0);
+}
+
+int
+as3722_gpio_map_gpios(device_t dev, phandle_t pdev, phandle_t gparent,
+    int gcells, pcell_t *gpios, uint32_t *pin, uint32_t *flags)
+{
+
+	if (gcells != 2)
+		return (ERANGE);
+	*pin = gpios[0];
+	*flags= gpios[1];
+	return (0);
+}
+
+int
+as3722_gpio_attach(struct as3722_softc *sc, phandle_t node)
+{
+	struct as3722_gpio_pin *pin;
+	int i, rv;
+
+	sx_init(&sc->gpio_lock, "AS3722 GPIO lock");
+	sc->gpio_npins = NGPIO;
+	sc->gpio_pins = malloc(sizeof(struct as3722_gpio_pin *) *
+	    sc->gpio_npins, M_AS3722_GPIO, M_WAITOK | M_ZERO);
+
+
+	sc->gpio_busdev = gpiobus_attach_bus(sc->dev);
+	if (sc->gpio_busdev == NULL)
+		return (ENXIO);
+	for (i = 0; i < sc->gpio_npins; i++) {
+		sc->gpio_pins[i] = malloc(sizeof(struct as3722_gpio_pin),
+		    M_AS3722_GPIO, M_WAITOK | M_ZERO);
+		pin = sc->gpio_pins[i];
+		sprintf(pin->pin_name, "gpio%d", i);
+		pin->pin_caps = GPIO_PIN_INPUT | GPIO_PIN_OUTPUT  |
+		    GPIO_PIN_OPENDRAIN | GPIO_PIN_PUSHPULL | GPIO_PIN_TRISTATE |
+		    GPIO_PIN_PULLUP | GPIO_PIN_PULLDOWN | GPIO_PIN_INVIN |
+		    GPIO_PIN_INVOUT;
+		rv = RD1(sc, AS3722_GPIO0_CONTROL + i, &pin->pin_ctrl_reg);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			    "Cannot read configuration for pin %s\n",
+			    sc->gpio_pins[i]->pin_name);
+		}
+	}
+	return (0);
+}
diff --git a/sys/arm/nvidia/as3722_regulators.c b/sys/arm/nvidia/as3722_regulators.c
new file mode 100644
index 0000000..0bca8e1
--- /dev/null
+++ b/sys/arm/nvidia/as3722_regulators.c
@@ -0,0 +1,811 @@
+/*-
+ * Copyright 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/gpio.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/sx.h>
+
+#include <machine/bus.h>
+
+#include <dev/extres/regulator/regulator.h>
+#include <dev/gpio/gpiobusvar.h>
+
+#include <gnu/dts/include/dt-bindings/mfd/as3722.h>
+
+#include "as3722.h"
+
+MALLOC_DEFINE(M_AS3722_REG, "AS3722 regulator", "AS3722 power regulator");
+
+#define	DIV_ROUND_UP(n,d) (((n) + (d) - 1) / (d))
+
+enum as3722_reg_id {
+	AS3722_REG_ID_SD0,
+	AS3722_REG_ID_SD1,
+	AS3722_REG_ID_SD2,
+	AS3722_REG_ID_SD3,
+	AS3722_REG_ID_SD4,
+	AS3722_REG_ID_SD5,
+	AS3722_REG_ID_SD6,
+	AS3722_REG_ID_LDO0,
+	AS3722_REG_ID_LDO1,
+	AS3722_REG_ID_LDO2,
+	AS3722_REG_ID_LDO3,
+	AS3722_REG_ID_LDO4,
+	AS3722_REG_ID_LDO5,
+	AS3722_REG_ID_LDO6,
+	AS3722_REG_ID_LDO7,
+	AS3722_REG_ID_LDO9,
+	AS3722_REG_ID_LDO10,
+	AS3722_REG_ID_LDO11,
+};
+
+struct regulator_range {
+	u_int	min_uvolt;
+	u_int	step_uvolt;
+	u_int	min_sel;
+	u_int	max_sel;
+};
+
+
+/* Regulator HW definition. */
+struct reg_def {
+	intptr_t		id;		/* ID */
+	char			*name;		/* Regulator name */
+	char			*supply_name;	/* Source property name */
+	uint8_t			volt_reg;
+	uint8_t			volt_vsel_mask;
+	uint8_t			enable_reg;
+	uint8_t			enable_mask;
+	uint8_t			ext_enable_reg;
+	uint8_t			ext_enable_mask;
+	struct regulator_range	*ranges;
+	int			nranges;
+};
+
+struct as3722_reg_sc {
+	struct regnode		*regnode;
+	struct as3722_softc	*base_sc;
+	struct reg_def		*def;
+	phandle_t		xref;
+
+	struct regnode_std_param *param;
+	int 			ext_control;
+	int	 		enable_tracking;
+
+	int			enable_usec;
+};
+
+#define	RANGE_INIT(_min_sel, _max_sel, _min_uvolt, _step_uvolt)		\
+{									\
+	.min_sel	= _min_sel,					\
+	.max_sel	= _max_sel,					\
+	.min_uvolt	= _min_uvolt,					\
+	.step_uvolt	= _step_uvolt,					\
+}
+
+static struct regulator_range as3722_sd016_ranges[] = {
+	RANGE_INIT(0x00, 0x00,       0,     0),
+	RANGE_INIT(0x01, 0x5A,  610000, 10000),
+};
+
+static struct regulator_range as3722_sd0_lv_ranges[] = {
+	RANGE_INIT(0x00, 0x00,       0,     0),
+	RANGE_INIT(0x01, 0x6E,  410000, 10000),
+};
+
+static struct regulator_range as3722_sd_ranges[] = {
+	RANGE_INIT(0x00, 0x00,       0,     0),
+	RANGE_INIT(0x01, 0x40,  612500, 12500),
+	RANGE_INIT(0x41, 0x70, 1425000, 25000),
+	RANGE_INIT(0x71, 0x7F, 2650000, 50000),
+};
+
+static struct regulator_range as3722_ldo3_ranges[] = {
+	RANGE_INIT(0x00, 0x00,       0,     0),
+	RANGE_INIT(0x01, 0x2D,  620000, 20000),
+};
+
+static struct regulator_range as3722_ldo_ranges[] = {
+	RANGE_INIT(0x00, 0x00,       0,     0),
+	RANGE_INIT(0x01, 0x24,  825000, 25000),
+	RANGE_INIT(0x40, 0x7F, 1725000, 25000),
+};
+
+static struct reg_def as3722s_def[] = {
+	{
+		.id = AS3722_REG_ID_SD0,
+		.name = "sd0",
+		.volt_reg = AS3722_SD0_VOLTAGE,
+		.volt_vsel_mask = AS3722_SD_VSEL_MASK,
+		.enable_reg = AS3722_SD_CONTROL,
+		.enable_mask = AS3722_SDN_CTRL(0),
+		.ext_enable_reg = AS3722_ENABLE_CTRL1,
+		.ext_enable_mask = AS3722_SD0_EXT_ENABLE_MASK,
+		.ranges = as3722_sd016_ranges,
+		.nranges = nitems(as3722_sd016_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_SD1,
+		.name = "sd1",
+		.volt_reg = AS3722_SD1_VOLTAGE,
+		.volt_vsel_mask = AS3722_SD_VSEL_MASK,
+		.enable_reg = AS3722_SD_CONTROL,
+		.enable_mask = AS3722_SDN_CTRL(1),
+		.ext_enable_reg = AS3722_ENABLE_CTRL1,
+		.ext_enable_mask = AS3722_SD1_EXT_ENABLE_MASK,
+		.ranges = as3722_sd_ranges,
+		.nranges = nitems(as3722_sd_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_SD2,
+		.name = "sd2",
+		.supply_name = "vsup-sd2",
+		.volt_reg = AS3722_SD2_VOLTAGE,
+		.volt_vsel_mask = AS3722_SD_VSEL_MASK,
+		.enable_reg = AS3722_SD_CONTROL,
+		.enable_mask = AS3722_SDN_CTRL(2),
+		.ext_enable_reg = AS3722_ENABLE_CTRL1,
+		.ext_enable_mask = AS3722_SD2_EXT_ENABLE_MASK,
+		.ranges = as3722_sd_ranges,
+		.nranges = nitems(as3722_sd_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_SD3,
+		.name = "sd3",
+		.supply_name = "vsup-sd3",
+		.volt_reg = AS3722_SD3_VOLTAGE,
+		.volt_vsel_mask = AS3722_SD_VSEL_MASK,
+		.enable_reg = AS3722_SD_CONTROL,
+		.enable_mask = AS3722_SDN_CTRL(3),
+		.ext_enable_reg = AS3722_ENABLE_CTRL1,
+		.ext_enable_mask = AS3722_SD3_EXT_ENABLE_MASK,
+		.ranges = as3722_sd_ranges,
+		.nranges = nitems(as3722_sd_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_SD4,
+		.name = "sd4",
+		.supply_name = "vsup-sd4",
+		.volt_reg = AS3722_SD4_VOLTAGE,
+		.volt_vsel_mask = AS3722_SD_VSEL_MASK,
+		.enable_reg = AS3722_SD_CONTROL,
+		.enable_mask = AS3722_SDN_CTRL(4),
+		.ext_enable_reg = AS3722_ENABLE_CTRL2,
+		.ext_enable_mask = AS3722_SD4_EXT_ENABLE_MASK,
+		.ranges = as3722_sd_ranges,
+		.nranges = nitems(as3722_sd_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_SD5,
+		.name = "sd5",
+		.supply_name = "vsup-sd5",
+		.volt_reg = AS3722_SD5_VOLTAGE,
+		.volt_vsel_mask = AS3722_SD_VSEL_MASK,
+		.enable_reg = AS3722_SD_CONTROL,
+		.enable_mask = AS3722_SDN_CTRL(5),
+		.ext_enable_reg = AS3722_ENABLE_CTRL2,
+		.ext_enable_mask = AS3722_SD5_EXT_ENABLE_MASK,
+		.ranges = as3722_sd_ranges,
+		.nranges = nitems(as3722_sd_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_SD6,
+		.name = "sd6",
+		.volt_reg = AS3722_SD6_VOLTAGE,
+		.volt_vsel_mask = AS3722_SD_VSEL_MASK,
+		.enable_reg = AS3722_SD_CONTROL,
+		.enable_mask = AS3722_SDN_CTRL(6),
+		.ext_enable_reg = AS3722_ENABLE_CTRL2,
+		.ext_enable_mask = AS3722_SD6_EXT_ENABLE_MASK,
+		.ranges = as3722_sd016_ranges,
+		.nranges = nitems(as3722_sd016_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO0,
+		.name = "ldo0",
+		.supply_name = "vin-ldo0",
+		.volt_reg = AS3722_LDO0_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO0_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL0,
+		.enable_mask = AS3722_LDO0_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL3,
+		.ext_enable_mask = AS3722_LDO0_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO1,
+		.name = "ldo1",
+		.supply_name = "vin-ldo1-6",
+		.volt_reg = AS3722_LDO1_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL0,
+		.enable_mask = AS3722_LDO1_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL3,
+		.ext_enable_mask = AS3722_LDO1_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO2,
+		.name = "ldo2",
+		.supply_name = "vin-ldo2-5-7",
+		.volt_reg = AS3722_LDO2_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL0,
+		.enable_mask = AS3722_LDO2_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL3,
+		.ext_enable_mask = AS3722_LDO2_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO3,
+		.name = "ldo3",
+		.supply_name = "vin-ldo3-4",
+		.volt_reg = AS3722_LDO3_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO3_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL0,
+		.enable_mask = AS3722_LDO3_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL3,
+		.ext_enable_mask = AS3722_LDO3_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo3_ranges,
+		.nranges = nitems(as3722_ldo3_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO4,
+		.name = "ldo4",
+		.supply_name = "vin-ldo3-4",
+		.volt_reg = AS3722_LDO4_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL0,
+		.enable_mask = AS3722_LDO4_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL4,
+		.ext_enable_mask = AS3722_LDO4_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO5,
+		.name = "ldo5",
+		.supply_name = "vin-ldo2-5-7",
+		.volt_reg = AS3722_LDO5_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL0,
+		.enable_mask = AS3722_LDO5_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL4,
+		.ext_enable_mask = AS3722_LDO5_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO6,
+		.name = "ldo6",
+		.supply_name = "vin-ldo1-6",
+		.volt_reg = AS3722_LDO6_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL0,
+		.enable_mask = AS3722_LDO6_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL4,
+		.ext_enable_mask = AS3722_LDO6_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO7,
+		.name = "ldo7",
+		.supply_name = "vin-ldo2-5-7",
+		.volt_reg = AS3722_LDO7_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL0,
+		.enable_mask = AS3722_LDO7_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL4,
+		.ext_enable_mask = AS3722_LDO7_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO9,
+		.name = "ldo9",
+		.supply_name = "vin-ldo9-10",
+		.volt_reg = AS3722_LDO9_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL1,
+		.enable_mask = AS3722_LDO9_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL5,
+		.ext_enable_mask = AS3722_LDO9_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO10,
+		.name = "ldo10",
+		.supply_name = "vin-ldo9-10",
+		.volt_reg = AS3722_LDO10_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL1,
+		.enable_mask = AS3722_LDO10_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL5,
+		.ext_enable_mask = AS3722_LDO10_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+	{
+		.id = AS3722_REG_ID_LDO11,
+		.name = "ldo11",
+		.supply_name = "vin-ldo11",
+		.volt_reg = AS3722_LDO11_VOLTAGE,
+		.volt_vsel_mask = AS3722_LDO_VSEL_MASK,
+		.enable_reg = AS3722_LDO_CONTROL1,
+		.enable_mask = AS3722_LDO11_CTRL,
+		.ext_enable_reg = AS3722_ENABLE_CTRL5,
+		.ext_enable_mask = AS3722_LDO11_EXT_ENABLE_MASK,
+		.ranges = as3722_ldo_ranges,
+		.nranges = nitems(as3722_ldo_ranges),
+	},
+};
+
+
+struct as3722_regnode_init_def {
+	struct regnode_init_def	reg_init_def;
+	int 			ext_control;
+	int	 		enable_tracking;
+};
+
+static int as3722_regnode_init(struct regnode *regnode);
+static int as3722_regnode_enable(struct regnode *regnode, bool enable,
+    int *udelay);
+static int as3722_regnode_set_volt(struct regnode *regnode, int min_uvolt,
+    int max_uvolt, int *udelay);
+static int as3722_regnode_get_volt(struct regnode *regnode, int *uvolt);
+static regnode_method_t as3722_regnode_methods[] = {
+	/* Regulator interface */
+	REGNODEMETHOD(regnode_init,		as3722_regnode_init),
+	REGNODEMETHOD(regnode_enable,		as3722_regnode_enable),
+	REGNODEMETHOD(regnode_set_voltage,	as3722_regnode_set_volt),
+	REGNODEMETHOD(regnode_get_voltage,	as3722_regnode_get_volt),
+	REGNODEMETHOD_END
+};
+DEFINE_CLASS_1(as3722_regnode, as3722_regnode_class, as3722_regnode_methods,
+   sizeof(struct as3722_reg_sc), regnode_class);
+
+static int
+regulator_range_sel_to_volt(struct as3722_reg_sc *sc, uint8_t sel, int *volt)
+{
+	struct regulator_range *range;
+	struct reg_def *def;
+	int i;
+
+	def = sc->def;
+	if (def->nranges == 0)
+		panic("Voltage regulator have zero ranges\n");
+
+	for (i = 0; i < def->nranges ; i++) {
+		range = def->ranges  + i;
+
+		if (!(sel >= range->min_sel &&
+		      sel <= range->max_sel))
+			continue;
+
+		sel -= range->min_sel;
+
+		*volt = range->min_uvolt + sel * range->step_uvolt;
+		return (0);
+	}
+
+	return (ERANGE);
+}
+
+static int
+regulator_range_volt_to_sel(struct as3722_reg_sc *sc, int min_uvolt,
+    int max_uvolt, uint8_t *out_sel)
+{
+	struct regulator_range *range;
+	struct reg_def *def;
+	uint8_t sel;
+	int uvolt;
+	int rv, i;
+
+	def = sc->def;
+	if (def->nranges == 0)
+		panic("Voltage regulator have zero ranges\n");
+
+	for (i = 0; i < def->nranges; i++) {
+		range = def->ranges  + i;
+		uvolt = range->min_uvolt +
+			(range->max_sel - range->min_sel) * range->step_uvolt;
+
+		if ((min_uvolt > uvolt) ||
+		    (max_uvolt < range->min_uvolt))
+			continue;
+
+		if (min_uvolt <= range->min_uvolt)
+			min_uvolt = range->min_uvolt;
+
+		/* If step is zero then range is fixed voltage range. */
+		if (range->step_uvolt == 0)
+			sel = 0;
+		else
+			sel = DIV_ROUND_UP(min_uvolt - range->min_uvolt,
+			   range->step_uvolt);
+
+
+		sel += range->min_sel;
+
+		break;
+	}
+
+	if (i >= def->nranges)
+		return (ERANGE);
+
+	/* Verify new settings. */
+	rv = regulator_range_sel_to_volt(sc, sel, &uvolt);
+	if (rv != 0)
+		return (rv);
+	if ((uvolt < min_uvolt) || (uvolt > max_uvolt))
+		return (ERANGE);
+
+	*out_sel = sel;
+	return (0);
+}
+
+
+static int
+as3722_read_sel(struct as3722_reg_sc *sc, uint8_t *sel)
+{
+	int rv;
+
+	rv = RD1(sc->base_sc, sc->def->volt_reg, sel);
+	if (rv != 0)
+		return (rv);
+	*sel &= sc->def->volt_vsel_mask;
+	*sel >>= ffs(sc->def->volt_vsel_mask) - 1;
+	return (0);
+}
+
+static int
+as3722_write_sel(struct as3722_reg_sc *sc, uint8_t sel)
+{
+	int rv;
+
+	sel <<= ffs(sc->def->volt_vsel_mask) - 1;
+	sel &= sc->def->volt_vsel_mask;
+
+	rv = RM1(sc->base_sc, sc->def->volt_reg,
+	    sc->def->volt_vsel_mask, sel);
+	if (rv != 0)
+		return (rv);
+	return (rv);
+}
+
+static bool
+as3722_sd0_is_low_voltage(struct as3722_reg_sc *sc)
+{
+	uint8_t val;
+	int rv;
+
+	rv = RD1(sc->base_sc, AS3722_FUSE7, &val);
+	if (rv != 0)
+		return (rv);
+	return (val & AS3722_FUSE7_SD0_LOW_VOLTAGE ? true : false);
+}
+
+static int
+as3722_reg_extreg_setup(struct as3722_reg_sc *sc, int ext_pwr_ctrl)
+{
+	uint8_t val;
+	int rv;
+
+	val =  ext_pwr_ctrl << (ffs(sc->def->ext_enable_mask) - 1);
+	rv = RM1(sc->base_sc, sc->def->ext_enable_reg,
+	    sc->def->ext_enable_mask, val);
+	return (rv);
+}
+
+static int
+as3722_reg_enable(struct as3722_reg_sc *sc)
+{
+	int rv;
+
+	rv = RM1(sc->base_sc, sc->def->enable_reg,
+	    sc->def->enable_mask, sc->def->enable_mask);
+	return (rv);
+}
+
+static int
+as3722_reg_disable(struct as3722_reg_sc *sc)
+{
+	int rv;
+
+	rv = RM1(sc->base_sc, sc->def->enable_reg,
+	    sc->def->enable_mask, 0);
+	return (rv);
+}
+
+static int
+as3722_regnode_init(struct regnode *regnode)
+{
+	struct as3722_reg_sc *sc;
+	int rv;
+
+	sc = regnode_get_softc(regnode);
+
+	sc->enable_usec = 500;
+	if (sc->def->id == AS3722_REG_ID_SD0) {
+		if (as3722_sd0_is_low_voltage(sc)) {
+			sc->def->ranges = as3722_sd0_lv_ranges;
+			sc->def->nranges = nitems(as3722_sd0_lv_ranges);
+		}
+		sc->enable_usec = 600;
+	} else if (sc->def->id == AS3722_REG_ID_LDO3) {
+		if (sc->enable_tracking) {
+			rv = RM1(sc->base_sc, sc->def->volt_reg,
+			    AS3722_LDO3_MODE_MASK,
+			    AS3722_LDO3_MODE_PMOS_TRACKING);
+			if (rv < 0) {
+				device_printf(sc->base_sc->dev,
+					"LDO3 tracking failed: %d\n", rv);
+				return (rv);
+			}
+		}
+	}
+
+	if (sc->ext_control) {
+
+		rv = as3722_reg_enable(sc);
+		if (rv < 0) {
+			device_printf(sc->base_sc->dev,
+				"Failed to enable %s regulator: %d\n",
+				sc->def->name, rv);
+			return (rv);
+		}
+		rv = as3722_reg_extreg_setup(sc, sc->ext_control);
+		if (rv < 0) {
+			device_printf(sc->base_sc->dev,
+				"%s ext control failed: %d", sc->def->name, rv);
+			return (rv);
+		}
+	}
+	return (0);
+}
+
+static void
+as3722_fdt_parse(struct as3722_softc *sc, phandle_t node, struct reg_def *def,
+struct as3722_regnode_init_def *init_def)
+{
+	int rv;
+	phandle_t parent, supply_node;
+	char prop_name[64]; /* Maximum OFW property name length. */
+
+	rv = regulator_parse_ofw_stdparam(sc->dev, node,
+	    &init_def->reg_init_def);
+
+	rv = OF_getencprop(node, "ams,ext-control", &init_def->ext_control,
+	    sizeof(init_def->ext_control));
+	if (rv <= 0)
+		init_def->ext_control = 0;
+	if (init_def->ext_control > 3) {
+		device_printf(sc->dev,
+		    "Invalid value for ams,ext-control property: %d\n",
+		    init_def->ext_control);
+		init_def->ext_control = 0;
+	}
+	if (OF_hasprop(node, "ams,enable-tracking"))
+		init_def->enable_tracking = 1;
+
+
+	/* Get parent supply. */
+	if (def->supply_name == NULL)
+		 return;
+
+	parent = OF_parent(node);
+	snprintf(prop_name, sizeof(prop_name), "%s-supply",
+	    def->supply_name);
+	rv = OF_getencprop(parent, prop_name, &supply_node,
+	    sizeof(supply_node));
+	if (rv <= 0)
+		return;
+	supply_node = OF_node_from_xref(supply_node);
+	rv = OF_getprop_alloc(supply_node, "regulator-name", 1,
+	    (void **)&init_def->reg_init_def.parent_name);
+	if (rv <= 0)
+		init_def->reg_init_def.parent_name = NULL;
+}
+
+static struct as3722_reg_sc *
+as3722_attach(struct as3722_softc *sc, phandle_t node, struct reg_def *def)
+{
+	struct as3722_reg_sc *reg_sc;
+	struct as3722_regnode_init_def init_def;
+	struct regnode *regnode;
+
+	bzero(&init_def, sizeof(init_def));
+
+	as3722_fdt_parse(sc, node, def, &init_def);
+	init_def.reg_init_def.id = def->id;
+	init_def.reg_init_def.ofw_node = node;
+	regnode = regnode_create(sc->dev, &as3722_regnode_class,
+	    &init_def.reg_init_def);
+	if (regnode == NULL) {
+		device_printf(sc->dev, "Cannot create regulator.\n");
+		return (NULL);
+	}
+	reg_sc = regnode_get_softc(regnode);
+
+	/* Init regulator softc. */
+	reg_sc->regnode = regnode;
+	reg_sc->base_sc = sc;
+	reg_sc->def = def;
+	reg_sc->xref = OF_xref_from_node(node);
+
+	reg_sc->param = regnode_get_stdparam(regnode);
+	reg_sc->ext_control = init_def.ext_control;
+	reg_sc->enable_tracking = init_def.enable_tracking;
+
+	regnode_register(regnode);
+	if (bootverbose) {
+		int volt, rv;
+		regnode_topo_slock();
+		rv = regnode_get_voltage(regnode, &volt);
+		if (rv == ENODEV) {
+			device_printf(sc->dev,
+			   " Regulator %s: parent doesn't exist yet.\n",
+			   regnode_get_name(regnode));
+		} else if (rv != 0) {
+			device_printf(sc->dev,
+			   " Regulator %s: voltage: INVALID!!!\n",
+			   regnode_get_name(regnode));
+		} else {
+			device_printf(sc->dev,
+			    " Regulator %s: voltage: %d uV\n",
+			    regnode_get_name(regnode), volt);
+		}
+		regnode_topo_unlock();
+	}
+
+	return (reg_sc);
+}
+
+int
+as3722_regulator_attach(struct as3722_softc *sc, phandle_t node)
+{
+	struct as3722_reg_sc *reg;
+	phandle_t child, rnode;
+	int i;
+
+	rnode = ofw_bus_find_child(node, "regulators");
+	if (rnode <= 0) {
+		device_printf(sc->dev, " Cannot find regulators subnode\n");
+		return (ENXIO);
+	}
+
+	sc->nregs = nitems(as3722s_def);
+	sc->regs = malloc(sizeof(struct as3722_reg_sc *) * sc->nregs,
+	    M_AS3722_REG, M_WAITOK | M_ZERO);
+
+
+	/* Attach all known regulators if exist in DT. */
+	for (i = 0; i < sc->nregs; i++) {
+		child = ofw_bus_find_child(rnode, as3722s_def[i].name);
+		if (child == 0) {
+			if (bootverbose)
+				device_printf(sc->dev,
+				    "Regulator %s missing in DT\n",
+				    as3722s_def[i].name);
+			continue;
+		}
+		reg = as3722_attach(sc, child, as3722s_def + i);
+		if (reg == NULL) {
+			device_printf(sc->dev, "Cannot attach regulator: %s\n",
+			    as3722s_def[i].name);
+			return (ENXIO);
+		}
+		sc->regs[i] = reg;
+	}
+	return (0);
+}
+
+int
+as3722_regulator_map(device_t dev, phandle_t xref, int ncells,
+    pcell_t *cells, int *num)
+{
+	struct as3722_softc *sc;
+	int i;
+
+	sc = device_get_softc(dev);
+	for (i = 0; i < sc->nregs; i++) {
+		if (sc->regs[i] == NULL)
+			continue;
+		if (sc->regs[i]->xref == xref) {
+			*num = sc->regs[i]->def->id;
+			return (0);
+		}
+	}
+	return (ENXIO);
+}
+
+static int
+as3722_regnode_enable(struct regnode *regnode, bool val, int *udelay)
+{
+	struct as3722_reg_sc *sc;
+	int rv;
+
+	sc = regnode_get_softc(regnode);
+
+	if (val)
+		rv = as3722_reg_enable(sc);
+	else
+		rv = as3722_reg_disable(sc);
+	*udelay = sc->enable_usec;
+	return (rv);
+}
+
+static int
+as3722_regnode_set_volt(struct regnode *regnode, int min_uvolt, int max_uvolt,
+    int *udelay)
+{
+	struct as3722_reg_sc *sc;
+	uint8_t sel;
+	int rv;
+
+	sc = regnode_get_softc(regnode);
+
+	*udelay = 0;
+	rv = regulator_range_volt_to_sel(sc, min_uvolt, max_uvolt, &sel);
+	if (rv != 0)
+		return (rv);
+	rv = as3722_write_sel(sc, sel);
+	return (rv);
+
+}
+
+static int
+as3722_regnode_get_volt(struct regnode *regnode, int *uvolt)
+{
+	struct as3722_reg_sc *sc;
+	uint8_t sel;
+	int rv;
+
+	sc = regnode_get_softc(regnode);
+	rv = as3722_read_sel(sc, &sel);
+	if (rv != 0)
+		return (rv);
+
+	/* LDO6 have bypass. */
+	if (sc->def->id == AS3722_REG_ID_LDO6 && sel == AS3722_LDO6_SEL_BYPASS)
+		return (ENOENT);
+	rv = regulator_range_sel_to_volt(sc, sel, uvolt);
+	return (rv);
+}
diff --git a/sys/arm/nvidia/as3722_rtc.c b/sys/arm/nvidia/as3722_rtc.c
new file mode 100644
index 0000000..8f13104
--- /dev/null
+++ b/sys/arm/nvidia/as3722_rtc.c
@@ -0,0 +1,115 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/clock.h>
+#include <sys/kernel.h>
+
+#include <dev/ofw/ofw_bus.h>
+
+#include "clock_if.h"
+#include "as3722.h"
+
+#define	AS3722_RTC_START_YEAR	2000
+
+int
+as3722_rtc_gettime(device_t dev, struct timespec *ts)
+{
+	struct as3722_softc *sc;
+	struct clocktime ct;
+	uint8_t buf[6];
+	int rv;
+
+	sc = device_get_softc(dev);
+
+	rv = as3722_read_buf(sc, AS3722_RTC_SECOND, buf, 6);
+	if (rv != 0) {
+		device_printf(sc->dev, "Failed to read RTC data\n");
+		return (rv);
+	}
+	ct.nsec = 0;
+	ct.sec = bcd2bin(buf[0] & 0x7F);
+	ct.min = bcd2bin(buf[1] & 0x7F);
+	ct.hour = bcd2bin(buf[2] & 0x3F);
+	ct.day = bcd2bin(buf[3] & 0x3F);
+	ct.mon = bcd2bin(buf[4] & 0x1F);
+	ct.year = bcd2bin(buf[5] & 0x7F) + AS3722_RTC_START_YEAR;
+	ct.dow = -1;
+
+	return clock_ct_to_ts(&ct, ts);
+}
+
+int
+as3722_rtc_settime(device_t dev, struct timespec *ts)
+{
+	struct as3722_softc *sc;
+	struct clocktime ct;
+	uint8_t buf[6];
+	int rv;
+
+	sc = device_get_softc(dev);
+	clock_ts_to_ct(ts, &ct);
+
+	if (ct.year < AS3722_RTC_START_YEAR)
+		return (EINVAL);
+
+	buf[0] = bin2bcd(ct.sec);
+	buf[1] = bin2bcd(ct.min);
+	buf[2] = bin2bcd(ct.hour);
+	buf[3] = bin2bcd(ct.day);
+	buf[4] = bin2bcd(ct.mon);
+	buf[5] = bin2bcd(ct.year - AS3722_RTC_START_YEAR);
+
+	rv = as3722_write_buf(sc, AS3722_RTC_SECOND, buf, 6);
+	if (rv != 0) {
+		device_printf(sc->dev, "Failed to write RTC data\n");
+		return (rv);
+	}
+	return (0);
+}
+
+int
+as3722_rtc_attach(struct as3722_softc *sc, phandle_t node)
+{
+	int rv;
+
+	/* Enable RTC, set 24 hours mode  and alarms */
+	rv = RM1(sc, AS3722_RTC_CONTROL,
+	    AS3722_RTC_ON | AS3722_RTC_ALARM_WAKEUP_EN | AS3722_RTC_AM_PM_MODE,
+	    AS3722_RTC_ON | AS3722_RTC_ALARM_WAKEUP_EN);
+	if (rv < 0) {
+		device_printf(sc->dev, "Failed to initialize RTC controller\n");
+		return (ENXIO);
+	}
+	clock_register(sc->dev, 1000000);
+
+	return (0);
+}
diff --git a/sys/arm/nvidia/tegra124/files.tegra124 b/sys/arm/nvidia/tegra124/files.tegra124
new file mode 100644
index 0000000..d4d38f7
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/files.tegra124
@@ -0,0 +1,57 @@
+# $FreeBSD$
+
+#
+# Standard ARM support.
+#
+kern/kern_clocksource.c			standard
+dev/ofw/ofw_cpu.c			optional	fdt
+
+#
+# Standard tegra124 devices and support.
+#
+arm/nvidia/tegra124/tegra124_machdep.c	standard
+arm/nvidia/tegra124/tegra124_mp.c	optional	smp
+arm/nvidia/tegra124/tegra124_car.c	standard
+arm/nvidia/tegra124/tegra124_clk_pll.c	standard
+arm/nvidia/tegra124/tegra124_clk_per.c	standard
+arm/nvidia/tegra124/tegra124_clk_super.c standard
+arm/nvidia/tegra124/tegra124_xusbpadctl.c standard
+arm/nvidia/tegra124/tegra124_pmc.c	standard
+arm/nvidia/tegra124/tegra124_cpufreq.c	standard
+arm/nvidia/tegra124/tegra124_coretemp.c	standard
+arm/nvidia/tegra_usbphy.c		standard
+arm/nvidia/tegra_pinmux.c		standard
+arm/nvidia/tegra_uart.c			optional	uart
+arm/nvidia/tegra_sdhci.c		optional	sdhci
+arm/nvidia/tegra_gpio.c			optional	gpio
+arm/nvidia/tegra_ehci.c			optional	ehci
+arm/nvidia/tegra_ahci.c			optional	ahci
+arm/nvidia/tegra_pcie.c			optional	pci
+arm/nvidia/tegra_i2c.c			optional	iic
+arm/nvidia/tegra_rtc.c			standard
+arm/nvidia/tegra_abpmisc.c		standard
+arm/nvidia/tegra_efuse.c		standard
+arm/nvidia/tegra_soctherm_if.m		standard
+arm/nvidia/tegra_soctherm.c		standard
+arm/nvidia/tegra_lic.c			standard
+#arm/nvidia/tegra_hda.c			optional	snd_hda
+#arm/nvidia/drm2/hdmi.c			optional	drm2
+#arm/nvidia/drm2/tegra_drm_if.m		optional	drm2
+#arm/nvidia/drm2/tegra_drm_subr.c	optional	drm2
+#arm/nvidia/drm2/tegra_host1x.c		optional	drm2
+#arm/nvidia/drm2/tegra_hdmi.c		optional	drm2
+#arm/nvidia/drm2/tegra_dc_if.m		optional	drm2
+#arm/nvidia/drm2/tegra_dc.c		optional	drm2
+#arm/nvidia/drm2/tegra_fb.c		optional	drm2
+#arm/nvidia/drm2/tegra_bo.c		optional	drm2
+#
+# Optional devices.
+#
+
+#
+# Temporary/ to be moved stuff
+#
+arm/nvidia/as3722.c			optional	iic
+arm/nvidia/as3722_regulators.c		optional	iic
+arm/nvidia/as3722_rtc.c			optional	iic
+arm/nvidia/as3722_gpio.c		optional	iic
diff --git a/sys/arm/nvidia/tegra124/std.tegra124 b/sys/arm/nvidia/tegra124/std.tegra124
new file mode 100644
index 0000000..127b001
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/std.tegra124
@@ -0,0 +1,14 @@
+# $FreeBSD$
+cpu 		CPU_CORTEXA
+machine		arm	armv6
+makeoptions	CONF_CFLAGS="-march=armv7a"
+
+options		KERNVIRTADDR		= 0xc0200000
+makeoptions	KERNVIRTADDR		= 0xc0200000
+
+options		ARM_INTRNG
+
+options		IPI_IRQ_START=0
+options		IPI_IRQ_END=15
+
+files "../nvidia/tegra124/files.tegra124"
diff --git a/sys/arm/nvidia/tegra124/tegra124_car.c b/sys/arm/nvidia/tegra124/tegra124_car.c
new file mode 100644
index 0000000..c0e93c7
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_car.c
@@ -0,0 +1,613 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/kernel.h>
+#include <sys/kobj.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+
+#include <machine/bus.h>
+#include <machine/cpu.h>
+
+#include <dev/extres/clk/clk_div.h>
+#include <dev/extres/clk/clk_fixed.h>
+#include <dev/extres/clk/clk_gate.h>
+#include <dev/extres/clk/clk_mux.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <gnu/dts/include/dt-bindings/clock/tegra124-car.h>
+
+#include "clkdev_if.h"
+#include "hwreset_if.h"
+#include "tegra124_car.h"
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-car",	1},
+	{NULL,		 	0},
+};
+
+#define	PLIST(x) static const char *x[]
+
+/* Pure multiplexer. */
+#define	MUX(_id, cname, plists, o, s, w)				\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = plists,					\
+	.clkdef.parent_cnt = nitems(plists),				\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,			\
+	.offset = o,							\
+	.shift  = s,							\
+	.width = w,							\
+}
+
+/* Fractional divider (7.1). */
+#define	DIV7_1(_id, cname, plist, o, s)					\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = (const char *[]){plist},			\
+	.clkdef.parent_cnt = 1,						\
+	.clkdef.flags =  CLK_NODE_STATIC_STRINGS,			\
+	.offset = o,							\
+	.i_shift = (s) + 1,						\
+	.i_width = 7,							\
+	.f_shift = s,							\
+	.f_width = 1,							\
+}
+
+/* Integer divider. */
+#define	DIV(_id, cname, plist, o, s, w, f)				\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = (const char *[]){plist},			\
+	.clkdef.parent_cnt = 1,						\
+	.clkdef.flags =  CLK_NODE_STATIC_STRINGS,			\
+	.offset = o,							\
+	.i_shift = s,							\
+	.i_width = w,							\
+	.div_flags = f,							\
+}
+
+/* Gate in PLL block. */
+#define	GATE_PLL(_id, cname, plist, o, s)				\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = (const char *[]){plist},			\
+	.clkdef.parent_cnt = 1,						\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,			\
+	.offset = o,							\
+	.shift = s,							\
+	.mask = 3,							\
+	.on_value = 3,							\
+	.off_value = 0,							\
+}
+
+/* Standard gate. */
+#define	GATE(_id, cname, plist, o, s)					\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = (const char *[]){plist},			\
+	.clkdef.parent_cnt = 1,						\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,			\
+	.offset = o,							\
+	.shift = s,							\
+	.mask = 1,							\
+	.on_value = 1,							\
+	.off_value = 0,							\
+}
+
+/* Inverted gate. */
+#define	GATE_INV(_id, cname, plist, o, s)				\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = (const char *[]){plist},			\
+	.clkdef.parent_cnt = 1,						\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,			\
+	.offset = o,							\
+	.shift = s,							\
+	.mask = 1,							\
+	.on_value = 0,							\
+	.off_value = 1,							\
+}
+
+/* Fixed rate clock. */
+#define	FRATE(_id, cname, _freq)					\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = NULL,					\
+	.clkdef.parent_cnt = 0,						\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,			\
+	.freq = _freq,							\
+}
+
+/* Fixed rate multipier/divider. */
+#define	FACT(_id, cname, pname, _mult, _div)				\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = (const char *[]){pname},			\
+	.clkdef.parent_cnt = 1,						\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,			\
+	.mult = _mult,							\
+	.div = _div,							\
+}
+
+static uint32_t osc_freqs[16] = {
+	 [0] =  13000000,
+	 [1] =  16800000,
+	 [4] =  19200000,
+	 [5] =  38400000,
+	 [8] =  12000000,
+	 [9] =  48000000,
+	[12] = 260000000,
+};
+
+
+/* Parent lists. */
+PLIST(mux_pll_srcs) = {"osc_div_clk", NULL, "pllP_out0", NULL}; /* FIXME */
+PLIST(mux_plle_src1) = {"osc_div_clk", "pllP_out0"};
+PLIST(mux_plle_src) = {"pllE_src1", "pllREFE_out"};
+PLIST(mux_plld_out0_plld2_out0) = {"pllD_out0", "pllD2_out0"};
+PLIST(mux_pllmcp_clkm) = {"pllM_out0", "pllC_out0", "pllP_out0", "clk_m",
+    "pllM_UD", "pllC2_out0", "pllC3_out0", "pllC_UD"};
+PLIST(mux_xusb_hs) = {"pc_xusb_ss", "pllU_60"};
+PLIST(mux_xusb_ss) = {"pc_xusb_ss", "osc_div_clk"};
+
+
+/* Clocks ajusted online. */
+static struct clk_fixed_def fixed_clk_m =
+	FRATE(0, "clk_m", 12000000);
+static struct clk_fixed_def fixed_osc_div_clk =
+	FACT(0, "osc_div_clk", "clk_m", 1, 1);
+
+static struct clk_fixed_def tegra124_fixed_clks[] = {
+	/* Core clocks. */
+	FRATE(0, "clk_s", 32768),
+	FACT(0, "clk_m_div2", "clk_m", 1, 2),
+	FACT(0, "clk_m_div4", "clk_m", 1, 3),
+	FACT(0, "pllU_60", "pllU_out", 1, 8),
+	FACT(0, "pllU_48", "pllU_out", 1, 10),
+	FACT(0, "pllU_12", "pllU_out", 1, 40),
+	FACT(TEGRA124_CLK_PLL_D_OUT0, "pllD_out0", "pllD_out", 1, 2),
+	FACT(TEGRA124_CLK_PLL_D2_OUT0, "pllD2_out0", "pllD2_out", 1, 1),
+	FACT(0, "pllX_out0", "pllX_out", 1, 2),
+	FACT(0, "pllC_UD", "pllC_out0", 1, 1),
+	FACT(0, "pllM_UD", "pllM_out0", 1, 1),
+
+	/* Audio clocks. */
+	FRATE(0, "audio0", 10000000),
+	FRATE(0, "audio1", 10000000),
+	FRATE(0, "audio2", 10000000),
+	FRATE(0, "audio3", 10000000),
+	FRATE(0, "audio4", 10000000),
+	FRATE(0, "ext_vimclk", 10000000),
+};
+
+
+static struct clk_mux_def tegra124_mux_clks[] = {
+	/* Core clocks. */
+	MUX(0, "pllD2_src", mux_pll_srcs, PLLD2_BASE, 25, 2),
+	MUX(0, "pllDP_src", mux_pll_srcs, PLLDP_BASE, 25, 2),
+	MUX(0, "pllC4_src", mux_pll_srcs, PLLC4_BASE, 25, 2),
+	MUX(0, "pllE_src1", mux_plle_src1, PLLE_AUX, 2, 1),
+	MUX(0, "pllE_src",  mux_plle_src, PLLE_AUX, 28, 1),
+
+	/* Base peripheral clocks. */
+	MUX(0, "dsia_mux", mux_plld_out0_plld2_out0, PLLD_BASE, 25, 1),
+	MUX(0, "dsib_mux", mux_plld_out0_plld2_out0, PLLD2_BASE, 25, 1),
+	MUX(0, "emc_mux", mux_pllmcp_clkm, CLK_SOURCE_EMC, 29, 3),
+
+	/* USB. */
+	MUX(0, "xusb_hs", mux_xusb_hs, CLK_SOURCE_XUSB_SS, 25, 1),
+	MUX(0, "xusb_ss_mux", mux_xusb_ss, CLK_SOURCE_XUSB_SS, 24, 1),
+
+};
+
+
+static struct clk_gate_def tegra124_gate_clks[] = {
+	/* Core clocks. */
+	GATE_PLL(0, "pllC_out1", "pllC_out1_div", PLLC_OUT, 0),
+	GATE_PLL(0, "pllM_out1", "pllM_out1_div", PLLM_OUT, 0),
+	GATE_PLL(0, "pllU_480", "pllU_out", PLLU_BASE, 22),
+	GATE_PLL(0, "pllP_outX0", "pllP_outX0_div", PLLP_RESHIFT, 0),
+	GATE_PLL(0, "pllP_out1", "pllP_out1_div", PLLP_OUTA, 0),
+	GATE_PLL(0, "pllP_out2", "pllP_out2_div", PLLP_OUTA, 16),
+	GATE_PLL(0, "pllP_out3", "pllP_out3_div", PLLP_OUTB, 0),
+	GATE_PLL(0, "pllP_out4", "pllP_out4_div", PLLP_OUTB, 16),
+	GATE_PLL(0, "pllP_out5", "pllP_out5_div", PLLP_OUTC, 16),
+	GATE_PLL(0, "pllA_out0", "pllA_out1_div", PLLA_OUT, 0),
+
+	/* Base peripheral clocks. */
+	GATE(TEGRA124_CLK_CML0, "cml0", "pllE_out0", PLLE_AUX, 0),
+	GATE(TEGRA124_CLK_CML1, "cml1", "pllE_out0", PLLE_AUX, 1),
+	GATE_INV(TEGRA124_CLK_HCLK, "hclk", "hclk_div", CLK_SYSTEM_RATE, 7),
+	GATE_INV(TEGRA124_CLK_PCLK, "pclk", "pclk_div", CLK_SYSTEM_RATE, 3),
+};
+
+static struct clk_div_def tegra124_div_clks[] = {
+	/* Core clocks. */
+	DIV7_1(0, "pllC_out1_div", "pllC_out0", PLLC_OUT, 2),
+	DIV7_1(0, "pllM_out1_div", "pllM_out0", PLLM_OUT, 8),
+	DIV7_1(0, "pllP_outX0_div", "pllP_out0", PLLP_RESHIFT, 2),
+	DIV7_1(0, "pllP_out1_div", "pllP_out0", PLLP_OUTA, 8),
+	DIV7_1(0, "pllP_out2_div", "pllP_out0", PLLP_OUTA, 24),
+	DIV7_1(0, "pllP_out3_div", "pllP_out0", PLLP_OUTB, 8),
+	DIV7_1(0, "pllP_out4_div", "pllP_out0", PLLP_OUTB, 24),
+	DIV7_1(0, "pllP_out5_div", "pllP_out0", PLLP_OUTC, 24),
+	DIV7_1(0, "pllA_out1_div", "pllA_out", PLLA_OUT, 8),
+
+	/* Base peripheral clocks. */
+	DIV(0, "hclk_div", "sclk", CLK_SYSTEM_RATE, 4, 2, 0),
+	DIV(0, "pclk_div", "hclk", CLK_SYSTEM_RATE, 0, 2, 0),
+};
+
+/* Initial setup table. */
+static struct  tegra124_init_item clk_init_table[] = {
+	/* clock, partent, frequency, enable */
+	{"uarta", "pllP_out0", 408000000, 0},
+	{"uartb", "pllP_out0", 408000000, 0},
+	{"uartc", "pllP_out0", 408000000, 0},
+	{"uartd", "pllP_out0", 408000000, 0},
+	{"pllA_out", NULL, 282240000, 1},
+	{"pllA_out0", NULL, 11289600, 1},
+	{"extperiph1", "pllA_out0", 0, 1},
+	{"i2s0", "pllA_out0", 11289600, 0},
+	{"i2s1", "pllA_out0", 11289600, 0},
+	{"i2s2", "pllA_out0", 11289600, 0},
+	{"i2s3", "pllA_out0", 11289600, 0},
+	{"i2s4", "pllA_out0", 11289600, 0},
+	{"vde", "pllP_out0", 0, 0},
+	{"host1x", "pllP_out0", 136000000, 1},
+	{"sclk", "pllP_out2", 102000000, 1},
+	{"dvfs_soc", "pllP_out0", 51000000, 1},
+	{"dvfs_ref", "pllP_out0", 51000000, 1},
+	{"pllC_out0", NULL, 600000000, 0},
+	{"pllC_out1", NULL, 100000000, 0},
+	{"spi4", "pllP_out0", 12000000, 1},
+	{"tsec", "pllC3_out0", 0, 0},
+	{"msenc", "pllC3_out0", 0, 0},
+	{"pllREFE_out", NULL, 672000000, 0},
+	{"pc_xusb_ss", "pllU_480", 120000000, 0},
+	{"xusb_ss", "pc_xusb_ss", 120000000, 0},
+	{"pc_xusb_fs", "pllU_48", 48000000, 0},
+	{"xusb_hs", "pllU_60", 60000000, 0},
+	{"pc_xusb_falcon", "pllREFE_out", 224000000, 0},
+	{"xusb_core_host", "pllREFE_out", 112000000, 0},
+	{"sata", "pllP_out0", 102000000, 0},
+	{"sata_oob", "pllP_out0", 204000000, 0},
+	{"sata_cold", NULL, 0, 1},
+	{"emc", NULL, 0, 1},
+	{"mselect", NULL, 0, 1},
+	{"csite", NULL, 0, 1},
+	{"tsensor", "clk_m", 400000, 0},
+
+	/* tegra124 only*/
+	{"soc_therm", "pllP_out0", 51000000, 0},
+	{"cclk_g", NULL, 0, 1},
+	{"hda", "pllP_out0", 102000000, 0},
+	{"hda2codec_2x", "pllP_out0", 48000000, 0},
+};
+
+static void
+init_divs(struct tegra124_car_softc *sc, struct clk_div_def *clks, int nclks)
+{
+	int i, rv;
+
+	for (i = 0; i < nclks; i++) {
+		rv = clknode_div_register(sc->clkdom, clks + i);
+		if (rv != 0)
+			panic("clk_div_register failed");
+	}
+}
+
+static void
+init_gates(struct tegra124_car_softc *sc, struct clk_gate_def *clks, int nclks)
+{
+	int i, rv;
+
+
+	for (i = 0; i < nclks; i++) {
+		rv = clknode_gate_register(sc->clkdom, clks + i);
+		if (rv != 0)
+			panic("clk_gate_register failed");
+	}
+}
+
+static void
+init_muxes(struct tegra124_car_softc *sc, struct clk_mux_def *clks, int nclks)
+{
+	int i, rv;
+
+
+	for (i = 0; i < nclks; i++) {
+		rv = clknode_mux_register(sc->clkdom, clks + i);
+		if (rv != 0)
+			panic("clk_mux_register failed");
+	}
+}
+
+static void
+init_fixeds(struct tegra124_car_softc *sc, struct clk_fixed_def *clks,
+    int nclks)
+{
+	int i, rv;
+	uint32_t val;
+	int osc_idx;
+
+	CLKDEV_READ_4(sc->dev, OSC_CTRL, &val);
+	osc_idx = val >> OSC_CTRL_OSC_FREQ_SHIFT;
+	fixed_clk_m.freq = osc_freqs[osc_idx];
+	if (fixed_clk_m.freq == 0)
+		panic("Undefined input frequency");
+	rv = clknode_fixed_register(sc->clkdom, &fixed_clk_m);
+	if (rv != 0) panic("clk_fixed_register failed");
+
+	val = (val >> OSC_CTRL_PLL_REF_DIV_SHIFT) & 3;
+	fixed_osc_div_clk.div = 1 << val;
+	rv = clknode_fixed_register(sc->clkdom, &fixed_osc_div_clk);
+	if (rv != 0) panic("clk_fixed_register failed");
+
+	for (i = 0; i < nclks; i++) {
+		rv = clknode_fixed_register(sc->clkdom, clks + i);
+		if (rv != 0)
+			panic("clk_fixed_register failed");
+	}
+}
+
+static void
+postinit_clock(struct tegra124_car_softc *sc)
+{
+	int i;
+	struct tegra124_init_item *tbl;
+	struct clknode *clknode;
+	int rv;
+
+	for (i = 0; i < nitems(clk_init_table); i++) {
+		tbl = &clk_init_table[i];
+
+		clknode =  clknode_find_by_name(tbl->name);
+		if (clknode == NULL) {
+			device_printf(sc->dev, "Cannot find clock %s\n",
+			    tbl->name);
+			continue;
+		}
+		if (tbl->parent != NULL) {
+			rv = clknode_set_parent_by_name(clknode, tbl->parent);
+			if (rv != 0) {
+				device_printf(sc->dev,
+				    "Cannot set parent for %s (to %s): %d\n",
+				    tbl->name, tbl->parent, rv);
+				continue;
+			}
+		}
+		if (tbl->frequency != 0) {
+			rv = clknode_set_freq(clknode, tbl->frequency, 0 , 9999);
+			if (rv != 0) {
+				device_printf(sc->dev,
+				    "Cannot set frequency for %s: %d\n",
+				    tbl->name, rv);
+				continue;
+			}
+		}
+		if (tbl->enable!= 0) {
+			rv = clknode_enable(clknode);
+			if (rv != 0) {
+				device_printf(sc->dev,
+				    "Cannot enable %s: %d\n", tbl->name, rv);
+				continue;
+			}
+		}
+	}
+}
+
+static void
+register_clocks(device_t dev)
+{
+	struct tegra124_car_softc *sc;
+
+	sc = device_get_softc(dev);
+	sc->clkdom = clkdom_create(dev);
+	if (sc->clkdom == NULL)
+		panic("clkdom == NULL");
+
+	tegra124_init_plls(sc);
+	init_fixeds(sc, tegra124_fixed_clks, nitems(tegra124_fixed_clks));
+	init_muxes(sc, tegra124_mux_clks, nitems(tegra124_mux_clks));
+	init_divs(sc, tegra124_div_clks, nitems(tegra124_div_clks));
+	init_gates(sc, tegra124_gate_clks, nitems(tegra124_gate_clks));
+	tegra124_periph_clock(sc);
+	tegra124_super_mux_clock(sc);
+	clkdom_finit(sc->clkdom);
+	clkdom_xlock(sc->clkdom);
+	postinit_clock(sc);
+	clkdom_unlock(sc->clkdom);
+	if (bootverbose)
+		clkdom_dump(sc->clkdom);
+}
+
+static int
+tegra124_car_clkdev_read_4(device_t dev, bus_addr_t addr, uint32_t *val)
+{
+	struct tegra124_car_softc *sc;
+
+	sc = device_get_softc(dev);
+	*val = bus_read_4(sc->mem_res, addr);
+	return (0);
+}
+
+static int
+tegra124_car_clkdev_write_4(device_t dev, bus_addr_t addr, uint32_t val)
+{
+	struct tegra124_car_softc *sc;
+
+	sc = device_get_softc(dev);
+	bus_write_4(sc->mem_res, addr, val);
+	return (0);
+}
+
+static int
+tegra124_car_clkdev_modify_4(device_t dev, bus_addr_t addr, uint32_t clear_mask,
+    uint32_t set_mask)
+{
+	struct tegra124_car_softc *sc;
+	uint32_t reg;
+
+	sc = device_get_softc(dev);
+	reg = bus_read_4(sc->mem_res, addr);
+	reg &= ~clear_mask;
+	reg |= set_mask;
+	bus_write_4(sc->mem_res, addr, reg);
+	return (0);
+}
+
+static void
+tegra124_car_clkdev_device_lock(device_t dev)
+{
+	struct tegra124_car_softc *sc;
+
+	sc = device_get_softc(dev);
+	mtx_lock(&sc->mtx);
+}
+
+static void
+tegra124_car_clkdev_device_unlock(device_t dev)
+{
+	struct tegra124_car_softc *sc;
+
+	sc = device_get_softc(dev);
+	mtx_unlock(&sc->mtx);
+}
+
+static int
+tegra124_car_detach(device_t dev)
+{
+
+	device_printf(dev, "Error: Clock driver cannot be detached\n");
+	return (EBUSY);
+}
+
+static int
+tegra124_car_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data != 0) {
+		device_set_desc(dev, "Tegra Clock Driver");
+		return (BUS_PROBE_DEFAULT);
+	}
+
+	return (ENXIO);
+}
+
+static int
+tegra124_car_attach(device_t dev)
+{
+	struct tegra124_car_softc *sc = device_get_softc(dev);
+	int rid, rv;
+
+	sc->dev = dev;
+
+	mtx_init(&sc->mtx, device_get_nameunit(dev), NULL, MTX_DEF);
+	sc->type = ofw_bus_search_compatible(dev, compat_data)->ocd_data;
+
+	/* Resource setup. */
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (!sc->mem_res) {
+		device_printf(dev, "cannot allocate memory resource\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	register_clocks(dev);
+	hwreset_register_ofw_provider(dev);
+	return (0);
+
+fail:
+	if (sc->mem_res)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+
+	return (rv);
+}
+
+static int
+tegra124_car_hwreset_assert(device_t dev, intptr_t id, bool value)
+{
+	struct tegra124_car_softc *sc = device_get_softc(dev);
+
+	return (tegra124_hwreset_by_idx(sc, id, value));
+}
+
+static device_method_t tegra124_car_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		tegra124_car_probe),
+	DEVMETHOD(device_attach,	tegra124_car_attach),
+	DEVMETHOD(device_detach,	tegra124_car_detach),
+
+	/* Clkdev  interface*/
+	DEVMETHOD(clkdev_read_4,	tegra124_car_clkdev_read_4),
+	DEVMETHOD(clkdev_write_4,	tegra124_car_clkdev_write_4),
+	DEVMETHOD(clkdev_modify_4,	tegra124_car_clkdev_modify_4),
+	DEVMETHOD(clkdev_device_lock,	tegra124_car_clkdev_device_lock),
+	DEVMETHOD(clkdev_device_unlock,	tegra124_car_clkdev_device_unlock),
+
+	/* Reset interface */
+	DEVMETHOD(hwreset_assert,	tegra124_car_hwreset_assert),
+
+	DEVMETHOD_END
+};
+
+static devclass_t tegra124_car_devclass;
+
+static driver_t tegra124_car_driver = {
+	"tegra124_car",
+	tegra124_car_methods,
+	sizeof(struct tegra124_car_softc),
+};
+
+EARLY_DRIVER_MODULE(tegra124_car, simplebus, tegra124_car_driver,
+    tegra124_car_devclass, 0, 0, BUS_PASS_TIMER);
diff --git a/sys/arm/nvidia/tegra124/tegra124_car.h b/sys/arm/nvidia/tegra124/tegra124_car.h
new file mode 100644
index 0000000..b11742c
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_car.h
@@ -0,0 +1,337 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _TEGRA124_CAR_
+#define	_TEGRA124_CAR_
+
+#include "clkdev_if.h"
+
+#define	RD4(sc, reg, val)	CLKDEV_READ_4((sc)->clkdev, reg, val)
+#define	WR4(sc, reg, val)	CLKDEV_WRITE_4((sc)->clkdev, reg, val)
+#define	MD4(sc, reg, mask, set)	CLKDEV_MODIFY_4((sc)->clkdev, reg, mask, set)
+#define	DEVICE_LOCK(sc)		CLKDEV_DEVICE_LOCK((sc)->clkdev)
+#define	DEVICE_UNLOCK(sc)	CLKDEV_DEVICE_UNLOCK((sc)->clkdev)
+
+#define	RST_DEVICES_L			0x004
+#define	RST_DEVICES_H			0x008
+#define	RST_DEVICES_U			0x00C
+#define	CLK_OUT_ENB_L			0x010
+#define	CLK_OUT_ENB_H			0x014
+#define	CLK_OUT_ENB_U			0x018
+#define	CCLK_BURST_POLICY		0x020
+#define	SUPER_CCLK_DIVIDER		0x024
+#define	SCLK_BURST_POLICY		0x028
+#define	SUPER_SCLK_DIVIDER		0x02c
+#define	CLK_SYSTEM_RATE			0x030
+
+#define	OSC_CTRL			0x050
+ #define	OSC_CTRL_OSC_FREQ_SHIFT		28
+ #define	OSC_CTRL_PLL_REF_DIV_SHIFT		26
+
+#define	PLLE_SS_CNTL 			0x068
+#define	 PLLE_SS_CNTL_SSCINCINTRV_MASK		(0x3f << 24)
+#define	 PLLE_SS_CNTL_SSCINCINTRV_VAL 		(0x20 << 24)
+#define	 PLLE_SS_CNTL_SSCINC_MASK 		(0xff << 16)
+#define	 PLLE_SS_CNTL_SSCINC_VAL 		(0x1 << 16)
+#define	 PLLE_SS_CNTL_SSCINVERT 		(1 << 15)
+#define	 PLLE_SS_CNTL_SSCCENTER 		(1 << 14)
+#define	 PLLE_SS_CNTL_SSCBYP 			(1 << 12)
+#define	 PLLE_SS_CNTL_INTERP_RESET 		(1 << 11)
+#define	 PLLE_SS_CNTL_BYPASS_SS 		(1 << 10)
+#define	 PLLE_SS_CNTL_SSCMAX_MASK		0x1ff
+#define	 PLLE_SS_CNTL_SSCMAX_VAL 		0x25
+#define	 PLLE_SS_CNTL_DISABLE 			(PLLE_SS_CNTL_BYPASS_SS |    \
+						 PLLE_SS_CNTL_INTERP_RESET | \
+						 PLLE_SS_CNTL_SSCBYP)
+#define	 PLLE_SS_CNTL_COEFFICIENTS_MASK 	(PLLE_SS_CNTL_SSCMAX_MASK |  \
+						 PLLE_SS_CNTL_SSCINC_MASK |  \
+						 PLLE_SS_CNTL_SSCINCINTRV_MASK)
+#define	 PLLE_SS_CNTL_COEFFICIENTS_VAL 		(PLLE_SS_CNTL_SSCMAX_VAL |   \
+						 PLLE_SS_CNTL_SSCINC_VAL |   \
+						 PLLE_SS_CNTL_SSCINCINTRV_VAL)
+
+#define	PLLC_BASE			0x080
+#define	PLLC_OUT			0x084
+#define	PLLC_MISC2			0x088
+#define	PLLC_MISC			0x08c
+#define	PLLM_BASE			0x090
+#define	PLLM_OUT			0x094
+#define	PLLM_MISC			0x09c
+#define	PLLP_BASE			0x0a0
+#define	PLLP_MISC			0x0ac
+#define	PLLP_OUTA			0x0a4
+#define	PLLP_OUTB			0x0a8
+#define	PLLA_BASE			0x0b0
+#define	PLLA_OUT			0x0b4
+#define	PLLA_MISC			0x0bc
+#define	PLLU_BASE			0x0c0
+#define	PLLU_MISC			0x0cc
+#define	PLLD_BASE			0x0d0
+#define	PLLD_MISC			0x0dc
+#define	PLLX_BASE			0x0e0
+#define	PLLX_MISC			0x0e4
+#define	PLLE_BASE			0x0e8
+#define	 PLLE_BASE_LOCK_OVERRIDE		(1 << 29)
+#define	 PLLE_BASE_DIVCML_SHIFT 		24
+#define	 PLLE_BASE_DIVCML_MASK 			0xf
+
+#define	PLLE_MISC			0x0ec
+#define	 PLLE_MISC_SETUP_BASE_SHIFT 		16
+#define	 PLLE_MISC_SETUP_BASE_MASK 		(0xffff << PLLE_MISC_SETUP_BASE_SHIFT)
+#define	 PLLE_MISC_READY 			(1 << 15)
+#define	 PLLE_MISC_IDDQ_SWCTL			(1 << 14)
+#define	 PLLE_MISC_IDDQ_OVERRIDE_VALUE		(1 << 13)
+#define	 PLLE_MISC_LOCK 			(1 << 11)
+#define	 PLLE_MISC_REF_ENABLE 			(1 << 10)
+#define	 PLLE_MISC_LOCK_ENABLE 			(1 << 9)
+#define	 PLLE_MISC_PTS 				(1 << 8)
+#define	 PLLE_MISC_VREG_BG_CTRL_SHIFT		4
+#define	 PLLE_MISC_VREG_BG_CTRL_MASK		(3 << PLLE_MISC_VREG_BG_CTRL_SHIFT)
+#define	 PLLE_MISC_VREG_CTRL_SHIFT		2
+#define	 PLLE_MISC_VREG_CTRL_MASK		(2 << PLLE_MISC_VREG_CTRL_SHIFT)
+
+#define	CLK_SOURCE_I2S1			0x100
+#define	CLK_SOURCE_I2S2			0x104
+#define	CLK_SOURCE_SPDIF_OUT		0x108
+#define	CLK_SOURCE_SPDIF_IN		0x10c
+#define	CLK_SOURCE_PWM			0x110
+#define	CLK_SOURCE_SPI2			0x118
+#define	CLK_SOURCE_SPI3			0x11c
+#define	CLK_SOURCE_I2C1			0x124
+#define	CLK_SOURCE_I2C5			0x128
+#define	CLK_SOURCE_SPI1			0x134
+#define	CLK_SOURCE_DISP1		0x138
+#define	CLK_SOURCE_DISP2		0x13c
+#define	CLK_SOURCE_ISP			0x144
+#define	CLK_SOURCE_VI			0x148
+#define	CLK_SOURCE_SDMMC1		0x150
+#define	CLK_SOURCE_SDMMC2		0x154
+#define	CLK_SOURCE_SDMMC4		0x164
+#define	CLK_SOURCE_VFIR			0x168
+#define	CLK_SOURCE_HSI			0x174
+#define	CLK_SOURCE_UARTA		0x178
+#define	CLK_SOURCE_UARTB		0x17c
+#define	CLK_SOURCE_HOST1X		0x180
+#define	CLK_SOURCE_HDMI			0x18c
+#define	CLK_SOURCE_I2C2			0x198
+#define	CLK_SOURCE_EMC			0x19c
+#define	CLK_SOURCE_UARTC		0x1a0
+#define	CLK_SOURCE_VI_SENSOR		0x1a8
+#define	CLK_SOURCE_SPI4			0x1b4
+#define	CLK_SOURCE_I2C3			0x1b8
+#define	CLK_SOURCE_SDMMC3		0x1bc
+#define	CLK_SOURCE_UARTD		0x1c0
+#define	CLK_SOURCE_VDE			0x1c8
+#define	CLK_SOURCE_OWR			0x1cc
+#define	CLK_SOURCE_NOR			0x1d0
+#define	CLK_SOURCE_CSITE		0x1d4
+#define	CLK_SOURCE_I2S0			0x1d8
+#define	CLK_SOURCE_DTV			0x1dc
+#define	CLK_SOURCE_MSENC		0x1f0
+#define	CLK_SOURCE_TSEC			0x1f4
+#define	CLK_SOURCE_SPARE2		0x1f8
+
+#define	CLK_OUT_ENB_X			0x280
+#define	RST_DEVICES_X			0x28C
+
+#define	RST_DEVICES_V			0x358
+#define	RST_DEVICES_W			0x35C
+#define	CLK_OUT_ENB_V			0x360
+#define	CLK_OUT_ENB_W			0x364
+#define	CCLKG_BURST_POLICY		0x368
+#define	SUPER_CCLKG_DIVIDER		0x36C
+#define	CCLKLP_BURST_POLICY		0x370
+#define	SUPER_CCLKLP_DIVIDER		0x374
+
+#define	CLK_SOURCE_MSELECT		0x3b4
+#define	CLK_SOURCE_TSENSOR		0x3b8
+#define	CLK_SOURCE_I2S3			0x3bc
+#define	CLK_SOURCE_I2S4			0x3c0
+#define	CLK_SOURCE_I2C4			0x3c4
+#define	CLK_SOURCE_SPI5			0x3c8
+#define	CLK_SOURCE_SPI6			0x3cc
+#define	CLK_SOURCE_AUDIO		0x3d0
+#define	CLK_SOURCE_DAM0			0x3d8
+#define	CLK_SOURCE_DAM1			0x3dc
+#define	CLK_SOURCE_DAM2			0x3e0
+#define	CLK_SOURCE_HDA2CODEC_2X		0x3e4
+#define	CLK_SOURCE_ACTMON		0x3e8
+#define	CLK_SOURCE_EXTPERIPH1		0x3ec
+#define	CLK_SOURCE_EXTPERIPH2		0x3f0
+#define	CLK_SOURCE_EXTPERIPH3		0x3f4
+#define	CLK_SOURCE_I2C_SLOW		0x3fc
+
+#define	CLK_SOURCE_SYS			0x400
+#define	CLK_SOURCE_SOR0			0x414
+#define	CLK_SOURCE_SATA_OOB		0x420
+#define	CLK_SOURCE_SATA			0x424
+#define	CLK_SOURCE_HDA			0x428
+#define	UTMIP_PLL_CFG0			0x480
+#define	UTMIP_PLL_CFG1			0x484
+#define	 UTMIP_PLL_CFG1_FORCE_PLLU_POWERUP		(1 << 17)
+#define	 UTMIP_PLL_CFG1_FORCE_PLLU_POWERDOWN		(1 << 16)
+#define	 UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERUP	(1 << 15)
+#define	 UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN	(1 << 14)
+#define	 UTMIP_PLL_CFG1_FORCE_PLL_ACTIVE_POWERDOWN 	(1 << 12)
+#define	 UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(x)		(((x) & 0x1f) << 6)
+#define	 UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(x)		(((x) & 0xfff) << 0)
+
+#define	UTMIP_PLL_CFG2			0x488
+#define	 UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(x)		(((x) & 0x3f) << 18)
+#define	 UTMIP_PLL_CFG2_STABLE_COUNT(x)			(((x) & 0xffff) << 6)
+#define	 UTMIP_PLL_CFG2_FORCE_PD_SAMP_C_POWERDOWN 	(1 << 4)
+#define	 UTMIP_PLL_CFG2_FORCE_PD_SAMP_B_POWERDOWN 	(1 << 2)
+#define	 UTMIP_PLL_CFG2_FORCE_PD_SAMP_A_POWERDOWN	(1 << 0)
+
+#define	PLLE_AUX			0x48c
+#define	 PLLE_AUX_PLLRE_SEL				(1 << 28)
+#define	 PLLE_AUX_SEQ_START_STATE 			(1 << 25)
+#define	 PLLE_AUX_SEQ_ENABLE				(1 << 24)
+#define	 PLLE_AUX_SS_SWCTL				(1 << 6)
+#define	 PLLE_AUX_ENABLE_SWCTL				(1 << 4)
+#define	 PLLE_AUX_USE_LOCKDET				(1 << 3)
+#define	 PLLE_AUX_PLLP_SEL				(1 << 2)
+
+#define	SATA_PLL_CFG0			0x490
+#define	SATA_PLL_CFG0_SEQ_START_STATE			(1 << 25)
+#define	SATA_PLL_CFG0_SEQ_ENABLE			(1 << 24)
+#define	SATA_PLL_CFG0_SEQ_PADPLL_PD_INPUT_VALUE		(1 << 7)
+#define	SATA_PLL_CFG0_SEQ_LANE_PD_INPUT_VALUE		(1 << 6)
+#define	SATA_PLL_CFG0_SEQ_RESET_INPUT_VALUE		(1 << 5)
+#define	SATA_PLL_CFG0_SEQ_IN_SWCTL			(1 << 4)
+#define	SATA_PLL_CFG0_PADPLL_USE_LOCKDET		(1 << 2)
+#define	SATA_PLL_CFG0_PADPLL_RESET_OVERRIDE_VALUE	(1 << 1)
+#define	SATA_PLL_CFG0_PADPLL_RESET_SWCTL		(1 << 0)
+
+#define	SATA_PLL_CFG1			0x494
+#define	PCIE_PLL_CFG0			0x498
+#define	PCIE_PLL_CFG0_SEQ_START_STATE			(1 << 25)
+#define	PCIE_PLL_CFG0_SEQ_ENABLE			(1 << 24)
+
+#define	PLLD2_BASE			0x4b8
+#define	PLLD2_MISC			0x4bc
+#define	UTMIP_PLL_CFG3			0x4c0
+#define	PLLRE_BASE			0x4c4
+#define	PLLRE_MISC			0x4c8
+#define	PLLC2_BASE			0x4e8
+#define	PLLC2_MISC			0x4ec
+#define	PLLC3_BASE			0x4fc
+
+#define	PLLC3_MISC			0x500
+#define	PLLX_MISC2			0x514
+#define	PLLX_MISC2			0x514
+#define	PLLX_MISC3			0x518
+#define	 PLLX_MISC3_DYNRAMP_STEPB_MASK		0xFF
+#define	 PLLX_MISC3_DYNRAMP_STEPB_SHIFT		24
+#define	 PLLX_MISC3_DYNRAMP_STEPA_MASK		0xFF
+#define	 PLLX_MISC3_DYNRAMP_STEPA_SHIFT		16
+#define	 PLLX_MISC3_NDIV_NEW_MASK		0xFF
+#define	 PLLX_MISC3_NDIV_NEW_SHIFT		8
+#define	 PLLX_MISC3_EN_FSTLCK			(1 << 5)
+#define	 PLLX_MISC3_LOCK_OVERRIDE		(1 << 4)
+#define	 PLLX_MISC3_PLL_FREQLOCK		(1 << 3)
+#define	 PLLX_MISC3_DYNRAMP_DONE		(1 << 2)
+#define	 PLLX_MISC3_CLAMP_NDIV			(1 << 1)
+#define	 PLLX_MISC3_EN_DYNRAMP			(1 << 0)
+#define	XUSBIO_PLL_CFG0			0x51c
+#define	 XUSBIO_PLL_CFG0_SEQ_START_STATE		(1 << 25)
+#define	 XUSBIO_PLL_CFG0_SEQ_ENABLE			(1 << 24)
+#define	 XUSBIO_PLL_CFG0_PADPLL_USE_LOCKDET		(1 << 6)
+#define	 XUSBIO_PLL_CFG0_CLK_ENABLE_SWCTL		(1 << 2)
+#define	 XUSBIO_PLL_CFG0_PADPLL_RESET_SWCTL		(1 << 0)
+
+#define	PLLP_RESHIFT			0x528
+#define	UTMIPLL_HW_PWRDN_CFG0		0x52c
+#define	 UTMIPLL_HW_PWRDN_CFG0_SEQ_START_STATE		(1 << 25)
+#define	 UTMIPLL_HW_PWRDN_CFG0_SEQ_ENABLE		(1 << 24)
+#define	 UTMIPLL_HW_PWRDN_CFG0_USE_LOCKDET		(1 << 6)
+#define	 UTMIPLL_HW_PWRDN_CFG0_SEQ_RESET_INPUT_VALUE	(1 << 5)
+#define	 UTMIPLL_HW_PWRDN_CFG0_SEQ_IN_SWCTL		(1 << 4)
+#define	 UTMIPLL_HW_PWRDN_CFG0_CLK_ENABLE_SWCTL		(1 << 2)
+#define	 UTMIPLL_HW_PWRDN_CFG0_IDDQ_OVERRIDE		(1 << 1)
+#define	 UTMIPLL_HW_PWRDN_CFG0_IDDQ_SWCTL		(1 << 0)
+
+#define	PLLDP_BASE			0x590
+#define	PLLDP_MISC			0x594
+#define	PLLC4_BASE			0x5a4
+#define	PLLC4_MISC			0x5a8
+
+#define	CLK_SOURCE_XUSB_CORE_HOST	0x600
+#define	CLK_SOURCE_XUSB_FALCON		0x604
+#define	CLK_SOURCE_XUSB_FS		0x608
+#define	CLK_SOURCE_XUSB_CORE_DEV	0x60c
+#define	CLK_SOURCE_XUSB_SS		0x610
+#define	CLK_SOURCE_CILAB		0x614
+#define	CLK_SOURCE_CILCD		0x618
+#define	CLK_SOURCE_CILE			0x61c
+#define	CLK_SOURCE_DSIA_LP		0x620
+#define	CLK_SOURCE_DSIB_LP		0x624
+#define	CLK_SOURCE_ENTROPY		0x628
+#define	CLK_SOURCE_DVFS_REF		0x62c
+#define	CLK_SOURCE_DVFS_SOC		0x630
+#define	CLK_SOURCE_TRACECLKIN		0x634
+#define	CLK_SOURCE_ADX			0x638
+#define	CLK_SOURCE_AMX			0x63c
+#define	CLK_SOURCE_EMC_LATENCY		0x640
+#define	CLK_SOURCE_SOC_THERM		0x644
+#define	CLK_SOURCE_VI_SENSOR2		0x658
+#define	CLK_SOURCE_I2C6			0x65c
+#define	CLK_SOURCE_EMC_DLL		0x664
+#define	CLK_SOURCE_HDMI_AUDIO		0x668
+#define	CLK_SOURCE_CLK72MHZ		0x66c
+#define	CLK_SOURCE_ADX1			0x670
+#define	CLK_SOURCE_AMX1			0x674
+#define	CLK_SOURCE_VIC			0x678
+#define	PLLP_OUTC			0x67c
+#define	PLLP_MISC1			0x680
+
+
+struct tegra124_car_softc {
+	device_t		dev;
+	struct resource *	mem_res;
+	struct mtx		mtx;
+	struct clkdom 		*clkdom;
+	int			type;
+};
+
+struct tegra124_init_item {
+	char 		*name;
+	char 		*parent;
+	uint64_t	frequency;
+	int 		enable;
+};
+
+void tegra124_init_plls(struct tegra124_car_softc *sc);
+
+void tegra124_periph_clock(struct tegra124_car_softc *sc);
+void tegra124_super_mux_clock(struct tegra124_car_softc *sc);
+
+int tegra124_hwreset_by_idx(struct tegra124_car_softc *sc, intptr_t idx,
+    bool reset);
+
+#endif /*_TEGRA124_CAR_*/
\ No newline at end of file
diff --git a/sys/arm/nvidia/tegra124/tegra124_clk_per.c b/sys/arm/nvidia/tegra124/tegra124_clk_per.c
new file mode 100644
index 0000000..65ae5d9
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_clk_per.c
@@ -0,0 +1,810 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/lock.h>
+#include <sys/mutex.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+
+#include <dev/extres/clk/clk.h>
+
+#include <gnu/dts/include/dt-bindings/clock/tegra124-car.h>
+#include "tegra124_car.h"
+
+/* Bits in base register. */
+#define	PERLCK_AMUX_MASK	0x0F
+#define	PERLCK_AMUX_SHIFT	16
+#define	PERLCK_AMUX_DIS		(1 << 20)
+#define	PERLCK_UDIV_DIS		(1 << 24)
+#define	PERLCK_ENA_MASK		(1 << 28)
+#define	PERLCK_MUX_SHIFT	29
+#define	PERLCK_MUX_MASK		0x07
+
+
+struct periph_def {
+	struct clknode_init_def	clkdef;
+	uint32_t		base_reg;
+	uint32_t		div_width;
+	uint32_t		div_mask;
+	uint32_t		div_f_width;
+	uint32_t		div_f_mask;
+	uint32_t		flags;
+};
+
+struct pgate_def {
+	struct clknode_init_def	clkdef;
+	uint32_t		idx;
+	uint32_t		flags;
+};
+#define	PLIST(x) static const char *x[]
+
+#define	GATE(_id, cname, plist, _idx)					\
+{									\
+	.clkdef.id = TEGRA124_CLK_##_id,				\
+	.clkdef.name = cname,						\
+	.clkdef.parent_names = (const char *[]){plist},			\
+	.clkdef.parent_cnt = 1,						\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,			\
+	.idx = _idx,							\
+	.flags = 0,							\
+}
+
+/* Sources for multiplexors. */
+PLIST(mux_a_N_audio_N_p_N_clkm) =
+    {"pllA_out0", NULL, "audio",  NULL,
+     "pllP_out0", NULL, "clk_m"};
+PLIST(mux_a_N_audio0_N_p_N_clkm) =
+    {"pllA_out0", NULL, "audio0", NULL,
+     "pllP_out0", NULL, "clk_m"};
+PLIST(mux_a_N_audio1_N_p_N_clkm) =
+    {"pllA_out0", NULL, "audio1", NULL,
+     "pllP_out0", NULL, "clk_m"};
+PLIST(mux_a_N_audio2_N_p_N_clkm) =
+    {"pllA_out0", NULL, "audio2", NULL,
+     "pllP_out0", NULL, "clk_m"};
+PLIST(mux_a_N_audio3_N_p_N_clkm) =
+    {"pllA_out0", NULL, "audio3", NULL,
+     "pllP_out0", NULL, "clk_m"};
+PLIST(mux_a_N_audio4_N_p_N_clkm) =
+    {"pllA_out0", NULL, "audio4", NULL,
+     "pllP_out0", NULL, "clk_m"};
+PLIST(mux_a_clks_p_clkm_e) =
+    {"pllA_out0", "clk_s", "pllP_out0",
+     "clk_m", "pllE_out0"};
+PLIST(mux_a_c2_c_c3_p_N_clkm) =
+    {"pllA_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "pllP_out0", NULL, "clk_m"};
+
+PLIST(mux_m_c_p_a_c2_c3) =
+    {"pllM_out0", "pllC_out0", "pllP_out0", "pllA_out0",
+     "pllC2_out0", "pllC3_out0"};
+PLIST(mux_m_c_p_a_c2_c3_clkm) =
+    {"pllM_out0", "pllC_out0", "pllP_out0", "pllA_out0",
+     "pllC2_out0", "pllC3_out0", "clk_m"};
+PLIST(mux_m_c_p_a_c2_c3_clkm_c4) =
+    {"pllM_out0", "pllC_out0", "pllP_out0", "pllA_out0",
+     "pllC2_out0", "pllC3_out0", "clk_m", "pllC4_out0"};
+PLIST(mux_m_c_p_clkm_mud_c2_c3) =
+    {"pllM_out0", "pllC_out0", "pllP_out0", "clk_m",
+     "pllM_UD", "pllC2_out0", "pllC3_out0"};
+PLIST(mux_m_c2_c_c3_p_N_a) =
+    {"pllM_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "pllP_out0", NULL, "pllA_out0"};
+PLIST(mux_m_c2_c_c3_p_N_a_c4) =
+    {"pllM_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     NULL, "pllA_out0", "pllC4_out0"};
+
+PLIST(mux_p_N_c_N_N_N_clkm) =
+    {"pllP_out0", NULL, "pllC_out0", NULL,
+     NULL, NULL, "clk_m"};
+PLIST(mux_p_N_c_N_m_N_clkm) =
+    {"pllP_out0", NULL, "pllC_out0", NULL,
+     "pllM_out0", NULL, "clk_m"};
+PLIST(mux_p_c_c2_clkm) =
+    {"pllP_out0", "pllC_out0", "pllC2_out0", "clk_m"};
+PLIST(mux_p_c2_c_c3_m) =
+    {"pllP_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "pllM_out0"};
+PLIST(mux_p_c2_c_c3_m_N_clkm) =
+    {"pllP_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "pllM_out0", NULL, "clk_m"};
+PLIST(mux_p_c2_c_c3_m_e_clkm) =
+    {"pllP_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "pllM_out0", "pllE_out0", "clk_m"};
+PLIST(mux_p_c2_c_c3_m_a_clkm) =
+    {"pllP_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "pllM_out0", "pllA_out0", "clk_m"};
+PLIST(mux_p_c2_c_c3_m_clks_clkm) =
+    {"pllP_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "pllM_out0", "clk_s", "clk_m"};
+PLIST(mux_p_c2_c_c3_clks_N_clkm) =
+    {"pllP_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "clk_s", NULL, "clk_m"};
+PLIST(mux_p_c2_c_c3_clkm_N_clks) =
+    {"pllP_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+     "clk_m", NULL, "clk_s"};
+PLIST(mux_p_clkm_clks_E) =
+    {"pllP_out0", "clk_m", "clk_s", "pllE_out0"};
+PLIST(mux_p_m_d_a_c_d2_clkm) =
+    {"pllP_out0", "pllM_out0", "pllD_out0", "pllA_out0",
+     "pllC_out0", "pllD2_out0", "clk_m"};
+
+PLIST(mux_clkm_N_u48_N_p_N_u480) =
+    {"clk_m", NULL, "pllU_48", NULL,
+     "pllP_out0", NULL, "pllU_480"};
+PLIST(mux_clkm_p_c2_c_c3_refre) =
+    {"clk_m", "pllP_out0", "pllC2_out0", "pllC_out0",
+     "pllC3_out0", "pllREFE_out"};
+PLIST(mux_clkm_refe_clks_u480_c_c2_c3_oscdiv) =
+    {"clk_m", "pllREFE_out", "clk_s", "pllU_480",
+     "pllC_out0", "pllC2_out0", "pllC3_out0", "osc_div_clk"};
+
+PLIST(mux_sep_audio) =
+   {"pllA_out0", "pllC2_out0", "pllC_out0", "pllC3_out0",
+    "pllP_out0", NULL, "clk_m", NULL,
+    "spdif_in", "i2s0", "i2s1", "i2s2",
+    "i2s4", "pllA_out0", "ext_vimclk"};
+
+static uint32_t clk_enabale_reg[] = {
+	CLK_OUT_ENB_L,
+	CLK_OUT_ENB_H,
+	CLK_OUT_ENB_U,
+	CLK_OUT_ENB_V,
+	CLK_OUT_ENB_W,
+	CLK_OUT_ENB_X,
+};
+
+static uint32_t clk_reset_reg[] = {
+	RST_DEVICES_L,
+	RST_DEVICES_H,
+	RST_DEVICES_U,
+	RST_DEVICES_V,
+	RST_DEVICES_W,
+	RST_DEVICES_X,
+};
+
+#define	L(n)  ((0 * 32) + (n))
+#define	H(n)  ((1 * 32) + (n))
+#define	U(n)  ((2 * 32) + (n))
+#define	V(n)  ((3 * 32) + (n))
+#define	W(n)  ((4 * 32) + (n))
+#define	X(n)  ((5 * 32) + (n))
+
+static struct pgate_def pgate_def[] = {
+	/* bank L ->  0-31 */
+	/* GATE(CPU, "cpu", "clk_m", L(0)), */
+	GATE(ISPB, "ispb", "clk_m", L(3)),
+	GATE(RTC, "rtc", "clk_s", L(4)),
+	GATE(TIMER, "timer", "clk_m", L(5)),
+	GATE(UARTA, "uarta", "pc_uarta" , L(6)),
+	GATE(UARTB, "uartb", "pc_uartb", L(7)),
+	GATE(VFIR, "vfir", "pc_vfir", L(7)),
+	/* GATE(GPIO, "gpio", "clk_m", L(8)), */
+	GATE(SDMMC2, "sdmmc2", "pc_sdmmc2", L(9)),
+	GATE(SPDIF_OUT, "spdif_out", "pc_spdif_out", L(10)),
+	GATE(SPDIF_IN, "spdif_in", "pc_spdif_in", L(10)),
+	GATE(I2S1, "i2s1", "pc_i2s1", L(11)),
+	GATE(I2C1, "i2c1", "pc_i2c1", L(12)),
+	GATE(SDMMC1, "sdmmc1", "pc_sdmmc1", L(14)),
+	GATE(SDMMC4, "sdmmc4", "pc_sdmmc4", L(15)),
+	GATE(PWM, "pwm", "pc_pwm", L(17)),
+	GATE(I2S2, "i2s2", "pc_i2s2", L(18)),
+	GATE(VI, "vi", "pc_vi", L(20)),
+	GATE(USBD, "usbd", "clk_m", L(22)),
+	GATE(ISP, "isp", "pc_isp", L(23)),
+	GATE(DISP2, "disp2", "pc_disp2", L(26)),
+	GATE(DISP1, "disp1", "pc_disp1", L(27)),
+	GATE(HOST1X, "host1x", "pc_host1x", L(28)),
+	GATE(VCP, "vcp", "clk_m", L(29)),
+	GATE(I2S0, "i2s0", "pc_i2s0", L(30)),
+	/* GATE(CACHE2, "ccache2", "clk_m", L(31)), */
+
+	/* bank H -> 32-63 */
+	GATE(MC, "mem", "clk_m", H(0)),
+	/* GATE(AHBDMA, "ahbdma", "clk_m", H(1)), */
+	GATE(APBDMA, "apbdma", "clk_m", H(2)),
+	GATE(KBC, "kbc", "clk_s", H(4)),
+	/* GATE(STAT_MON, "stat_mon", "clk_s", H(5)), */
+	/* GATE(PMC, "pmc", "clk_s", H(6)), */
+	GATE(FUSE, "fuse", "clk_m", H(7)),
+	GATE(KFUSE, "kfuse", "clk_m", H(8)),
+	GATE(SBC1, "spi1", "pc_spi1", H(9)),
+	GATE(NOR, "snor", "pc_snor", H(10)),
+	/* GATE(JTAG2TBC, "jtag2tbc", "clk_m", H(11)), */
+	GATE(SBC2, "spi2", "pc_spi2", H(12)),
+	GATE(SBC3, "spi3", "pc_spi3", H(14)),
+	GATE(I2C5, "i2c5", "pc_i2c5", H(15)),
+	GATE(DSIA, "dsia", "dsia_mux", H(16)),
+	GATE(MIPI, "hsi", "pc_hsi", H(18)),
+	GATE(HDMI, "hdmi", "pc_hdmi", H(19)),
+	GATE(CSI, "csi", "pllP_out3", H(20)),
+	GATE(I2C2, "i2c2", "pc_i2c2", H(22)),
+	GATE(UARTC, "uartc", "pc_uartc", H(23)),
+	GATE(MIPI_CAL, "mipi_cal", "clk_m", H(24)),
+	GATE(EMC, "emc", "emc_mux", H(25)),
+	GATE(USB2, "usb2", "clk_m", H(26)),
+	GATE(USB3, "usb3", "clk_m", H(27)),
+	GATE(VDE, "vde", "pc_vde", H(29)),
+	GATE(BSEA, "bsea", "clk_m", H(30)),
+	GATE(BSEV, "bsev", "clk_m", H(31)),
+
+	/* bank U  -> 64-95 */
+	GATE(UARTD, "uartd", "pc_uartd", U(1)),
+	GATE(I2C3, "i2c3", "pc_i2c3", U(3)),
+	GATE(SBC4, "spi4", "pc_spi4", U(4)),
+	GATE(SDMMC3, "sdmmc3", "pc_sdmmc3", U(5)),
+	GATE(PCIE, "pcie", "clk_m", U(6)),
+	GATE(OWR, "owr", "pc_owr", U(7)),
+	GATE(AFI, "afi", "clk_m", U(8)),
+	GATE(CSITE, "csite", "pc_csite", U(9)),
+	/* GATE(AVPUCQ, "avpucq", clk_m, U(11)), */
+	GATE(TRACE, "traceclkin", "pc_traceclkin", U(13)),
+	GATE(SOC_THERM, "soc_therm", "pc_soc_therm", U(14)),
+	GATE(DTV, "dtv", "clk_m", U(15)),
+	GATE(I2CSLOW, "i2c_slow", "pc_i2c_slow", U(17)),
+	GATE(DSIB, "dsib", "dsib_mux", U(18)),
+	GATE(TSEC, "tsec", "pc_tsec", U(19)),
+	/* GATE(IRAMA, "irama", "clk_m", U(20)), */
+	/* GATE(IRAMB, "iramb", "clk_m", U(21)), */
+	/* GATE(IRAMC, "iramc", "clk_m", U(22)), */
+	/* GATE(IRAMD, "iramd", "clk_m", U(23)), */
+	/* GATE(CRAM2, "cram2", "clk_m", U(24)), */
+	GATE(XUSB_HOST, "xusb_core_host", "pc_xusb_core_host", U(25)),
+	/* GATE(M_DOUBLER, "m_doubler", "clk_m", U(26)), */
+	GATE(MSENC, "msenc", "pc_msenc", U(27)),
+	GATE(CSUS, "sus_out", "clk_m", U(28)),
+	/* GATE(DEVD2_OUT, "devd2_out", "clk_m", U(29)), */
+	/* GATE(DEVD1_OUT, "devd1_out", "clk_m", U(30)), */
+	GATE(XUSB_DEV_SRC, "xusb_core_dev", "pc_xusb_core_dev", U(31)),
+
+	/* bank V  -> 96-127 */
+	/* GATE(CPUG, "cpug", "clk_m", V(0)), */
+	/* GATE(CPULP, "cpuLP", "clk_m", V(1)), */
+	GATE(MSELECT, "mselect", "pc_mselect", V(3)),
+	GATE(TSENSOR, "tsensor", "pc_tsensor", V(4)),
+	GATE(I2S3, "i2s3", "pc_i2s3", V(5)),
+	GATE(I2S4, "i2s4", "pc_i2s4", V(6)),
+	GATE(I2C4, "i2c4", "pc_i2c4", V(7)),
+	GATE(SBC5, "spi5", "pc_spi5", V(8)),
+	GATE(SBC6, "spi6", "pc_spi6", V(9)),
+	GATE(D_AUDIO, "audio", "pc_audio", V(10)),
+	GATE(APBIF, "apbif", "clk_m", V(11)),
+	GATE(DAM0, "dam0", "pc_dam0", V(12)),
+	GATE(DAM1, "dam1", "pc_dam1", V(13)),
+	GATE(DAM2, "dam2",  "pc_dam2", V(14)),
+	GATE(HDA2CODEC_2X, "hda2codec_2x", "pc_hda2codec_2x", V(15)),
+	/* GATE(ATOMICS, "atomics", "clk_m", V(16)), */
+	/* GATE(SPDIF_DOUBLER, "spdif_doubler", "clk_m", V(22)), */
+	GATE(ACTMON, "actmon", "pc_actmon", V(23)),
+	GATE(EXTERN1, "extperiph1", "pc_extperiph1", V(24)),
+	GATE(EXTERN2, "extperiph2", "pc_extperiph2", V(25)),
+	GATE(EXTERN3, "extperiph3", "pc_extperiph3", V(26)),
+	GATE(SATA_OOB, "sata_oob", "pc_sata_oob", V(27)),
+	GATE(SATA, "sata", "pc_sata", V(28)),
+	GATE(HDA, "hda", "pc_hda", V(29)),
+
+	/* bank W   -> 128-159*/
+	GATE(HDA2HDMI, "hda2hdmi", "clk_m", W(0)),
+	GATE(SATA_COLD, "sata_cold", "clk_m", W(1)), /* Reset only */
+	/* GATE(PCIERX0, "pcierx0", "clk_m", W(2)), */
+	/* GATE(PCIERX1, "pcierx1", "clk_m", W(3)), */
+	/* GATE(PCIERX2, "pcierx2", "clk_m", W(4)), */
+	/* GATE(PCIERX3, "pcierx3", "clk_m", W(5)), */
+	/* GATE(PCIERX4, "pcierx4", "clk_m", W(6)), */
+	/* GATE(PCIERX5, "pcierx5", "clk_m", W(7)), */
+	/* GATE(CEC, "cec", "clk_m", W(8)), */
+	/* GATE(PCIE2_IOBIST, "pcie2_iobist", "clk_m", W(9)), */
+	/* GATE(EMC_IOBIST, "emc_iobist", "clk_m", W(10)), */
+	/* GATE(HDMI_IOBIST, "hdmi_iobist", "clk_m", W(11)), */
+	/* GATE(SATA_IOBIST, "sata_iobist", "clk_m", W(12)), */
+	/* GATE(MIPI_IOBIST, "mipi_iobist", "clk_m", W(13)), */
+	/* GATE(XUSB_IOBIST, "xusb_iobist", "clk_m", W(15)), */
+	GATE(CILAB, "cilab", "pc_cilab", W(16)),
+	GATE(CILCD, "cilcd", "pc_cilcd", W(17)),
+	GATE(CILE, "cile", "pc_cile", W(18)),
+	GATE(DSIALP, "dsia_lp", "pc_dsia_lp", W(19)),
+	GATE(DSIBLP, "dsib_lp", "pc_dsib_lp", W(20)),
+	GATE(ENTROPY, "entropy", "pc_entropy", W(21)),
+	GATE(AMX, "amx", "pc_amx", W(25)),
+	GATE(ADX, "adx", "pc_adx", W(26)),
+	GATE(DFLL_REF, "dvfs_ref", "pc_dvfs_ref", X(27)),
+	GATE(DFLL_SOC, "dvfs_soc", "pc_dvfs_soc",  X(27)),
+	GATE(XUSB_SS_SRC, "xusb_ss", "xusb_ss_mux", X(28)),
+	/* GATE(EMC_LATENCY, "emc_latency", "pc_emc_latency", X(29)), */
+
+	/* bank X -> 160-191*/
+	/* GATE(SPARE, "spare", "clk_m", X(0)), */
+	/* GATE(CAM_MCLK, "CAM_MCLK", "clk_m", X(4)), */
+	/* GATE(CAM_MCLK2, "CAM_MCLK2", "clk_m", X(5)), */
+	GATE(I2C6, "i2c6", "pc_i2c6", X(6)),
+	/* GATE(VIM2_CLK, "vim2_clk", clk_m, X(11)), */
+	/* GATE(EMC_DLL, "emc_dll", "pc_emc_dll", X(14)), */
+	GATE(HDMI_AUDIO, "hdmi_audio", "pc_hdmi_audio", X(16)),
+	GATE(CLK72MHZ, "clk72mhz", "pc_clk72mhz", X(17)),
+	GATE(VIC03, "vic", "pc_vic", X(18)),
+	GATE(ADX1, "adx1", "pc_adx1", X(20)),
+	GATE(DPAUX, "dpaux", "clk_m", X(21)),
+	GATE(SOR0_LVDS, "sor0", "pc_sor0", X(22)),
+	GATE(GPU, "gpu", "osc_div_clk", X(24)),
+	GATE(AMX1, "amx1", "pc_amx1", X(26)),
+};
+
+/* Peripheral clock clock */
+#define	DCF_HAVE_MUX		0x0100 /* Block with multipexor */
+#define	DCF_HAVE_ENA		0x0200 /* Block with enable bit */
+#define	DCF_HAVE_DIV		0x0400 /* Block with divider */
+
+/* Mark block with additional bis / functionality. */
+#define	DCF_IS_MASK		0x00FF
+#define	DCF_IS_UART		0x0001
+#define	DCF_IS_VI		0x0002
+#define	DCF_IS_HOST1X		0x0003
+#define	DCF_IS_XUSB_SS		0x0004
+#define	DCF_IS_EMC_DLL		0x0005
+#define	FDS_IS_SATA		0x0006
+#define	DCF_IS_VIC		0x0007
+#define	DCF_IS_AUDIO		0x0008
+#define	DCF_IS_SOR0		0x0009
+
+/* Basic pheripheral clock */
+#define	PER_CLK(_id, cn, pl, r, diw, fiw, f)				\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cn,						\
+	.clkdef.parent_names = pl,					\
+	.clkdef.parent_cnt = nitems(pl),				\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,			\
+	.base_reg = r,							\
+	.div_width = diw,						\
+	.div_f_width = fiw,						\
+	.flags = f,							\
+}
+
+/* Mux with fractional 8.1 divider. */
+#define	CLK_8_1(cn, pl, r,  f)						\
+	PER_CLK(0, cn, pl, r,  8, 1, (f) | DCF_HAVE_MUX | DCF_HAVE_DIV)
+/* Mux with fractional 16.1 divider. */
+#define	CLK16_1(cn, pl, r,  f)						\
+	PER_CLK(0, cn, pl, r,  16, 1, (f) | DCF_HAVE_MUX | DCF_HAVE_DIV)
+/* Mux with integer 16bits divider. */
+#define	CLK16_0(cn, pl, r,  f)						\
+	PER_CLK(0, cn, pl, r,  16, 0, (f) | DCF_HAVE_MUX | DCF_HAVE_DIV)
+/* Mux wihout divider. */
+#define	CLK_0_0(cn, pl, r,  f)						\
+	PER_CLK(0, cn, pl, r,  0, 0, (f) | DCF_HAVE_MUX)
+
+static struct periph_def periph_def[] = {
+	CLK_8_1("pc_i2s1", mux_a_N_audio1_N_p_N_clkm, CLK_SOURCE_I2S1, DCF_HAVE_ENA),
+	CLK_8_1("pc_i2s2", mux_a_N_audio2_N_p_N_clkm, CLK_SOURCE_I2S2, DCF_HAVE_ENA),
+	CLK_8_1("pc_spdif_out", mux_a_N_audio_N_p_N_clkm, CLK_SOURCE_SPDIF_OUT, 0),
+	CLK_8_1("pc_spdif_in", mux_p_c2_c_c3_m, CLK_SOURCE_SPDIF_IN, 0),
+	CLK_8_1("pc_pwm", mux_p_c2_c_c3_clks_N_clkm, CLK_SOURCE_PWM, 0),
+	CLK_8_1("pc_spi2", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_SPI2, 0),
+	CLK_8_1("pc_spi3", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_SPI3, 0),
+	CLK16_0("pc_i2c5", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_I2C5, 0),
+	CLK16_0("pc_i2c1", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_I2C1, 0),
+	CLK_8_1("pc_spi1", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_SPI1, 0),
+	CLK_0_0("pc_disp1", mux_p_m_d_a_c_d2_clkm, CLK_SOURCE_DISP1, 0),
+	CLK_0_0("pc_disp2", mux_p_m_d_a_c_d2_clkm, CLK_SOURCE_DISP2, 0),
+	CLK_8_1("pc_isp", mux_m_c_p_a_c2_c3_clkm_c4, CLK_SOURCE_ISP, 0),
+	CLK_8_1("pc_vi", mux_m_c2_c_c3_p_N_a_c4, CLK_SOURCE_VI, DCF_IS_VI),
+	CLK_8_1("pc_sdmmc1", mux_p_c2_c_c3_m_e_clkm, CLK_SOURCE_SDMMC1, 0),
+	CLK_8_1("pc_sdmmc2", mux_p_c2_c_c3_m_e_clkm, CLK_SOURCE_SDMMC2, 0),
+	CLK_8_1("pc_sdmmc4", mux_p_c2_c_c3_m_e_clkm, CLK_SOURCE_SDMMC4, 0),
+	CLK_8_1("pc_vfir", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_VFIR, 0),
+	CLK_8_1("pc_hsi", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_HSI, 0),
+	CLK16_1("pc_uarta", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_UARTA, DCF_IS_UART),
+	CLK16_1("pc_uartb", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_UARTB, DCF_IS_UART),
+	CLK_8_1("pc_host1x", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_HOST1X, DCF_IS_HOST1X),
+	CLK_8_1("pc_hdmi", mux_p_m_d_a_c_d2_clkm, CLK_SOURCE_HDMI, 0),
+	CLK16_0("pc_i2c2", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_I2C2, 0),
+/* EMC  8 */
+	CLK16_1("pc_uartc", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_UARTC, DCF_IS_UART),
+	CLK_8_1("pc_vi_sensor", mux_m_c2_c_c3_p_N_a, CLK_SOURCE_VI_SENSOR, 0),
+	CLK_8_1("pc_spi4", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_SPI4, 0),
+	CLK16_0("pc_i2c3", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_I2C3, 0),
+	CLK_8_1("pc_sdmmc3", mux_p_c2_c_c3_m_e_clkm, CLK_SOURCE_SDMMC3, 0),
+	CLK16_1("pc_uartd", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_UARTD, DCF_IS_UART),
+	CLK_8_1("pc_vde", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_VDE, 0),
+	CLK_8_1("pc_owr", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_OWR, 0),
+	CLK_8_1("pc_snor", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_NOR, 0),
+	CLK_8_1("pc_csite", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_CSITE, 0),
+	CLK_8_1("pc_i2s0", mux_a_N_audio0_N_p_N_clkm, CLK_SOURCE_I2S0, 0),
+/* DTV xxx */
+	CLK_8_1("pc_msenc", mux_m_c2_c_c3_p_N_a, CLK_SOURCE_MSENC, 0),
+	CLK_8_1("pc_tsec", mux_p_c2_c_c3_m_a_clkm, CLK_SOURCE_TSEC, 0),
+/* SPARE2 */
+
+
+	CLK_8_1("pc_mselect", mux_p_c2_c_c3_m_clks_clkm, CLK_SOURCE_MSELECT, 0),
+	CLK_8_1("pc_tsensor", mux_p_c2_c_c3_clkm_N_clks, CLK_SOURCE_TSENSOR, 0),
+	CLK_8_1("pc_i2s3", mux_a_N_audio3_N_p_N_clkm, CLK_SOURCE_I2S3, DCF_HAVE_ENA),
+	CLK_8_1("pc_i2s4", mux_a_N_audio4_N_p_N_clkm, CLK_SOURCE_I2S4, DCF_HAVE_ENA),
+	CLK16_0("pc_i2c4", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_I2C4, 0),
+	CLK_8_1("pc_spi5", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_SPI5, 0),
+	CLK_8_1("pc_spi6", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_SPI6, 0),
+	CLK_8_1("pc_audio", mux_sep_audio, CLK_SOURCE_AUDIO, DCF_IS_AUDIO),
+	CLK_8_1("pc_dam0", mux_sep_audio, CLK_SOURCE_DAM0, DCF_IS_AUDIO),
+	CLK_8_1("pc_dam1", mux_sep_audio, CLK_SOURCE_DAM1, DCF_IS_AUDIO),
+	CLK_8_1("pc_dam2",  mux_sep_audio, CLK_SOURCE_DAM2, DCF_IS_AUDIO),
+	CLK_8_1("pc_hda2codec_2x", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_HDA2CODEC_2X, 0),
+	CLK_8_1("pc_actmon", mux_p_c2_c_c3_clks_N_clkm, CLK_SOURCE_ACTMON, 0),
+	CLK_8_1("pc_extperiph1", mux_a_clks_p_clkm_e, CLK_SOURCE_EXTPERIPH1, 0),
+	CLK_8_1("pc_extperiph2", mux_a_clks_p_clkm_e, CLK_SOURCE_EXTPERIPH2,  0),
+	CLK_8_1("pc_extperiph3", mux_a_clks_p_clkm_e, CLK_SOURCE_EXTPERIPH3, 0),
+	CLK_8_1("pc_i2c_slow", mux_p_c2_c_c3_clks_N_clkm, CLK_SOURCE_I2C_SLOW, 0),
+/* SYS */
+	CLK_8_1("pc_sor0", mux_p_m_d_a_c_d2_clkm,  CLK_SOURCE_SOR0, DCF_IS_SOR0),
+	CLK_8_1("pc_sata_oob", mux_p_N_c_N_m_N_clkm, CLK_SOURCE_SATA_OOB, 0),
+	CLK_8_1("pc_sata", mux_p_N_c_N_m_N_clkm, CLK_SOURCE_SATA, FDS_IS_SATA),
+	CLK_8_1("pc_hda", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_HDA, 0),
+
+
+	CLK_8_1("pc_xusb_core_host", mux_clkm_p_c2_c_c3_refre, CLK_SOURCE_XUSB_CORE_HOST, 0),
+	CLK_8_1("pc_xusb_falcon", mux_clkm_p_c2_c_c3_refre, CLK_SOURCE_XUSB_FALCON, 0),
+	CLK_8_1("pc_xusb_fs", mux_clkm_N_u48_N_p_N_u480, CLK_SOURCE_XUSB_FS, 0),
+	CLK_8_1("pc_xusb_core_dev", mux_clkm_p_c2_c_c3_refre, CLK_SOURCE_XUSB_CORE_DEV, 0),
+	CLK_8_1("pc_xusb_ss", mux_clkm_refe_clks_u480_c_c2_c3_oscdiv, CLK_SOURCE_XUSB_SS, DCF_IS_XUSB_SS),
+	CLK_8_1("pc_cilab", mux_p_N_c_N_N_N_clkm, CLK_SOURCE_CILAB, 0),
+	CLK_8_1("pc_cilcd", mux_p_N_c_N_N_N_clkm, CLK_SOURCE_CILCD, 0),
+	CLK_8_1("pc_cile", mux_p_N_c_N_N_N_clkm, CLK_SOURCE_CILE, 0),
+	CLK_8_1("pc_dsia_lp", mux_p_N_c_N_N_N_clkm, CLK_SOURCE_DSIA_LP, 0),
+	CLK_8_1("pc_dsib_lp", mux_p_N_c_N_N_N_clkm, CLK_SOURCE_DSIB_LP, 0),
+	CLK_8_1("pc_entropy", mux_p_clkm_clks_E, CLK_SOURCE_ENTROPY, 0),
+	CLK_8_1("pc_dvfs_ref", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_DVFS_REF, DCF_HAVE_ENA),
+	CLK_8_1("pc_dvfs_soc", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_DVFS_SOC, DCF_HAVE_ENA),
+	CLK_8_1("pc_traceclkin", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_TRACECLKIN, 0),
+	CLK_8_1("pc_adx", mux_a_c2_c_c3_p_N_clkm, CLK_SOURCE_ADX, DCF_HAVE_ENA),
+	CLK_8_1("pc_amx", mux_a_c2_c_c3_p_N_clkm, CLK_SOURCE_AMX, DCF_HAVE_ENA),
+	CLK_8_1("pc_emc_latency", mux_m_c_p_clkm_mud_c2_c3, CLK_SOURCE_EMC_LATENCY, 0),
+	CLK_8_1("pc_soc_therm", mux_m_c_p_a_c2_c3, CLK_SOURCE_SOC_THERM, 0),
+	CLK_8_1("pc_vi_sensor2", mux_m_c2_c_c3_p_N_a, CLK_SOURCE_VI_SENSOR2, 0),
+	CLK16_0("pc_i2c6", mux_p_c2_c_c3_m_N_clkm, CLK_SOURCE_I2C6, 0),
+	CLK_8_1("pc_emc_dll", mux_m_c_p_clkm_mud_c2_c3, CLK_SOURCE_EMC_DLL, DCF_IS_EMC_DLL),
+	CLK_8_1("pc_hdmi_audio", mux_p_c_c2_clkm, CLK_SOURCE_HDMI_AUDIO, 0),
+	CLK_8_1("pc_clk72mhz", mux_p_c_c2_clkm, CLK_SOURCE_CLK72MHZ, 0),
+	CLK_8_1("pc_adx1", mux_a_c2_c_c3_p_N_clkm, CLK_SOURCE_ADX1, DCF_HAVE_ENA),
+	CLK_8_1("pc_amx1", mux_a_c2_c_c3_p_N_clkm, CLK_SOURCE_AMX1, DCF_HAVE_ENA),
+	CLK_8_1("pc_vic", mux_m_c_p_a_c2_c3_clkm, CLK_SOURCE_VIC, DCF_IS_VIC),
+};
+
+static int periph_init(struct clknode *clk, device_t dev);
+static int periph_recalc(struct clknode *clk, uint64_t *freq);
+static int periph_set_freq(struct clknode *clk, uint64_t fin,
+    uint64_t *fout, int flags, int *stop);
+static int periph_set_mux(struct clknode *clk, int idx);
+
+struct periph_sc {
+	device_t		clkdev;
+	uint32_t		base_reg;
+	uint32_t		div_shift;
+	uint32_t		div_width;
+	uint32_t		div_mask;
+	uint32_t		div_f_width;
+	uint32_t		div_f_mask;
+	uint32_t		flags;
+
+	uint32_t		divider;
+	int 			mux;
+};
+
+static clknode_method_t periph_methods[] = {
+	/* Device interface */
+	CLKNODEMETHOD(clknode_init,		periph_init),
+	CLKNODEMETHOD(clknode_recalc_freq,	periph_recalc),
+	CLKNODEMETHOD(clknode_set_freq,		periph_set_freq),
+	CLKNODEMETHOD(clknode_set_mux, 		periph_set_mux),
+	CLKNODEMETHOD_END
+};
+DEFINE_CLASS_1(tegra124_periph, tegra124_periph_class, periph_methods,
+   sizeof(struct periph_sc), clknode_class);
+static int
+periph_init(struct clknode *clk, device_t dev)
+{
+	struct periph_sc *sc;
+	uint32_t reg;
+	sc = clknode_get_softc(clk);
+
+	DEVICE_LOCK(sc);
+	if (sc->flags & DCF_HAVE_ENA)
+		MD4(sc, sc->base_reg, PERLCK_ENA_MASK, PERLCK_ENA_MASK);
+
+	RD4(sc, sc->base_reg, &reg);
+	DEVICE_UNLOCK(sc);
+
+	/* Stnadard mux. */
+	if (sc->flags & DCF_HAVE_MUX)
+		sc->mux = (reg >> PERLCK_MUX_SHIFT) & PERLCK_MUX_MASK;
+	else
+		sc->mux = 0;
+	if (sc->flags & DCF_HAVE_DIV)
+		sc->divider = (reg & sc->div_mask) + 2;
+	else
+		sc->divider = 1;
+	if ((sc->flags & DCF_IS_MASK) == DCF_IS_UART) {
+		if (!(reg & PERLCK_UDIV_DIS))
+			sc->divider = 2;
+	}
+
+	/* AUDIO MUX */
+	if ((sc->flags & DCF_IS_MASK) == DCF_IS_AUDIO) {
+		if (!(reg & PERLCK_AMUX_DIS) && (sc->mux == 7)) {
+			sc->mux = 8 +
+			    ((reg >> PERLCK_AMUX_SHIFT) & PERLCK_MUX_MASK);
+		}
+	}
+	clknode_init_parent_idx(clk, sc->mux);
+	return(0);
+}
+
+static int
+periph_set_mux(struct clknode *clk, int idx)
+{
+	struct periph_sc *sc;
+	uint32_t reg;
+
+
+	sc = clknode_get_softc(clk);
+	if (!(sc->flags & DCF_HAVE_MUX))
+		return (ENXIO);
+
+	sc->mux = idx;
+	DEVICE_LOCK(sc);
+	RD4(sc, sc->base_reg, &reg);
+	reg &= ~(PERLCK_MUX_MASK << PERLCK_MUX_SHIFT);
+	if ((sc->flags & DCF_IS_MASK) == DCF_IS_AUDIO) {
+		reg &= ~PERLCK_AMUX_DIS;
+		reg &= ~(PERLCK_MUX_MASK << PERLCK_AMUX_SHIFT);
+
+		if (idx <= 7) {
+			reg |= idx << PERLCK_MUX_SHIFT;
+		} else {
+			reg |= 7 << PERLCK_MUX_SHIFT;
+			reg |= (idx - 8) << PERLCK_AMUX_SHIFT;
+		}
+	} else {
+		reg |= idx << PERLCK_MUX_SHIFT;
+	}
+	WR4(sc, sc->base_reg, reg);
+	DEVICE_UNLOCK(sc);
+
+	return(0);
+}
+
+static int
+periph_recalc(struct clknode *clk, uint64_t *freq)
+{
+	struct periph_sc *sc;
+	uint32_t reg;
+
+	sc = clknode_get_softc(clk);
+
+	if (sc->flags & DCF_HAVE_DIV) {
+		DEVICE_LOCK(sc);
+		RD4(sc, sc->base_reg, &reg);
+		DEVICE_UNLOCK(sc);
+		*freq = (*freq << sc->div_f_width) / sc->divider;
+	}
+	return (0);
+}
+
+static int
+periph_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
+   int flags, int *stop)
+{
+	struct periph_sc *sc;
+	uint64_t tmp, divider;
+
+	sc = clknode_get_softc(clk);
+	if (!(sc->flags & DCF_HAVE_DIV)) {
+		*stop = 0;
+		return (0);
+	}
+
+	tmp = fin << sc->div_f_width;
+	divider = tmp / *fout;
+	if ((tmp % *fout) != 0)
+		divider++;
+
+	if (divider < (1 << sc->div_f_width))
+		 divider = 1 << sc->div_f_width;
+
+	if ((*stop != 0) &&
+	    ((flags & (CLK_SET_ROUND_UP | CLK_SET_ROUND_DOWN)) == 0) &&
+	    (*fout != (tmp / divider)))
+		return (ERANGE);
+
+	if ((flags & CLK_SET_DRYRUN) == 0) {
+		DEVICE_LOCK(sc);
+		MD4(sc, sc->base_reg, sc->div_mask,
+		    (divider - (1 << sc->div_f_width)));
+		DEVICE_UNLOCK(sc);
+		sc->divider = divider;
+	}
+	*fout = tmp / divider;
+	*stop = 1;
+	return (0);
+}
+
+static int
+periph_register(struct clkdom *clkdom, struct periph_def *clkdef)
+{
+	struct clknode *clk;
+	struct periph_sc *sc;
+
+	clk = clknode_create(clkdom, &tegra124_periph_class, &clkdef->clkdef);
+	if (clk == NULL)
+		return (1);
+
+	sc = clknode_get_softc(clk);
+	sc->clkdev = clknode_get_device(clk);
+	sc->base_reg = clkdef->base_reg;
+	sc->div_width = clkdef->div_width;
+	sc->div_mask = (1 <<clkdef->div_width) - 1;
+	sc->div_f_width = clkdef->div_f_width;
+	sc->div_f_mask = (1 <<clkdef->div_f_width) - 1;
+	sc->flags = clkdef->flags;
+
+	clknode_register(clkdom, clk);
+	return (0);
+}
+
+/* -------------------------------------------------------------------------- */
+static int pgate_init(struct clknode *clk, device_t dev);
+static int pgate_set_gate(struct clknode *clk, bool enable);
+
+struct pgate_sc {
+	device_t		clkdev;
+	uint32_t		idx;
+	uint32_t		flags;
+	uint32_t		enabled;
+
+};
+
+static clknode_method_t pgate_methods[] = {
+	/* Device interface */
+	CLKNODEMETHOD(clknode_init,		pgate_init),
+	CLKNODEMETHOD(clknode_set_gate,		pgate_set_gate),
+	CLKNODEMETHOD_END
+};
+DEFINE_CLASS_1(tegra124_pgate, tegra124_pgate_class, pgate_methods,
+   sizeof(struct pgate_sc), clknode_class);
+
+static uint32_t
+get_enable_reg(int idx)
+{
+	KASSERT(idx / 32 < nitems(clk_enabale_reg),
+	    ("Invalid clock index for enable: %d", idx));
+	return (clk_enabale_reg[idx / 32]);
+}
+
+static uint32_t
+get_reset_reg(int idx)
+{
+	KASSERT(idx / 32 < nitems(clk_reset_reg),
+	    ("Invalid clock index for reset: %d", idx));
+	return (clk_reset_reg[idx / 32]);
+}
+
+static int
+pgate_init(struct clknode *clk, device_t dev)
+{
+	struct pgate_sc *sc;
+	uint32_t ena_reg, rst_reg, mask;
+
+	sc = clknode_get_softc(clk);
+	mask = 1 << (sc->idx % 32);
+
+	DEVICE_LOCK(sc);
+	RD4(sc, get_enable_reg(sc->idx), &ena_reg);
+	RD4(sc, get_reset_reg(sc->idx), &rst_reg);
+	DEVICE_UNLOCK(sc);
+
+	sc->enabled = ena_reg & mask ? 1 : 0;
+	clknode_init_parent_idx(clk, 0);
+
+	return(0);
+}
+
+static int
+pgate_set_gate(struct clknode *clk, bool enable)
+{
+	struct pgate_sc *sc;
+	uint32_t reg, mask, base_reg;
+
+	sc = clknode_get_softc(clk);
+	mask = 1 << (sc->idx % 32);
+	sc->enabled = enable;
+	base_reg = get_enable_reg(sc->idx);
+
+	DEVICE_LOCK(sc);
+	MD4(sc, base_reg, mask, enable ? mask : 0);
+	RD4(sc, base_reg, &reg);
+	DEVICE_UNLOCK(sc);
+
+	DELAY(2);
+	return(0);
+}
+
+int
+tegra124_hwreset_by_idx(struct tegra124_car_softc *sc, intptr_t idx, bool reset)
+{
+	uint32_t reg, mask, reset_reg;
+
+	mask = 1 << (idx % 32);
+	reset_reg = get_reset_reg(idx);
+
+	CLKDEV_DEVICE_LOCK(sc->dev);
+	CLKDEV_MODIFY_4(sc->dev, reset_reg, mask, reset ? mask : 0);
+	CLKDEV_READ_4(sc->dev, reset_reg, &reg);
+	CLKDEV_DEVICE_UNLOCK(sc->dev);
+
+	return(0);
+}
+
+static int
+pgate_register(struct clkdom *clkdom, struct pgate_def *clkdef)
+{
+	struct clknode *clk;
+	struct pgate_sc *sc;
+
+	clk = clknode_create(clkdom, &tegra124_pgate_class, &clkdef->clkdef);
+	if (clk == NULL)
+		return (1);
+
+	sc = clknode_get_softc(clk);
+	sc->clkdev = clknode_get_device(clk);
+	sc->idx = clkdef->idx;
+	sc->flags = clkdef->flags;
+
+	clknode_register(clkdom, clk);
+	return (0);
+}
+
+void
+tegra124_periph_clock(struct tegra124_car_softc *sc)
+{
+	int i, rv;
+
+	for (i = 0; i <  nitems(periph_def); i++) {
+		rv = periph_register(sc->clkdom, &periph_def[i]);
+		if (rv != 0)
+			panic("tegra124_periph_register failed");
+	}
+	for (i = 0; i <  nitems(pgate_def); i++) {
+		rv = pgate_register(sc->clkdom, &pgate_def[i]);
+		if (rv != 0)
+			panic("tegra124_pgate_register failed");
+	}
+
+}
diff --git a/sys/arm/nvidia/tegra124/tegra124_clk_pll.c b/sys/arm/nvidia/tegra124/tegra124_clk_pll.c
new file mode 100644
index 0000000..6adfd32
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_clk_pll.c
@@ -0,0 +1,1066 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/lock.h>
+#include <sys/mutex.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+
+#include <dev/extres/clk/clk.h>
+
+#include <gnu/dts/include/dt-bindings/clock/tegra124-car.h>
+#include "tegra124_car.h"
+
+/* #define TEGRA_PLL_DEBUG */
+#ifdef TEGRA_PLL_DEBUG
+#define dprintf(...) printf(__VA_ARGS__)
+#else
+#define dprintf(...)
+#endif
+
+/* All PLLs. */
+enum pll_type {
+	PLL_M,
+	PLL_X,
+	PLL_C,
+	PLL_C2,
+	PLL_C3,
+	PLL_C4,
+	PLL_P,
+	PLL_A,
+	PLL_U,
+	PLL_D,
+	PLL_D2,
+	PLL_DP,
+	PLL_E,
+	PLL_REFE};
+
+/* Common base register bits. */
+#define	PLL_BASE_BYPASS		(1U << 31)
+#define	PLL_BASE_ENABLE		(1  << 30)
+#define	PLL_BASE_REFDISABLE	(1  << 29)
+#define	PLL_BASE_LOCK		(1  << 27)
+#define	PLL_BASE_DIVM_SHIFT	0
+#define	PLL_BASE_DIVN_SHIFT	8
+
+#define	PLLRE_MISC_LOCK		(1 << 24)
+
+#define	PLL_MISC_LOCK_ENABLE	(1 << 18)
+#define	PLLC_MISC_LOCK_ENABLE	(1 << 24)
+#define	PLLDU_MISC_LOCK_ENABLE	(1 << 22)
+#define	PLLRE_MISC_LOCK_ENABLE	(1 << 30)
+#define	PLLSS_MISC_LOCK_ENABLE	(1 << 30)
+
+#define	PLLC_IDDQ_BIT		26
+#define	PLLX_IDDQ_BIT		3
+#define	PLLRE_IDDQ_BIT		16
+#define	PLLSS_IDDQ_BIT		19
+
+#define	PLL_LOCK_TIMEOUT 1000
+
+/* Post divider <-> register value mapping. */
+struct pdiv_table {
+	uint32_t divider;	/* real divider */
+	uint32_t value;		/* register value */
+};
+
+/* Bits definition of M, N and P fields. */
+struct mnp_bits {
+	uint32_t	m_width;
+	uint32_t	n_width;
+	uint32_t	p_width;
+	uint32_t	p_shift;
+};
+
+struct clk_pll_def {
+	struct clknode_init_def	clkdef;
+	enum pll_type		type;
+	uint32_t		base_reg;
+	uint32_t		misc_reg;
+	uint32_t		lock_mask;
+	uint32_t		lock_enable;
+	uint32_t		iddq_reg;
+	uint32_t		iddq_mask;
+	uint32_t		flags;
+	struct pdiv_table 	*pdiv_table;
+	struct mnp_bits		mnp_bits;
+};
+
+#define	PLL(_id, cname, pname)					\
+	.clkdef.id = _id,					\
+	.clkdef.name = cname,					\
+	.clkdef.parent_names = (const char *[]){pname},		\
+	.clkdef.parent_cnt = 1,				\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS
+
+/* Tegra K1 PLLs
+ PLLM: Clock source for EMC 2x clock
+ PLLX: Clock source for the fast CPU cluster and the shadow CPU
+ PLLC: Clock source for general use
+ PLLC2: Clock source for engine scaling
+ PLLC3: Clock source for engine scaling
+ PLLC4: Clock source for ISP/VI units
+ PLLP: Clock source for most peripherals
+ PLLA: Audio clock sources: (11.2896 MHz, 12.288 MHz, 24.576 MHz)
+ PLLU: Clock source for USB PHY, provides 12/60/480 MHz
+ PLLD: Clock sources for the DSI and display subsystem
+ PLLD2: Clock sources for the DSI and display subsystem
+ refPLLe:
+ PLLE: generate the 100 MHz reference clock for USB 3.0 (spread spectrum)
+ PLLDP: Clock source for eDP/LVDS (spread spectrum)
+
+ DFLLCPU: DFLL clock source for the fast CPU cluster
+ GPCPLL: Clock source for the GPU
+*/
+
+static struct pdiv_table pllm_map[] = {
+	{1, 0},
+	{2, 1},
+	{0, 0}
+};
+
+static struct pdiv_table pllxc_map[] = {
+	{ 1,  0},
+	{ 2,  1},
+	{ 3,  2},
+	{ 4,  3},
+	{ 5,  4},
+	{ 6,  5},
+	{ 8,  6},
+	{10,  7},
+	{12,  8},
+	{16,  9},
+	{12, 10},
+	{16, 11},
+	{20, 12},
+	{24, 13},
+	{32, 14},
+	{ 0,  0}
+};
+
+static struct pdiv_table pllc_map[] = {
+	{ 1, 0},
+	{ 2, 1},
+	{ 3, 2},
+	{ 4, 3},
+	{ 6, 4},
+	{ 8, 5},
+	{12, 6},
+	{16, 7},
+	{ 0,  0}
+};
+
+static struct pdiv_table pll12g_ssd_esd_map[] = {
+	{ 1,  0},
+	{ 2,  1},
+	{ 3,  2},
+	{ 4,  3},
+	{ 5,  4},
+	{ 6,  5},
+	{ 8,  6},
+	{10,  7},
+	{12,  8},
+	{16,  9},
+	{12, 10},
+	{16, 11},
+	{20, 12},
+	{24, 13},
+	{32, 14},
+	{ 0,  0}
+};
+
+static struct pdiv_table pllu_map[] = {
+	{1, 1},
+	{2, 0},
+	{0, 0}
+};
+
+static struct clk_pll_def pll_clks[] = {
+/* PLLM: 880 MHz Clock source for EMC 2x clock */
+	{
+		PLL(TEGRA124_CLK_PLL_M, "pllM_out0", "osc_div_clk"),
+		.type = PLL_M,
+		.base_reg = PLLM_BASE,
+		.misc_reg = PLLM_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLL_MISC_LOCK_ENABLE,
+		.pdiv_table = pllm_map,
+		.mnp_bits = {8, 8, 1, 20},
+	},
+/* PLLX: 1GHz Clock source for the fast CPU cluster and the shadow CPU */
+	{
+		PLL(TEGRA124_CLK_PLL_X, "pllX_out", "osc_div_clk"),
+		.type = PLL_X,
+		.base_reg = PLLX_BASE,
+		.misc_reg = PLLX_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLL_MISC_LOCK_ENABLE,
+		.iddq_reg = PLLX_MISC3,
+		.iddq_mask = 1 << PLLX_IDDQ_BIT,
+		.pdiv_table = pllxc_map,
+		.mnp_bits = {8, 8, 4, 20},
+	},
+/* PLLC: 600 MHz Clock source for general use */
+	{
+		PLL(TEGRA124_CLK_PLL_C, "pllC_out0", "osc_div_clk"),
+		.type = PLL_C,
+		.base_reg = PLLC_BASE,
+		.misc_reg = PLLC_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLLC_MISC_LOCK_ENABLE,
+		.iddq_reg = PLLC_MISC,
+		.iddq_mask = 1 << PLLC_IDDQ_BIT,
+		.pdiv_table = pllc_map,
+		.mnp_bits = {8, 8, 4, 20},
+	},
+/* PLLC2: 600 MHz Clock source for engine scaling */
+	{
+		PLL(TEGRA124_CLK_PLL_C2, "pllC2_out0", "osc_div_clk"),
+		.type = PLL_C2,
+		.base_reg = PLLC2_BASE,
+		.misc_reg = PLLC2_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLL_MISC_LOCK_ENABLE,
+		.pdiv_table = pllc_map,
+		.mnp_bits = {2, 8, 3, 20},
+	},
+/* PLLC3: 600 MHz Clock source for engine scaling */
+	{
+		PLL(TEGRA124_CLK_PLL_C3, "pllC3_out0", "osc_div_clk"),
+		.type = PLL_C3,
+		.base_reg = PLLC3_BASE,
+		.misc_reg = PLLC3_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLL_MISC_LOCK_ENABLE,
+		.pdiv_table = pllc_map,
+		.mnp_bits = {2, 8, 3, 20},
+	},
+/* PLLC4: 600 MHz Clock source for ISP/VI units */
+	{
+		PLL(TEGRA124_CLK_PLL_C4, "pllC4_out0", "pllC4_src"),
+		.type = PLL_C4,
+		.base_reg = PLLC4_BASE,
+		.misc_reg = PLLC4_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLLSS_MISC_LOCK_ENABLE,
+		.iddq_reg = PLLC4_BASE,
+		.iddq_mask = 1 << PLLSS_IDDQ_BIT,
+		.pdiv_table = pll12g_ssd_esd_map,
+		.mnp_bits = {8, 8, 4, 20},
+	},
+/* PLLP: 408 MHz Clock source for most peripherals */
+	{
+		PLL(TEGRA124_CLK_PLL_P, "pllP_out0", "osc_div_clk"),
+		.type = PLL_P,
+		.base_reg = PLLP_BASE,
+		.misc_reg = PLLP_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLL_MISC_LOCK_ENABLE,
+		.mnp_bits = {5, 10, 3,  20},
+	},
+/* PLLA: Audio clock sources: (11.2896 MHz, 12.288 MHz, 24.576 MHz) */
+	{
+		PLL(TEGRA124_CLK_PLL_A, "pllA_out", "pllP_out1"),
+		.type = PLL_A,
+		.base_reg = PLLA_BASE,
+		.misc_reg = PLLA_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLL_MISC_LOCK_ENABLE,
+		.mnp_bits = {5, 10, 3,  20},
+	},
+/* PLLU: 480 MHz Clock source for USB PHY, provides 12/60/480 MHz */
+	{
+		PLL(TEGRA124_CLK_PLL_U, "pllU_out", "osc_div_clk"),
+		.type = PLL_U,
+		.base_reg = PLLU_BASE,
+		.misc_reg = PLLU_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLLDU_MISC_LOCK_ENABLE,
+		.pdiv_table = pllu_map,
+		.mnp_bits = {5, 10, 1, 20},
+	},
+/* PLLD: 600 MHz Clock sources for the DSI and display subsystem */
+	{
+		PLL(TEGRA124_CLK_PLL_D, "pllD_out", "osc_div_clk"),
+		.type = PLL_D,
+		.base_reg = PLLD_BASE,
+		.misc_reg = PLLD_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLL_MISC_LOCK_ENABLE,
+		.mnp_bits = {5, 11, 3, 20},
+	},
+/* PLLD2: 600 MHz Clock sources for the DSI and display subsystem */
+	{
+		PLL(TEGRA124_CLK_PLL_D2, "pllD2_out", "pllD2_src"),
+		.type = PLL_D2,
+		.base_reg = PLLD2_BASE,
+		.misc_reg = PLLD2_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLLSS_MISC_LOCK_ENABLE,
+		.iddq_reg = PLLD2_BASE,
+		.iddq_mask =  1 << PLLSS_IDDQ_BIT,
+		.pdiv_table = pll12g_ssd_esd_map,
+		.mnp_bits = {8, 8, 4, 20},
+	},
+/* refPLLe:  */
+	{
+		PLL(0, "pllREFE_out", "osc_div_clk"),
+		.type = PLL_REFE,
+		.base_reg = PLLRE_BASE,
+		.misc_reg = PLLRE_MISC,
+		.lock_mask = PLLRE_MISC_LOCK,
+		.lock_enable = PLLRE_MISC_LOCK_ENABLE,
+		.iddq_reg = PLLRE_MISC,
+		.iddq_mask = 1 << PLLRE_IDDQ_BIT,
+		.mnp_bits = {8, 8, 4, 16},
+	},
+/* PLLE: generate the 100 MHz reference clock for USB 3.0 (spread spectrum) */
+	{
+		PLL(TEGRA124_CLK_PLL_E, "pllE_out0", "pllE_src"),
+		.type = PLL_E,
+		.base_reg = PLLE_BASE,
+		.misc_reg = PLLE_MISC,
+		.lock_mask = PLLE_MISC_LOCK,
+		.lock_enable = PLLE_MISC_LOCK_ENABLE,
+		.mnp_bits = {8, 8, 4, 24},
+	},
+/* PLLDP: 600 MHz Clock source for eDP/LVDS (spread spectrum) */
+	{
+		PLL(0, "pllDP_out0", "pllDP_src"),
+		.type = PLL_DP,
+		.base_reg = PLLDP_BASE,
+		.misc_reg = PLLDP_MISC,
+		.lock_mask = PLL_BASE_LOCK,
+		.lock_enable = PLLSS_MISC_LOCK_ENABLE,
+		.iddq_reg = PLLDP_BASE,
+		.iddq_mask =  1 << PLLSS_IDDQ_BIT,
+		.pdiv_table = pll12g_ssd_esd_map,
+		.mnp_bits = {8, 8, 4, 20},
+	},
+};
+
+static int tegra124_pll_init(struct clknode *clk, device_t dev);
+static int tegra124_pll_set_gate(struct clknode *clk, bool enable);
+static int tegra124_pll_recalc(struct clknode *clk, uint64_t *freq);
+static int tegra124_pll_set_freq(struct clknode *clknode, uint64_t fin,
+    uint64_t *fout, int flags, int *stop);
+struct pll_sc {
+	device_t		clkdev;
+	enum pll_type		type;
+	uint32_t		base_reg;
+	uint32_t		misc_reg;
+	uint32_t		lock_mask;
+	uint32_t		lock_enable;
+	uint32_t		iddq_reg;
+	uint32_t		iddq_mask;
+	uint32_t		flags;
+	struct pdiv_table 	*pdiv_table;
+	struct mnp_bits		mnp_bits;
+};
+
+static clknode_method_t tegra124_pll_methods[] = {
+	/* Device interface */
+	CLKNODEMETHOD(clknode_init,		tegra124_pll_init),
+	CLKNODEMETHOD(clknode_set_gate,		tegra124_pll_set_gate),
+	CLKNODEMETHOD(clknode_recalc_freq,	tegra124_pll_recalc),
+	CLKNODEMETHOD(clknode_set_freq,		tegra124_pll_set_freq),
+	CLKNODEMETHOD_END
+};
+DEFINE_CLASS_1(tegra124_pll, tegra124_pll_class, tegra124_pll_methods,
+   sizeof(struct pll_sc), clknode_class);
+
+static int
+pll_enable(struct pll_sc *sc)
+{
+	uint32_t reg;
+
+
+	RD4(sc, sc->base_reg, &reg);
+	if (sc->type != PLL_E)
+		reg &= ~PLL_BASE_BYPASS;
+	reg |= PLL_BASE_ENABLE;
+	WR4(sc, sc->base_reg, reg);
+	return (0);
+}
+
+static int
+pll_disable(struct pll_sc *sc)
+{
+	uint32_t reg;
+
+	RD4(sc, sc->base_reg, &reg);
+	if (sc->type != PLL_E)
+		reg |= PLL_BASE_BYPASS;
+	reg &= ~PLL_BASE_ENABLE;
+	WR4(sc, sc->base_reg, reg);
+	return (0);
+}
+
+static uint32_t
+pdiv_to_reg(struct pll_sc *sc, uint32_t p_div)
+{
+	struct pdiv_table *tbl;
+
+	tbl = sc->pdiv_table;
+	if (tbl == NULL)
+		return (ffs(p_div));
+
+	while (tbl->divider != 0) {
+		if (p_div <= tbl->divider)
+			return (tbl->value);
+		tbl++;
+	}
+	return ~0;
+}
+
+static uint32_t
+reg_to_pdiv(struct pll_sc *sc, uint32_t reg)
+{
+	struct pdiv_table *tbl;
+
+	tbl = sc->pdiv_table;
+	if (tbl != NULL) {
+		while (tbl->divider) {
+			if (reg == tbl->value)
+				return (tbl->divider);
+			tbl++;
+		}
+		return (0);
+	}
+	return (1 << reg);
+}
+
+static uint32_t
+get_masked(uint32_t val, uint32_t shift, uint32_t width)
+{
+
+	return ((val >> shift) & ((1 << width) - 1));
+}
+
+static uint32_t
+set_masked(uint32_t val, uint32_t v, uint32_t shift, uint32_t width)
+{
+
+	val &= ~(((1 << width) - 1) << shift);
+	val |= (v & ((1 << width) - 1)) << shift;
+	return (val);
+}
+
+static void
+get_divisors(struct pll_sc *sc, uint32_t *m, uint32_t *n, uint32_t *p)
+{
+	uint32_t val;
+	struct mnp_bits *mnp_bits;
+
+	mnp_bits = &sc->mnp_bits;
+	RD4(sc, sc->base_reg, &val);
+	*m = get_masked(val, PLL_BASE_DIVM_SHIFT, mnp_bits->m_width);
+	*n = get_masked(val, PLL_BASE_DIVN_SHIFT, mnp_bits->n_width);
+	*p = get_masked(val, mnp_bits->p_shift, mnp_bits->p_width);
+}
+
+static uint32_t
+set_divisors(struct pll_sc *sc, uint32_t val, uint32_t m, uint32_t n,
+    uint32_t p)
+{
+	struct mnp_bits *mnp_bits;
+
+	mnp_bits = &sc->mnp_bits;
+	val = set_masked(val, m, PLL_BASE_DIVM_SHIFT, mnp_bits->m_width);
+	val = set_masked(val, n, PLL_BASE_DIVN_SHIFT, mnp_bits->n_width);
+	val = set_masked(val, p, mnp_bits->p_shift, mnp_bits->p_width);
+	return (val);
+}
+
+static bool
+is_locked(struct pll_sc *sc)
+{
+	uint32_t reg;
+
+	switch (sc->type) {
+	case PLL_REFE:
+		RD4(sc, sc->misc_reg, &reg);
+		reg &=  PLLRE_MISC_LOCK;
+		break;
+
+	case PLL_E:
+		RD4(sc, sc->misc_reg, &reg);
+		reg &= PLLE_MISC_LOCK;
+		break;
+
+	default:
+		RD4(sc, sc->base_reg, &reg);
+		reg &= PLL_BASE_LOCK;
+		break;
+	}
+	return (reg != 0);
+}
+
+static int
+wait_for_lock(struct pll_sc *sc)
+{
+	int i;
+
+	for (i = PLL_LOCK_TIMEOUT / 10; i > 0; i--) {
+		if (is_locked(sc))
+			break;
+		DELAY(10);
+	}
+	if (i <= 0) {
+		printf("PLL lock timeout\n");
+		return (ETIMEDOUT);
+	}
+	return (0);
+}
+
+static int
+plle_enable(struct pll_sc *sc)
+{
+	uint32_t reg;
+	int rv;
+	struct mnp_bits *mnp_bits;
+	uint32_t pll_m = 1;
+	uint32_t pll_n = 200;
+	uint32_t pll_p = 13;
+	uint32_t pll_cml = 13;
+
+	mnp_bits = &sc->mnp_bits;
+
+
+	/* Disable lock override. */
+	RD4(sc, sc->base_reg, &reg);
+	reg &= ~PLLE_BASE_LOCK_OVERRIDE;
+	WR4(sc, sc->base_reg, reg);
+
+	RD4(sc, PLLE_AUX, &reg);
+	reg |= PLLE_AUX_ENABLE_SWCTL;
+	reg &= ~PLLE_AUX_SEQ_ENABLE;
+	WR4(sc, PLLE_AUX, reg);
+	DELAY(10);
+
+	RD4(sc, sc->misc_reg, &reg);
+	reg |= PLLE_MISC_LOCK_ENABLE;
+	reg |= PLLE_MISC_IDDQ_SWCTL;
+	reg &= ~PLLE_MISC_IDDQ_OVERRIDE_VALUE;
+	reg |= PLLE_MISC_PTS;
+	reg |= PLLE_MISC_VREG_BG_CTRL_MASK;
+	reg |= PLLE_MISC_VREG_CTRL_MASK;
+	WR4(sc, sc->misc_reg, reg);
+	DELAY(10);
+
+	RD4(sc, PLLE_SS_CNTL, &reg);
+	reg |= PLLE_SS_CNTL_DISABLE;
+	WR4(sc, PLLE_SS_CNTL, reg);
+
+	RD4(sc, sc->base_reg, &reg);
+	reg = set_divisors(sc, reg, pll_m, pll_n, pll_p);
+	reg &= ~(PLLE_BASE_DIVCML_MASK << PLLE_BASE_DIVCML_SHIFT);
+	reg |= pll_cml << PLLE_BASE_DIVCML_SHIFT;
+	WR4(sc, sc->base_reg, reg);
+	DELAY(10);
+
+	pll_enable(sc);
+	rv = wait_for_lock(sc);
+	if (rv != 0)
+		return (rv);
+
+	RD4(sc, PLLE_SS_CNTL, &reg);
+	reg &= ~PLLE_SS_CNTL_SSCCENTER;
+	reg &= ~PLLE_SS_CNTL_SSCINVERT;
+	reg &= ~PLLE_SS_CNTL_COEFFICIENTS_MASK;
+	reg |= PLLE_SS_CNTL_COEFFICIENTS_VAL;
+	WR4(sc, PLLE_SS_CNTL, reg);
+	reg &= ~PLLE_SS_CNTL_SSCBYP;
+	reg &= ~PLLE_SS_CNTL_BYPASS_SS;
+	WR4(sc, PLLE_SS_CNTL, reg);
+	DELAY(10);
+
+	reg &= ~PLLE_SS_CNTL_INTERP_RESET;
+	WR4(sc, PLLE_SS_CNTL, reg);
+	DELAY(10);
+
+	/* HW control of brick pll. */
+	RD4(sc, sc->misc_reg, &reg);
+	reg &= ~PLLE_MISC_IDDQ_SWCTL;
+	WR4(sc, sc->misc_reg, reg);
+
+	RD4(sc, PLLE_AUX, &reg);
+	reg |= PLLE_AUX_USE_LOCKDET;
+	reg |= PLLE_AUX_SEQ_START_STATE;
+	reg &= ~PLLE_AUX_ENABLE_SWCTL;
+	reg &= ~PLLE_AUX_SS_SWCTL;
+	WR4(sc, PLLE_AUX, reg);
+	reg |= PLLE_AUX_SEQ_START_STATE;
+	DELAY(10);
+	reg |= PLLE_AUX_SEQ_ENABLE;
+	WR4(sc, PLLE_AUX, reg);
+
+	RD4(sc, XUSBIO_PLL_CFG0, &reg);
+	reg |= XUSBIO_PLL_CFG0_PADPLL_USE_LOCKDET;
+	reg |= XUSBIO_PLL_CFG0_SEQ_START_STATE;
+	reg &= ~XUSBIO_PLL_CFG0_CLK_ENABLE_SWCTL;
+	reg &= ~XUSBIO_PLL_CFG0_PADPLL_RESET_SWCTL;
+	WR4(sc, XUSBIO_PLL_CFG0, reg);
+	DELAY(10);
+
+	reg |= XUSBIO_PLL_CFG0_SEQ_ENABLE;
+	WR4(sc, XUSBIO_PLL_CFG0, reg);
+
+
+	/* Enable HW control and unreset SATA PLL. */
+	RD4(sc, SATA_PLL_CFG0, &reg);
+	reg &= ~SATA_PLL_CFG0_PADPLL_RESET_SWCTL;
+	reg &= ~SATA_PLL_CFG0_PADPLL_RESET_OVERRIDE_VALUE;
+	reg |=  SATA_PLL_CFG0_PADPLL_USE_LOCKDET;
+	reg &= ~SATA_PLL_CFG0_SEQ_IN_SWCTL;
+	reg &= ~SATA_PLL_CFG0_SEQ_RESET_INPUT_VALUE;
+	reg &= ~SATA_PLL_CFG0_SEQ_LANE_PD_INPUT_VALUE;
+	reg &= ~SATA_PLL_CFG0_SEQ_PADPLL_PD_INPUT_VALUE;
+	reg &= ~SATA_PLL_CFG0_SEQ_ENABLE;
+	reg |=  SATA_PLL_CFG0_SEQ_START_STATE;
+	WR4(sc, SATA_PLL_CFG0, reg);
+	DELAY(10);
+	reg |= SATA_PLL_CFG0_SEQ_ENABLE;
+	WR4(sc, SATA_PLL_CFG0, reg);
+
+	/* Enable HW control of PCIe PLL. */
+	RD4(sc, PCIE_PLL_CFG0, &reg);
+	reg |= PCIE_PLL_CFG0_SEQ_ENABLE;
+	WR4(sc, PCIE_PLL_CFG0, reg);
+
+	return (0);
+}
+
+static int
+tegra124_pll_set_gate(struct clknode *clknode, bool enable)
+{
+	int rv;
+	struct pll_sc *sc;
+
+	sc = clknode_get_softc(clknode);
+	if (enable == 0) {
+		rv = pll_disable(sc);
+		return(rv);
+	}
+
+	if (sc->type == PLL_E)
+		rv = plle_enable(sc);
+	else
+		rv = pll_enable(sc);
+	return (rv);
+}
+
+static int
+pll_set_std(struct pll_sc *sc, uint64_t fin, uint64_t *fout, int flags,
+    uint32_t m, uint32_t n, uint32_t p)
+{
+	uint32_t reg;
+	struct mnp_bits *mnp_bits;
+	int rv;
+
+	mnp_bits = &sc->mnp_bits;
+	if (m >= (1 << mnp_bits->m_width))
+		return (ERANGE);
+	if (n >= (1 << mnp_bits->n_width))
+		return (ERANGE);
+	if (pdiv_to_reg(sc, p) >= (1 << mnp_bits->p_width))
+		return (ERANGE);
+
+	if (flags & CLK_SET_DRYRUN) {
+		if (((flags & (CLK_SET_ROUND_UP | CLK_SET_ROUND_DOWN)) == 0) &&
+		    (*fout != (((fin / m) * n) /p)))
+			return (ERANGE);
+
+		*fout = ((fin / m) * n) /p;
+		return (0);
+	}
+
+	pll_disable(sc);
+
+	/* take pll out of IDDQ */
+	if (sc->iddq_reg != 0)
+		MD4(sc, sc->iddq_reg, sc->iddq_mask, 0);
+
+	RD4(sc, sc->base_reg, &reg);
+	reg = set_masked(reg, m, PLL_BASE_DIVM_SHIFT, mnp_bits->m_width);
+	reg = set_masked(reg, n, PLL_BASE_DIVN_SHIFT, mnp_bits->n_width);
+	reg = set_masked(reg, pdiv_to_reg(sc, p), mnp_bits->p_shift,
+	    mnp_bits->p_width);
+	WR4(sc, sc->base_reg, reg);
+
+	/* Enable PLL. */
+	RD4(sc, sc->base_reg, &reg);
+	reg |= PLL_BASE_ENABLE;
+	WR4(sc, sc->base_reg, reg);
+
+	/* Enable lock detection. */
+	RD4(sc, sc->misc_reg, &reg);
+	reg |= sc->lock_enable;
+	WR4(sc, sc->misc_reg, reg);
+
+	rv = wait_for_lock(sc);
+	if (rv != 0) {
+		/* Disable PLL */
+		RD4(sc, sc->base_reg, &reg);
+		reg &= ~PLL_BASE_ENABLE;
+		WR4(sc, sc->base_reg, reg);
+		return (rv);
+	}
+	RD4(sc, sc->misc_reg, &reg);
+
+	pll_enable(sc);
+	*fout = ((fin / m) * n) / p;
+	return 0;
+}
+
+static int
+plla_set_freq(struct pll_sc *sc, uint64_t fin, uint64_t *fout, int flags)
+{
+	uint32_t m, n, p;
+
+	p = 1;
+	m = 5;
+	n = (*fout * p * m + fin / 2)/ fin;
+	dprintf("%s: m: %d, n: %d, p: %d\n", __func__, m, n, p);
+	return (pll_set_std(sc,  fin, fout, flags, m, n, p));
+}
+
+static int
+pllc_set_freq(struct pll_sc *sc, uint64_t fin, uint64_t *fout, int flags)
+{
+	uint32_t m, n, p;
+
+
+	p = 2;
+	m = 1;
+	n = (*fout * p * m + fin / 2)/ fin;
+	dprintf("%s: m: %d, n: %d, p: %d\n", __func__, m, n, p);
+	return (pll_set_std( sc, fin, fout, flags, m, n, p));
+}
+
+static int
+plld2_set_freq(struct pll_sc *sc, uint64_t fin, uint64_t *fout, int flags)
+{
+	uint32_t m, n, p;
+
+	p = 2;
+	m = 1;
+	n = (*fout * p * m + fin / 2)/ fin;
+	dprintf("%s: m: %d, n: %d, p: %d\n", __func__, m, n, p);
+	return (pll_set_std(sc, fin, fout, flags, m, n, p));
+}
+
+
+
+static int
+pllrefe_set_freq(struct pll_sc *sc, uint64_t fin, uint64_t *fout, int flags)
+{
+	uint32_t m, n, p;
+
+	m = 1;
+	p = 1;
+	n = *fout * p * m / fin;
+	return (pll_set_std(sc, fin, fout, flags, m, n, p));
+}
+
+static int
+pllx_set_freq(struct pll_sc *sc, uint64_t fin, uint64_t *fout, int flags)
+{
+	uint32_t reg;
+	uint32_t m, n, p;
+	struct mnp_bits *mnp_bits;
+	int rv;
+
+	mnp_bits = &sc->mnp_bits;
+
+	p = 1;
+	m = 1;
+	n = (*fout * p * m + fin / 2)/ fin;
+	dprintf("%s: m: %d, n: %d, p: %d\n", __func__, m, n, p);
+
+	if (m >= (1 << mnp_bits->m_width))
+		return (ERANGE);
+	if (n >= (1 << mnp_bits->n_width))
+		return (ERANGE);
+	if (pdiv_to_reg(sc, p) >= (1 << mnp_bits->p_width))
+		return (ERANGE);
+
+	if (flags & CLK_SET_DRYRUN) {
+		if (((flags & (CLK_SET_ROUND_UP | CLK_SET_ROUND_DOWN)) == 0) &&
+		    (*fout != (((fin / m) * n) /p)))
+			return (ERANGE);
+		*fout = ((fin / m) * n) /p;
+		return (0);
+	}
+
+	/* Set bypass. */
+	RD4(sc, sc->base_reg, &reg);
+	reg |= PLL_BASE_BYPASS;
+	WR4(sc, sc->base_reg, reg);
+	RD4(sc, sc->base_reg, &reg);
+	DELAY(100);
+
+	/* Set PLL. */
+	RD4(sc, sc->base_reg, &reg);
+	reg = set_masked(reg, m, PLL_BASE_DIVM_SHIFT, mnp_bits->m_width);
+	reg = set_masked(reg, n, PLL_BASE_DIVN_SHIFT, mnp_bits->n_width);
+	reg = set_masked(reg, pdiv_to_reg(sc, p), mnp_bits->p_shift,
+	    mnp_bits->p_width);
+	WR4(sc, sc->base_reg, reg);
+	RD4(sc, sc->base_reg, &reg);
+	DELAY(100);
+
+	/* Enable PLL. */
+	RD4(sc, sc->base_reg, &reg);
+	reg |= PLL_BASE_ENABLE;
+	WR4(sc, sc->base_reg, reg);
+
+	/* Enable lock detection */
+	RD4(sc, sc->misc_reg, &reg);
+	reg |= sc->lock_enable;
+	WR4(sc, sc->misc_reg, reg);
+
+	rv = wait_for_lock(sc);
+	if (rv != 0) {
+		/* Disable PLL */
+		RD4(sc, sc->base_reg, &reg);
+		reg &= ~PLL_BASE_ENABLE;
+		WR4(sc, sc->base_reg, reg);
+		return (rv);
+	}
+	RD4(sc, sc->misc_reg, &reg);
+
+	/* Clear bypass. */
+	RD4(sc, sc->base_reg, &reg);
+	reg &= ~PLL_BASE_BYPASS;
+	WR4(sc, sc->base_reg, reg);
+	*fout = ((fin / m) * n) / p;
+	return (0);
+}
+
+static int
+tegra124_pll_set_freq(struct clknode *clknode, uint64_t fin, uint64_t *fout,
+    int flags, int *stop)
+{
+	*stop = 1;
+	int rv;
+	struct pll_sc *sc;
+
+	sc = clknode_get_softc(clknode);
+	dprintf("%s: Requested freq: %llu, input freq: %llu\n", __func__,
+	    *fout, fin);
+	switch (sc->type) {
+	case PLL_A:
+		rv = plla_set_freq(sc, fin, fout, flags);
+		break;
+	case PLL_C:
+		rv = pllc_set_freq(sc, fin, fout, flags);
+		break;
+	case PLL_D2:
+		rv = plld2_set_freq(sc, fin, fout, flags);
+		break;
+
+	case PLL_REFE:
+		rv = pllrefe_set_freq(sc, fin, fout, flags);
+		break;
+
+	case PLL_X:
+		rv = pllx_set_freq(sc, fin, fout, flags);
+		break;
+
+	case PLL_U:
+		if (*fout == 480000000)  /* PLLU is fixed to 480 MHz */
+			rv = 0;
+		else
+			rv = ERANGE;
+		break;
+	default:
+		rv = ENXIO;
+		break;
+	}
+	return (rv);
+}
+
+
+static int
+tegra124_pll_init(struct clknode *clk, device_t dev)
+{
+	struct pll_sc *sc;
+	uint32_t reg;
+
+	sc = clknode_get_softc(clk);
+
+	/* If PLL is enabled, enable lock detect too. */
+	RD4(sc, sc->base_reg, &reg);
+	if (reg & PLL_BASE_ENABLE) {
+		RD4(sc, sc->misc_reg, &reg);
+		reg |= sc->lock_enable;
+		WR4(sc, sc->misc_reg, reg);
+	}
+
+	clknode_init_parent_idx(clk, 0);
+	return(0);
+}
+
+static int
+tegra124_pll_recalc(struct clknode *clk, uint64_t *freq)
+{
+	struct pll_sc *sc;
+	uint32_t m, n, p, pr;
+	uint32_t reg, misc_reg;
+	int locked;
+
+	sc = clknode_get_softc(clk);
+
+	RD4(sc, sc->base_reg, &reg);
+	RD4(sc, sc->misc_reg, &misc_reg);
+
+	get_divisors(sc, &m, &n, &pr);
+	if (sc->type != PLL_E)
+		p = reg_to_pdiv(sc, pr);
+	else
+		p = 2 * (pr - 1);
+	locked = is_locked(sc);
+
+	dprintf("%s: %s (0x%08x, 0x%08x) - m: %d, n: %d, p: %d (%d): "
+	    "e: %d, r: %d, o: %d - %s\n", __func__,
+	    clknode_get_name(clk), reg, misc_reg, m, n, p, pr,
+	    (reg >> 30) & 1, (reg >> 29) & 1, (reg >> 28) & 1,
+	    locked ? "locked" : "unlocked");
+
+	if ((m == 0) || (n == 0) || (p == 0)) {
+		*freq = 0;
+		return (EINVAL);
+	}
+	*freq = ((*freq / m) * n) / p;
+	return (0);
+}
+
+static int
+pll_register(struct clkdom *clkdom, struct clk_pll_def *clkdef)
+{
+	struct clknode *clk;
+	struct pll_sc *sc;
+
+	clk = clknode_create(clkdom, &tegra124_pll_class, &clkdef->clkdef);
+	if (clk == NULL)
+		return (ENXIO);
+
+	sc = clknode_get_softc(clk);
+	sc->clkdev = clknode_get_device(clk);
+	sc->type = clkdef->type;
+	sc->base_reg = clkdef->base_reg;
+	sc->misc_reg = clkdef->misc_reg;
+	sc->lock_mask = clkdef->lock_mask;
+	sc->lock_enable = clkdef->lock_enable;
+	sc->iddq_reg = clkdef->iddq_reg;
+	sc->iddq_mask = clkdef->iddq_mask;
+	sc->flags = clkdef->flags;
+	sc->pdiv_table = clkdef->pdiv_table;
+	sc->mnp_bits = clkdef->mnp_bits;
+	clknode_register(clkdom, clk);
+	return (0);
+}
+
+static void config_utmi_pll(struct tegra124_car_softc *sc)
+{
+	uint32_t reg;
+	/*
+	 * XXX Simplified UTMIP settings for 12MHz base clock.
+	 */
+#define	ENABLE_DELAY_COUNT 	0x02
+#define	STABLE_COUNT		0x2F
+#define	ACTIVE_DELAY_COUNT	0x04
+#define	XTAL_FREQ_COUNT		0x76
+
+	CLKDEV_READ_4(sc->dev, UTMIP_PLL_CFG2, &reg);
+	reg &= ~UTMIP_PLL_CFG2_STABLE_COUNT(~0);
+	reg |= UTMIP_PLL_CFG2_STABLE_COUNT(STABLE_COUNT);
+	reg &= ~UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(~0);
+	reg |= UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(ACTIVE_DELAY_COUNT);
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_A_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_B_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_C_POWERDOWN;
+	CLKDEV_WRITE_4(sc->dev, UTMIP_PLL_CFG2, reg);
+
+	CLKDEV_READ_4(sc->dev, UTMIP_PLL_CFG1, &reg);
+	reg &= ~UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(~0);
+	reg |= UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(ENABLE_DELAY_COUNT);
+	reg &= ~UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(~0);
+	reg |= UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(XTAL_FREQ_COUNT);
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ACTIVE_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLLU_POWERUP;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLLU_POWERDOWN;
+	CLKDEV_WRITE_4(sc->dev, UTMIP_PLL_CFG1, reg);
+
+	/* Prepare UTMIP requencer. */
+	CLKDEV_READ_4(sc->dev, UTMIPLL_HW_PWRDN_CFG0, &reg);
+	reg |= UTMIPLL_HW_PWRDN_CFG0_USE_LOCKDET;
+	reg &= ~UTMIPLL_HW_PWRDN_CFG0_CLK_ENABLE_SWCTL;
+	reg |= UTMIPLL_HW_PWRDN_CFG0_SEQ_START_STATE;
+	CLKDEV_WRITE_4(sc->dev, UTMIPLL_HW_PWRDN_CFG0, reg);
+
+	/* Powerup UTMIP. */
+	CLKDEV_READ_4(sc->dev, UTMIP_PLL_CFG1, &reg);
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERUP;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN;
+	CLKDEV_WRITE_4(sc->dev, UTMIP_PLL_CFG1, reg);
+	DELAY(10);
+
+	/* SW override for UTMIPLL */
+	CLKDEV_READ_4(sc->dev, UTMIPLL_HW_PWRDN_CFG0, &reg);
+	reg |= UTMIPLL_HW_PWRDN_CFG0_IDDQ_SWCTL;
+	reg &= ~UTMIPLL_HW_PWRDN_CFG0_IDDQ_OVERRIDE;
+	CLKDEV_WRITE_4(sc->dev, UTMIPLL_HW_PWRDN_CFG0, reg);
+	DELAY(10);
+
+	/* HW control of UTMIPLL. */
+	CLKDEV_READ_4(sc->dev, UTMIPLL_HW_PWRDN_CFG0, &reg);
+	reg |= UTMIPLL_HW_PWRDN_CFG0_SEQ_ENABLE;
+	CLKDEV_WRITE_4(sc->dev, UTMIPLL_HW_PWRDN_CFG0, reg);
+}
+
+void
+tegra124_init_plls(struct tegra124_car_softc *sc)
+{
+	int i, rv;
+
+	for (i = 0; i < nitems(pll_clks); i++) {
+		rv = pll_register(sc->clkdom, pll_clks + i);
+		if (rv != 0)
+			panic("pll_register failed");
+	}
+	config_utmi_pll(sc);
+
+}
diff --git a/sys/arm/nvidia/tegra124/tegra124_clk_super.c b/sys/arm/nvidia/tegra124/tegra124_clk_super.c
new file mode 100644
index 0000000..7f32eba
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_clk_super.c
@@ -0,0 +1,265 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/lock.h>
+#include <sys/mutex.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+
+#include <dev/extres/clk/clk.h>
+
+#include <gnu/dts/include/dt-bindings/clock/tegra124-car.h>
+#include "tegra124_car.h"
+
+
+/* Flags */
+#define	SMF_HAVE_DIVIDER_2	1
+
+struct super_mux_def {
+	struct clknode_init_def	clkdef;
+	uint32_t		base_reg;
+	uint32_t		flags;
+	int			src_pllx;
+	int			src_div2;
+};
+
+#define	PLIST(x) static const char *x[]
+#define	SM(_id, cn, pl, r, x, d, f)				\
+{									\
+	.clkdef.id = _id,						\
+	.clkdef.name = cn,						\
+	.clkdef.parent_names = pl,					\
+	.clkdef.parent_cnt = nitems(pl),				\
+	.clkdef.flags = CLK_NODE_STATIC_STRINGS,				\
+	.base_reg = r,							\
+	.src_pllx = x,							\
+	.src_div2 = d,							\
+	.flags = f,							\
+}
+
+PLIST(cclk_g_parents) = {
+	"clk_m", "pllC_out0", "clk_s", "pllM_out0",
+	"pllP_out0", "pllP_out4", "pllC2_out0", "pllC3_out0",
+	"pllX_out", NULL, NULL, NULL,
+	NULL, NULL, NULL,NULL, // "dfllCPU_out0"
+};
+
+PLIST(cclk_lp_parents) = {
+	"clk_m", "pllC_out0", "clk_s", "pllM_out0",
+	"pllP_out0", "pllP_out4", "pllC2_out0", "pllC3_out0",
+	"pllX_out", NULL, NULL, NULL,
+	NULL, NULL, NULL, NULL,
+	"pllX_out0"
+};
+
+PLIST(sclk_parents) = {
+	"clk_m", "pllC_out1", "pllP_out4", "pllP_out0",
+	"pllP_out2", "pllC_out0", "clk_s", "pllM_out1",
+};
+
+static struct super_mux_def super_mux_def[] = {
+ SM(TEGRA124_CLK_CCLK_G, "cclk_g", cclk_g_parents, CCLKG_BURST_POLICY, 0, 0, 0),
+ SM(TEGRA124_CLK_CCLK_LP, "cclk_lp", cclk_lp_parents, CCLKLP_BURST_POLICY, 8, 16, SMF_HAVE_DIVIDER_2),
+ SM(TEGRA124_CLK_SCLK, "sclk", sclk_parents, SCLK_BURST_POLICY, 0, 0, 0),
+};
+
+static int super_mux_init(struct clknode *clk, device_t dev);
+static int super_mux_set_mux(struct clknode *clk, int idx);
+
+struct super_mux_sc {
+	device_t		clkdev;
+	uint32_t		base_reg;
+	int			src_pllx;
+	int			src_div2;
+	uint32_t		flags;
+
+	int 			mux;
+};
+
+static clknode_method_t super_mux_methods[] = {
+	/* Device interface */
+	CLKNODEMETHOD(clknode_init,		super_mux_init),
+	CLKNODEMETHOD(clknode_set_mux, 		super_mux_set_mux),
+	CLKNODEMETHOD_END
+};
+DEFINE_CLASS_1(tegra124_super_mux, tegra124_super_mux_class, super_mux_methods,
+   sizeof(struct super_mux_sc), clknode_class);
+
+/* Mux status. */
+#define	SUPER_MUX_STATE_STDBY		0
+#define	SUPER_MUX_STATE_IDLE		1
+#define	SUPER_MUX_STATE_RUN		2
+#define	SUPER_MUX_STATE_IRQ		3
+#define	SUPER_MUX_STATE_FIQ		4
+
+/* Mux register bits. */
+#define	SUPER_MUX_STATE_BIT_SHIFT	28
+#define	SUPER_MUX_STATE_BIT_MASK	0xF
+/* State is Priority encoded */
+#define	SUPER_MUX_STATE_BIT_STDBY	0x00
+#define	SUPER_MUX_STATE_BIT_IDLE	0x01
+#define	SUPER_MUX_STATE_BIT_RUN		0x02
+#define	SUPER_MUX_STATE_BIT_IRQ		0x04
+#define	SUPER_MUX_STATE_BIT_FIQ		0x08
+
+#define	SUPER_MUX_MUX_WIDTH		4
+#define	SUPER_MUX_LP_DIV2_BYPASS	(1 << 16)
+
+static uint32_t
+super_mux_get_state(uint32_t reg)
+{
+	reg = (reg >> SUPER_MUX_STATE_BIT_SHIFT) & SUPER_MUX_STATE_BIT_MASK;
+	if (reg & SUPER_MUX_STATE_BIT_FIQ)
+		 return (SUPER_MUX_STATE_FIQ);
+	if (reg & SUPER_MUX_STATE_BIT_IRQ)
+		 return (SUPER_MUX_STATE_IRQ);
+	if (reg & SUPER_MUX_STATE_BIT_RUN)
+		 return (SUPER_MUX_STATE_RUN);
+	if (reg & SUPER_MUX_STATE_BIT_IDLE)
+		 return (SUPER_MUX_STATE_IDLE);
+	return (SUPER_MUX_STATE_STDBY);
+}
+
+static int
+super_mux_init(struct clknode *clk, device_t dev)
+{
+	struct super_mux_sc *sc;
+	uint32_t reg;
+	int shift, state;
+
+	sc = clknode_get_softc(clk);
+
+	DEVICE_LOCK(sc);
+	RD4(sc, sc->base_reg, &reg);
+	DEVICE_UNLOCK(sc);
+	state = super_mux_get_state(reg);
+
+	if ((state != SUPER_MUX_STATE_RUN) &&
+	    (state != SUPER_MUX_STATE_IDLE)) {
+		panic("Unexpected super mux state: %u", state);
+	}
+
+	shift = state * SUPER_MUX_MUX_WIDTH;
+
+	sc->mux = (reg >> shift) & ((1 << SUPER_MUX_MUX_WIDTH) - 1);
+
+	/*
+	 * CCLKLP uses PLLX/2 as source if LP_DIV2_BYPASS isn't set
+	 * and source mux is set to PLLX.
+	 */
+	if (sc->flags & SMF_HAVE_DIVIDER_2) {
+		if (((reg & SUPER_MUX_LP_DIV2_BYPASS) == 0) &&
+		    (sc->mux == sc->src_pllx))
+		sc->mux = sc->src_div2;
+	}
+	clknode_init_parent_idx(clk, sc->mux);
+
+	return(0);
+}
+
+static int
+super_mux_set_mux(struct clknode *clk, int idx)
+{
+
+	struct super_mux_sc *sc;
+	int shift, state;
+	uint32_t reg, dummy;
+
+	sc = clknode_get_softc(clk);
+
+	DEVICE_LOCK(sc);
+	RD4(sc, sc->base_reg, &reg);
+	state = super_mux_get_state(reg);
+
+	if ((state != SUPER_MUX_STATE_RUN) &&
+	    (state != SUPER_MUX_STATE_IDLE)) {
+		panic("Unexpected super mux state: %u", state);
+	}
+
+	shift = state * SUPER_MUX_MUX_WIDTH;
+	sc->mux = idx;
+	if (sc->flags & SMF_HAVE_DIVIDER_2) {
+		if (idx == sc->src_div2) {
+			idx = sc->src_pllx;
+			reg &= ~SUPER_MUX_LP_DIV2_BYPASS;
+			WR4(sc, sc->base_reg, reg);
+			RD4(sc, sc->base_reg, &dummy);
+		} else if (idx == sc->src_pllx) {
+			reg = SUPER_MUX_LP_DIV2_BYPASS;
+			WR4(sc, sc->base_reg, reg);
+			RD4(sc, sc->base_reg, &dummy);
+		}
+	}
+	reg &= ~(((1 << SUPER_MUX_MUX_WIDTH) - 1) << shift);
+	reg |= idx << shift;
+	WR4(sc, sc->base_reg, reg);
+	RD4(sc, sc->base_reg, &dummy);
+	DEVICE_UNLOCK(sc);
+
+	return(0);
+}
+
+static int
+super_mux_register(struct clkdom *clkdom, struct super_mux_def *clkdef)
+{
+	struct clknode *clk;
+	struct super_mux_sc *sc;
+
+	clk = clknode_create(clkdom, &tegra124_super_mux_class,
+	    &clkdef->clkdef);
+	if (clk == NULL)
+		return (1);
+
+	sc = clknode_get_softc(clk);
+	sc->clkdev = clknode_get_device(clk);
+	sc->base_reg = clkdef->base_reg;
+	sc->src_pllx = clkdef->src_pllx;
+	sc->src_div2 = clkdef->src_div2;
+	sc->flags = clkdef->flags;
+
+	clknode_register(clkdom, clk);
+	return (0);
+}
+
+void
+tegra124_super_mux_clock(struct tegra124_car_softc *sc)
+{
+	int i, rv;
+
+	for (i = 0; i <  nitems(super_mux_def); i++) {
+		rv = super_mux_register(sc->clkdom, &super_mux_def[i]);
+		if (rv != 0)
+			panic("super_mux_register failed");
+	}
+
+}
diff --git a/sys/arm/nvidia/tegra124/tegra124_coretemp.c b/sys/arm/nvidia/tegra124/tegra124_coretemp.c
new file mode 100644
index 0000000..c29fe8e
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_coretemp.c
@@ -0,0 +1,273 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/cpu.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/module.h>
+#include <sys/sysctl.h>
+
+#include <machine/bus.h>
+#include <machine/cpu.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include "tegra_soctherm_if.h"
+
+
+enum therm_info {
+	CORETEMP_TEMP,
+	CORETEMP_DELTA,
+	CORETEMP_RESOLUTION,
+	CORETEMP_TJMAX,
+};
+
+struct tegra124_coretemp_softc {
+	device_t		dev;
+	int			overheat_log;
+	int			core_max_temp;
+	int			cpu_id;
+	device_t		tsens_dev;
+	intptr_t		tsens_id;
+};
+
+static int
+coretemp_get_val_sysctl(SYSCTL_HANDLER_ARGS)
+{
+	device_t dev;
+	int val, temp, rv;
+	struct tegra124_coretemp_softc *sc;
+	enum therm_info type;
+	char stemp[16];
+
+
+	dev = (device_t) arg1;
+	sc = device_get_softc(dev);
+	type = arg2;
+
+
+	rv = TEGRA_SOCTHERM_GET_TEMPERATURE(sc->tsens_dev, sc->dev,
+	     sc->tsens_id, &temp);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot read temperature sensor %d:  %d\n",
+		    sc->tsens_id, rv);
+		return (rv);
+	}
+
+	switch (type) {
+	case CORETEMP_TEMP:
+		val = temp / 100;
+		val +=  2731;
+		break;
+	case CORETEMP_DELTA:
+		val = (sc->core_max_temp - temp) / 1000;
+		break;
+	case CORETEMP_RESOLUTION:
+		val = 1;
+		break;
+	case CORETEMP_TJMAX:
+		val = sc->core_max_temp / 100;
+		val +=  2731;
+		break;
+	}
+
+
+	if ((temp > sc->core_max_temp)  && !sc->overheat_log) {
+		sc->overheat_log = 1;
+
+		/*
+		 * Check for Critical Temperature Status and Critical
+		 * Temperature Log.  It doesn't really matter if the
+		 * current temperature is invalid because the "Critical
+		 * Temperature Log" bit will tell us if the Critical
+		 * Temperature has * been reached in past. It's not
+		 * directly related to the current temperature.
+		 *
+		 * If we reach a critical level, allow devctl(4)
+		 * to catch this and shutdown the system.
+		 */
+		device_printf(dev, "critical temperature detected, "
+		    "suggest system shutdown\n");
+		snprintf(stemp, sizeof(stemp), "%d", val);
+		devctl_notify("coretemp", "Thermal", stemp,
+		    "notify=0xcc");
+	} else {
+		sc->overheat_log = 0;
+	}
+
+	return (sysctl_handle_int(oidp, 0, val, req));
+}
+
+static int
+tegra124_coretemp_ofw_parse(struct tegra124_coretemp_softc *sc)
+{
+	int rv, ncells;
+	phandle_t node, xnode;
+	pcell_t *cells;
+
+	node = OF_peer(0);
+	node = ofw_bus_find_child(node, "thermal-zones");
+	if (node <= 0) {
+		device_printf(sc->dev, "Cannot find 'thermal-zones'.\n");
+		return (ENXIO);
+	}
+
+	node = ofw_bus_find_child(node, "cpu");
+	if (node <= 0) {
+		device_printf(sc->dev, "Cannot find 'cpu'\n");
+		return (ENXIO);
+	}
+	rv = ofw_bus_parse_xref_list_alloc(node, "thermal-sensors",
+	    "#thermal-sensor-cells", 0, &xnode, &ncells, &cells);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot parse 'thermal-sensors' property.\n");
+		return (ENXIO);
+	}
+	if (ncells != 1) {
+		device_printf(sc->dev,
+		    "Invalid format of 'thermal-sensors' property(%d).\n",
+		    ncells);
+		return (ENXIO);
+	}
+
+	sc->tsens_id = 0x100 + sc->cpu_id; //cells[0];
+	free(cells, M_OFWPROP);
+
+	sc->tsens_dev = OF_device_from_xref(xnode);
+	if (sc->tsens_dev == NULL) {
+		device_printf(sc->dev,
+		    "Cannot find thermal sensors device.");
+		return (ENXIO);
+	}
+	return (0);
+}
+
+static void
+tegra124_coretemp_identify(driver_t *driver, device_t parent)
+{
+
+	if (device_find_child(parent, "tegra124_coretemp", -1) != NULL)
+		return;
+	if (BUS_ADD_CHILD(parent, 0, "tegra124_coretemp", -1) == NULL)
+		device_printf(parent, "add child failed\n");
+}
+
+static int
+tegra124_coretemp_probe(device_t dev)
+{
+
+	device_set_desc(dev, "CPU Frequency Control");
+	return (0);
+}
+
+static int
+tegra124_coretemp_attach(device_t dev)
+{
+	struct tegra124_coretemp_softc *sc;
+	device_t pdev;
+	struct sysctl_oid *oid;
+	struct sysctl_ctx_list *ctx;
+	int rv;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	sc->cpu_id = device_get_unit(dev);
+	sc->core_max_temp = 102000;
+	pdev = device_get_parent(dev);
+
+	rv = tegra124_coretemp_ofw_parse(sc);
+	if (rv != 0)
+		return (rv);
+
+	ctx = device_get_sysctl_ctx(dev);
+
+	oid = SYSCTL_ADD_NODE(ctx,
+	    SYSCTL_CHILDREN(device_get_sysctl_tree(pdev)), OID_AUTO,
+	    "coretemp", CTLFLAG_RD, NULL, "Per-CPU thermal information");
+
+	/*
+	 * Add the MIBs to dev.cpu.N and dev.cpu.N.coretemp.
+	 */
+	SYSCTL_ADD_PROC(ctx, SYSCTL_CHILDREN(device_get_sysctl_tree(pdev)),
+	    OID_AUTO, "temperature", CTLTYPE_INT | CTLFLAG_RD | CTLFLAG_MPSAFE,
+	    dev, CORETEMP_TEMP, coretemp_get_val_sysctl, "IK",
+	    "Current temperature");
+	SYSCTL_ADD_PROC(ctx, SYSCTL_CHILDREN(oid), OID_AUTO, "delta",
+	    CTLTYPE_INT | CTLFLAG_RD | CTLFLAG_MPSAFE, dev, CORETEMP_DELTA,
+	    coretemp_get_val_sysctl, "I",
+	    "Delta between TCC activation and current temperature");
+	SYSCTL_ADD_PROC(ctx, SYSCTL_CHILDREN(oid), OID_AUTO, "resolution",
+	    CTLTYPE_INT | CTLFLAG_RD | CTLFLAG_MPSAFE, dev, CORETEMP_RESOLUTION,
+	    coretemp_get_val_sysctl, "I",
+	    "Resolution of CPU thermal sensor");
+	SYSCTL_ADD_PROC(ctx, SYSCTL_CHILDREN(oid), OID_AUTO, "tjmax",
+	    CTLTYPE_INT | CTLFLAG_RD | CTLFLAG_MPSAFE, dev, CORETEMP_TJMAX,
+	    coretemp_get_val_sysctl, "IK",
+	    "TCC activation temperature");
+
+	return (0);
+}
+
+static int
+tegra124_coretemp_detach(device_t dev)
+{
+	struct tegra124_coretemp_softc *sc;
+
+	sc = device_get_softc(dev);
+	return (0);
+}
+
+
+static device_method_t tegra124_coretemp_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_identify,	tegra124_coretemp_identify),
+	DEVMETHOD(device_probe,		tegra124_coretemp_probe),
+	DEVMETHOD(device_attach,	tegra124_coretemp_attach),
+	DEVMETHOD(device_detach,	tegra124_coretemp_detach),
+
+
+	DEVMETHOD_END
+};
+
+static devclass_t tegra124_coretemp_devclass;
+static driver_t tegra124_coretemp_driver = {
+	"tegra124_coretemp",
+	tegra124_coretemp_methods,
+	sizeof(struct tegra124_coretemp_softc),
+};
+
+DRIVER_MODULE(tegra124_coretemp, cpu, tegra124_coretemp_driver,
+    tegra124_coretemp_devclass, 0, 0);
diff --git a/sys/arm/nvidia/tegra124/tegra124_cpufreq.c b/sys/arm/nvidia/tegra124/tegra124_cpufreq.c
new file mode 100644
index 0000000..1227c81
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_cpufreq.c
@@ -0,0 +1,583 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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/cpu.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/module.h>
+
+#include <machine/bus.h>
+#include <machine/cpu.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/regulator/regulator.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <arm/nvidia/tegra_efuse.h>
+
+#include "cpufreq_if.h"
+
+#define	XXX
+
+/* CPU voltage table entry */
+struct speedo_entry {
+	uint64_t		freq; 	/* Frequency point */
+	int			c0; 	/* Coeeficient values for */
+	int			c1;	/* quadratic equation: */
+	int 			c2;	/* c2 * speedo^2 + c1 * speedo + c0 */
+};
+
+struct cpu_volt_def {
+	int			min_uvolt;	/* Min allowed CPU voltage */
+	int			max_uvolt;	/* Max allowed CPU voltage */
+	int 			step_uvolt; 	/* Step of CPU voltage */
+	int			speedo_scale;	/* Scaling factor for cvt */
+	int			speedo_nitems;	/* Size of speedo table */
+	struct speedo_entry	*speedo_tbl;	/* CPU voltage table */
+};
+
+struct cpu_speed_point {
+	uint64_t		freq;		/* Frequecy */
+	int			uvolt;		/* Requested voltage */
+};
+
+static struct speedo_entry tegra124_speedo_dpll_tbl[] =
+{
+	{ 204000000ULL,	1112619, -29295, 402},
+	{ 306000000ULL,	1150460, -30585, 402},
+	{ 408000000ULL,	1190122, -31865, 402},
+	{ 510000000ULL,	1231606, -33155, 402},
+	{ 612000000ULL,	1274912, -34435, 402},
+	{ 714000000ULL,	1320040, -35725, 402},
+	{ 816000000ULL,	1366990, -37005, 402},
+	{ 918000000ULL,	1415762, -38295, 402},
+	{1020000000ULL,	1466355, -39575, 402},
+	{1122000000ULL,	1518771, -40865, 402},
+	{1224000000ULL,	1573009, -42145, 402},
+	{1326000000ULL,	1629068, -43435, 402},
+	{1428000000ULL,	1686950, -44715, 402},
+	{1530000000ULL,	1746653, -46005, 402},
+	{1632000000ULL,	1808179, -47285, 402},
+	{1734000000ULL,	1871526, -48575, 402},
+	{1836000000ULL,	1936696, -49855, 402},
+	{1938000000ULL,	2003687, -51145, 402},
+	{2014500000ULL,	2054787, -52095, 402},
+	{2116500000ULL,	2124957, -53385, 402},
+	{2218500000ULL,	2196950, -54665, 402},
+	{2320500000ULL,	2270765, -55955, 402},
+	{2320500000ULL,	2270765, -55955, 402},
+	{2422500000ULL,	2346401, -57235, 402},
+	{2524500000ULL,	2437299, -58535, 402},
+};
+
+static struct cpu_volt_def tegra124_cpu_volt_dpll_def =
+{
+	.min_uvolt =  900000,		/* 0.9 V */
+	.max_uvolt = 1260000,		/* 1.26 */
+	.step_uvolt =  10000,		/* 10 mV */
+	.speedo_scale = 100,
+	.speedo_nitems = nitems(tegra124_speedo_dpll_tbl),
+	.speedo_tbl = tegra124_speedo_dpll_tbl,
+};
+
+static struct speedo_entry tegra124_speedo_pllx_tbl[] =
+{
+	{ 204000000ULL,	 800000, 0, 0},
+	{ 306000000ULL,	 800000, 0, 0},
+	{ 408000000ULL,	 800000, 0, 0},
+	{ 510000000ULL,	 800000, 0, 0},
+	{ 612000000ULL,	 800000, 0, 0},
+	{ 714000000ULL,	 800000, 0, 0},
+	{ 816000000ULL,	 820000, 0, 0},
+	{ 918000000ULL,	 840000, 0, 0},
+	{1020000000ULL,	 880000, 0, 0},
+	{1122000000ULL,	 900000, 0, 0},
+	{1224000000ULL,	 930000, 0, 0},
+	{1326000000ULL,	 960000, 0, 0},
+	{1428000000ULL,	 990000, 0, 0},
+	{1530000000ULL,	1020000, 0, 0},
+	{1632000000ULL,	1070000, 0, 0},
+	{1734000000ULL,	1100000, 0, 0},
+	{1836000000ULL,	1140000, 0, 0},
+	{1938000000ULL,	1180000, 0, 0},
+	{2014500000ULL,	1220000, 0, 0},
+	{2116500000ULL,	1260000, 0, 0},
+	{2218500000ULL,	1310000, 0, 0},
+	{2320500000ULL,	1360000, 0, 0},
+	{2397000000ULL,	1400000, 0, 0},
+	{2499000000ULL,	1400000, 0, 0},
+};
+
+
+static struct cpu_volt_def tegra124_cpu_volt_pllx_def =
+{
+	.min_uvolt =  900000,		/* 0.9 V */
+	.max_uvolt = 1260000,		/* 1.26 */
+	.step_uvolt =  10000,		/* 10 mV */
+	.speedo_scale = 100,
+	.speedo_nitems = nitems(tegra124_speedo_pllx_tbl),
+	.speedo_tbl = tegra124_speedo_pllx_tbl,
+};
+
+static uint64_t cpu_freq_tbl[] = {
+	 204000000ULL,
+	 306000000ULL,
+	 408000000ULL,
+	 510000000ULL,
+	 612000000ULL,
+	 714000000ULL,
+	 816000000ULL,
+	 918000000ULL,
+	1020000000ULL,
+	1122000000ULL,
+	1224000000ULL,
+	1326000000ULL,
+	1428000000ULL,
+	1530000000ULL,
+	1632000000ULL,
+	1734000000ULL,
+	1836000000ULL,
+	1938000000ULL,
+	2014000000ULL,
+	2116000000ULL,
+	2218000000ULL,
+	2320000000ULL,
+	2320000000ULL,
+	2422000000ULL,
+	2524000000ULL,
+};
+
+static uint64_t cpu_max_freq[] = {
+	2014500000ULL,
+	2320500000ULL,
+	2116500000ULL,
+	2524500000ULL,
+};
+
+struct tegra124_cpufreq_softc {
+	device_t		dev;
+	phandle_t		node;
+
+	regulator_t		supply_vdd_cpu;
+	clk_t			clk_cpu_g;
+	clk_t			clk_cpu_lp;
+	clk_t			clk_pll_x;
+	clk_t			clk_pll_p;
+	clk_t			clk_dfll;
+
+	int 			process_id;
+	int 			speedo_id;
+	int 			speedo_value;
+
+	uint64_t		cpu_max_freq;
+	struct cpu_volt_def	*cpu_def;
+	struct cpu_speed_point	*speed_points;
+	int			nspeed_points;
+
+	struct cpu_speed_point	*act_speed_point;
+
+	int			latency;
+};
+
+static int cpufreq_lowest_freq = 1;
+TUNABLE_INT("hw.tegra124.cpufreq.lowest_freq", &cpufreq_lowest_freq);
+
+#define	DIV_ROUND_CLOSEST(val, div)	(((val) + ((div) / 2)) / (div))
+
+#define	ROUND_UP(val, div)	((((val) + ((div) - 1)) / (div)) * (div))
+#define	ROUND_DOWN(val, div)	(((val) / (div)) * (div))
+
+/*
+ * Compute requesetd voltage for given frequency and SoC process variations,
+ * - compute base voltage from speedo value using speedo table
+ * - round up voltage to next regulator step
+ * - clamp it to regulator limits
+ */
+static int
+freq_to_voltage(struct tegra124_cpufreq_softc *sc, uint64_t freq)
+{
+	int uv, scale, min_uvolt, max_uvolt, step_uvolt;
+	struct speedo_entry *ent;
+	int i;
+
+	/* Get speedo entry with higher frequency */
+	ent = NULL;
+	for (i = 0; i < sc->cpu_def->speedo_nitems; i++) {
+		if (sc->cpu_def->speedo_tbl[i].freq >= freq) {
+			ent = &sc->cpu_def->speedo_tbl[i];
+			break;
+		}
+	}
+	if (ent == NULL)
+		ent = &sc->cpu_def->speedo_tbl[sc->cpu_def->speedo_nitems - 1];
+	scale = sc->cpu_def->speedo_scale;
+
+
+	/* uV = (c2 * speedo / scale + c1) * speedo / scale + c0) */
+	uv = DIV_ROUND_CLOSEST(ent->c2 * sc->speedo_value, scale);
+	uv = DIV_ROUND_CLOSEST((uv + ent->c1) * sc->speedo_value, scale) +
+	    ent->c0;
+	step_uvolt = sc->cpu_def->step_uvolt;
+	/* Round up it to next regulator step */
+	uv = ROUND_UP(uv, step_uvolt);
+
+	/* Clamp result */
+	min_uvolt = ROUND_UP(sc->cpu_def->min_uvolt, step_uvolt);
+	max_uvolt = ROUND_DOWN(sc->cpu_def->max_uvolt, step_uvolt);
+	if (uv < min_uvolt)
+		uv =  min_uvolt;
+	if (uv > max_uvolt)
+		uv =  max_uvolt;
+	return (uv);
+
+}
+
+static void
+build_speed_points(struct tegra124_cpufreq_softc *sc) {
+	int i;
+
+	sc->nspeed_points = nitems(cpu_freq_tbl);
+	sc->speed_points = malloc(sizeof(struct cpu_speed_point) *
+	    sc->nspeed_points, M_DEVBUF, M_NOWAIT);
+	for (i = 0; i < sc->nspeed_points; i++) {
+		sc->speed_points[i].freq = cpu_freq_tbl[i];
+		sc->speed_points[i].uvolt = freq_to_voltage(sc,
+		    cpu_freq_tbl[i]);
+	}
+}
+
+static struct cpu_speed_point *
+get_speed_point(struct tegra124_cpufreq_softc *sc, uint64_t freq)
+{
+	int i;
+
+	if (sc->speed_points[0].freq >= freq)
+		return (sc->speed_points + 0);
+
+	for (i = 0; i < sc->nspeed_points - 1; i++) {
+		if (sc->speed_points[i + 1].freq > freq)
+			return (sc->speed_points + i);
+	}
+
+	return (sc->speed_points + sc->nspeed_points - 1);
+}
+
+static int
+tegra124_cpufreq_settings(device_t dev, struct cf_setting *sets, int *count)
+{
+	struct tegra124_cpufreq_softc *sc;
+	int i, j, max_cnt;
+
+	if (sets == NULL || count == NULL)
+		return (EINVAL);
+
+	sc = device_get_softc(dev);
+	memset(sets, CPUFREQ_VAL_UNKNOWN, sizeof(*sets) * (*count));
+
+	max_cnt = min(sc->nspeed_points, *count);
+	for (i = 0, j = sc->nspeed_points - 1; j >= 0; j--) {
+		if (sc->cpu_max_freq < sc->speed_points[j].freq)
+			continue;
+		sets[i].freq = sc->speed_points[j].freq / 1000000;
+		sets[i].volts = sc->speed_points[j].uvolt / 1000;
+		sets[i].lat = sc->latency;
+		sets[i].dev = dev;
+		i++;
+	}
+	*count = i;
+
+	return (0);
+}
+
+static int
+set_cpu_freq(struct tegra124_cpufreq_softc *sc, uint64_t freq)
+{
+	struct cpu_speed_point *point;
+	int rv;
+
+	point = get_speed_point(sc, freq);
+
+	if (sc->act_speed_point->uvolt < point->uvolt) {
+		/* set cpu voltage */
+		rv = regulator_set_voltage(sc->supply_vdd_cpu,
+		    point->uvolt, point->uvolt);
+		DELAY(10000);
+		if (rv != 0)
+			return (rv);
+	}
+	rv = clk_set_freq(sc->clk_cpu_g, point->freq, CLK_SET_ROUND_DOWN);
+	if (rv != 0) {
+		device_printf(sc->dev, "Can't set CPU clock frequency\n");
+		return (rv);
+	}
+
+	if (sc->act_speed_point->uvolt > point->uvolt) {
+		/* set cpu voltage */
+		rv = regulator_set_voltage(sc->supply_vdd_cpu,
+		    point->uvolt, point->uvolt);
+		if (rv != 0)
+			return (rv);
+	}
+
+	sc->act_speed_point = point;
+
+	return (0);
+}
+
+static int
+tegra124_cpufreq_set(device_t dev, const struct cf_setting *cf)
+{
+	struct tegra124_cpufreq_softc *sc;
+	uint64_t freq;
+	int rv;
+
+	if (cf == NULL || cf->freq < 0)
+		return (EINVAL);
+
+	sc = device_get_softc(dev);
+
+	freq = cf->freq;
+	if (freq < cpufreq_lowest_freq)
+		freq = cpufreq_lowest_freq;
+	freq *= 1000000;
+	if (freq >= sc->cpu_max_freq)
+		freq = sc->cpu_max_freq;
+	rv = set_cpu_freq(sc, freq);
+
+	return (rv);
+}
+
+static int
+tegra124_cpufreq_get(device_t dev, struct cf_setting *cf)
+{
+	struct tegra124_cpufreq_softc *sc;
+
+	if (cf == NULL)
+		return (EINVAL);
+
+	sc = device_get_softc(dev);
+	memset(cf, CPUFREQ_VAL_UNKNOWN, sizeof(*cf));
+	cf->dev = NULL;
+	cf->freq = sc->act_speed_point->freq / 1000000;
+	cf->volts = sc->act_speed_point->uvolt / 1000;
+	/* Transition latency in us. */
+	cf->lat = sc->latency;
+	/* Driver providing this setting. */
+	cf->dev = dev;
+
+	return (0);
+}
+
+
+static int
+tegra124_cpufreq_type(device_t dev, int *type)
+{
+
+	if (type == NULL)
+		return (EINVAL);
+	*type = CPUFREQ_TYPE_ABSOLUTE;
+
+	return (0);
+}
+
+static int
+get_fdt_resources(struct tegra124_cpufreq_softc *sc, phandle_t node)
+{
+	int rv;
+	device_t parent_dev;
+
+	parent_dev =  device_get_parent(sc->dev);
+	rv = regulator_get_by_ofw_property(parent_dev, "vdd-cpu-supply",
+	    &sc->supply_vdd_cpu);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'vdd-cpu' regulator\n");
+		return (rv);
+	}
+
+	rv = clk_get_by_ofw_name(parent_dev, "cpu_g", &sc->clk_cpu_g);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'cpu_g' clock: %d\n", rv);
+		return (ENXIO);
+	}
+
+	rv = clk_get_by_ofw_name(parent_dev, "cpu_lp", &sc->clk_cpu_lp);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'cpu_lp' clock\n");
+		return (ENXIO);
+	}
+
+	rv = clk_get_by_ofw_name(parent_dev, "pll_x", &sc->clk_pll_x);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'pll_x' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(parent_dev, "pll_p", &sc->clk_pll_p);
+	if (rv != 0) {
+		device_printf(parent_dev, "Cannot get 'pll_p' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(parent_dev, "dfll", &sc->clk_dfll);
+	if (rv != 0) {
+		/* XXX DPLL is not implemented yet */
+/*
+		device_printf(sc->dev, "Cannot get 'dfll' clock\n");
+		return (ENXIO);
+*/
+	}
+	return (0);
+}
+
+static void
+tegra124_cpufreq_identify(driver_t *driver, device_t parent)
+{
+
+	if (device_find_child(parent, "tegra124_cpufreq", -1) != NULL)
+		return;
+	if (BUS_ADD_CHILD(parent, 0, "tegra124_cpufreq", -1) == NULL)
+		device_printf(parent, "add child failed\n");
+}
+
+static int
+tegra124_cpufreq_probe(device_t dev)
+{
+
+	if (device_get_unit(dev) != 0)
+		return (ENXIO);
+	device_set_desc(dev, "CPU Frequency Control");
+
+	return (0);
+}
+
+static int
+tegra124_cpufreq_attach(device_t dev)
+{
+	struct tegra124_cpufreq_softc *sc;
+	uint64_t freq;
+	int rv;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	sc->node = ofw_bus_get_node(device_get_parent(dev));
+
+	sc->process_id = tegra_sku_info.cpu_process_id;
+	sc->speedo_id = tegra_sku_info.cpu_speedo_id;
+	sc->speedo_value = tegra_sku_info.cpu_speedo_value;
+
+	/* Tegra 124 */
+	/* XXX DPLL is not implemented yet */
+	if (1)
+		sc->cpu_def = &tegra124_cpu_volt_pllx_def;
+	else
+		sc->cpu_def = &tegra124_cpu_volt_dpll_def;
+
+
+	rv = get_fdt_resources(sc, sc->node);
+	if (rv !=  0) {
+		return (rv);
+	}
+
+	build_speed_points(sc);
+
+	rv = clk_get_freq(sc->clk_cpu_g, &freq);
+	if (rv != 0) {
+		device_printf(dev, "Can't get CPU clock frequency\n");
+		return (rv);
+	}
+	if (sc->speedo_id < nitems(cpu_max_freq))
+		sc->cpu_max_freq = cpu_max_freq[sc->speedo_id];
+	else
+		sc->cpu_max_freq = cpu_max_freq[0];
+	sc->act_speed_point = get_speed_point(sc, freq);
+
+	/* Set safe startup CPU frequency. */
+	rv = set_cpu_freq(sc, 1632000000);
+	if (rv != 0) {
+		device_printf(dev, "Can't set initial CPU clock frequency\n");
+		return (rv);
+	}
+
+	/* This device is controlled by cpufreq(4). */
+	cpufreq_register(dev);
+
+	return (0);
+}
+
+static int
+tegra124_cpufreq_detach(device_t dev)
+{
+	struct tegra124_cpufreq_softc *sc;
+
+	sc = device_get_softc(dev);
+	cpufreq_unregister(dev);
+
+	if (sc->supply_vdd_cpu != NULL)
+		regulator_release(sc->supply_vdd_cpu);
+
+	if (sc->clk_cpu_g != NULL)
+		clk_release(sc->clk_cpu_g);
+	if (sc->clk_cpu_lp != NULL)
+		clk_release(sc->clk_cpu_lp);
+	if (sc->clk_pll_x != NULL)
+		clk_release(sc->clk_pll_x);
+	if (sc->clk_pll_p != NULL)
+		clk_release(sc->clk_pll_p);
+	if (sc->clk_dfll != NULL)
+		clk_release(sc->clk_dfll);
+	return (0);
+}
+
+static device_method_t tegra124_cpufreq_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_identify,	tegra124_cpufreq_identify),
+	DEVMETHOD(device_probe,		tegra124_cpufreq_probe),
+	DEVMETHOD(device_attach,	tegra124_cpufreq_attach),
+	DEVMETHOD(device_detach,	tegra124_cpufreq_detach),
+
+	/* cpufreq interface */
+	DEVMETHOD(cpufreq_drv_set,	tegra124_cpufreq_set),
+	DEVMETHOD(cpufreq_drv_get,	tegra124_cpufreq_get),
+	DEVMETHOD(cpufreq_drv_settings,	tegra124_cpufreq_settings),
+	DEVMETHOD(cpufreq_drv_type,	tegra124_cpufreq_type),
+
+	DEVMETHOD_END
+};
+
+static devclass_t tegra124_cpufreq_devclass;
+static driver_t tegra124_cpufreq_driver = {
+	"tegra124_cpufreq",
+	tegra124_cpufreq_methods,
+	sizeof(struct tegra124_cpufreq_softc),
+};
+
+DRIVER_MODULE(tegra124_cpufreq, cpu, tegra124_cpufreq_driver,
+    tegra124_cpufreq_devclass, 0, 0);
diff --git a/sys/arm/nvidia/tegra124/tegra124_machdep.c b/sys/arm/nvidia/tegra124/tegra124_machdep.c
new file mode 100644
index 0000000..e74a05b
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_machdep.c
@@ -0,0 +1,173 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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.
+ */
+
+#define	_ARM32_BUS_DMA_PRIVATE
+#include "opt_platform.h"
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/reboot.h>
+
+#include <vm/vm.h>
+
+#include <machine/bus.h>
+#include <machine/devmap.h>
+#include <machine/fdt.h>
+#include <machine/intr.h>
+#include <machine/machdep.h>
+#include <machine/platformvar.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+
+#include <arm/nvidia/tegra124/tegra124_mp.h>
+
+#include "platform_if.h"
+
+#define	PMC_PHYSBASE		0x7000e400
+#define	PMC_SIZE		0x400
+#define	PMC_CONTROL_REG		0x0
+#define	PMC_SCRATCH0		0x50
+#define	 PMC_SCRATCH0_MODE_RECOVERY	(1 << 31)
+#define	 PMC_SCRATCH0_MODE_BOOTLOADER	(1 << 30)
+#define	 PMC_SCRATCH0_MODE_RCM		(1 << 1)
+#define	 PMC_SCRATCH0_MODE_MASK		(PMC_SCRATCH0_MODE_RECOVERY | \
+					PMC_SCRATCH0_MODE_BOOTLOADER | \
+					PMC_SCRATCH0_MODE_RCM)
+
+struct fdt_fixup_entry fdt_fixup_table[] = {
+	{ NULL, NULL }
+};
+
+struct arm32_dma_range *
+bus_dma_get_range(void)
+{
+
+	return (NULL);
+}
+
+int
+bus_dma_get_range_nb(void)
+{
+
+	return (0);
+}
+
+static vm_offset_t
+tegra124_lastaddr(platform_t plat)
+{
+
+	return (arm_devmap_lastaddr());
+}
+
+static int
+tegra124_attach(platform_t plat)
+{
+
+	return (0);
+}
+
+static void
+tegra124_late_init(platform_t plat)
+{
+
+}
+
+/*
+ * Set up static device mappings.
+ *
+ */
+static int
+tegra124_devmap_init(platform_t plat)
+{
+
+	arm_devmap_add_entry(0x70000000, 0x01000000);
+	return (0);
+}
+
+void
+cpu_reset(void)
+{
+	bus_space_handle_t pmc;
+	uint32_t reg;
+
+	printf("Resetting...\n");
+	bus_space_map(fdtbus_bs_tag, PMC_PHYSBASE, PMC_SIZE, 0, &pmc);
+
+	reg = bus_space_read_4(fdtbus_bs_tag, pmc, PMC_SCRATCH0);
+	reg &= PMC_SCRATCH0_MODE_MASK;
+	bus_space_write_4(fdtbus_bs_tag, pmc, PMC_SCRATCH0,
+	   reg | PMC_SCRATCH0_MODE_BOOTLOADER); 	/* boot to bootloader */
+	bus_space_read_4(fdtbus_bs_tag, pmc, PMC_SCRATCH0);
+
+	reg = bus_space_read_4(fdtbus_bs_tag, pmc, PMC_CONTROL_REG);
+	spinlock_enter();
+	dsb();
+	bus_space_write_4(fdtbus_bs_tag, pmc, PMC_CONTROL_REG, reg | 0x10);
+	bus_space_read_4(fdtbus_bs_tag, pmc, PMC_CONTROL_REG);
+	while(1)
+		;
+
+}
+
+/*
+ * Early putc routine for EARLY_PRINTF support.  To use, add to kernel config:
+ *   option SOCDEV_PA=0x02000000
+ *   option SOCDEV_VA=0x02000000
+ *   option EARLY_PRINTF
+ */
+#if 0
+static void
+tegra124_early_putc(int c)
+{
+	volatile uint32_t * UART_STAT_REG = (uint32_t *)0x02020098;
+	volatile uint32_t * UART_TX_REG   = (uint32_t *)0x02020040;
+	const uint32_t      UART_TXRDY    = (1 << 3);
+
+	while ((*UART_STAT_REG & UART_TXRDY) == 0)
+		continue;
+	*UART_TX_REG = c;
+}
+early_putc_t *early_putc = tegra124_early_putc;
+#endif
+
+static platform_method_t tegra124_methods[] = {
+	PLATFORMMETHOD(platform_attach,		tegra124_attach),
+	PLATFORMMETHOD(platform_lastaddr,	tegra124_lastaddr),
+	PLATFORMMETHOD(platform_devmap_init,	tegra124_devmap_init),
+	PLATFORMMETHOD(platform_late_init,	tegra124_late_init),
+#ifdef SMP
+	PLATFORMMETHOD(platform_mp_start_ap,	tegra124_mp_start_ap),
+	PLATFORMMETHOD(platform_mp_setmaxid,	tegra124_mp_setmaxid),
+#endif
+	PLATFORMMETHOD_END,
+};
+
+FDT_PLATFORM_DEF(tegra124, "Nvidia Jetson-TK1", 0, "nvidia,jetson-tk1");
diff --git a/sys/arm/nvidia/tegra124/tegra124_mp.c b/sys/arm/nvidia/tegra124/tegra124_mp.c
new file mode 100644
index 0000000..36b3b7c
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_mp.c
@@ -0,0 +1,127 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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 ``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 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/kernel.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/smp.h>
+
+#include <vm/vm.h>
+#include <vm/pmap.h>
+
+#include <machine/cpu.h>
+#include <machine/intr.h>
+#include <machine/fdt.h>
+#include <machine/smp.h>
+#include <machine/platformvar.h>
+#include <machine/pmap.h>
+
+#include <arm/nvidia/tegra124/tegra124_mp.h>
+
+#define	PMC_PHYSBASE			0x7000e400
+#define	PMC_SIZE			0x400
+#define	PMC_CONTROL_REG			0x0
+#define	PMC_PWRGATE_TOGGLE		0x30
+#define	 PCM_PWRGATE_TOGGLE_START	(1 << 8)
+#define	PMC_PWRGATE_STATUS		0x38
+
+#define	TEGRA_EXCEPTION_VECTORS_BASE	0x6000F000 /* exception vectors */
+#define	TEGRA_EXCEPTION_VECTORS_SIZE	1024
+#define	 TEGRA_EXCEPTION_VECTOR_ENTRY	0x100
+
+void
+tegra124_mp_setmaxid(platform_t plat)
+{
+	int ncpu;
+
+	/* If we've already set the global vars don't bother to do it again. */
+	if (mp_ncpus != 0)
+		return;
+
+	/* Read current CP15 Cache Size ID Register */
+	ncpu = cp15_l2ctlr_get();
+	ncpu = CPUV7_L2CTLR_NPROC(ncpu);
+
+	mp_ncpus = ncpu;
+	mp_maxid = ncpu - 1;
+}
+
+void
+tegra124_mp_start_ap(platform_t plat)
+{
+	bus_space_handle_t pmc;
+	bus_space_handle_t exvec;
+	int i;
+	uint32_t val;
+	uint32_t mask;
+
+	if (bus_space_map(fdtbus_bs_tag, PMC_PHYSBASE, PMC_SIZE, 0, &pmc) != 0)
+		panic("Couldn't map the PMC\n");
+	if (bus_space_map(fdtbus_bs_tag, TEGRA_EXCEPTION_VECTORS_BASE,
+	    TEGRA_EXCEPTION_VECTORS_SIZE, 0, &exvec) != 0)
+		panic("Couldn't map the exception vectors\n");
+
+	bus_space_write_4(fdtbus_bs_tag, exvec , TEGRA_EXCEPTION_VECTOR_ENTRY,
+			  pmap_kextract((vm_offset_t)mpentry));
+	bus_space_read_4(fdtbus_bs_tag, exvec , TEGRA_EXCEPTION_VECTOR_ENTRY);
+
+
+	/* Wait until POWERGATE is ready (max 20 APB cycles). */
+	do {
+		val = bus_space_read_4(fdtbus_bs_tag, pmc,
+		    PMC_PWRGATE_TOGGLE);
+	} while ((val & PCM_PWRGATE_TOGGLE_START) != 0);
+
+	for (i = 1; i < mp_ncpus; i++) {
+		val = bus_space_read_4(fdtbus_bs_tag, pmc, PMC_PWRGATE_STATUS);
+		mask = 1 << (i + 8);	/* cpu mask */
+		if ((val & mask) == 0) {
+			/* Wait until POWERGATE is ready (max 20 APB cycles). */
+			do {
+				val = bus_space_read_4(fdtbus_bs_tag, pmc,
+				PMC_PWRGATE_TOGGLE);
+			} while ((val & PCM_PWRGATE_TOGGLE_START) != 0);
+			bus_space_write_4(fdtbus_bs_tag, pmc,
+			    PMC_PWRGATE_TOGGLE,
+			    PCM_PWRGATE_TOGGLE_START | (8 + i));
+
+			/* Wait until CPU is powered */
+			do {
+				val = bus_space_read_4(fdtbus_bs_tag, pmc,
+				    PMC_PWRGATE_STATUS);
+			} while ((val & mask) == 0);
+		}
+
+	}
+	armv7_sev();
+	bus_space_unmap(fdtbus_bs_tag, pmc, PMC_SIZE);
+	bus_space_unmap(fdtbus_bs_tag, exvec, TEGRA_EXCEPTION_VECTORS_SIZE);
+}
diff --git a/sys/arm/nvidia/tegra124/tegra124_mp.h b/sys/arm/nvidia/tegra124/tegra124_mp.h
new file mode 100644
index 0000000..57785aa
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_mp.h
@@ -0,0 +1,35 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _TEGRA124_MP_H_
+#define	_TEGRA124_MP_H_
+
+void tegra124_mp_setmaxid(platform_t plat);
+void tegra124_mp_start_ap(platform_t plat);
+
+#endif /*_TEGRA124_MP_H_*/
\ No newline at end of file
diff --git a/sys/arm/nvidia/tegra124/tegra124_pmc.c b/sys/arm/nvidia/tegra124/tegra124_pmc.c
new file mode 100644
index 0000000..7ff7307
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_pmc.c
@@ -0,0 +1,566 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <arm/nvidia/tegra_pmc.h>
+
+#define	PMC_CNTRL			0x000
+#define	 PMC_CNTRL_CPUPWRGOOD_SEL_MASK		(0x3 << 20)
+#define	 PMC_CNTRL_CPUPWRGOOD_SEL_SHIFT		20
+#define	 PMC_CNTRL_CPUPWRGOOD_EN		(1 << 19)
+#define	 PMC_CNTRL_FUSE_OVERRIDE		(1 << 18)
+#define	 PMC_CNTRL_INTR_POLARITY		(1 << 17)
+#define	 PMC_CNTRL_CPU_PWRREQ_OE		(1 << 16)
+#define	 PMC_CNTRL_CPU_PWRREQ_POLARITY		(1 << 15)
+#define	 PMC_CNTRL_SIDE_EFFECT_LP0		(1 << 14)
+#define	 PMC_CNTRL_AOINIT			(1 << 13)
+#define	 PMC_CNTRL_PWRGATE_DIS			(1 << 12)
+#define	 PMC_CNTRL_SYSCLK_OE			(1 << 11)
+#define	 PMC_CNTRL_SYSCLK_POLARITY		(1 << 10)
+#define	 PMC_CNTRL_PWRREQ_OE			(1 <<  9)
+#define	 PMC_CNTRL_PWRREQ_POLARITY		(1 <<  8)
+#define	 PMC_CNTRL_BLINK_EN			(1 <<  7)
+#define	 PMC_CNTRL_GLITCHDET_DIS		(1 <<  6)
+#define	 PMC_CNTRL_LATCHWAKE_EN			(1 <<  5)
+#define	 PMC_CNTRL_MAIN_RST			(1 <<  4)
+#define	 PMC_CNTRL_KBC_RST			(1 <<  3)
+#define	 PMC_CNTRL_RTC_RST			(1 <<  2)
+#define	 PMC_CNTRL_RTC_CLK_DIS			(1 <<  1)
+#define	 PMC_CNTRL_KBC_CLK_DIS			(1 <<  0)
+
+#define	PMC_DPD_SAMPLE			0x020
+
+#define	PMC_CLAMP_STATUS		0x02C
+#define	  PMC_CLAMP_STATUS_PARTID(x)		(1 << ((x) & 0x1F))
+
+#define	PMC_PWRGATE_TOGGLE		0x030
+#define	 PMC_PWRGATE_TOGGLE_START		(1 << 8)
+#define	 PMC_PWRGATE_TOGGLE_PARTID(x)		(((x) & 0x1F) << 0)
+
+#define	PMC_REMOVE_CLAMPING_CMD		0x034
+#define	  PMC_REMOVE_CLAMPING_CMD_PARTID(x)	(1 << ((x) & 0x1F))
+
+#define	PMC_PWRGATE_STATUS		0x038
+#define	PMC_PWRGATE_STATUS_PARTID(x)		(1 << ((x) & 0x1F))
+
+#define	PMC_SCRATCH0			0x050
+#define	 PMC_SCRATCH0_MODE_RECOVERY		(1 << 31)
+#define	 PMC_SCRATCH0_MODE_BOOTLOADER		(1 << 30)
+#define	 PMC_SCRATCH0_MODE_RCM			(1 << 1)
+#define	 PMC_SCRATCH0_MODE_MASK			(PMC_SCRATCH0_MODE_RECOVERY | \
+						PMC_SCRATCH0_MODE_BOOTLOADER | \
+						PMC_SCRATCH0_MODE_RCM)
+
+#define	PMC_CPUPWRGOOD_TIMER		0x0c8
+#define	PMC_CPUPWROFF_TIMER		0x0cc
+
+#define	PMC_SCRATCH41			0x140
+
+#define	PMC_SENSOR_CTRL			0x1b0
+#define	PMC_SENSOR_CTRL_BLOCK_SCRATCH_WRITE	(1 << 2)
+#define	PMC_SENSOR_CTRL_ENABLE_RST		(1 << 1)
+#define	PMC_SENSOR_CTRL_ENABLE_PG		(1 << 0)
+
+#define	PMC_IO_DPD_REQ			0x1b8
+#define	 PMC_IO_DPD_REQ_CODE_IDLE		(0 << 30)
+#define	 PMC_IO_DPD_REQ_CODE_OFF		(1 << 30)
+#define	 PMC_IO_DPD_REQ_CODE_ON			(2 << 30)
+#define	 PMC_IO_DPD_REQ_CODE_MASK		(3 << 30)
+
+#define	PMC_IO_DPD_STATUS		0x1bc
+#define	 PMC_IO_DPD_STATUS_HDMI			(1 << 28)
+#define	PMC_IO_DPD2_REQ			0x1c0
+#define	PMC_IO_DPD2_STATUS		0x1c4
+#define	 PMC_IO_DPD2_STATUS_HV			(1 << 6)
+#define	PMC_SEL_DPD_TIM			0x1c8
+
+#define	PMC_SCRATCH54			0x258
+#define	PMC_SCRATCH54_DATA_SHIFT		8
+#define	PMC_SCRATCH54_ADDR_SHIFT		0
+
+#define	PMC_SCRATCH55			0x25c
+#define	PMC_SCRATCH55_RST_ENABLE		(1 << 31)
+#define	PMC_SCRATCH55_CNTRL_TYPE		(1 << 30)
+#define	PMC_SCRATCH55_CNTRL_ID_SHIFT		27
+#define	PMC_SCRATCH55_CNTRL_ID_MASK		0x07
+#define	PMC_SCRATCH55_PINMUX_SHIFT		24
+#define	PMC_SCRATCH55_PINMUX_MASK		0x07
+#define	PMC_SCRATCH55_CHECKSUM_SHIFT		16
+#define	PMC_SCRATCH55_CHECKSUM_MASK		0xFF
+#define	PMC_SCRATCH55_16BITOP			(1 << 15)
+#define	PMC_SCRATCH55_I2CSLV1_SHIFT		0
+#define	PMC_SCRATCH55_I2CSLV1_MASK		0x7F
+
+#define	PMC_GPU_RG_CNTRL		0x2d4
+
+#define	WR4(_sc, _r, _v)	bus_write_4((_sc)->mem_res, (_r), (_v))
+#define	RD4(_sc, _r)		bus_read_4((_sc)->mem_res, (_r))
+
+#define	PMC_LOCK(_sc)		mtx_lock(&(_sc)->mtx)
+#define	PMC_UNLOCK(_sc)		mtx_unlock(&(_sc)->mtx)
+#define	PMC_LOCK_INIT(_sc)	mtx_init(&(_sc)->mtx, 			\
+	    device_get_nameunit(_sc->dev), "tegra124_pmc", MTX_DEF)
+#define	PMC_LOCK_DESTROY(_sc)	mtx_destroy(&(_sc)->mtx);
+#define	PMC_ASSERT_LOCKED(_sc)	mtx_assert(&(_sc)->mtx, MA_OWNED);
+#define	PMC_ASSERT_UNLOCKED(_sc) mtx_assert(&(_sc)->mtx, MA_NOTOWNED);
+
+struct tegra124_pmc_softc {
+	device_t		dev;
+	struct resource		*mem_res;
+	clk_t			clk;
+	struct mtx		mtx;
+
+	uint32_t		rate;
+	enum tegra_suspend_mode suspend_mode;
+	uint32_t		cpu_good_time;
+	uint32_t		cpu_off_time;
+	uint32_t		core_osc_time;
+	uint32_t		core_pmu_time;
+	uint32_t		core_off_time;
+	int			corereq_high;
+	int			sysclkreq_high;
+	int			combined_req;
+	int			cpu_pwr_good_en;
+	uint32_t		lp0_vec_phys;
+	uint32_t		lp0_vec_size;
+};
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-pmc",		1},
+	{NULL,				0},
+};
+
+static struct tegra124_pmc_softc *pmc_sc;
+
+static inline struct tegra124_pmc_softc *
+tegra124_pmc_get_sc(void)
+{
+	if (pmc_sc == NULL)
+		panic("To early call to Tegra PMC driver.\n");
+	return (pmc_sc);
+}
+
+static int
+tegra124_pmc_set_powergate(struct tegra124_pmc_softc *sc,
+    enum tegra_powergate_id id, int ena)
+{
+	uint32_t reg;
+	int i;
+
+	PMC_LOCK(sc);
+
+	reg = RD4(sc, PMC_PWRGATE_STATUS) & PMC_PWRGATE_STATUS_PARTID(id);
+	if (((reg != 0) && ena) || ((reg == 0) && !ena)) {
+		PMC_UNLOCK(sc);
+		return (0);
+	}
+
+	for (i = 100; i > 0; i--) {
+		reg = RD4(sc, PMC_PWRGATE_TOGGLE);
+		if ((reg & PMC_PWRGATE_TOGGLE_START) == 0)
+			break;
+		DELAY(1);
+	}
+	if (i <= 0)
+		device_printf(sc->dev,
+		    "Timeout when waiting for TOGGLE_START\n");
+
+	WR4(sc, PMC_PWRGATE_TOGGLE,
+	    PMC_PWRGATE_TOGGLE_START | PMC_PWRGATE_TOGGLE_PARTID(id));
+
+	for (i = 100; i > 0; i--) {
+		reg = RD4(sc, PMC_PWRGATE_TOGGLE);
+		if ((reg & PMC_PWRGATE_TOGGLE_START) == 0)
+			break;
+		DELAY(1);
+	}
+	if (i <= 0)
+		device_printf(sc->dev,
+		    "Timeout when waiting for TOGGLE_START\n");
+		PMC_UNLOCK(sc);
+	return (0);
+}
+
+int
+tegra_powergate_remove_clamping(enum tegra_powergate_id  id)
+{
+	struct tegra124_pmc_softc *sc;
+	uint32_t reg;
+	enum tegra_powergate_id swid;
+	int i;
+
+	sc = tegra124_pmc_get_sc();
+
+	if (id == TEGRA_POWERGATE_3D) {
+		WR4(sc, PMC_GPU_RG_CNTRL, 0);
+		return (0);
+	}
+
+	reg = RD4(sc, PMC_PWRGATE_STATUS);
+	if ((reg & PMC_PWRGATE_STATUS_PARTID(id)) == 0)
+		panic("Attempt to remove clamping for unpowered partition.\n");
+
+	if (id == TEGRA_POWERGATE_PCX)
+		swid = TEGRA_POWERGATE_VDE;
+	else if (id == TEGRA_POWERGATE_VDE)
+		swid = TEGRA_POWERGATE_PCX;
+	else
+		swid = id;
+	WR4(sc, PMC_REMOVE_CLAMPING_CMD, PMC_REMOVE_CLAMPING_CMD_PARTID(swid));
+
+	for (i = 100; i > 0; i--) {
+		reg = RD4(sc, PMC_REMOVE_CLAMPING_CMD);
+		if ((reg & PMC_REMOVE_CLAMPING_CMD_PARTID(swid)) == 0)
+			break;
+		DELAY(1);
+	}
+	if (i <= 0)
+		device_printf(sc->dev, "Timeout when remove clamping\n");
+
+	reg = RD4(sc, PMC_CLAMP_STATUS);
+	if ((reg & PMC_CLAMP_STATUS_PARTID(id)) != 0)
+		panic("Cannot remove clamping\n");
+
+	return (0);
+}
+
+int
+tegra_powergate_is_powered(enum tegra_powergate_id id)
+{
+	struct tegra124_pmc_softc *sc;
+	uint32_t reg;
+
+	sc = tegra124_pmc_get_sc();
+
+	reg = RD4(sc, PMC_PWRGATE_STATUS);
+	return ((reg & PMC_PWRGATE_STATUS_PARTID(id)) ? 1 : 0);
+}
+
+int
+tegra_powergate_power_on(enum tegra_powergate_id id)
+{
+	struct tegra124_pmc_softc *sc;
+	int rv, i;
+
+	sc = tegra124_pmc_get_sc();
+
+	rv = tegra124_pmc_set_powergate(sc, id, 1);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot set powergate: %d\n", id);
+		return (rv);
+	}
+
+	for (i = 100; i > 0; i--) {
+		if (tegra_powergate_is_powered(id))
+			break;
+		DELAY(1);
+	}
+	if (i <= 0)
+		device_printf(sc->dev, "Timeout when waiting on power up\n");
+
+	return (rv);
+}
+
+int
+tegra_powergate_power_off(enum tegra_powergate_id id)
+{
+	struct tegra124_pmc_softc *sc;
+	int rv, i;
+
+	sc = tegra124_pmc_get_sc();
+
+	rv = tegra124_pmc_set_powergate(sc, id, 0);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot set powergate: %d\n", id);
+		return (rv);
+	}
+	for (i = 100; i > 0; i--) {
+		if (!tegra_powergate_is_powered(id))
+			break;
+		DELAY(1);
+	}
+	if (i <= 0)
+		device_printf(sc->dev, "Timeout when waiting on power off\n");
+
+	return (rv);
+}
+
+int
+tegra_powergate_sequence_power_up(enum tegra_powergate_id id, clk_t clk,
+    hwreset_t rst)
+{
+	struct tegra124_pmc_softc *sc;
+	int rv;
+
+	sc = tegra124_pmc_get_sc();
+
+	rv = hwreset_assert(rst);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot assert reset\n");
+		return (rv);
+	}
+
+	rv = clk_stop(clk);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot stop clock\n");
+		goto clk_fail;
+	}
+
+	rv = tegra_powergate_power_on(id);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot power on powergate\n");
+		goto clk_fail;
+	}
+
+	rv = clk_enable(clk);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable clock\n");
+		goto clk_fail;
+	}
+	DELAY(20);
+
+	rv = tegra_powergate_remove_clamping(id);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot remove clamping\n");
+		goto fail;
+	}
+	rv = hwreset_deassert(rst);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot unreset reset\n");
+		goto fail;
+	}
+	return 0;
+
+fail:
+	clk_disable(clk);
+clk_fail:
+	hwreset_assert(rst);
+	tegra_powergate_power_off(id);
+	return (rv);
+}
+
+static int
+tegra124_pmc_parse_fdt(struct tegra124_pmc_softc *sc, phandle_t node)
+{
+	int rv;
+	uint32_t tmp;
+	uint32_t tmparr[2];
+
+	rv = OF_getencprop(node, "nvidia,suspend-mode", &tmp, sizeof(tmp));
+	if (rv > 0) {
+		switch (tmp) {
+		case 0:
+			sc->suspend_mode = TEGRA_SUSPEND_LP0;
+			break;
+
+		case 1:
+			sc->suspend_mode = TEGRA_SUSPEND_LP1;
+			break;
+
+		case 2:
+			sc->suspend_mode = TEGRA_SUSPEND_LP2;
+			break;
+
+		default:
+			sc->suspend_mode = TEGRA_SUSPEND_NONE;
+			break;
+		}
+	}
+
+	rv = OF_getencprop(node, "nvidia,cpu-pwr-good-time", &tmp, sizeof(tmp));
+	if (rv > 0) {
+		sc->cpu_good_time = tmp;
+		sc->suspend_mode = TEGRA_SUSPEND_NONE;
+	}
+
+	rv = OF_getencprop(node, "nvidia,cpu-pwr-off-time", &tmp, sizeof(tmp));
+	if (rv > 0) {
+		sc->cpu_off_time = tmp;
+		sc->suspend_mode = TEGRA_SUSPEND_NONE;
+	}
+
+	rv = OF_getencprop(node, "nvidia,core-pwr-good-time", tmparr,
+	    sizeof(tmparr));
+	if (rv == sizeof(tmparr)) {
+		sc->core_osc_time = tmparr[0];
+		sc->core_pmu_time = tmparr[1];
+		sc->suspend_mode = TEGRA_SUSPEND_NONE;
+	}
+
+	rv = OF_getencprop(node, "nvidia,core-pwr-off-time", &tmp, sizeof(tmp));
+	if (rv > 0) {
+		sc->core_off_time = tmp;
+		sc->suspend_mode = TEGRA_SUSPEND_NONE;
+	}
+
+	sc->corereq_high =
+	    OF_hasprop(node, "nvidia,core-power-req-active-high");
+	sc->sysclkreq_high =
+	    OF_hasprop(node, "nvidia,sys-clock-req-active-high");
+	sc->combined_req =
+	    OF_hasprop(node, "nvidia,combined-power-req");
+	sc->cpu_pwr_good_en =
+	    OF_hasprop(node, "nvidia,cpu-pwr-good-en");
+
+	rv = OF_getencprop(node, "nvidia,lp0-vec", tmparr, sizeof(tmparr));
+	if (rv == sizeof(tmparr)) {
+
+		sc->lp0_vec_phys = tmparr[0];
+		sc->core_pmu_time = tmparr[1];
+		sc->lp0_vec_size = TEGRA_SUSPEND_NONE;
+		if (sc->suspend_mode == TEGRA_SUSPEND_LP0)
+			sc->suspend_mode = TEGRA_SUSPEND_LP1;
+	}
+	return 0;
+}
+
+static int
+tegra124_pmc_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_search_compatible(dev, compat_data)->ocd_data)
+		return (ENXIO);
+
+	device_set_desc(dev, "Tegra PMC");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+tegra124_pmc_detach(device_t dev)
+{
+
+	/* This device is always present. */
+	return (EBUSY);
+}
+
+static int
+tegra124_pmc_attach(device_t dev)
+{
+	struct tegra124_pmc_softc *sc;
+	int rid, rv;
+	uint32_t reg;
+	phandle_t node;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	node = ofw_bus_get_node(dev);
+
+	rv = tegra124_pmc_parse_fdt(sc, node);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot parse FDT data\n");
+		return (rv);
+	}
+
+	rv = clk_get_by_ofw_name(sc->dev, "pclk", &sc->clk);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get \"pclk\" clock\n");
+		return (ENXIO);
+	}
+
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	PMC_LOCK_INIT(sc);
+
+	/* Enable CPU power request. */
+	reg = RD4(sc, PMC_CNTRL);
+	reg |= PMC_CNTRL_CPU_PWRREQ_OE;
+	WR4(sc, PMC_CNTRL, reg);
+
+	/* Set sysclk output polarity */
+	reg = RD4(sc, PMC_CNTRL);
+	if (sc->sysclkreq_high)
+		reg &= ~PMC_CNTRL_SYSCLK_POLARITY;
+	else
+		reg |= PMC_CNTRL_SYSCLK_POLARITY;
+	WR4(sc, PMC_CNTRL, reg);
+
+	/* Enable sysclk request. */
+	reg = RD4(sc, PMC_CNTRL);
+	reg |= PMC_CNTRL_SYSCLK_OE;
+	WR4(sc, PMC_CNTRL, reg);
+
+	/*
+	 * Remove HDMI from deep power down mode.
+	 * XXX mote this to HDMI driver
+	 */
+	reg = RD4(sc, PMC_IO_DPD_STATUS);
+	reg &= ~ PMC_IO_DPD_STATUS_HDMI;
+	WR4(sc, PMC_IO_DPD_STATUS, reg);
+
+	reg = RD4(sc, PMC_IO_DPD2_STATUS);
+	reg &= ~ PMC_IO_DPD2_STATUS_HV;
+	WR4(sc, PMC_IO_DPD2_STATUS, reg);
+
+	if (pmc_sc != NULL)
+		panic("tegra124_pmc: double driver attach");
+	pmc_sc = sc;
+	return (0);
+}
+
+static device_method_t tegra124_pmc_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		tegra124_pmc_probe),
+	DEVMETHOD(device_attach,	tegra124_pmc_attach),
+	DEVMETHOD(device_detach,	tegra124_pmc_detach),
+
+	DEVMETHOD_END
+};
+
+static driver_t tegra124_pmc_driver = {
+	"tegra124_pmc",
+	tegra124_pmc_methods,
+	sizeof(struct tegra124_pmc_softc),
+};
+
+static devclass_t tegra124_pmc_devclass;
+EARLY_DRIVER_MODULE(tegra124_pmc, simplebus, tegra124_pmc_driver,
+    tegra124_pmc_devclass, 0, 0, 70);
diff --git a/sys/arm/nvidia/tegra124/tegra124_xusbpadctl.c b/sys/arm/nvidia/tegra124/tegra124_xusbpadctl.c
new file mode 100644
index 0000000..e06d4bf
--- /dev/null
+++ b/sys/arm/nvidia/tegra124/tegra124_xusbpadctl.c
@@ -0,0 +1,603 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+#include <machine/fdt.h>
+
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/extres/phy/phy.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/fdt/fdt_pinctrl.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <gnu/dts/include/dt-bindings/pinctrl/pinctrl-tegra-xusb.h>
+
+#include "phy_if.h"
+
+#define	XUSB_PADCTL_USB2_PAD_MUX		0x004
+
+#define	XUSB_PADCTL_ELPG_PROGRAM		0x01C
+#define	 ELPG_PROGRAM_AUX_MUX_LP0_VCORE_DOWN		(1 << 26)
+#define	 ELPG_PROGRAM_AUX_MUX_LP0_CLAMP_EN_EARLY	(1 << 25)
+#define	 ELPG_PROGRAM_AUX_MUX_LP0_CLAMP_EN		(1 << 24)
+
+#define	XUSB_PADCTL_IOPHY_PLL_P0_CTL1		0x040
+#define	 IOPHY_PLL_P0_CTL1_PLL0_LOCKDET			(1 << 19)
+#define	 IOPHY_PLL_P0_CTL1_REFCLK_SEL_MASK		(0xf<< 12)
+#define	 IOPHY_PLL_P0_CTL1_PLL_RST			(1 << 1)
+
+#define	XUSB_PADCTL_IOPHY_PLL_P0_CTL2		0x044
+#define	 IOPHY_PLL_P0_CTL2_REFCLKBUF_EN			(1 << 6)
+#define	 IOPHY_PLL_P0_CTL2_TXCLKREF_EN			(1 << 5)
+#define	 IOPHY_PLL_P0_CTL2_TXCLKREF_SEL			(1 << 4)
+
+
+#define	XUSB_PADCTL_USB3_PAD_MUX		0x134
+
+#define	XUSB_PADCTL_IOPHY_PLL_S0_CTL1		0x138
+#define	 IOPHY_PLL_S0_CTL1_PLL1_LOCKDET			(1 << 27)
+#define	 IOPHY_PLL_S0_CTL1_PLL1_MODE			(1 << 24)
+#define	 IOPHY_PLL_S0_CTL1_PLL_PWR_OVRD			(1 << 3)
+#define	 IOPHY_PLL_S0_CTL1_PLL_RST_L			(1 << 1)
+#define	 IOPHY_PLL_S0_CTL1_PLL_IDDQ			(1 << 0)
+
+#define	XUSB_PADCTL_IOPHY_PLL_S0_CTL2		0x13C
+#define	XUSB_PADCTL_IOPHY_PLL_S0_CTL3		0x140
+#define	XUSB_PADCTL_IOPHY_PLL_S0_CTL4		0x144
+
+#define	XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL1	0x148
+#define	 IOPHY_MISC_PAD_S0_CTL1_IDDQ_OVRD		(1 << 1)
+#define	 IOPHY_MISC_PAD_S0_CTL1_IDDQ			(1 << 0)
+
+#define	XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL2	0x14C
+#define	XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL3	0x150
+#define	XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL4	0x154
+#define	XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL5	0x158
+#define	XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL6	0x15C
+
+struct lane_cfg {
+	char	*function;
+	char 	**lanes;
+	int 	iddq;
+};
+
+struct xusbpadctl_softc {
+	device_t	dev;
+	struct resource	*mem_res;
+	hwreset_t		rst;
+	int		phy_ena_cnt;
+};
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-xusb-padctl",	1},
+	{NULL,				0},
+};
+
+struct padctl_lane {
+	const char *name;
+	bus_size_t reg;
+	uint32_t shift;
+	uint32_t mask;
+	int iddq;
+	char **mux;
+	int nmux;
+};
+
+static char *otg_mux[] = {"snps", "xusb", "uart", "rsvd"};
+static char *usb_mux[] = {"snps", "xusb"};
+static char *pci_mux[] = {"pcie", "usb3", "sata", "rsvd"};
+
+#define	LANE(n, r, s, m, i, mx)					\
+{								\
+	.name = n,						\
+	.reg = r,						\
+	.shift = s,						\
+	.mask = m,						\
+	.iddq = i,						\
+	.mux = mx,						\
+	.nmux = nitems(mx),					\
+}
+
+static const struct padctl_lane lanes_tbl[] = {
+	LANE("otg-0",  XUSB_PADCTL_USB2_PAD_MUX,  0, 0x3, -1, otg_mux),
+	LANE("otg-1",  XUSB_PADCTL_USB2_PAD_MUX,  2, 0x3, -1, otg_mux),
+	LANE("otg-2",  XUSB_PADCTL_USB2_PAD_MUX,  4, 0x3, -1, otg_mux),
+	LANE("ulpi-0", XUSB_PADCTL_USB2_PAD_MUX, 12, 0x1, -1, usb_mux),
+	LANE("hsic-0", XUSB_PADCTL_USB2_PAD_MUX, 14, 0x1, -1, usb_mux),
+	LANE("hsic-1", XUSB_PADCTL_USB2_PAD_MUX, 15, 0x1, -1, usb_mux),
+	LANE("pcie-0", XUSB_PADCTL_USB3_PAD_MUX, 16, 0x3,  1, pci_mux),
+	LANE("pcie-1", XUSB_PADCTL_USB3_PAD_MUX, 18, 0x3,  2, pci_mux),
+	LANE("pcie-2", XUSB_PADCTL_USB3_PAD_MUX, 20, 0x3,  3, pci_mux),
+	LANE("pcie-3", XUSB_PADCTL_USB3_PAD_MUX, 22, 0x3,  4, pci_mux),
+	LANE("pcie-4", XUSB_PADCTL_USB3_PAD_MUX, 24, 0x3,  5, pci_mux),
+	LANE("sata-0", XUSB_PADCTL_USB3_PAD_MUX, 26, 0x3,  6, pci_mux),
+};
+
+static int
+xusbpadctl_mux_function(const struct padctl_lane *lane, char *fnc_name)
+{
+	int i;
+
+	for (i = 0; i < lane->nmux; i++) {
+		if (strcmp(fnc_name, lane->mux[i]) == 0)
+			return 	(i);
+	}
+
+	return (-1);
+}
+
+static int
+xusbpadctl_config_lane(struct xusbpadctl_softc *sc, char *lane_name,
+    const struct padctl_lane *lane, struct lane_cfg *cfg)
+{
+
+	int tmp;
+	uint32_t reg;
+
+	reg = bus_read_4(sc->mem_res, lane->reg);
+	if (cfg->function != NULL) {
+		tmp = xusbpadctl_mux_function(lane, cfg->function);
+		if (tmp == -1) {
+			device_printf(sc->dev,
+			    "Unknown function %s for lane %s\n", cfg->function,
+			    lane_name);
+			return (EINVAL);
+		}
+		reg &= ~(lane->mask << lane->shift);
+		reg |=  (tmp & lane->mask) << lane->shift;
+	}
+	if (cfg->iddq != -1) {
+		if (lane->iddq == -1) {
+			device_printf(sc->dev, "Invalid IDDQ for lane %s\n",
+			lane_name);
+			return (EINVAL);
+		}
+		if (cfg->iddq != 0)
+			reg &= ~(1 << lane->iddq);
+		else
+			reg |= 1 << lane->iddq;
+	}
+
+	bus_write_4(sc->mem_res, lane->reg, reg);
+	return (0);
+}
+
+static const struct padctl_lane *
+xusbpadctl_search_lane(char *lane_name)
+{
+	int i;
+
+	for (i = 0; i < nitems(lanes_tbl); i++) {
+		if (strcmp(lane_name, lanes_tbl[i].name) == 0)
+			return 	(&lanes_tbl[i]);
+	}
+
+	return (NULL);
+}
+
+static int
+xusbpadctl_config_node(struct xusbpadctl_softc *sc, char *lane_name,
+    struct lane_cfg *cfg)
+{
+	const struct padctl_lane *lane;
+	int rv;
+
+	lane = xusbpadctl_search_lane(lane_name);
+	if (lane == NULL) {
+		device_printf(sc->dev, "Unknown lane: %s\n", lane_name);
+		return (ENXIO);
+	}
+	rv = xusbpadctl_config_lane(sc, lane_name, lane, cfg);
+	return (rv);
+}
+
+static int
+xusbpadctl_read_node(struct xusbpadctl_softc *sc, phandle_t node,
+    struct lane_cfg *cfg, char **lanes, int *llanes)
+{
+	int rv;
+
+	*llanes = OF_getprop_alloc(node, "nvidia,lanes", 1, (void **)lanes);
+	if (*llanes <= 0)
+		return (ENOENT);
+
+	/* Read function (mux) settings. */
+	rv = OF_getprop_alloc(node, "nvidia,function", 1,
+	    (void **)&cfg->function);
+	if (rv <= 0)
+		cfg->function = NULL;
+	/* Read numeric properties. */
+	rv = OF_getencprop(node, "nvidia,iddq", &cfg->iddq,
+	    sizeof(cfg->iddq));
+	if (rv <= 0)
+		cfg->iddq = -1;
+	return (0);
+}
+
+static int
+xusbpadctl_process_node(struct xusbpadctl_softc *sc, phandle_t node)
+{
+	struct lane_cfg cfg;
+	char *lanes, *lname;
+	int i, len, llanes, rv;
+
+	rv = xusbpadctl_read_node(sc, node, &cfg, &lanes, &llanes);
+	if (rv != 0)
+		return (rv);
+
+	len = 0;
+	lname = lanes;
+	do {
+		i = strlen(lname) + 1;
+		rv = xusbpadctl_config_node(sc, lname, &cfg);
+		if (rv != 0)
+			device_printf(sc->dev,
+			    "Cannot configure lane: %s: %d\n", lname, rv);
+
+		len += i;
+		lname += i;
+	} while (len < llanes);
+
+	if (lanes != NULL)
+		free(lanes, M_OFWPROP);
+	if (cfg.function != NULL)
+		free(cfg.function, M_OFWPROP);
+	return (rv);
+}
+
+
+static int
+xusbpadctl_pinctrl_cfg(device_t dev, phandle_t cfgxref)
+{
+	struct xusbpadctl_softc *sc;
+	phandle_t node, cfgnode;
+	int rv;
+
+	sc = device_get_softc(dev);
+	cfgnode = OF_node_from_xref(cfgxref);
+
+	rv = 0;
+	for (node = OF_child(cfgnode); node != 0; node = OF_peer(node)) {
+		if (!fdt_is_enabled(node))
+			continue;
+		rv = xusbpadctl_process_node(sc, node);
+		if (rv != 0)
+			return (rv);
+	}
+
+	return (rv);
+}
+
+static int
+xusbpadctl_phy_pcie_powerup(struct xusbpadctl_softc *sc)
+{
+	uint32_t reg;
+	int i;
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL1);
+	reg &= ~IOPHY_PLL_P0_CTL1_REFCLK_SEL_MASK;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL1, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL2);
+	reg |= IOPHY_PLL_P0_CTL2_REFCLKBUF_EN;
+	reg |= IOPHY_PLL_P0_CTL2_TXCLKREF_EN;
+	reg |= IOPHY_PLL_P0_CTL2_TXCLKREF_SEL;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL2, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL1);
+	reg |= IOPHY_PLL_P0_CTL1_PLL_RST;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL1, reg);
+	DELAY(100);
+
+	for (i = 0; i < 100; i++) {
+		reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL1);
+		if (reg & IOPHY_PLL_P0_CTL1_PLL0_LOCKDET)
+			return (0);
+		DELAY(10);
+	}
+
+	return (ETIMEDOUT);
+}
+
+
+static int
+xusbpadctl_phy_pcie_powerdown(struct xusbpadctl_softc *sc)
+{
+	uint32_t reg;
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL1);
+	reg &= ~IOPHY_PLL_P0_CTL1_PLL_RST;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_P0_CTL1, reg);
+	DELAY(100);
+	return (0);
+
+}
+
+static int
+xusbpadctl_phy_sata_powerup(struct xusbpadctl_softc *sc)
+{
+	uint32_t reg;
+	int i;
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL1);
+	reg &= ~IOPHY_MISC_PAD_S0_CTL1_IDDQ_OVRD;
+	reg &= ~IOPHY_MISC_PAD_S0_CTL1_IDDQ;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL1, reg);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1);
+	reg &= ~IOPHY_PLL_S0_CTL1_PLL_PWR_OVRD;
+	reg &= ~IOPHY_PLL_S0_CTL1_PLL_IDDQ;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1, reg);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1);
+	reg |= IOPHY_PLL_S0_CTL1_PLL1_MODE;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1, reg);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1);
+	reg |= IOPHY_PLL_S0_CTL1_PLL_RST_L;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1, reg);
+
+	for (i = 100; i >= 0; i--) {
+		reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1);
+		if (reg & IOPHY_PLL_S0_CTL1_PLL1_LOCKDET)
+			break;
+		DELAY(100);
+	}
+	if (i <= 0) {
+		device_printf(sc->dev, "Failed to power up SATA phy\n");
+		return (ETIMEDOUT);
+	}
+
+	return (0);
+}
+
+static int
+xusbpadctl_phy_sata_powerdown(struct xusbpadctl_softc *sc)
+{
+	uint32_t reg;
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1);
+	reg &= ~IOPHY_PLL_S0_CTL1_PLL_RST_L;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1);
+	reg &= ~IOPHY_PLL_S0_CTL1_PLL1_MODE;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1);
+	reg |= IOPHY_PLL_S0_CTL1_PLL_PWR_OVRD;
+	reg |= IOPHY_PLL_S0_CTL1_PLL_IDDQ;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_PLL_S0_CTL1, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL1);
+	reg |= IOPHY_MISC_PAD_S0_CTL1_IDDQ_OVRD;
+	reg |= IOPHY_MISC_PAD_S0_CTL1_IDDQ;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_IOPHY_MISC_PAD_S0_CTL1, reg);
+	DELAY(100);
+
+	return (0);
+}
+
+static int
+xusbpadctl_phy_powerup(struct xusbpadctl_softc *sc)
+{
+	uint32_t reg;
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM);
+	reg &= ~ELPG_PROGRAM_AUX_MUX_LP0_CLAMP_EN;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM);
+	reg &= ~ELPG_PROGRAM_AUX_MUX_LP0_CLAMP_EN_EARLY;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM);
+	reg &= ~ELPG_PROGRAM_AUX_MUX_LP0_VCORE_DOWN;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM, reg);
+	DELAY(100);
+
+	return (0);
+}
+
+static int
+xusbpadctl_phy_powerdown(struct xusbpadctl_softc *sc)
+{
+	uint32_t reg;
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM);
+	reg |= ELPG_PROGRAM_AUX_MUX_LP0_VCORE_DOWN;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM);
+	reg |= ELPG_PROGRAM_AUX_MUX_LP0_CLAMP_EN_EARLY;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM, reg);
+	DELAY(100);
+
+	reg = bus_read_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM);
+	reg |= ELPG_PROGRAM_AUX_MUX_LP0_CLAMP_EN;
+	bus_write_4(sc->mem_res, XUSB_PADCTL_ELPG_PROGRAM, reg);
+	DELAY(100);
+
+	return (0);
+}
+
+static int
+xusbpadctl_phy_enable(device_t dev, intptr_t id, bool enable)
+{
+	struct xusbpadctl_softc *sc;
+	int rv;
+
+	sc = device_get_softc(dev);
+
+	if ((id != TEGRA_XUSB_PADCTL_PCIE) &&
+	    (id != TEGRA_XUSB_PADCTL_SATA)) {
+		device_printf(dev, "Unknown phy: %d\n", id);
+		return (ENXIO);
+	}
+
+	rv = 0;
+	if (enable) {
+		if (sc->phy_ena_cnt == 0) {
+			rv = xusbpadctl_phy_powerup(sc);
+			if (rv != 0)
+				return (rv);
+		}
+		sc->phy_ena_cnt++;
+	}
+
+	if (id == TEGRA_XUSB_PADCTL_PCIE) {
+		if (enable)
+			rv = xusbpadctl_phy_pcie_powerup(sc);
+		else
+			rv = xusbpadctl_phy_pcie_powerdown(sc);
+		if (rv != 0)
+			return (rv);
+	} else if (id == TEGRA_XUSB_PADCTL_SATA) {
+		if (enable)
+			rv = xusbpadctl_phy_sata_powerup(sc);
+		else
+			rv = xusbpadctl_phy_sata_powerdown(sc);
+		if (rv != 0)
+			return (rv);
+	}
+	if (!enable) {
+		 if (sc->phy_ena_cnt == 1) {
+			rv = xusbpadctl_phy_powerdown(sc);
+			if (rv != 0)
+				return (rv);
+		}
+		sc->phy_ena_cnt--;
+	}
+
+	return (0);
+}
+
+static int
+xusbpadctl_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_search_compatible(dev, compat_data)->ocd_data)
+		return (ENXIO);
+
+	device_set_desc(dev, "Tegra XUSB phy");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+xusbpadctl_detach(device_t dev)
+{
+
+	/* This device is always present. */
+	return (EBUSY);
+}
+
+static int
+xusbpadctl_attach(device_t dev)
+{
+	struct xusbpadctl_softc * sc;
+	int rid, rv;
+	phandle_t node;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	node = ofw_bus_get_node(dev);
+	rv = hwreset_get_by_ofw_name(dev, "padctl", &sc->rst);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get 'padctl' reset: %d\n", rv);
+		return (rv);
+	}
+	rv = hwreset_deassert(sc->rst);
+	if (rv != 0) {
+		device_printf(dev, "Cannot unreset 'padctl' reset: %d\n", rv);
+		return (rv);
+	}
+
+	/* Register as a pinctrl device and use default configuration */
+	fdt_pinctrl_register(dev, NULL);
+	fdt_pinctrl_configure_by_name(dev, "default");
+	phy_register_provider(dev);
+
+	return (0);
+}
+
+
+static device_method_t tegra_xusbpadctl_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,         xusbpadctl_probe),
+	DEVMETHOD(device_attach,        xusbpadctl_attach),
+	DEVMETHOD(device_detach,        xusbpadctl_detach),
+
+	/* fdt_pinctrl interface */
+	DEVMETHOD(fdt_pinctrl_configure, xusbpadctl_pinctrl_cfg),
+
+	/* phy interface */
+	DEVMETHOD(phy_enable,		xusbpadctl_phy_enable),
+
+	DEVMETHOD_END
+};
+
+static driver_t tegra_xusbpadctl_driver = {
+	"tegra_xusbpadctl",
+	tegra_xusbpadctl_methods,
+	sizeof(struct xusbpadctl_softc),
+};
+
+static devclass_t tegra_xusbpadctl_devclass;
+
+EARLY_DRIVER_MODULE(tegra_xusbpadctl, simplebus, tegra_xusbpadctl_driver,
+    tegra_xusbpadctl_devclass, 0, 0, 73);
diff --git a/sys/arm/nvidia/tegra_abpmisc.c b/sys/arm/nvidia/tegra_abpmisc.c
new file mode 100644
index 0000000..83ee44f
--- /dev/null
+++ b/sys/arm/nvidia/tegra_abpmisc.c
@@ -0,0 +1,194 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * SoC misc configuration and indentification driver.
+ */
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/clock.h>
+#include <sys/kernel.h>
+#include <sys/limits.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/module.h>
+#include <sys/resource.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+#include <sys/rman.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <arm/nvidia/tegra_efuse.h>
+
+#define	PMC_STRAPPING_OPT_A	0  	/* 0x464 */
+
+#define	PMC_STRAPPING_OPT_A_RAM_CODE_SHIFT	4
+#define	PMC_STRAPPING_OPT_A_RAM_CODE_MASK_LONG	\
+	(0xf << PMC_STRAPPING_OPT_A_RAM_CODE_SHIFT)
+#define	PMC_STRAPPING_OPT_A_RAM_CODE_MASK_SHORT	\
+	(0x3 << PMC_STRAPPING_OPT_A_RAM_CODE_SHIFT)
+
+
+#define	ABP_RD4(_sc, _r)	bus_read_4((_sc)->abp_misc_res, (_r))
+#define	STR_RD4(_sc, _r)	bus_read_4((_sc)->strap_opt_res, (_r))
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-apbmisc",	1},
+	{NULL,				0}
+};
+
+struct tegra_abpmisc_softc {
+	device_t		dev;
+
+	struct resource		*abp_misc_res;
+	struct resource		*strap_opt_res;
+};
+
+static struct tegra_abpmisc_softc *dev_sc;
+
+static void
+tegra_abpmisc_read_revision(struct tegra_abpmisc_softc *sc)
+{
+	uint32_t id, chip_id, minor_rev;
+	int rev;
+
+	id = ABP_RD4(sc, 4);
+	chip_id = (id >> 8) & 0xff;
+	minor_rev = (id >> 16) & 0xf;
+
+	switch (minor_rev) {
+	case 1:
+		rev = TEGRA_REVISION_A01;
+		break;
+	case 2:
+		rev = TEGRA_REVISION_A02;
+		break;
+	case 3:
+		rev = TEGRA_REVISION_A03;
+		break;
+	case 4:
+		rev = TEGRA_REVISION_A04;
+		break;
+	default:
+		rev = TEGRA_REVISION_UNKNOWN;
+	}
+
+	tegra_sku_info.chip_id = chip_id;
+	tegra_sku_info.revision = rev;
+}
+
+static int
+tegra_abpmisc_probe(device_t dev)
+{
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+tegra_abpmisc_attach(device_t dev)
+{
+	int rid;
+	struct tegra_abpmisc_softc *sc;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	rid = 0;
+	sc->abp_misc_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE | RF_SHAREABLE);
+	if (sc->abp_misc_res == NULL) {
+		device_printf(dev, "Cannot map ABP misc registers.\n");
+		goto fail;
+	}
+
+	rid = 1;
+	sc->strap_opt_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->strap_opt_res == NULL) {
+		device_printf(dev, "Cannot map strapping options registers.\n");
+		goto fail;
+	}
+
+	tegra_abpmisc_read_revision(sc);
+
+	/* XXX - Hack - address collision with pinmux. */
+	if (sc->abp_misc_res != NULL) {
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->abp_misc_res);
+		sc->abp_misc_res = NULL;
+	}
+
+	dev_sc = sc;
+	return (bus_generic_attach(dev));
+
+fail:
+	if (sc->abp_misc_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->abp_misc_res);
+	if (sc->strap_opt_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 1, sc->strap_opt_res);
+
+	return (ENXIO);
+}
+
+static int
+tegra_abpmisc_detach(device_t dev)
+{
+	struct tegra_abpmisc_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (sc->abp_misc_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->abp_misc_res);
+	if (sc->strap_opt_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 1, sc->strap_opt_res);
+	return (bus_generic_detach(dev));
+}
+
+static device_method_t tegra_abpmisc_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		tegra_abpmisc_probe),
+	DEVMETHOD(device_attach,	tegra_abpmisc_attach),
+	DEVMETHOD(device_detach,	tegra_abpmisc_detach),
+
+	DEVMETHOD_END
+};
+
+DEFINE_CLASS_0(tegra_abpmisc, tegra_abpmisc_driver, tegra_abpmisc_methods,
+    sizeof(struct tegra_abpmisc_softc));
+static devclass_t tegra_abpmisc_devclass;
+EARLY_DRIVER_MODULE(tegra_abpmisc, simplebus, tegra_abpmisc_driver,
+    tegra_abpmisc_devclass, 0, 0, BUS_PASS_TIMER);
diff --git a/sys/arm/nvidia/tegra_ahci.c b/sys/arm/nvidia/tegra_ahci.c
new file mode 100644
index 0000000..4afba17
--- /dev/null
+++ b/sys/arm/nvidia/tegra_ahci.c
@@ -0,0 +1,627 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * AHCI driver for Tegra SoCs.
+ */
+#include <sys/param.h>
+#include <sys/module.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/endian.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/mutex.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+
+#include <dev/ahci/ahci.h>
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/extres/phy/phy.h>
+#include <dev/extres/regulator/regulator.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/fdt/fdt_pinctrl.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <arm/nvidia/tegra_efuse.h>
+#include <arm/nvidia/tegra_pmc.h>
+
+#define	AHCI_WR4(_sc, _r, _v)	bus_write_4((_sc)->ctlr.r_mem, (_r), (_v))
+#define	AHCI_RD4(_sc, _r)	bus_read_4((_sc)->ctlr.r_mem, (_r))
+#define	SATA_WR4(_sc, _r, _v)	bus_write_4((_sc)->sata_mem, (_r), (_v))
+#define	SATA_RD4(_sc, _r)	bus_read_4((_sc)->sata_mem, (_r))
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-ahci", 	1},
+	{NULL,			0}
+};
+
+struct tegra_ahci_sc {
+	struct ahci_controller	ctlr;	/* Must be first */
+	device_t		dev;
+	struct resource		*sata_mem;
+	clk_t			clk_sata;
+	clk_t			clk_sata_oob;
+	clk_t			clk_pll_e;
+	clk_t			clk_cml;
+	hwreset_t		hwreset_sata;
+	hwreset_t		hwreset_sata_oob;
+	hwreset_t		hwreset_sata_cold;
+	regulator_t		supply_hvdd;
+	regulator_t		supply_vddio;
+	regulator_t		supply_avdd;
+	regulator_t		supply_target_5v;
+	regulator_t		supply_target_12v;
+	phy_t			phy;
+};
+
+struct sata_pad_calibration {
+	uint32_t gen1_tx_amp;
+	uint32_t gen1_tx_peak;
+	uint32_t gen2_tx_amp;
+	uint32_t gen2_tx_peak;
+};
+
+static const struct sata_pad_calibration tegra124_pad_calibration[] = {
+	{0x18, 0x04, 0x18, 0x0a},
+	{0x0e, 0x04, 0x14, 0x0a},
+	{0x0e, 0x07, 0x1a, 0x0e},
+	{0x14, 0x0e, 0x1a, 0x0e},
+};
+
+#define	SATA_CONFIGURATION			0x180
+#define	 SATA_CONFIGURATION_EN_FPCI			(1 << 0)
+
+#define	SATA_FPCI_BAR5				0x94
+#define	 SATA_FPCI_BAR5_START_SHIFT			4
+
+#define	SATA_INTR_MASK				0x188
+#define	SATA_INTR_MASK_IP_INT_MASK			(1 << 16)
+
+#define	SCFG_OFFSET				0x1000
+
+#define	T_SATA0_CFG_1				0x04
+#define	 T_SATA0_CFG_1_IO_SPACE				(1 << 0)
+#define	 T_SATA0_CFG_1_MEMORY_SPACE			(1 << 1)
+#define	 T_SATA0_CFG_1_BUS_MASTER			(1 << 2)
+#define	 T_SATA0_CFG_1_SERR				(1 << 8)
+
+#define	T_SATA0_CFG_9				0x24
+#define	 T_SATA0_CFG_9_BASE_ADDRESS_SHIFT		13
+
+#define	T_SATA0_AHCI_HBA_CAP_BKDR		0x300
+#define	T_SATA0_BKDOOR_CC			0x4a4
+#define	T_SATA0_CFG_SATA			0x54c
+#define	 T_SATA0_CFG_SATA_BACKDOOR_PROG_IF_EN		(1 << 12)
+
+#define	T_SATA0_CFG_MISC			0x550
+#define	T_SATA0_INDEX				0x680
+
+#define	T_SATA0_CHX_PHY_CTRL1_GEN1		0x690
+#define	 T_SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_MASK	0xff
+#define	 T_SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_SHIFT	8
+#define	 T_SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_MASK		0xff
+#define	 T_SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_SHIFT	0
+
+
+#define	T_SATA0_CHX_PHY_CTRL1_GEN2		0x694
+#define	 T_SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_MASK	0xff
+#define	 T_SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_SHIFT	12
+#define	 T_SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_MASK		0xff
+#define	 T_SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_SHIFT	0
+
+#define	T_SATA0_CHX_PHY_CTRL2			0x69c
+#define	 T_SATA0_CHX_PHY_CTRL2_CDR_CNTL_GEN1		0x23
+
+#define	T_SATA0_CHX_PHY_CTRL11			0x6d0
+#define	 T_SATA0_CHX_PHY_CTRL11_GEN2_RX_EQ		(0x2800 << 16)
+
+#define	FUSE_SATA_CALIB				0x124
+#define	FUSE_SATA_CALIB_MASK			0x3
+
+
+#define	SATA_AUX_MISC_CNTL			0x1108
+#define	SATA_AUX_PAD_PLL_CTRL_0			0x1120
+#define	SATA_AUX_PAD_PLL_CTRL_1			0x1124
+#define	SATA_AUX_PAD_PLL_CTRL_2			0x1128
+#define	SATA_AUX_PAD_PLL_CTRL_3			0x112c
+
+#define	T_AHCI_HBA_CCC_PORTS			0x0018
+#define	T_AHCI_HBA_CAP_BKDR			0x00A0
+#define	 T_AHCI_HBA_CAP_BKDR_S64A			(1 << 31)
+#define	 T_AHCI_HBA_CAP_BKDR_SNCQ			(1 << 30)
+#define	 T_AHCI_HBA_CAP_BKDR_SSNTF			(1 << 29)
+#define	 T_AHCI_HBA_CAP_BKDR_SMPS			(1 << 28)
+#define	 T_AHCI_HBA_CAP_BKDR_SUPP_STG_SPUP		(1 << 27)
+#define	 T_AHCI_HBA_CAP_BKDR_SALP			(1 << 26)
+#define	 T_AHCI_HBA_CAP_BKDR_SAL			(1 << 25)
+#define	 T_AHCI_HBA_CAP_BKDR_SUPP_CLO			(1 << 24)
+#define	 T_AHCI_HBA_CAP_BKDR_INTF_SPD_SUPP(x)		(((x) & 0xF) << 20)
+#define	 T_AHCI_HBA_CAP_BKDR_SUPP_NONZERO_OFFSET	(1 << 19)
+#define	 T_AHCI_HBA_CAP_BKDR_SUPP_AHCI_ONLY		(1 << 18)
+#define	 T_AHCI_HBA_CAP_BKDR_SUPP_PM			(1 << 17)
+#define	 T_AHCI_HBA_CAP_BKDR_FIS_SWITCHING		(1 << 16)
+#define	 T_AHCI_HBA_CAP_BKDR_PIO_MULT_DRQ_BLK		(1 << 15)
+#define	 T_AHCI_HBA_CAP_BKDR_SLUMBER_ST_CAP		(1 << 14)
+#define	 T_AHCI_HBA_CAP_BKDR_PARTIAL_ST_CAP		(1 << 13)
+#define	 T_AHCI_HBA_CAP_BKDR_NUM_CMD_SLOTS(x)		(((x) & 0x1F) <<  8)
+#define	 T_AHCI_HBA_CAP_BKDR_CMD_CMPL_COALESING		(1 <<  7)
+#define	 T_AHCI_HBA_CAP_BKDR_ENCL_MGMT_SUPP		(1 <<  6)
+#define	 T_AHCI_HBA_CAP_BKDR_EXT_SATA			(1 <<  5)
+#define	 T_AHCI_HBA_CAP_BKDR_NUM_PORTS(x)		(((x) & 0xF) <<  0)
+
+#define	T_AHCI_PORT_BKDR			0x0170
+
+#define	 T_AHCI_PORT_BKDR_PXDEVSLP_DETO_OVERRIDE_VAL(x)	(((x) & 0xFF) << 24)
+#define	 T_AHCI_PORT_BKDR_PXDEVSLP_MDAT_OVERRIDE_VAL(x)	(((x) & 0x1F) << 16)
+#define	 T_AHCI_PORT_BKDR_PXDEVSLP_DETO_OVERRIDE	(1 << 15)
+#define	 T_AHCI_PORT_BKDR_PXDEVSLP_MDAT_OVERRIDE	(1 << 14)
+#define	 T_AHCI_PORT_BKDR_PXDEVSLP_DM(x)		(((x) & 0xF) << 10)
+#define	 T_AHCI_PORT_BKDR_PORT_UNCONNECTED		(1 <<  9)
+#define	 T_AHCI_PORT_BKDR_CLK_CLAMP_CTRL_CLAMP_THIS_CH	(1 <<  8)
+#define	 T_AHCI_PORT_BKDR_CLK_CLAMP_CTRL_TXRXCLK_UNCLAMP (1 <<  7)
+#define	 T_AHCI_PORT_BKDR_CLK_CLAMP_CTRL_TXRXCLK_CLAMP	(1 <<  6)
+#define	 T_AHCI_PORT_BKDR_CLK_CLAMP_CTRL_DEVCLK_UNCLAMP	(1 <<  5)
+#define	 T_AHCI_PORT_BKDR_CLK_CLAMP_CTRL_DEVCLK_CLAMP	(1 <<  4)
+#define	 T_AHCI_PORT_BKDR_HOTPLUG_CAP			(1 <<  3)
+#define	 T_AHCI_PORT_BKDR_MECH_SWITCH			(1 <<  2)
+#define	 T_AHCI_PORT_BKDR_COLD_PRSN_DET			(1 <<  1)
+#define	 T_AHCI_PORT_BKDR_EXT_SATA_SUPP			(1 <<  0)
+
+static int
+get_fdt_resources(struct tegra_ahci_sc *sc, phandle_t node)
+{
+	int rv;
+
+
+	rv = regulator_get_by_ofw_property(sc->dev, "hvdd-supply",
+	    &sc->supply_hvdd );
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'hvdd' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "vddio-supply",
+	    &sc->supply_vddio);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'vddio' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "avdd-supply",
+	    &sc->supply_avdd);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'avdd' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "target-5v-supply",
+	    &sc->supply_target_5v);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'target-5v' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "target-12v-supply",
+	    &sc->supply_target_12v);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'target-12v' regulator\n");
+		return (ENXIO);
+	}
+
+	rv = hwreset_get_by_ofw_name(sc->dev, "sata", &sc->hwreset_sata );
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'sata' reset\n");
+		return (ENXIO);
+	}
+	rv = hwreset_get_by_ofw_name(sc->dev, "sata-oob",
+	    &sc->hwreset_sata_oob);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'sata oob' reset\n");
+		return (ENXIO);
+	}
+	rv = hwreset_get_by_ofw_name(sc->dev, "sata-cold",
+	    &sc->hwreset_sata_cold);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'sata cold' reset\n");
+		return (ENXIO);
+	}
+
+	rv = phy_get_by_ofw_name(sc->dev, "sata-phy", &sc->phy);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'sata' phy\n");
+		return (ENXIO);
+	}
+
+	rv = clk_get_by_ofw_name(sc->dev, "sata", &sc->clk_sata);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'sata' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(sc->dev, "sata-oob", &sc->clk_sata_oob);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'sata oob' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(sc->dev, "cml1", &sc->clk_cml);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'cml1' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(sc->dev, "pll_e", &sc->clk_pll_e);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'pll_e' clock\n");
+		return (ENXIO);
+	}
+	return (0);
+}
+
+static int
+enable_fdt_resources(struct tegra_ahci_sc *sc)
+{
+	int rv;
+
+	rv = regulator_enable(sc->supply_hvdd);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable  'hvdd' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_vddio);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable  'vddio' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_avdd);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable  'avdd' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_target_5v);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable  'target-5v' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_target_12v);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable  'sc->target-12v' regulator\n");
+		return (rv);
+	}
+
+	/* Stop clocks */
+	clk_stop(sc->clk_sata);
+	clk_stop(sc->clk_sata_oob);
+	tegra_powergate_power_off(TEGRA_POWERGATE_SAX);
+
+	rv = hwreset_assert(sc->hwreset_sata);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot assert 'sata' reset\n");
+		return (rv);
+	}
+	rv = hwreset_assert(sc->hwreset_sata_oob);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot assert 'sata oob' reset\n");
+		return (rv);
+	}
+
+	rv = hwreset_assert(sc->hwreset_sata_cold);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot assert 'sata cold' reset\n");
+		return (rv);
+	}
+	rv = tegra_powergate_sequence_power_up(TEGRA_POWERGATE_SAX,
+	    sc->clk_sata, sc->hwreset_sata);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'SAX' powergate\n");
+		return (rv);
+	}
+
+	rv = clk_enable(sc->clk_sata_oob);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'sata oob' clock\n");
+		return (rv);
+	}
+	rv = clk_enable(sc->clk_cml);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'cml' clock\n");
+		return (rv);
+	}
+	rv = clk_enable(sc->clk_pll_e);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'pll e' clock\n");
+		return (rv);
+	}
+
+	rv = hwreset_deassert(sc->hwreset_sata_cold);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot unreset 'sata cold' reset\n");
+		return (rv);
+	}
+	rv = hwreset_deassert(sc->hwreset_sata_oob);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot unreset 'sata oob' reset\n");
+		return (rv);
+	}
+
+	rv = phy_enable(sc->dev, sc->phy);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable SATA phy\n");
+		return (rv);
+	}
+
+	return (0);
+}
+
+static int
+tegra_ahci_ctrl_init(struct tegra_ahci_sc *sc)
+{
+	uint32_t val;
+	const struct sata_pad_calibration *calib;
+
+	val = SATA_RD4(sc, SATA_CONFIGURATION);
+	val |= SATA_CONFIGURATION_EN_FPCI;
+	SATA_WR4(sc, SATA_CONFIGURATION, val);
+
+
+	/* Pad calibration. */
+	val = tegra_fuse_read_4(FUSE_SATA_CALIB);
+	calib = tegra124_pad_calibration + (val & FUSE_SATA_CALIB_MASK);
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_INDEX, 1);
+
+	val = SATA_RD4(sc, SCFG_OFFSET + T_SATA0_CHX_PHY_CTRL1_GEN1);
+	val &= ~(T_SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_MASK <<
+	    T_SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_SHIFT);
+	val &= ~(T_SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_MASK <<
+	    T_SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_SHIFT);
+	val |= calib->gen1_tx_amp << T_SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_SHIFT;
+	val |= calib->gen1_tx_peak << T_SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_SHIFT;
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_CHX_PHY_CTRL1_GEN1, val);
+
+	val = SATA_RD4(sc, SCFG_OFFSET + T_SATA0_CHX_PHY_CTRL1_GEN2);
+	val &= ~(T_SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_MASK <<
+	    T_SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_SHIFT);
+	val &= ~(T_SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_MASK <<
+	    T_SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_SHIFT);
+	val |= calib->gen2_tx_amp << T_SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_SHIFT;
+	val |= calib->gen2_tx_peak << T_SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_SHIFT;
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_CHX_PHY_CTRL1_GEN2, val);
+
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_CHX_PHY_CTRL11,
+	    T_SATA0_CHX_PHY_CTRL11_GEN2_RX_EQ);
+
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_CHX_PHY_CTRL2,
+	    T_SATA0_CHX_PHY_CTRL2_CDR_CNTL_GEN1);
+
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_INDEX, 0);
+
+	/* Set device ID. */
+	val = SATA_RD4(sc, SCFG_OFFSET + T_SATA0_CFG_SATA);
+	val |= T_SATA0_CFG_SATA_BACKDOOR_PROG_IF_EN;
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_CFG_SATA, val);
+
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_BKDOOR_CC, 0x01060100);
+
+	val = SATA_RD4(sc, SCFG_OFFSET + T_SATA0_CFG_SATA);
+	val &= ~T_SATA0_CFG_SATA_BACKDOOR_PROG_IF_EN;
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_CFG_SATA, val);
+
+	/* Enable IO & memory access, bus master mode */
+	val = SATA_RD4(sc, SCFG_OFFSET + T_SATA0_CFG_1);
+	val |= T_SATA0_CFG_1_IO_SPACE;
+	val |= T_SATA0_CFG_1_MEMORY_SPACE;
+	val |= T_SATA0_CFG_1_BUS_MASTER;
+	val |= T_SATA0_CFG_1_SERR;
+	SATA_WR4(sc, SCFG_OFFSET + T_SATA0_CFG_1, val);
+
+	/* SATA MMIO. */
+	SATA_WR4(sc, SATA_FPCI_BAR5, 0x10000 << SATA_FPCI_BAR5_START_SHIFT);
+	/* AHCI bar */
+	SATA_WR4(sc,  SCFG_OFFSET + T_SATA0_CFG_9,
+	    0x08000 << T_SATA0_CFG_9_BASE_ADDRESS_SHIFT);
+
+	/* Unmask  interrupts. */
+	val = SATA_RD4(sc, SATA_INTR_MASK);
+	val |= SATA_INTR_MASK_IP_INT_MASK;
+	SATA_WR4(sc, SATA_INTR_MASK, val);
+
+	return (0);
+}
+
+static int
+tegra_ahci_ctlr_reset(device_t dev)
+{
+	struct tegra_ahci_sc *sc;
+	int rv;
+	uint32_t reg;
+
+	sc = device_get_softc(dev);
+	rv = ahci_ctlr_reset(dev);
+	if (rv != 0)
+		return (0);
+	AHCI_WR4(sc, T_AHCI_HBA_CCC_PORTS, 1);
+
+	/* Overwrite AHCI capabilites. */
+	reg  = AHCI_RD4(sc, T_AHCI_HBA_CAP_BKDR);
+	reg &= ~T_AHCI_HBA_CAP_BKDR_NUM_PORTS(~0);
+	reg |= T_AHCI_HBA_CAP_BKDR_NUM_PORTS(0);
+	reg |= T_AHCI_HBA_CAP_BKDR_EXT_SATA;
+	reg |= T_AHCI_HBA_CAP_BKDR_ENCL_MGMT_SUPP;
+	reg |= T_AHCI_HBA_CAP_BKDR_CMD_CMPL_COALESING;
+	reg |= T_AHCI_HBA_CAP_BKDR_FIS_SWITCHING;
+	reg |= T_AHCI_HBA_CAP_BKDR_SUPP_PM;
+	reg |= T_AHCI_HBA_CAP_BKDR_SUPP_CLO;
+	reg |= T_AHCI_HBA_CAP_BKDR_SUPP_STG_SPUP;
+	AHCI_WR4(sc, T_AHCI_HBA_CAP_BKDR, reg);
+
+	/* Overwrite AHCI portcapabilites. */
+	reg  = AHCI_RD4(sc, T_AHCI_PORT_BKDR);
+	reg |= T_AHCI_PORT_BKDR_COLD_PRSN_DET;
+	reg |= T_AHCI_PORT_BKDR_HOTPLUG_CAP;
+	reg |= T_AHCI_PORT_BKDR_EXT_SATA_SUPP;
+	AHCI_WR4(sc, T_AHCI_PORT_BKDR, reg);
+
+	return (0);
+}
+
+static int
+tegra_ahci_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_search_compatible(dev, compat_data)->ocd_data)
+		return (ENXIO);
+
+	device_set_desc_copy(dev, "AHCI SATA controller");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+tegra_ahci_attach(device_t dev)
+{
+	struct tegra_ahci_sc *sc;
+	struct ahci_controller *ctlr;
+	phandle_t node;
+	int rv, rid;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	ctlr = &sc->ctlr;
+	node = ofw_bus_get_node(dev);
+
+	ctlr->r_rid = 0;
+	ctlr->r_mem = bus_alloc_resource_any(dev, SYS_RES_MEMORY,
+	    &ctlr->r_rid, RF_ACTIVE);
+	if (ctlr->r_mem == NULL)
+		return (ENXIO);
+
+	rid = 1;
+	sc->sata_mem = bus_alloc_resource_any(dev, SYS_RES_MEMORY,
+	    &rid, RF_ACTIVE);
+	if (sc->sata_mem == NULL) {
+		rv = ENXIO;
+		goto fail;
+	}
+	rv = get_fdt_resources(sc, node);
+	if (rv != 0) {
+		device_printf(sc->dev, "Failed to allocate FDT resource(s)\n");
+		goto fail;
+	}
+
+	rv = enable_fdt_resources(sc);
+	if (rv != 0) {
+		device_printf(sc->dev, "Failed to enable FDT resource(s)\n");
+		goto fail;
+	}
+	rv = tegra_ahci_ctrl_init(sc);
+	if (rv != 0) {
+		device_printf(sc->dev, "Failed to initialize controller)\n");
+		goto fail;
+	}
+
+	/* Setup controller defaults. */
+	ctlr->msi = 0;
+	ctlr->numirqs = 1;
+	ctlr->ccc = 0;
+
+	/* Reset controller. */
+	rv = tegra_ahci_ctlr_reset(dev);
+	if (rv != 0)
+		goto fail;
+	rv = ahci_attach(dev);
+	return (rv);
+
+fail:
+	/* XXX FDT  stuff */
+	if (sc->sata_mem != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 1, sc->sata_mem);
+	if (ctlr->r_mem != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, ctlr->r_rid,
+		    ctlr->r_mem);
+	return (rv);
+}
+
+static int
+tegra_ahci_detach(device_t dev)
+{
+
+	ahci_detach(dev);
+	return (0);
+}
+
+static int
+tegra_ahci_suspend(device_t dev)
+{
+	struct tegra_ahci_sc *sc = device_get_softc(dev);
+
+	bus_generic_suspend(dev);
+	/* Disable interupts, so the state change(s) doesn't trigger. */
+	ATA_OUTL(sc->ctlr.r_mem, AHCI_GHC,
+	     ATA_INL(sc->ctlr.r_mem, AHCI_GHC) & (~AHCI_GHC_IE));
+	return (0);
+}
+
+static int
+tegra_ahci_resume(device_t dev)
+{
+	int res;
+
+	if ((res = tegra_ahci_ctlr_reset(dev)) != 0)
+		return (res);
+	ahci_ctlr_setup(dev);
+	return (bus_generic_resume(dev));
+}
+
+devclass_t genahci_devclass;
+static device_method_t genahci_methods[] = {
+	DEVMETHOD(device_probe,		tegra_ahci_probe),
+	DEVMETHOD(device_attach,	tegra_ahci_attach),
+	DEVMETHOD(device_detach,	tegra_ahci_detach),
+	DEVMETHOD(device_suspend,	tegra_ahci_suspend),
+	DEVMETHOD(device_resume,	tegra_ahci_resume),
+	DEVMETHOD(bus_print_child,	ahci_print_child),
+	DEVMETHOD(bus_alloc_resource,	ahci_alloc_resource),
+	DEVMETHOD(bus_release_resource,	ahci_release_resource),
+	DEVMETHOD(bus_setup_intr,	ahci_setup_intr),
+	DEVMETHOD(bus_teardown_intr,	ahci_teardown_intr),
+	DEVMETHOD(bus_child_location_str, ahci_child_location_str),
+	DEVMETHOD(bus_get_dma_tag,	ahci_get_dma_tag),
+
+	DEVMETHOD_END
+};
+static driver_t genahci_driver = {
+	"ahci",
+	genahci_methods,
+	sizeof(struct tegra_ahci_sc)
+};
+DRIVER_MODULE(genahci, simplebus, genahci_driver, genahci_devclass, NULL, NULL);
diff --git a/sys/arm/nvidia/tegra_efuse.c b/sys/arm/nvidia/tegra_efuse.c
new file mode 100644
index 0000000..ae3f9ef
--- /dev/null
+++ b/sys/arm/nvidia/tegra_efuse.c
@@ -0,0 +1,368 @@
+/*-
+ * Copyright (c) 2015 Michal Meloun
+ * 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/clock.h>
+#include <sys/kernel.h>
+#include <sys/limits.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/module.h>
+#include <sys/resource.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <arm/nvidia/tegra_efuse.h>
+
+
+#define	RD4(_sc, _r)	bus_read_4((_sc)->mem_res, (_sc)->fuse_begin + (_r))
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-efuse",	1},
+	{NULL,			0}
+};
+
+struct tegra_efuse_softc {
+	device_t		dev;
+	struct resource		*mem_res;
+
+	int			fuse_begin;
+	clk_t			clk;
+	hwreset_t			reset;
+};
+struct tegra_efuse_softc *dev_sc;
+
+struct tegra_sku_info tegra_sku_info;
+static char *tegra_rev_name[] = {
+	[TEGRA_REVISION_UNKNOWN] = "unknown",
+	[TEGRA_REVISION_A01]     = "A01",
+	[TEGRA_REVISION_A02]     = "A02",
+	[TEGRA_REVISION_A03]     = "A03",
+	[TEGRA_REVISION_A03p]    = "A03 prime",
+	[TEGRA_REVISION_A04]     = "A04",
+};
+
+/* Tegra30 and later */
+#define	FUSE_VENDOR_CODE	0x100
+#define	FUSE_FAB_CODE		0x104
+#define	FUSE_LOT_CODE_0		0x108
+#define	FUSE_LOT_CODE_1		0x10c
+#define	FUSE_WAFER_ID		0x110
+#define	FUSE_X_COORDINATE	0x114
+#define	FUSE_Y_COORDINATE	0x118
+
+/* ---------------------- Tegra 124 specific code & data --------------- */
+#define	TEGRA124_FUSE_BEGIN		0x100
+
+#define	TEGRA124_CPU_PROCESS_CORNERS	2
+#define	TEGRA124_GPU_PROCESS_CORNERS	2
+#define	TEGRA124_SOC_PROCESS_CORNERS	2
+
+#define	TEGRA124_FUSE_SKU_INFO		0x10
+#define	TEGRA124_FUSE_CPU_SPEEDO_0	0x14
+#define	TEGRA124_FUSE_CPU_IDDQ		0x18
+#define	TEGRA124_FUSE_FT_REV		0x28
+#define	TEGRA124_FUSE_CPU_SPEEDO_1	0x2c
+#define	TEGRA124_FUSE_CPU_SPEEDO_2	0x30
+#define	TEGRA124_FUSE_SOC_SPEEDO_0	0x34
+#define	TEGRA124_FUSE_SOC_SPEEDO_1	0x38
+#define	TEGRA124_FUSE_SOC_SPEEDO_2	0x3c
+#define	TEGRA124_FUSE_SOC_IDDQ		0x40
+#define	TEGRA124_FUSE_GPU_IDDQ		0x128
+
+enum {
+	TEGRA124_THRESHOLD_INDEX_0,
+	TEGRA124_THRESHOLD_INDEX_1,
+	TEGRA124_THRESHOLD_INDEX_COUNT,
+};
+
+static uint32_t tegra124_cpu_process_speedos[][TEGRA124_CPU_PROCESS_CORNERS] =
+{
+	{2190,	UINT_MAX},
+	{0,	UINT_MAX},
+};
+
+static uint32_t tegra124_gpu_process_speedos[][TEGRA124_GPU_PROCESS_CORNERS] =
+{
+	{1965,	UINT_MAX},
+	{0,	UINT_MAX},
+};
+
+static uint32_t tegra124_soc_process_speedos[][TEGRA124_SOC_PROCESS_CORNERS] =
+{
+	{2101,	UINT_MAX},
+	{0,	UINT_MAX},
+};
+
+static void
+tegra124_rev_sku_to_speedo_ids(struct tegra_efuse_softc *sc,
+    struct tegra_sku_info *sku, int *threshold)
+{
+
+	/* Assign to default */
+	sku->cpu_speedo_id = 0;
+	sku->soc_speedo_id = 0;
+	sku->gpu_speedo_id = 0;
+	*threshold = TEGRA124_THRESHOLD_INDEX_0;
+
+	switch (sku->sku_id) {
+	case 0x00: /* Eng sku */
+	case 0x0F:
+	case 0x23:
+		/* Using the default */
+		break;
+	case 0x83:
+		sku->cpu_speedo_id = 2;
+		break;
+
+	case 0x1F:
+	case 0x87:
+	case 0x27:
+		sku->cpu_speedo_id = 2;
+		sku->soc_speedo_id = 0;
+		sku->gpu_speedo_id = 1;
+		*threshold = TEGRA124_THRESHOLD_INDEX_0;
+		break;
+	case 0x81:
+	case 0x21:
+	case 0x07:
+		sku->cpu_speedo_id = 1;
+		sku->soc_speedo_id = 1;
+		sku->gpu_speedo_id = 1;
+		*threshold = TEGRA124_THRESHOLD_INDEX_1;
+		break;
+	case 0x49:
+	case 0x4A:
+	case 0x48:
+		sku->cpu_speedo_id = 4;
+		sku->soc_speedo_id = 2;
+		sku->gpu_speedo_id = 3;
+		*threshold = TEGRA124_THRESHOLD_INDEX_1;
+		break;
+	default:
+		device_printf(sc->dev, " Unknown SKU ID %d\n", sku->sku_id);
+		break;
+	}
+}
+
+
+static void
+tegra124_init_speedo(struct tegra_efuse_softc *sc, struct tegra_sku_info *sku)
+{
+	int i, threshold;
+
+	sku->sku_id = RD4(sc, TEGRA124_FUSE_SKU_INFO);
+	sku->soc_iddq_value = RD4(sc, TEGRA124_FUSE_SOC_IDDQ);
+	sku->cpu_iddq_value = RD4(sc, TEGRA124_FUSE_CPU_IDDQ);
+	sku->gpu_iddq_value = RD4(sc, TEGRA124_FUSE_GPU_IDDQ);
+	sku->soc_speedo_value = RD4(sc, TEGRA124_FUSE_SOC_SPEEDO_0);
+	sku->cpu_speedo_value = RD4(sc, TEGRA124_FUSE_CPU_SPEEDO_0);
+	sku->gpu_speedo_value = RD4(sc, TEGRA124_FUSE_CPU_SPEEDO_2);
+
+	if (sku->cpu_speedo_value == 0) {
+		device_printf(sc->dev, "CPU Speedo value is not fused.\n");
+		return;
+	}
+
+	tegra124_rev_sku_to_speedo_ids(sc, sku, &threshold);
+
+	for (i = 0; i < TEGRA124_SOC_PROCESS_CORNERS; i++) {
+		if (sku->soc_speedo_value <
+			tegra124_soc_process_speedos[threshold][i])
+			break;
+	}
+	sku->soc_process_id = i;
+
+	for (i = 0; i < TEGRA124_CPU_PROCESS_CORNERS; i++) {
+		if (sku->cpu_speedo_value <
+			tegra124_cpu_process_speedos[threshold][i])
+				break;
+	}
+	sku->cpu_process_id = i;
+
+	for (i = 0; i < TEGRA124_GPU_PROCESS_CORNERS; i++) {
+		if (sku->gpu_speedo_value <
+			tegra124_gpu_process_speedos[threshold][i])
+			break;
+	}
+	sku->gpu_process_id = i;
+
+}
+
+/* ----------------- End of Tegra 124 specific code & data --------------- */
+
+uint32_t
+tegra_fuse_read_4(int addr) {
+
+	if (dev_sc == NULL)
+		panic("tegra_fuse_read_4 called too early");
+	return (RD4(dev_sc, addr));
+}
+
+
+static void
+tegra_efuse_dump_sku()
+{
+	printf(" TEGRA SKU Info:\n");
+	printf("  chip_id: %u\n", tegra_sku_info.chip_id);
+	printf("  sku_id: %u\n", tegra_sku_info.sku_id);
+	printf("  cpu_process_id: %u\n", tegra_sku_info.cpu_process_id);
+	printf("  cpu_speedo_id: %u\n", tegra_sku_info.cpu_speedo_id);
+	printf("  cpu_speedo_value: %u\n", tegra_sku_info.cpu_speedo_value);
+	printf("  cpu_iddq_value: %u\n", tegra_sku_info.cpu_iddq_value);
+	printf("  soc_process_id: %u\n", tegra_sku_info.soc_process_id);
+	printf("  soc_speedo_id: %u\n", tegra_sku_info.soc_speedo_id);
+	printf("  soc_speedo_value: %u\n", tegra_sku_info.soc_speedo_value);
+	printf("  soc_iddq_value: %u\n", tegra_sku_info.soc_iddq_value);
+	printf("  gpu_process_id: %u\n", tegra_sku_info.gpu_process_id);
+	printf("  gpu_speedo_id: %u\n", tegra_sku_info.gpu_speedo_id);
+	printf("  gpu_speedo_value: %u\n", tegra_sku_info.gpu_speedo_value);
+	printf("  gpu_iddq_value: %u\n", tegra_sku_info.gpu_iddq_value);
+	printf("  revision: %s\n", tegra_rev_name[tegra_sku_info.revision]);
+}
+
+static int
+tegra_efuse_probe(device_t dev)
+{
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+tegra_efuse_attach(device_t dev)
+{
+	int rv, rid;
+	phandle_t node;
+	struct tegra_efuse_softc *sc;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	node = ofw_bus_get_node(dev);
+
+	/* Get the memory resource for the register mapping. */
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot map registers.\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	/* OFW resources. */
+	rv = clk_get_by_ofw_name(dev, "fuse", &sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get fuse clock: %d\n", rv);
+		goto fail;
+	}
+	rv = clk_enable(sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable clock: %d\n", rv);
+		goto fail;
+	}
+	rv = hwreset_get_by_ofw_name(sc->dev, "fuse", &sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get fuse reset\n");
+		goto fail;
+	}
+	rv = hwreset_deassert(sc->reset);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot clear reset\n");
+		goto fail;
+	}
+
+	/* Tegra124 specific init. */
+	sc->fuse_begin = TEGRA124_FUSE_BEGIN;
+	tegra124_init_speedo(sc, &tegra_sku_info);
+
+	dev_sc = sc;
+
+	if (bootverbose)
+		tegra_efuse_dump_sku();
+	return (bus_generic_attach(dev));
+
+fail:
+	dev_sc = NULL;
+	if (sc->clk != NULL)
+		clk_release(sc->clk);
+	if (sc->reset != NULL)
+		hwreset_release(sc->reset);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+
+	return (rv);
+}
+
+static int
+tegra_efuse_detach(device_t dev)
+{
+	struct tegra_efuse_softc *sc;
+
+	sc = device_get_softc(dev);
+	dev_sc = NULL;
+	if (sc->clk != NULL)
+		clk_release(sc->clk);
+	if (sc->reset != NULL)
+		hwreset_release(sc->reset);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+
+	return (bus_generic_detach(dev));
+}
+
+static device_method_t tegra_efuse_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		tegra_efuse_probe),
+	DEVMETHOD(device_attach,	tegra_efuse_attach),
+	DEVMETHOD(device_detach,	tegra_efuse_detach),
+
+
+	DEVMETHOD_END
+};
+
+DEFINE_CLASS_0(tegra_efuse, tegra_efuse_driver, tegra_efuse_methods,
+    sizeof(struct tegra_efuse_softc));
+static devclass_t tegra_efuse_devclass;
+EARLY_DRIVER_MODULE(tegra_efuse, simplebus, tegra_efuse_driver,
+    tegra_efuse_devclass, 0, 0, BUS_PASS_TIMER);
diff --git a/sys/arm/nvidia/tegra_efuse.h b/sys/arm/nvidia/tegra_efuse.h
new file mode 100644
index 0000000..36804d06
--- /dev/null
+++ b/sys/arm/nvidia/tegra_efuse.h
@@ -0,0 +1,61 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _TEGRA_EFUSE_H_
+
+enum tegra_revision {
+	TEGRA_REVISION_UNKNOWN = 0,
+	TEGRA_REVISION_A01,
+	TEGRA_REVISION_A02,
+	TEGRA_REVISION_A03,
+	TEGRA_REVISION_A03p,
+	TEGRA_REVISION_A04,
+};
+
+struct tegra_sku_info {
+	u_int chip_id;
+	u_int sku_id;
+	u_int cpu_process_id;
+	u_int cpu_speedo_id;
+	u_int cpu_speedo_value;
+	u_int cpu_iddq_value;
+	u_int soc_process_id;
+	u_int soc_speedo_id;
+	u_int soc_speedo_value;
+	u_int soc_iddq_value;
+	u_int gpu_process_id;
+	u_int gpu_speedo_id;
+	u_int gpu_speedo_value;
+	u_int gpu_iddq_value;
+	enum tegra_revision revision;
+};
+
+extern struct tegra_sku_info tegra_sku_info;
+uint32_t tegra_fuse_read_4(int addr);
+
+#endif /* _TEGRA_EFUSE_H_ */
diff --git a/sys/arm/nvidia/tegra_ehci.c b/sys/arm/nvidia/tegra_ehci.c
new file mode 100644
index 0000000..e7f7b22
--- /dev/null
+++ b/sys/arm/nvidia/tegra_ehci.c
@@ -0,0 +1,322 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * EHCI driver for Tegra SoCs.
+ */
+#include "opt_bus.h"
+#include "opt_platform.h"
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/bus.h>
+#include <sys/condvar.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/extres/phy/phy.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+#include <dev/usb/usb.h>
+#include <dev/usb/usbdi.h>
+#include <dev/usb/usb_busdma.h>
+#include <dev/usb/usb_process.h>
+#include <dev/usb/usb_controller.h>
+#include <dev/usb/usb_bus.h>
+#include <dev/usb/controller/ehci.h>
+#include <dev/usb/controller/ehcireg.h>
+
+#include "usbdevs.h"
+
+#define	TEGRA_EHCI_REG_OFF	0x100
+#define	TEGRA_EHCI_REG_SIZE	0x100
+
+/* Compatible devices. */
+#define	TEGRA124_EHCI		1
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-ehci",	(uintptr_t)TEGRA124_EHCI},
+	{NULL,		 	0},
+};
+
+struct tegra_ehci_softc {
+	ehci_softc_t	ehci_softc;
+	device_t	dev;
+	struct resource	*ehci_mem_res;	/* EHCI core regs. */
+	struct resource	*ehci_irq_res;	/* EHCI core IRQ. */
+	int		usb_alloc_called;
+	clk_t		clk;
+	phy_t 		phy;
+	hwreset_t 	reset;
+};
+
+static void
+tegra_ehci_post_reset(struct ehci_softc *ehci_softc)
+{
+	uint32_t usbmode;
+
+	/* Force HOST mode. */
+	usbmode = EOREAD4(ehci_softc, EHCI_USBMODE_LPM);
+	usbmode &= ~EHCI_UM_CM;
+	usbmode |= EHCI_UM_CM_HOST;
+	device_printf(ehci_softc->sc_bus.bdev, "set host controller mode\n");
+	EOWRITE4(ehci_softc, EHCI_USBMODE_LPM, usbmode);
+}
+
+static int
+tegra_ehci_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data != 0) {
+		device_set_desc(dev, "Nvidia Tegra EHCI controller");
+		return (BUS_PROBE_DEFAULT);
+	}
+	return (ENXIO);
+}
+
+static int
+tegra_ehci_detach(device_t dev)
+{
+	struct tegra_ehci_softc *sc;
+	ehci_softc_t *esc;
+
+	sc = device_get_softc(dev);
+
+	esc = &sc->ehci_softc;
+	if (sc->clk != NULL)
+		clk_release(sc->clk);
+	if (esc->sc_bus.bdev != NULL)
+		device_delete_child(dev, esc->sc_bus.bdev);
+	if (esc->sc_flags & EHCI_SCFLG_DONEINIT)
+		ehci_detach(esc);
+	if (esc->sc_intr_hdl != NULL)
+		bus_teardown_intr(dev, esc->sc_irq_res,
+		    esc->sc_intr_hdl);
+	if (sc->ehci_irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0,
+		    sc->ehci_irq_res);
+	if (sc->ehci_mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0,
+		    sc->ehci_mem_res);
+	if (sc->usb_alloc_called)
+		usb_bus_mem_free_all(&esc->sc_bus, &ehci_iterate_hw_softc);
+
+	/* During module unload there are lots of children leftover. */
+	device_delete_children(dev);
+
+	return (0);
+}
+
+static int
+tegra_ehci_attach(device_t dev)
+{
+	struct tegra_ehci_softc *sc;
+	ehci_softc_t *esc;
+	int rv, rid;
+	uint64_t freq;
+	phandle_t node;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	node = ofw_bus_get_node(dev);
+	esc = &sc->ehci_softc;
+
+	/* Allocate resources. */
+	rid = 0;
+	sc->ehci_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE | RF_SHAREABLE);
+	if (sc->ehci_mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	rid = 0;
+	sc->ehci_irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	    RF_ACTIVE);
+	if (sc->ehci_irq_res == NULL) {
+		device_printf(dev, "Cannot allocate IRQ resources\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	rv = hwreset_get_by_ofw_name(dev, "usb", &sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get reset\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	rv = phy_get_by_ofw_property(sc->dev, "nvidia,phy", &sc->phy);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'nvidia,phy' phy\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	rv = clk_get_by_ofw_index(sc->dev, 0, &sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get clock\n");
+		goto out;
+	}
+
+	rv = clk_enable(sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable clock\n");
+		goto out;
+	}
+
+	freq = 0;
+	rv = clk_get_freq(sc->clk, &freq);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get clock frequency\n");
+		goto out;
+	}
+
+	rv = hwreset_deassert(sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot clear reset: %d\n", rv);
+		rv = ENXIO;
+		goto out;
+	}
+
+	rv = phy_enable(sc->dev, sc->phy);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable phy: %d\n", rv);
+		goto out;
+	}
+
+	/* Fill data for EHCI driver. */
+	esc->sc_vendor_get_port_speed = ehci_get_port_speed_hostc;
+	esc->sc_vendor_post_reset = tegra_ehci_post_reset;
+	esc->sc_io_tag = rman_get_bustag(sc->ehci_mem_res);
+	esc->sc_bus.parent = dev;
+	esc->sc_bus.devices = esc->sc_devices;
+	esc->sc_bus.devices_max = EHCI_MAX_DEVICES;
+	esc->sc_bus.dma_bits = 32;
+
+	/* Allocate all DMA memory. */
+	rv = usb_bus_mem_alloc_all(&esc->sc_bus, USB_GET_DMA_TAG(dev),
+	    &ehci_iterate_hw_softc);
+	sc->usb_alloc_called = 1;
+	if (rv != 0) {
+		device_printf(dev, "usb_bus_mem_alloc_all() failed\n");
+		rv = ENOMEM;
+		goto out;
+	}
+
+	/*
+	 * Set handle to USB related registers subregion used by
+	 * generic EHCI driver.
+	 */
+	rv = bus_space_subregion(esc->sc_io_tag,
+	    rman_get_bushandle(sc->ehci_mem_res),
+	    TEGRA_EHCI_REG_OFF, TEGRA_EHCI_REG_SIZE, &esc->sc_io_hdl);
+	if (rv != 0) {
+		device_printf(dev, "Could not create USB memory subregion\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	/* Setup interrupt handler. */
+	rv = bus_setup_intr(dev, sc->ehci_irq_res, INTR_TYPE_BIO, NULL,
+	    (driver_intr_t *)ehci_interrupt, esc, &esc->sc_intr_hdl);
+	if (rv != 0) {
+		device_printf(dev, "Could not setup IRQ\n");
+		goto out;
+	}
+
+	/* Add USB bus device. */
+	esc->sc_bus.bdev = device_add_child(dev, "usbus", -1);
+	if (esc->sc_bus.bdev == NULL) {
+		device_printf(dev, "Could not add USB device\n");
+		goto out;
+	}
+	device_set_ivars(esc->sc_bus.bdev, &esc->sc_bus);
+
+	esc->sc_id_vendor = USB_VENDOR_FREESCALE;
+	strlcpy(esc->sc_vendor, "Nvidia", sizeof(esc->sc_vendor));
+
+	/* Set flags that affect ehci_init() behavior. */
+	esc->sc_flags |= EHCI_SCFLG_TT;
+	esc->sc_flags |= EHCI_SCFLG_NORESTERM;
+	rv = ehci_init(esc);
+	if (rv != 0) {
+		device_printf(dev, "USB init failed: %d\n",
+		    rv);
+		goto out;
+	}
+	esc->sc_flags |= EHCI_SCFLG_DONEINIT;
+
+	/* Probe the bus. */
+	rv = device_probe_and_attach(esc->sc_bus.bdev);
+	if (rv != 0) {
+		device_printf(dev,
+		    "device_probe_and_attach() failed\n");
+		goto out;
+	}
+	return (0);
+
+out:
+	tegra_ehci_detach(dev);
+	return (rv);
+}
+
+static device_method_t ehci_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe, tegra_ehci_probe),
+	DEVMETHOD(device_attach, tegra_ehci_attach),
+	DEVMETHOD(device_detach, tegra_ehci_detach),
+	DEVMETHOD(device_suspend, bus_generic_suspend),
+	DEVMETHOD(device_resume, bus_generic_resume),
+	DEVMETHOD(device_shutdown, bus_generic_shutdown),
+
+	/* Bus interface */
+	DEVMETHOD(bus_print_child, bus_generic_print_child),
+
+	DEVMETHOD_END
+};
+
+static driver_t ehci_driver = {
+	"ehci",
+	ehci_methods,
+	sizeof(struct tegra_ehci_softc)
+};
+
+static devclass_t ehci_devclass;
+DRIVER_MODULE(ehci, simplebus, ehci_driver, ehci_devclass, 0, 0);
+MODULE_DEPEND(ehci, usb, 1, 1, 1);
\ No newline at end of file
diff --git a/sys/arm/nvidia/tegra_gpio.c b/sys/arm/nvidia/tegra_gpio.c
new file mode 100644
index 0000000..f9a1d4d
--- /dev/null
+++ b/sys/arm/nvidia/tegra_gpio.c
@@ -0,0 +1,480 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * Tegra GPIO driver.
+ */
+#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/rman.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/gpio.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/gpio/gpiobusvar.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+
+#define	GPIO_LOCK(_sc)		mtx_lock(&(_sc)->sc_mtx)
+#define	GPIO_UNLOCK(_sc)	mtx_unlock(&(_sc)->sc_mtx)
+#define	GPIO_LOCK_INIT(_sc)	mtx_init(&_sc->sc_mtx, 			\
+	    device_get_nameunit(_sc->sc_dev), "tegra_gpio", MTX_DEF)
+#define	GPIO_LOCK_DESTROY(_sc)	mtx_destroy(&_sc->sc_mtx);
+#define	GPIO_ASSERT_LOCKED(_sc)	mtx_assert(&_sc->sc_mtx, MA_OWNED);
+#define	GPIO_ASSERT_UNLOCKED(_sc) mtx_assert(&_sc->sc_mtx, MA_NOTOWNED);
+
+#define	WR4(_sc, _r, _v)	bus_write_4((_sc)->mem_res, (_r), (_v))
+#define	RD4(_sc, _r)		bus_read_4((_sc)->mem_res, (_r))
+
+#define	GPIO_BANK_OFFS		0x100	/* Bank offset */
+#define	GPIO_NUM_BANKS		8	/* Total number per bank */
+#define	GPIO_REGS_IN_BANK	4	/* Total registers in bank */
+#define	GPIO_PINS_IN_REG	8	/* Total pin in register */
+
+#define	GPIO_BANKNUM(n)		((n) / (GPIO_REGS_IN_BANK * GPIO_PINS_IN_REG))
+#define	GPIO_PORTNUM(n)		(((n) / GPIO_PINS_IN_REG) % GPIO_REGS_IN_BANK)
+#define	GPIO_BIT(n)		((n) % GPIO_PINS_IN_REG)
+
+#define	GPIO_REGNUM(n)		(GPIO_BANKNUM(n) * GPIO_BANK_OFFS + \
+				    GPIO_PORTNUM(n) * 4)
+
+#define	NGPIO	((GPIO_NUM_BANKS * GPIO_REGS_IN_BANK * GPIO_PINS_IN_REG) - 8)
+
+/* Register offsets */
+#define	GPIO_CNF		0x00
+#define	GPIO_OE			0x10
+#define	GPIO_OUT		0x20
+#define	GPIO_IN			0x30
+#define	GPIO_INT_STA		0x40
+#define	GPIO_INT_ENB		0x50
+#define	GPIO_INT_LVL		0x60
+#define	GPIO_INT_CLR		0x70
+#define	GPIO_MSK_CNF		0x80
+#define	GPIO_MSK_OE		0x90
+#define	GPIO_MSK_OUT		0xA0
+#define	GPIO_MSK_INT_STA	0xC0
+#define	GPIO_MSK_INT_ENB	0xD0
+#define	GPIO_MSK_INT_LVL	0xE0
+
+char *tegra_gpio_port_names[] = {
+	 "A",  "B",  "C",  "D", /* Bank 0 */
+	 "E",  "F",  "G",  "H", /* Bank 1 */
+	 "I",  "J",  "K",  "L", /* Bank 2 */
+	 "M",  "N",  "O",  "P", /* Bank 3 */
+	 "Q",  "R",  "S",  "T", /* Bank 4 */
+	 "U",  "V",  "W",  "X", /* Bank 5 */
+	 "Y",  "Z", "AA", "BB", /* Bank 5 */
+	"CC", "DD", "EE"	/* Bank 5 */
+};
+
+struct tegra_gpio_softc {
+	device_t		dev;
+	device_t		sc_busdev;
+	struct mtx		sc_mtx;
+	struct resource		*mem_res;
+	struct resource		*irq_res;
+	void			*gpio_ih;
+	int			gpio_npins;
+	struct gpio_pin		gpio_pins[NGPIO];
+};
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-gpio", 1},
+	{NULL,			0}
+};
+
+static inline void
+gpio_write_masked(struct tegra_gpio_softc *sc, bus_size_t reg,
+    struct gpio_pin *pin, uint32_t val)
+{
+	uint32_t tmp;
+	int bit;
+
+	bit = GPIO_BIT(pin->gp_pin);
+	tmp = 0x100 << bit;		/* mask */
+	tmp |= (val & 1) << bit;	/* value */
+	bus_write_4(sc->mem_res, reg + GPIO_REGNUM(pin->gp_pin), tmp);
+}
+static inline uint32_t
+gpio_read(struct tegra_gpio_softc *sc, bus_size_t reg, struct gpio_pin *pin)
+{
+	int bit;
+	uint32_t val;
+
+	bit = GPIO_BIT(pin->gp_pin);
+	val = bus_read_4(sc->mem_res, reg + GPIO_REGNUM(pin->gp_pin));
+	return (val >> bit) & 1;
+}
+
+static void
+tegra_gpio_pin_configure(struct tegra_gpio_softc *sc, struct gpio_pin *pin,
+    unsigned int flags)
+{
+
+	if ((flags & (GPIO_PIN_INPUT|GPIO_PIN_OUTPUT)) == 0)
+		return;
+
+	/* Manage input/output */
+	pin->gp_flags &= ~(GPIO_PIN_INPUT | GPIO_PIN_OUTPUT);
+	if (flags & GPIO_PIN_OUTPUT) {
+		pin->gp_flags |= GPIO_PIN_OUTPUT;
+		gpio_write_masked(sc, GPIO_MSK_OE, pin, 1);
+	} else {
+		pin->gp_flags |= GPIO_PIN_INPUT;
+		gpio_write_masked(sc, GPIO_MSK_OE, pin, 0);
+	}
+}
+
+static device_t
+tegra_gpio_get_bus(device_t dev)
+{
+	struct tegra_gpio_softc *sc;
+
+	sc = device_get_softc(dev);
+	return (sc->sc_busdev);
+}
+
+static int
+tegra_gpio_pin_max(device_t dev, int *maxpin)
+{
+
+	*maxpin = NGPIO - 1;
+	return (0);
+}
+
+static int
+tegra_gpio_pin_getcaps(device_t dev, uint32_t pin, uint32_t *caps)
+{
+	struct tegra_gpio_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	*caps = sc->gpio_pins[pin].gp_caps;
+	GPIO_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+tegra_gpio_pin_getflags(device_t dev, uint32_t pin, uint32_t *flags)
+{
+	struct tegra_gpio_softc *sc;
+	int cnf;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	cnf = gpio_read(sc, GPIO_CNF, &sc->gpio_pins[pin]);
+	if (cnf == 0) {
+		GPIO_UNLOCK(sc);
+		return (ENXIO);
+	}
+	*flags = sc->gpio_pins[pin].gp_flags;
+	GPIO_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+tegra_gpio_pin_getname(device_t dev, uint32_t pin, char *name)
+{
+	struct tegra_gpio_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	memcpy(name, sc->gpio_pins[pin].gp_name, GPIOMAXNAME);
+	GPIO_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+tegra_gpio_pin_setflags(device_t dev, uint32_t pin, uint32_t flags)
+{
+	struct tegra_gpio_softc *sc;
+	int cnf;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	cnf = gpio_read(sc, GPIO_CNF,  &sc->gpio_pins[pin]);
+	if (cnf == 0) {
+		/* XXX - allow this for while ....
+		GPIO_UNLOCK(sc);
+		return (ENXIO);
+		*/
+		gpio_write_masked(sc, GPIO_MSK_CNF,  &sc->gpio_pins[pin], 1);
+	}
+	tegra_gpio_pin_configure(sc, &sc->gpio_pins[pin], flags);
+	GPIO_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+tegra_gpio_pin_set(device_t dev, uint32_t pin, unsigned int value)
+{
+	struct tegra_gpio_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+	GPIO_LOCK(sc);
+	gpio_write_masked(sc, GPIO_MSK_OUT, &sc->gpio_pins[pin], value);
+	GPIO_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+tegra_gpio_pin_get(device_t dev, uint32_t pin, unsigned int *val)
+{
+	struct tegra_gpio_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	*val = gpio_read(sc, GPIO_IN, &sc->gpio_pins[pin]);
+	GPIO_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+tegra_gpio_pin_toggle(device_t dev, uint32_t pin)
+{
+	struct tegra_gpio_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (pin >= sc->gpio_npins)
+		return (EINVAL);
+
+	GPIO_LOCK(sc);
+	gpio_write_masked(sc, GPIO_MSK_OE, &sc->gpio_pins[pin],
+	     gpio_read(sc, GPIO_IN, &sc->gpio_pins[pin]) ^ 1);
+	GPIO_UNLOCK(sc);
+
+	return (0);
+}
+
+static int
+tegra_gpio_intr(void *arg)
+{
+	struct tegra_gpio_softc *sc;
+	uint32_t val;
+	int i;
+
+	sc = arg;
+	for (i = 0; i < NGPIO; i += GPIO_PINS_IN_REG) {
+		/* Clear interrupt */
+		val = bus_read_4(sc->mem_res, GPIO_INT_STA + GPIO_REGNUM(i));
+		val &= bus_read_4(sc->mem_res, GPIO_INT_ENB + GPIO_REGNUM(i));
+		bus_write_4(sc->mem_res, GPIO_INT_CLR + GPIO_REGNUM(i), val);
+		/* Interrupt handling */
+#ifdef not_yet
+		for (j = 0; j < GPIO_PINS_IN_REG; j++) {
+			if (val & (1 << j))
+				handle_irq(i + j);
+		}
+		*/
+#endif
+	}
+	return (FILTER_HANDLED);
+}
+
+static int
+tegra_gpio_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data != 0) {
+		device_set_desc(dev, "Tegra GPIO Controller");
+		return (BUS_PROBE_DEFAULT);
+	}
+
+	return (ENXIO);
+}
+
+static int
+tegra_gpio_detach(device_t dev)
+{
+	struct tegra_gpio_softc *sc;
+
+	sc = device_get_softc(dev);
+
+	KASSERT(mtx_initialized(&sc->sc_mtx), ("gpio mutex not initialized"));
+
+	gpiobus_detach_bus(dev);
+	if (sc->gpio_ih != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->gpio_ih);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+	mtx_destroy(&sc->sc_mtx);
+
+	return(0);
+}
+
+static int
+tegra_gpio_attach(device_t dev)
+{
+	struct tegra_gpio_softc *sc;
+	int i, rid;
+
+	sc = device_get_softc(dev);
+	mtx_init(&sc->sc_mtx, device_get_nameunit(dev), NULL, MTX_DEF);
+
+	/* Allocate bus_space resources. */
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		tegra_gpio_detach(dev);
+		return (ENXIO);
+	}
+
+	rid = 0;
+	sc->irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid, RF_ACTIVE);
+	if (sc->irq_res == NULL) {
+		device_printf(dev, "Cannot allocate IRQ resources\n");
+		tegra_gpio_detach(dev);
+		return (ENXIO);
+	}
+
+	sc->dev = dev;
+	sc->gpio_npins = NGPIO;
+
+	if ((bus_setup_intr(dev, sc->irq_res, INTR_TYPE_MISC,
+	    tegra_gpio_intr, NULL, sc, &sc->gpio_ih))) {
+		device_printf(dev,
+		    "WARNING: unable to register interrupt handler\n");
+		tegra_gpio_detach(dev);
+		return (ENXIO);
+	}
+
+	for (i = 0; i < sc->gpio_npins; i++) {
+		sc->gpio_pins[i].gp_pin = i;
+		sc->gpio_pins[i].gp_caps = GPIO_PIN_INPUT | GPIO_PIN_OUTPUT;
+		snprintf(sc->gpio_pins[i].gp_name, GPIOMAXNAME, "gpio_%s.%d",
+		    tegra_gpio_port_names[ i / GPIO_PINS_IN_REG],
+		    i % GPIO_PINS_IN_REG);
+		sc->gpio_pins[i].gp_flags =
+		    gpio_read(sc, GPIO_OE, &sc->gpio_pins[i]) != 0 ?
+		    GPIO_PIN_OUTPUT : GPIO_PIN_INPUT;
+	}
+
+	sc->sc_busdev = gpiobus_attach_bus(dev);
+	if (sc->sc_busdev == NULL) {
+		tegra_gpio_detach(dev);
+		return (ENXIO);
+	}
+
+	return (bus_generic_attach(dev));
+}
+
+static int
+tegra_map_gpios(device_t dev, phandle_t pdev, phandle_t gparent,
+    int gcells, pcell_t *gpios, uint32_t *pin, uint32_t *flags)
+{
+
+	if (gcells != 2)
+		return (ERANGE);
+	*pin = gpios[0];
+	*flags= gpios[1];
+	return (0);
+}
+
+static phandle_t
+tegra_gpio_get_node(device_t bus, device_t dev)
+{
+
+	/* We only have one child, the GPIO bus, which needs our own node. */
+	return (ofw_bus_get_node(bus));
+}
+
+static device_method_t tegra_gpio_methods[] = {
+	DEVMETHOD(device_probe,		tegra_gpio_probe),
+	DEVMETHOD(device_attach,	tegra_gpio_attach),
+	DEVMETHOD(device_detach,	tegra_gpio_detach),
+
+	/* GPIO protocol */
+	DEVMETHOD(gpio_get_bus,		tegra_gpio_get_bus),
+	DEVMETHOD(gpio_pin_max,		tegra_gpio_pin_max),
+	DEVMETHOD(gpio_pin_getname,	tegra_gpio_pin_getname),
+	DEVMETHOD(gpio_pin_getflags,	tegra_gpio_pin_getflags),
+	DEVMETHOD(gpio_pin_getcaps,	tegra_gpio_pin_getcaps),
+	DEVMETHOD(gpio_pin_setflags,	tegra_gpio_pin_setflags),
+	DEVMETHOD(gpio_pin_get,		tegra_gpio_pin_get),
+	DEVMETHOD(gpio_pin_set,		tegra_gpio_pin_set),
+	DEVMETHOD(gpio_pin_toggle,	tegra_gpio_pin_toggle),
+	DEVMETHOD(gpio_map_gpios,	tegra_map_gpios),
+
+	/* ofw_bus interface */
+	DEVMETHOD(ofw_bus_get_node,	tegra_gpio_get_node),
+
+	DEVMETHOD_END
+};
+
+static driver_t tegra_gpio_driver = {
+	"gpio",
+	tegra_gpio_methods,
+	sizeof(struct tegra_gpio_softc),
+};
+static devclass_t tegra_gpio_devclass;
+
+EARLY_DRIVER_MODULE(tegra_gpio, simplebus, tegra_gpio_driver,
+    tegra_gpio_devclass, 0, 0, 70);
diff --git a/sys/arm/nvidia/tegra_i2c.c b/sys/arm/nvidia/tegra_i2c.c
new file mode 100644
index 0000000..65d8935
--- /dev/null
+++ b/sys/arm/nvidia/tegra_i2c.c
@@ -0,0 +1,804 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * I2C driver for Tegra SoCs.
+ */
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/limits.h>
+#include <sys/module.h>
+#include <sys/resource.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+#include <sys/rman.h>
+
+#include <sys/lock.h>
+#include <sys/mutex.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/iicbus/iiconf.h>
+#include <dev/iicbus/iicbus.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include "iicbus_if.h"
+
+#define	I2C_CNFG				0x000
+#define	 I2C_CNFG_MSTR_CLR_BUS_ON_TIMEOUT		(1 << 15)
+#define	 I2C_CNFG_DEBOUNCE_CNT(x)			(((x) & 0x07) << 12)
+#define	 I2C_CNFG_NEW_MASTER_FSM			(1 << 11)
+#define	 I2C_CNFG_PACKET_MODE_EN			(1 << 10)
+#define	 I2C_CNFG_SEND					(1 <<  9)
+#define	 I2C_CNFG_NOACK					(1 <<  8)
+#define	 I2C_CNFG_CMD2					(1 <<  7)
+#define	 I2C_CNFG_CMD1					(1 <<  6)
+#define	 I2C_CNFG_START					(1 <<  5)
+#define	 I2C_CNFG_SLV2					(1 <<  4)
+#define	 I2C_CNFG_LENGTH_SHIFT				1
+#define	 I2C_CNFG_LENGTH_MASK				0x7
+#define	 I2C_CNFG_A_MOD					(1 <<  0)
+
+#define	I2C_CMD_ADDR0				0x004
+#define	I2C_CMD_ADDR1				0x008
+#define	I2C_CMD_DATA1				0x00c
+#define	I2C_CMD_DATA2				0x010
+#define	I2C_STATUS				0x01c
+#define	I2C_SL_CNFG				0x020
+#define	I2C_SL_RCVD				0x024
+#define	I2C_SL_STATUS				0x028
+#define	I2C_SL_ADDR1				0x02c
+#define	I2C_SL_ADDR2				0x030
+#define	I2C_TLOW_SEXT				0x034
+#define	I2C_SL_DELAY_COUNT			0x03c
+#define	I2C_SL_INT_MASK				0x040
+#define	I2C_SL_INT_SOURCE			0x044
+#define	I2C_SL_INT_SET				0x048
+#define	I2C_TX_PACKET_FIFO			0x050
+#define	I2C_RX_FIFO				0x054
+#define	I2C_PACKET_TRANSFER_STATUS		0x058
+#define	I2C_FIFO_CONTROL			0x05c
+#define	 I2C_FIFO_CONTROL_SLV_TX_FIFO_TRIG(x)		(((x) & 0x07) << 13)
+#define	 I2C_FIFO_CONTROL_SLV_RX_FIFO_TRIG(x)		(((x) & 0x07) << 10)
+#define	 I2C_FIFO_CONTROL_SLV_TX_FIFO_FLUSH		(1 <<  9)
+#define	 I2C_FIFO_CONTROL_SLV_RX_FIFO_FLUSH		(1 <<  8)
+#define	 I2C_FIFO_CONTROL_TX_FIFO_TRIG(x)		(((x) & 0x07) << 5)
+#define	 I2C_FIFO_CONTROL_RX_FIFO_TRIG(x)		(((x) & 0x07) << 2)
+#define	 I2C_FIFO_CONTROL_TX_FIFO_FLUSH			(1 << 1)
+#define	 I2C_FIFO_CONTROL_RX_FIFO_FLUSH			(1 << 0)
+
+#define	I2C_FIFO_STATUS				0x060
+#define	 I2C_FIFO_STATUS_SLV_XFER_ERR_REASON		(1 << 25)
+#define	 I2C_FIFO_STATUS_TX_FIFO_SLV_EMPTY_CNT(x)	(((x) >> 20) & 0xF)
+#define	 I2C_FIFO_STATUS_RX_FIFO_SLV_FULL_CNT(x)	(((x) >> 16) & 0xF)
+#define	 I2C_FIFO_STATUS_TX_FIFO_EMPTY_CNT(x)		(((x) >>  4) & 0xF)
+#define	 I2C_FIFO_STATUS_RX_FIFO_FULL_CNT(x)		(((x) >>  0) & 0xF)
+
+#define	I2C_INTERRUPT_MASK_REGISTER		0x064
+#define	I2C_INTERRUPT_STATUS_REGISTER		0x068
+#define	 I2C_INT_SLV_ACK_WITHHELD			(1 << 28)
+#define	 I2C_INT_SLV_RD2WR				(1 << 27)
+#define	 I2C_INT_SLV_WR2RD				(1 << 26)
+#define	 I2C_INT_SLV_PKT_XFER_ERR			(1 << 25)
+#define	 I2C_INT_SLV_TX_BUFFER_REQ			(1 << 24)
+#define	 I2C_INT_SLV_RX_BUFFER_FILLED			(1 << 23)
+#define	 I2C_INT_SLV_PACKET_XFER_COMPLETE		(1 << 22)
+#define	 I2C_INT_SLV_TFIFO_OVF				(1 << 21)
+#define	 I2C_INT_SLV_RFIFO_UNF				(1 << 20)
+#define	 I2C_INT_SLV_TFIFO_DATA_REQ			(1 << 17)
+#define	 I2C_INT_SLV_RFIFO_DATA_REQ			(1 << 16)
+#define	 I2C_INT_BUS_CLEAR_DONE				(1 << 11)
+#define	 I2C_INT_TLOW_MEXT_TIMEOUT			(1 << 10)
+#define	 I2C_INT_TLOW_SEXT_TIMEOUT			(1 <<  9)
+#define	 I2C_INT_TIMEOUT				(1 <<  8)
+#define	 I2C_INT_PACKET_XFER_COMPLETE			(1 <<  7)
+#define	 I2C_INT_ALL_PACKETS_XFER_COMPLETE		(1 <<  6)
+#define	 I2C_INT_TFIFO_OVR				(1 <<  5)
+#define	 I2C_INT_RFIFO_UNF				(1 <<  4)
+#define	 I2C_INT_NOACK					(1 <<  3)
+#define	 I2C_INT_ARB_LOST				(1 <<  2)
+#define	 I2C_INT_TFIFO_DATA_REQ				(1 <<  1)
+#define	 I2C_INT_RFIFO_DATA_REQ				(1 <<  0)
+#define	 I2C_ERROR_MASK		(I2C_INT_ARB_LOST | I2C_INT_NOACK |	 \
+				I2C_INT_RFIFO_UNF | I2C_INT_TFIFO_OVR)
+
+#define	I2C_CLK_DIVISOR				0x06c
+#define	 I2C_CLK_DIVISOR_STD_FAST_MODE_SHIFT		16
+#define	 I2C_CLK_DIVISOR_STD_FAST_MODE_MASK		0xffff
+#define	 I2C_CLK_DIVISOR_HSMODE_SHIFT			0
+#define	 I2C_CLK_DIVISOR_HSMODE_MASK			0xffff
+#define	I2C_INTERRUPT_SOURCE_REGISTER		0x070
+#define	I2C_INTERRUPT_SET_REGISTER		0x074
+#define	I2C_SLV_TX_PACKET_FIFO			0x07c
+#define	I2C_SLV_PACKET_STATUS			0x080
+#define	I2C_BUS_CLEAR_CONFIG			0x084
+#define	 I2C_BUS_CLEAR_CONFIG_BC_SCLK_THRESHOLD(x)	(((x) & 0xFF) << 16)
+#define	 I2C_BUS_CLEAR_CONFIG_BC_STOP_COND		(1 << 2)
+#define	 I2C_BUS_CLEAR_CONFIG_BC_TERMINATE		(1 << 1)
+#define	 I2C_BUS_CLEAR_CONFIG_BC_ENABLE			(1 << 0)
+
+#define	I2C_BUS_CLEAR_STATUS			0x088
+#define	 I2C_BUS_CLEAR_STATUS_BC_STATUS			(1 << 0)
+
+#define	I2C_CONFIG_LOAD				0x08c
+#define	 I2C_CONFIG_LOAD_TIMEOUT_CONFIG_LOAD		(1 << 2)
+#define	 I2C_CONFIG_LOAD_SLV_CONFIG_LOAD		(1 << 1)
+#define	 I2C_CONFIG_LOAD_MSTR_CONFIG_LOAD		(1 << 0)
+
+#define	I2C_INTERFACE_TIMING_0			0x094
+#define	I2C_INTERFACE_TIMING_1			0x098
+#define	I2C_HS_INTERFACE_TIMING_0		0x09c
+#define	I2C_HS_INTERFACE_TIMING_1		0x0a0
+
+/* Protocol header 0 */
+#define	PACKET_HEADER0_HEADER_SIZE_SHIFT	28
+#define	PACKET_HEADER0_HEADER_SIZE_MASK		0x3
+#define	PACKET_HEADER0_PACKET_ID_SHIFT		16
+#define	PACKET_HEADER0_PACKET_ID_MASK		0xff
+#define	PACKET_HEADER0_CONT_ID_SHIFT		12
+#define	PACKET_HEADER0_CONT_ID_MASK		0xf
+#define	PACKET_HEADER0_PROTOCOL_I2C		(1 << 4)
+#define	PACKET_HEADER0_TYPE_SHIFT		0
+#define	PACKET_HEADER0_TYPE_MASK		0x7
+
+/* I2C header */
+#define	I2C_HEADER_HIGHSPEED_MODE		(1 << 22)
+#define	I2C_HEADER_CONT_ON_NAK			(1 << 21)
+#define	I2C_HEADER_SEND_START_BYTE		(1 << 20)
+#define	I2C_HEADER_READ				(1 << 19)
+#define	I2C_HEADER_10BIT_ADDR			(1 << 18)
+#define	I2C_HEADER_IE_ENABLE			(1 << 17)
+#define	I2C_HEADER_REPEAT_START			(1 << 16)
+#define	I2C_HEADER_CONTINUE_XFER		(1 << 15)
+#define	I2C_HEADER_MASTER_ADDR_SHIFT		12
+#define	I2C_HEADER_MASTER_ADDR_MASK		0x7
+#define	I2C_HEADER_SLAVE_ADDR_SHIFT		0
+#define	I2C_HEADER_SLAVE_ADDR_MASK		0x3ff
+
+#define	I2C_CLK_DIVISOR_STD_FAST_MODE		0x19
+#define	I2C_CLK_MULTIPLIER_STD_FAST_MODE	8
+
+#define	I2C_REQUEST_TIMEOUT			(5 * hz)
+
+#define	WR4(_sc, _r, _v)	bus_write_4((_sc)->mem_res, (_r), (_v))
+#define	RD4(_sc, _r)		bus_read_4((_sc)->mem_res, (_r))
+
+#define	LOCK(_sc)		mtx_lock(&(_sc)->mtx)
+#define	UNLOCK(_sc)		mtx_unlock(&(_sc)->mtx)
+#define	SLEEP(_sc, timeout)						\
+	mtx_sleep(sc, &sc->mtx, 0, "i2cbuswait", timeout);
+#define	LOCK_INIT(_sc)							\
+	mtx_init(&_sc->mtx, device_get_nameunit(_sc->dev), "tegra_i2c", MTX_DEF)
+#define	LOCK_DESTROY(_sc)	mtx_destroy(&_sc->mtx)
+#define	ASSERT_LOCKED(_sc)	mtx_assert(&_sc->mtx, MA_OWNED)
+#define	ASSERT_UNLOCKED(_sc)	mtx_assert(&_sc->mtx, MA_NOTOWNED)
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-i2c",	1},
+	{NULL,			0}
+};
+enum tegra_i2c_xfer_type {
+	XFER_STOP, 		/* Send stop condition after xfer */
+	XFER_REPEAT_START,	/* Send repeated start after xfer */
+	XFER_CONTINUE		/* Don't send nothing */
+} ;
+
+struct tegra_i2c_softc {
+	device_t		dev;
+	struct mtx		mtx;
+
+	struct resource		*mem_res;
+	struct resource		*irq_res;
+	void			*irq_h;
+
+	device_t		iicbus;
+	clk_t			clk;
+	hwreset_t		reset;
+	uint32_t		core_freq;
+	uint32_t		bus_freq;
+	int			bus_inuse;
+
+	struct iic_msg		*msg;
+	int			msg_idx;
+	uint32_t		bus_err;
+	int			done;
+};
+
+static int
+tegra_i2c_flush_fifo(struct tegra_i2c_softc *sc)
+{
+	int timeout;
+	uint32_t reg;
+
+	reg = RD4(sc, I2C_FIFO_CONTROL);
+	reg |= I2C_FIFO_CONTROL_TX_FIFO_FLUSH | I2C_FIFO_CONTROL_RX_FIFO_FLUSH;
+	WR4(sc, I2C_FIFO_CONTROL, reg);
+
+	timeout = 10;
+	while (timeout > 0) {
+		reg = RD4(sc, I2C_FIFO_CONTROL);
+		reg &= I2C_FIFO_CONTROL_TX_FIFO_FLUSH |
+		    I2C_FIFO_CONTROL_RX_FIFO_FLUSH;
+		if (reg == 0)
+			break;
+		DELAY(10);
+	}
+	if (timeout <= 0) {
+		device_printf(sc->dev, "FIFO flush timedout\n");
+		return (ETIMEDOUT);
+	}
+	return (0);
+}
+
+static void
+tegra_i2c_setup_clk(struct tegra_i2c_softc *sc, int clk_freq)
+{
+	int div;
+
+	div = ((sc->core_freq  / clk_freq)  / 10) - 1;
+	if ((sc->core_freq / (10 * (div + 1)))  > clk_freq)
+		div++;
+	if (div > 65535)
+		div = 65535;
+	WR4(sc, I2C_CLK_DIVISOR,
+	    (1 << I2C_CLK_DIVISOR_HSMODE_SHIFT) |
+	    (div << I2C_CLK_DIVISOR_STD_FAST_MODE_SHIFT));
+}
+
+static void
+tegra_i2c_bus_clear(struct tegra_i2c_softc *sc)
+{
+	int timeout;
+	uint32_t reg, status;
+
+	WR4(sc, I2C_BUS_CLEAR_CONFIG,
+	    I2C_BUS_CLEAR_CONFIG_BC_SCLK_THRESHOLD(18) |
+	    I2C_BUS_CLEAR_CONFIG_BC_STOP_COND |
+	    I2C_BUS_CLEAR_CONFIG_BC_TERMINATE);
+
+	WR4(sc, I2C_CONFIG_LOAD, I2C_CONFIG_LOAD_MSTR_CONFIG_LOAD);
+	for (timeout = 1000; timeout > 0; timeout--) {
+		if (RD4(sc, I2C_CONFIG_LOAD) == 0)
+			break;
+		DELAY(10);
+	}
+	if (timeout <= 0)
+		device_printf(sc->dev, "config load timeouted\n");
+	reg = RD4(sc, I2C_BUS_CLEAR_CONFIG);
+	reg |= I2C_BUS_CLEAR_CONFIG_BC_ENABLE;
+	WR4(sc, I2C_BUS_CLEAR_CONFIG,reg);
+
+	for (timeout = 1000; timeout > 0; timeout--) {
+		if ((RD4(sc, I2C_BUS_CLEAR_CONFIG) &
+		    I2C_BUS_CLEAR_CONFIG_BC_ENABLE) == 0)
+			break;
+		DELAY(10);
+	}
+	if (timeout <= 0)
+		device_printf(sc->dev, "bus clear timeouted\n");
+
+	status = RD4(sc, I2C_BUS_CLEAR_STATUS);
+	if ((status & I2C_BUS_CLEAR_STATUS_BC_STATUS) == 0)
+		device_printf(sc->dev, "bus clear failed\n");
+}
+
+static int
+tegra_i2c_hw_init(struct tegra_i2c_softc *sc)
+{
+	int rv, timeout;
+
+	/* Reset the core. */
+	rv = hwreset_assert(sc->reset);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot assert reset\n");
+		return (rv);
+	}
+	DELAY(10);
+	rv = hwreset_deassert(sc->reset);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot clear reset\n");
+		return (rv);
+	}
+
+	WR4(sc, I2C_INTERRUPT_MASK_REGISTER, 0);
+	WR4(sc, I2C_INTERRUPT_STATUS_REGISTER, 0xFFFFFFFF);
+	WR4(sc, I2C_CNFG, I2C_CNFG_NEW_MASTER_FSM | I2C_CNFG_PACKET_MODE_EN |
+	    I2C_CNFG_DEBOUNCE_CNT(2));
+
+	tegra_i2c_setup_clk(sc, sc->bus_freq);
+
+	WR4(sc, I2C_FIFO_CONTROL, I2C_FIFO_CONTROL_TX_FIFO_TRIG(7) |
+	    I2C_FIFO_CONTROL_RX_FIFO_TRIG(0));
+
+	WR4(sc, I2C_CONFIG_LOAD, I2C_CONFIG_LOAD_MSTR_CONFIG_LOAD);
+	for (timeout = 1000; timeout > 0; timeout--) {
+		if (RD4(sc, I2C_CONFIG_LOAD) == 0)
+			break;
+		DELAY(10);
+	}
+	if (timeout <= 0)
+		device_printf(sc->dev, "config load timeouted\n");
+
+	tegra_i2c_bus_clear(sc);
+	return (0);
+}
+
+static int
+tegra_i2c_tx(struct tegra_i2c_softc *sc)
+{
+	uint32_t reg;
+	int cnt, i;
+
+	if (sc->msg_idx >= sc->msg->len)
+		panic("Invalid call to tegra_i2c_tx\n");
+
+	while(sc->msg_idx < sc->msg->len) {
+		reg = RD4(sc, I2C_FIFO_STATUS);
+		if (I2C_FIFO_STATUS_TX_FIFO_EMPTY_CNT(reg) == 0)
+			break;
+		cnt = min(4, sc->msg->len - sc->msg_idx);
+		reg = 0;
+		for (i = 0;  i < cnt; i++) {
+			reg |=  sc->msg->buf[sc->msg_idx] << (i * 8);
+			sc->msg_idx++;
+		}
+		WR4(sc, I2C_TX_PACKET_FIFO, reg);
+	}
+	if (sc->msg_idx >= sc->msg->len)
+		return (0);
+	return (sc->msg->len - sc->msg_idx - 1);
+}
+
+static int
+tegra_i2c_rx(struct tegra_i2c_softc *sc)
+{
+	uint32_t reg;
+	int cnt, i;
+
+	if (sc->msg_idx >= sc->msg->len)
+		panic("Invalid call to tegra_i2c_rx\n");
+
+	while(sc->msg_idx < sc->msg->len) {
+		reg = RD4(sc, I2C_FIFO_STATUS);
+		if (I2C_FIFO_STATUS_RX_FIFO_FULL_CNT(reg) == 0)
+			break;
+		cnt = min(4, sc->msg->len - sc->msg_idx);
+		reg = RD4(sc, I2C_RX_FIFO);
+		for (i = 0;  i < cnt; i++) {
+			sc->msg->buf[sc->msg_idx] = (reg >> (i * 8)) & 0xFF;
+			sc->msg_idx++;
+		}
+	}
+
+	if (sc->msg_idx >= sc->msg->len)
+		return (0);
+	return (sc->msg->len - sc->msg_idx - 1);
+}
+
+static void
+tegra_i2c_intr(void *arg)
+{
+	struct tegra_i2c_softc *sc;
+	uint32_t status, reg;
+	int rv;
+
+	sc = (struct tegra_i2c_softc *)arg;
+
+	LOCK(sc);
+	status = RD4(sc, I2C_INTERRUPT_SOURCE_REGISTER);
+	if (sc->msg == NULL) {
+		/* Unexpected interrupt - disable FIFOs, clear reset. */
+		reg = RD4(sc, I2C_INTERRUPT_MASK_REGISTER);
+		reg &= ~I2C_INT_TFIFO_DATA_REQ;
+		reg &= ~I2C_INT_RFIFO_DATA_REQ;
+		WR4(sc, I2C_INTERRUPT_MASK_REGISTER, 0);
+		WR4(sc, I2C_INTERRUPT_STATUS_REGISTER, status);
+		UNLOCK(sc);
+		return;
+	}
+
+	if ((status & I2C_ERROR_MASK) != 0) {
+		if (status & I2C_INT_NOACK)
+			sc->bus_err = IIC_ENOACK;
+		if (status & I2C_INT_ARB_LOST)
+			sc->bus_err = IIC_EBUSERR;
+		if ((status & I2C_INT_TFIFO_OVR) ||
+		    (status & I2C_INT_RFIFO_UNF))
+			sc->bus_err = IIC_EBUSERR;
+		sc->done = 1;
+	} else if ((status & I2C_INT_RFIFO_DATA_REQ) &&
+	    (sc->msg != NULL) && (sc->msg->flags & IIC_M_RD)) {
+		rv = tegra_i2c_rx(sc);
+		if (rv == 0) {
+			reg = RD4(sc, I2C_INTERRUPT_MASK_REGISTER);
+			reg &= ~I2C_INT_RFIFO_DATA_REQ;
+			WR4(sc, I2C_INTERRUPT_MASK_REGISTER, reg);
+		}
+	} else if ((status & I2C_INT_TFIFO_DATA_REQ) &&
+	    (sc->msg != NULL) && !(sc->msg->flags & IIC_M_RD)) {
+		rv = tegra_i2c_tx(sc);
+		if (rv == 0) {
+			reg = RD4(sc, I2C_INTERRUPT_MASK_REGISTER);
+			reg &= ~I2C_INT_TFIFO_DATA_REQ;
+			WR4(sc, I2C_INTERRUPT_MASK_REGISTER, reg);
+		}
+	} else if ((status & I2C_INT_RFIFO_DATA_REQ) ||
+		    (status & I2C_INT_TFIFO_DATA_REQ)) {
+		device_printf(sc->dev, "Unexpected data interrupt: 0x%08X\n",
+		    status);
+		reg = RD4(sc, I2C_INTERRUPT_MASK_REGISTER);
+		reg &= ~I2C_INT_TFIFO_DATA_REQ;
+		reg &= ~I2C_INT_RFIFO_DATA_REQ;
+		WR4(sc, I2C_INTERRUPT_MASK_REGISTER, reg);
+	}
+	if (status & I2C_INT_PACKET_XFER_COMPLETE)
+		sc->done = 1;
+	WR4(sc, I2C_INTERRUPT_STATUS_REGISTER, status);
+	if (sc->done) {
+		WR4(sc, I2C_INTERRUPT_MASK_REGISTER, 0);
+		wakeup(&(sc->done));
+	}
+	UNLOCK(sc);
+}
+
+static void
+tegra_i2c_start_msg(struct tegra_i2c_softc *sc, struct iic_msg *msg,
+    enum tegra_i2c_xfer_type xtype)
+{
+	uint32_t tmp, mask;
+
+	/* Packet header. */
+	tmp =  (0 << PACKET_HEADER0_HEADER_SIZE_SHIFT) |
+	   PACKET_HEADER0_PROTOCOL_I2C |
+	   (1 << PACKET_HEADER0_CONT_ID_SHIFT) |
+	   (1 << PACKET_HEADER0_PACKET_ID_SHIFT);
+	WR4(sc, I2C_TX_PACKET_FIFO, tmp);
+
+
+	/* Packet size. */
+	WR4(sc, I2C_TX_PACKET_FIFO, msg->len - 1);
+
+	/* I2C header. */
+	tmp = I2C_HEADER_IE_ENABLE;
+	if (xtype == XFER_CONTINUE)
+		tmp |= I2C_HEADER_CONTINUE_XFER;
+	else if (xtype == XFER_REPEAT_START)
+		tmp |= I2C_HEADER_REPEAT_START;
+	tmp |= msg->slave << I2C_HEADER_SLAVE_ADDR_SHIFT;
+	if (msg->flags & IIC_M_RD) {
+		tmp |= I2C_HEADER_READ;
+		tmp |= 1 << I2C_HEADER_SLAVE_ADDR_SHIFT;
+	} else
+		tmp &= ~(1 << I2C_HEADER_SLAVE_ADDR_SHIFT);
+
+	WR4(sc, I2C_TX_PACKET_FIFO, tmp);
+
+	/* Interrupt mask. */
+	mask = I2C_INT_NOACK | I2C_INT_ARB_LOST | I2C_INT_PACKET_XFER_COMPLETE;
+	if (msg->flags & IIC_M_RD)
+		mask |= I2C_INT_RFIFO_DATA_REQ;
+	else
+		mask |= I2C_INT_TFIFO_DATA_REQ;
+	WR4(sc, I2C_INTERRUPT_MASK_REGISTER, mask);
+}
+
+static int
+tegra_i2c_poll(struct tegra_i2c_softc *sc)
+{
+	int timeout;
+
+	for(timeout = 10000; timeout > 0; timeout--)  {
+		UNLOCK(sc);
+		tegra_i2c_intr(sc);
+		LOCK(sc);
+		if (sc->done != 0)
+			 break;
+		DELAY(1);
+	}
+	if (timeout <= 0)
+		return (ETIMEDOUT);
+	return (0);
+}
+
+static int
+tegra_i2c_transfer(device_t dev, struct iic_msg *msgs, uint32_t nmsgs)
+{
+	int rv, i;
+	struct tegra_i2c_softc *sc;
+	enum tegra_i2c_xfer_type xtype;
+
+	sc = device_get_softc(dev);
+	LOCK(sc);
+
+	/* Get the bus. */
+	while (sc->bus_inuse == 1)
+		SLEEP(sc,  0);
+	sc->bus_inuse = 1;
+
+	rv = 0;
+	for (i = 0; i < nmsgs; i++) {
+		sc->msg = &msgs[i];
+		sc->msg_idx = 0;
+		sc->bus_err = 0;
+		sc->done = 0;
+		/* Check for valid parameters. */
+		if (sc->msg == NULL || sc->msg->buf == NULL ||
+		    sc->msg->len == 0) {
+			rv = EINVAL;
+			break;
+		}
+
+		/* Get flags for next transfer. */
+		if (i == (nmsgs - 1)) {
+			if (msgs[i].flags & IIC_M_NOSTOP)
+				xtype = XFER_CONTINUE;
+			else
+				xtype = XFER_STOP;
+		} else {
+			if (msgs[i + 1].flags & IIC_M_NOSTART)
+				xtype = XFER_CONTINUE;
+			else
+				xtype = XFER_REPEAT_START;
+		}
+		tegra_i2c_start_msg(sc, sc->msg, xtype);
+		if (cold)
+			rv = tegra_i2c_poll(sc);
+		else
+			rv = msleep(&sc->done, &sc->mtx, PZERO, "iic",
+			    I2C_REQUEST_TIMEOUT);
+
+		WR4(sc, I2C_INTERRUPT_MASK_REGISTER, 0);
+		WR4(sc, I2C_INTERRUPT_STATUS_REGISTER, 0xFFFFFFFF);
+		if (rv == 0)
+			rv = sc->bus_err;
+		if (rv != 0)
+			break;
+	}
+
+	if (rv != 0) {
+		tegra_i2c_hw_init(sc);
+		tegra_i2c_flush_fifo(sc);
+	}
+
+	sc->msg = NULL;
+	sc->msg_idx = 0;
+	sc->bus_err = 0;
+	sc->done = 0;
+
+	/* Wake up the processes that are waiting for the bus. */
+	sc->bus_inuse = 0;
+	wakeup(sc);
+	UNLOCK(sc);
+
+	return (rv);
+}
+
+static int
+tegra_i2c_iicbus_reset(device_t dev, u_char speed, u_char addr, u_char *oldaddr)
+{
+	struct tegra_i2c_softc *sc;
+	int busfreq;
+
+	sc = device_get_softc(dev);
+	busfreq = IICBUS_GET_FREQUENCY(sc->iicbus, speed);
+	sc = device_get_softc(dev);
+	LOCK(sc);
+	tegra_i2c_setup_clk(sc, busfreq);
+	UNLOCK(sc);
+	return (0);
+}
+
+static int
+tegra_i2c_probe(device_t dev)
+{
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+tegra_i2c_attach(device_t dev)
+{
+	int rv, rid;
+	phandle_t node;
+	struct tegra_i2c_softc *sc;
+	uint64_t freq;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	node = ofw_bus_get_node(dev);
+
+	LOCK_INIT(sc);
+
+	/* Get the memory resource for the register mapping. */
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot map registers.\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	/* Allocate our IRQ resource. */
+	rid = 0;
+	sc->irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	    RF_ACTIVE);
+	if (sc->irq_res == NULL) {
+		device_printf(dev, "Cannot allocate interrupt.\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	/* FDT resources. */
+	rv = clk_get_by_ofw_name(dev, "div-clk", &sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get i2c clock: %d\n", rv);
+		goto fail;
+	}
+	rv = hwreset_get_by_ofw_name(sc->dev, "i2c", &sc->reset);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get i2c reset\n");
+		return (ENXIO);
+	}
+	rv = OF_getencprop(node, "clock-frequency", &sc->bus_freq,
+	    sizeof(sc->bus_freq));
+	if (rv != sizeof(sc->bus_freq)) {
+		sc->bus_freq = 100000;
+		goto fail;
+	}
+
+	/* Request maximum frequency for I2C block 136MHz (408MHz / 3). */
+	rv = clk_set_freq(sc->clk, 136000000, CLK_SET_ROUND_DOWN);
+	if (rv != 0) {
+		device_printf(dev, "Cannot set clock frequency\n");
+		goto fail;
+	}
+	rv = clk_get_freq(sc->clk, &freq);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get clock frequency\n");
+		goto fail;
+	}
+	sc->core_freq = (uint32_t)freq;
+
+	rv = clk_enable(sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable clock: %d\n", rv);
+		goto fail;
+	}
+
+	/* Init hardware. */
+	rv = tegra_i2c_hw_init(sc);
+	if (rv) {
+		device_printf(dev, "tegra_i2c_activate failed\n");
+		goto fail;
+	}
+
+	/* Setup interrupt. */
+	rv = bus_setup_intr(dev, sc->irq_res, INTR_TYPE_MISC | INTR_MPSAFE,
+	    NULL, tegra_i2c_intr, sc, &sc->irq_h);
+	if (rv) {
+		device_printf(dev, "Cannot setup interrupt.\n");
+		goto fail;
+	}
+
+	/* Attach the iicbus. */
+	sc->iicbus = device_add_child(dev, "iicbus", -1);
+	if (sc->iicbus == NULL) {
+		device_printf(dev, "Could not allocate iicbus instance.\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	/* Probe and attach the iicbus. */
+	return (bus_generic_attach(dev));
+
+fail:
+	if (sc->irq_h != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->irq_h);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+	LOCK_DESTROY(sc);
+
+	return (rv);
+}
+
+static int
+tegra_i2c_detach(device_t dev)
+{
+	struct tegra_i2c_softc *sc;
+	int rv;
+
+	sc = device_get_softc(dev);
+	tegra_i2c_hw_init(sc);
+	if (sc->irq_h != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->irq_h);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+
+	LOCK_DESTROY(sc);
+	if (sc->iicbus)
+	    rv = device_delete_child(dev, sc->iicbus);
+	return (bus_generic_detach(dev));
+}
+
+static phandle_t
+tegra_i2c_get_node(device_t bus, device_t dev)
+{
+
+	/* Share controller node with iibus device. */
+	return (ofw_bus_get_node(bus));
+}
+
+static device_method_t tegra_i2c_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		tegra_i2c_probe),
+	DEVMETHOD(device_attach,	tegra_i2c_attach),
+	DEVMETHOD(device_detach,	tegra_i2c_detach),
+
+	/* Bus interface */
+	DEVMETHOD(bus_setup_intr,	bus_generic_setup_intr),
+	DEVMETHOD(bus_teardown_intr,	bus_generic_teardown_intr),
+	DEVMETHOD(bus_alloc_resource,	bus_generic_alloc_resource),
+	DEVMETHOD(bus_release_resource,	bus_generic_release_resource),
+	DEVMETHOD(bus_activate_resource, bus_generic_activate_resource),
+	DEVMETHOD(bus_deactivate_resource, bus_generic_deactivate_resource),
+	DEVMETHOD(bus_adjust_resource,	bus_generic_adjust_resource),
+	DEVMETHOD(bus_set_resource,	bus_generic_rl_set_resource),
+	DEVMETHOD(bus_get_resource,	bus_generic_rl_get_resource),
+
+	/* OFW methods */
+	DEVMETHOD(ofw_bus_get_node,	tegra_i2c_get_node),
+
+	/* iicbus interface */
+	DEVMETHOD(iicbus_callback,	iicbus_null_callback),
+	DEVMETHOD(iicbus_reset,		tegra_i2c_iicbus_reset),
+	DEVMETHOD(iicbus_transfer,	tegra_i2c_transfer),
+
+	DEVMETHOD_END
+};
+
+DEFINE_CLASS_0(iichb, tegra_i2c_driver, tegra_i2c_methods,
+    sizeof(struct tegra_i2c_softc));
+static devclass_t tegra_i2c_devclass;
+EARLY_DRIVER_MODULE(iichb, simplebus, tegra_i2c_driver, tegra_i2c_devclass, 0,
+    0, 73);
diff --git a/sys/arm/nvidia/tegra_lic.c b/sys/arm/nvidia/tegra_lic.c
new file mode 100644
index 0000000..2e3e0a0
--- /dev/null
+++ b/sys/arm/nvidia/tegra_lic.c
@@ -0,0 +1,265 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * Local interrupt controller driver for Tegra SoCs.
+ */
+#include <sys/param.h>
+#include <sys/module.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/kernel.h>
+#include <sys/rman.h>
+
+#include <machine/fdt.h>
+#include <machine/intr.h>
+#include <machine/resource.h>
+
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include "pic_if.h"
+
+#define	LIC_VIRQ_CPU		0x00
+#define	LIC_VIRQ_COP		0x04
+#define	LIC_VFRQ_CPU		0x08
+#define	LIC_VFRQ_COP		0x0c
+#define	LIC_ISR			0x10
+#define	LIC_FIR			0x14
+#define	LIC_FIR_SET		0x18
+#define	LIC_FIR_CLR		0x1c
+#define	LIC_CPU_IER		0x20
+#define	LIC_CPU_IER_SET		0x24
+#define	LIC_CPU_IER_CLR		0x28
+#define	LIC_CPU_IEP_CLASS	0x2C
+#define	LIC_COP_IER		0x30
+#define	LIC_COP_IER_SET		0x34
+#define	LIC_COP_IER_CLR		0x38
+#define	LIC_COP_IEP_CLASS	0x3c
+
+#define	WR4(_sc, _b, _r, _v)	bus_write_4((_sc)->mem_res[_b], (_r), (_v))
+#define	RD4(_sc, _b, _r)	bus_read_4((_sc)->mem_res[_b], (_r))
+
+static struct resource_spec lic_spec[] = {
+	{ SYS_RES_MEMORY,	0,	RF_ACTIVE },
+	{ SYS_RES_MEMORY,	1,	RF_ACTIVE },
+	{ SYS_RES_MEMORY,	2,	RF_ACTIVE },
+	{ SYS_RES_MEMORY,	3,	RF_ACTIVE },
+	{ SYS_RES_MEMORY,	4,	RF_ACTIVE },
+	{ -1, 0 }
+};
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-ictlr", 	1},
+	{NULL,				0}
+};
+
+struct tegra_lic_sc {
+	device_t		dev;
+	struct resource		*mem_res[nitems(lic_spec)];
+	device_t		parent;
+};
+
+static int
+tegra_lic_register(device_t dev, struct intr_irqsrc *isrc, boolean_t *is_percpu)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	return (PIC_REGISTER(sc->parent, isrc, is_percpu));
+}
+
+static int
+tegra_lic_unregister(device_t dev, struct intr_irqsrc *isrc)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	return (PIC_UNREGISTER(sc->parent, isrc));
+}
+
+static void
+tegra_lic_enable_source(device_t dev, struct intr_irqsrc *isrc)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	PIC_ENABLE_SOURCE(sc->parent, isrc);
+}
+
+static void
+tegra_lic_disable_source(device_t dev, struct intr_irqsrc *isrc)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	PIC_DISABLE_SOURCE(sc->parent, isrc);
+}
+
+static void
+tegra_lic_enable_intr(device_t dev, struct intr_irqsrc *isrc)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	PIC_ENABLE_INTR(sc->parent, isrc);
+}
+
+static void
+tegra_lic_pre_ithread(device_t dev, struct intr_irqsrc *isrc)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	PIC_PRE_ITHREAD(sc->parent, isrc);
+}
+
+
+static void
+tegra_lic_post_ithread(device_t dev, struct intr_irqsrc *isrc)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	PIC_POST_ITHREAD(sc->parent, isrc);
+}
+
+static void
+tegra_lic_post_filter(device_t dev, struct intr_irqsrc *isrc)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	PIC_POST_FILTER(sc->parent, isrc);
+}
+
+#ifdef SMP
+static int
+tegra_lic_bind(device_t dev, struct intr_irqsrc *isrc)
+{
+	struct tegra_lic_sc *sc = device_get_softc(dev);
+
+	return (PIC_BIND(sc->parent, isrc));
+}
+#endif
+
+static int
+tegra_lic_probe(device_t dev)
+{
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+tegra_lic_attach(device_t dev)
+{
+	struct tegra_lic_sc *sc;
+	phandle_t node;
+	phandle_t parent_xref;
+	int i, rv;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	node = ofw_bus_get_node(dev);
+
+	rv = OF_getencprop(node, "interrupt-parent", &parent_xref,
+	    sizeof(parent_xref));
+	if (rv <= 0) {
+		device_printf(dev, "Cannot read parent node property\n");
+		goto fail;
+	}
+	sc->parent = OF_device_from_xref(parent_xref);
+	if (sc->parent == NULL) {
+		device_printf(dev, "Cannott find parent controller\n");
+		goto fail;
+	}
+
+	if (bus_alloc_resources(dev, lic_spec, sc->mem_res)) {
+		device_printf(dev, "Cannott allocate resources\n");
+		goto fail;
+	}
+
+	/* Disable all interrupts, route all to irq */
+	for (i = 0; i < nitems(lic_spec); i++) {
+		if (sc->mem_res[i] == NULL)
+			continue;
+		WR4(sc, i, LIC_CPU_IER_CLR, 0xFFFFFFFF);
+		WR4(sc, i, LIC_CPU_IEP_CLASS, 0);
+	}
+
+
+	if (intr_pic_register(dev, OF_xref_from_node(node)) != 0) {
+		device_printf(dev, "Cannot register PIC\n");
+		goto fail;
+	}
+	return (0);
+
+fail:
+	bus_release_resources(dev, lic_spec, sc->mem_res);
+	return (ENXIO);
+}
+
+static int
+tegra_lic_detach(device_t dev)
+{
+	struct tegra_lic_sc *sc;
+	int i;
+
+	sc = device_get_softc(dev);
+	for (i = 0; i < nitems(lic_spec); i++) {
+		if (sc->mem_res[i] == NULL)
+			continue;
+		bus_release_resource(dev, SYS_RES_MEMORY, i,
+		    sc->mem_res[i]);
+	}
+	return (0);
+}
+
+static device_method_t tegra_lic_methods[] = {
+	DEVMETHOD(device_probe,		tegra_lic_probe),
+	DEVMETHOD(device_attach,	tegra_lic_attach),
+	DEVMETHOD(device_detach,	tegra_lic_detach),
+
+	/* Interrupt controller interface */
+	DEVMETHOD(pic_register,		tegra_lic_register),
+	DEVMETHOD(pic_unregister,	tegra_lic_unregister),
+	DEVMETHOD(pic_enable_source,	tegra_lic_enable_source),
+	DEVMETHOD(pic_disable_source,	tegra_lic_disable_source),
+	DEVMETHOD(pic_enable_intr,	tegra_lic_enable_intr),
+	DEVMETHOD(pic_pre_ithread,	tegra_lic_pre_ithread),
+	DEVMETHOD(pic_post_ithread,	tegra_lic_post_ithread),
+	DEVMETHOD(pic_post_filter,	tegra_lic_post_filter),
+#ifdef SMP
+	DEVMETHOD(pic_bind,		tegra_lic_bind),
+#endif
+	DEVMETHOD_END
+};
+devclass_t tegra_lic_devclass;
+DEFINE_CLASS_0(tegra_lic, tegra_lic_driver, tegra_lic_methods,
+    sizeof(struct tegra_lic_sc));
+EARLY_DRIVER_MODULE(tegra_lic, simplebus, tegra_lic_driver, tegra_lic_devclass,
+    NULL, NULL, BUS_PASS_INTERRUPT + BUS_PASS_ORDER_MIDDLE + 1);
diff --git a/sys/arm/nvidia/tegra_pcie.c b/sys/arm/nvidia/tegra_pcie.c
new file mode 100644
index 0000000..1287e01
--- /dev/null
+++ b/sys/arm/nvidia/tegra_pcie.c
@@ -0,0 +1,1692 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * Nvidia Integrated PCI/PCI-Express controller driver.
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/module.h>
+#include <sys/mutex.h>
+#include <sys/queue.h>
+#include <sys/bus.h>
+#include <sys/rman.h>
+#include <sys/endian.h>
+
+#include <machine/intr.h>
+
+#include <vm/vm.h>
+#include <vm/pmap.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/extres/phy/phy.h>
+#include <dev/extres/regulator/regulator.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+#include <dev/ofw/ofw_pci.h>
+#include <dev/pci/pcivar.h>
+#include <dev/pci/pcireg.h>
+#include <dev/pci/pcib_private.h>
+
+#include <machine/devmap.h>
+#include <machine/resource.h>
+#include <machine/bus.h>
+
+#include "ofw_bus_if.h"
+#include "pcib_if.h"
+
+#include <arm/nvidia/tegra_pmc.h>
+
+/* --- Move to ofw_pci.c/.h ----------------------- */
+
+struct tegra_pci_range {
+	/* parsed phys.hi */
+	int 		nonrelocatable;
+	int		prefetchable;
+	int		aliased;
+	int		space_code;	/* In native format (not shifted)*/
+	int		bus;
+	int		device;
+	int		function;
+	int		reg;
+	pci_addr_t	pci_addr;	/* PCI Address */
+	bus_addr_t	host_addr;	/* Host bus address*/
+	bus_size_t	size;		/* Range size */
+};
+
+static int
+tegra_pci_get_ranges(phandle_t node,  struct tegra_pci_range **ranges)
+{
+	int host_address_cells, pci_address_cells, size_cells;
+	cell_t *base_ranges;
+	ssize_t nbase_ranges;
+	int nranges;
+	int i, j, k;
+	uint32_t flags;
+	uint64_t tmp;
+
+	host_address_cells = 1;
+	pci_address_cells = 3;
+	size_cells = 2;
+	OF_getencprop(OF_parent(node), "#address-cells", &host_address_cells,
+	    sizeof(host_address_cells));
+	OF_getencprop(node, "#address-cells", &pci_address_cells,
+	    sizeof(pci_address_cells));
+	OF_getencprop(node, "#size-cells", &size_cells, sizeof(size_cells));
+
+	nbase_ranges = OF_getproplen(node, "ranges");
+	if (nbase_ranges <= 0)
+		return (-1);
+	nranges = nbase_ranges / sizeof(cell_t) /
+	    (pci_address_cells + host_address_cells + size_cells);
+
+	*ranges = malloc(nranges * sizeof(struct tegra_pci_range),
+	    M_DEVBUF, M_WAITOK);
+	base_ranges = malloc(nbase_ranges, M_DEVBUF, M_WAITOK);
+	OF_getencprop(node, "ranges", base_ranges, nbase_ranges);
+
+	for (i = 0, j = 0; i < nranges; i++) {
+		flags =  base_ranges[j++];
+		(*ranges)[i].nonrelocatable =
+		   flags & OFW_PCI_PHYS_HI_NONRELOCATABLE ? 1 : 0;
+		(*ranges)[i].prefetchable =
+		   flags & OFW_PCI_PHYS_HI_PREFETCHABLE ? 1 : 0;
+		(*ranges)[i].aliased =
+		   flags & OFW_PCI_PHYS_HI_ALIASED ? 1 : 0;
+		(*ranges)[i].space_code = flags & OFW_PCI_PHYS_HI_SPACEMASK;
+		(*ranges)[i].bus = OFW_PCI_PHYS_HI_BUS(flags);
+		(*ranges)[i].device = OFW_PCI_PHYS_HI_DEVICE(flags);
+		(*ranges)[i].function = OFW_PCI_PHYS_HI_FUNCTION(flags);
+		(*ranges)[i].reg = flags & OFW_PCI_PHYS_HI_REGISTERMASK;
+
+		tmp = 0;
+		for (k = 0; k < pci_address_cells - 1; k++) {
+			tmp <<= 32;
+			tmp |= base_ranges[j++];
+		}
+		(*ranges)[i].pci_addr = (pci_addr_t)tmp;
+
+		tmp = 0;
+		for (k = 0; k < host_address_cells; k++) {
+			tmp <<= 32;
+			tmp |= base_ranges[j++];
+		}
+		(*ranges)[i].host_addr = (bus_addr_t)tmp;
+		tmp = 0;
+
+		for (k = 0; k < size_cells; k++) {
+			tmp <<= 32;
+			tmp |= base_ranges[j++];
+		}
+		(*ranges)[i].size = (bus_size_t)tmp;
+	}
+
+	free(base_ranges, M_DEVBUF);
+	return (nranges);
+}
+
+/* -------------------------------------------------------------------------- */
+#define	AFI_AXI_BAR0_SZ				0x000
+#define	AFI_AXI_BAR1_SZ				0x004
+#define	AFI_AXI_BAR2_SZ				0x008
+#define	AFI_AXI_BAR3_SZ				0x00c
+#define	AFI_AXI_BAR4_SZ				0x010
+#define	AFI_AXI_BAR5_SZ				0x014
+#define	AFI_AXI_BAR0_START			0x018
+#define	AFI_AXI_BAR1_START			0x01c
+#define	AFI_AXI_BAR2_START			0x020
+#define	AFI_AXI_BAR3_START			0x024
+#define	AFI_AXI_BAR4_START			0x028
+#define	AFI_AXI_BAR5_START			0x02c
+#define	AFI_FPCI_BAR0				0x030
+#define	AFI_FPCI_BAR1				0x034
+#define	AFI_FPCI_BAR2				0x038
+#define	AFI_FPCI_BAR3				0x03c
+#define	AFI_FPCI_BAR4				0x040
+#define	AFI_FPCI_BAR5				0x044
+#define	AFI_MSI_BAR_SZ				0x060
+#define	AFI_MSI_FPCI_BAR_ST			0x064
+#define	AFI_MSI_AXI_BAR_ST			0x068
+
+
+#define	AFI_AXI_BAR6_SZ				0x134
+#define	AFI_AXI_BAR7_SZ				0x138
+#define	AFI_AXI_BAR8_SZ				0x13c
+#define	AFI_AXI_BAR6_START			0x140
+#define	AFI_AXI_BAR7_START			0x144
+#define	AFI_AXI_BAR8_START			0x148
+#define	AFI_FPCI_BAR6				0x14c
+#define	AFI_FPCI_BAR7				0x150
+#define	AFI_FPCI_BAR8				0x154
+
+#define	AFI_CONFIGURATION			0x0ac
+#define	 AFI_CONFIGURATION_EN_FPCI			(1 << 0)
+
+#define	AFI_FPCI_ERROR_MASKS			0x0b0
+#define	AFI_INTR_MASK				0x0b4
+#define	 AFI_INTR_MASK_MSI_MASK				(1 << 8)
+#define	 AFI_INTR_MASK_INT_MASK				(1 << 0)
+
+#define	AFI_INTR_CODE				0x0b8
+#define	 AFI_INTR_CODE_MASK				0xf
+#define	 AFI_INTR_CODE_INT_CODE_INI_SLVERR		1
+#define	 AFI_INTR_CODE_INT_CODE_INI_DECERR		2
+#define	 AFI_INTR_CODE_INT_CODE_TGT_SLVERR		3
+#define	 AFI_INTR_CODE_INT_CODE_TGT_DECERR		4
+#define	 AFI_INTR_CODE_INT_CODE_TGT_WRERR		5
+#define	 AFI_INTR_CODE_INT_CODE_SM_MSG			6
+#define	 AFI_INTR_CODE_INT_CODE_DFPCI_DECERR		7
+#define	 AFI_INTR_CODE_INT_CODE_AXI_DECERR		8
+#define	 AFI_INTR_CODE_INT_CODE_FPCI_TIMEOUT		9
+#define	 AFI_INTR_CODE_INT_CODE_PE_PRSNT_SENSE		10
+#define	 AFI_INTR_CODE_INT_CODE_PE_CLKREQ_SENSE		11
+#define	 AFI_INTR_CODE_INT_CODE_CLKCLAMP_SENSE		12
+#define	 AFI_INTR_CODE_INT_CODE_RDY4PD_SENSE		13
+#define	 AFI_INTR_CODE_INT_CODE_P2P_ERROR		14
+
+
+#define	AFI_INTR_SIGNATURE			0x0bc
+#define	AFI_UPPER_FPCI_ADDRESS			0x0c0
+#define	AFI_SM_INTR_ENABLE			0x0c4
+#define	 AFI_SM_INTR_RP_DEASSERT			(1 << 14)
+#define	 AFI_SM_INTR_RP_ASSERT				(1 << 13)
+#define	 AFI_SM_INTR_HOTPLUG				(1 << 12)
+#define	 AFI_SM_INTR_PME				(1 << 11)
+#define	 AFI_SM_INTR_FATAL_ERROR			(1 << 10)
+#define	 AFI_SM_INTR_UNCORR_ERROR			(1 <<  9)
+#define	 AFI_SM_INTR_CORR_ERROR				(1 <<  8)
+#define	 AFI_SM_INTR_INTD_DEASSERT			(1 <<  7)
+#define	 AFI_SM_INTR_INTC_DEASSERT			(1 <<  6)
+#define	 AFI_SM_INTR_INTB_DEASSERT			(1 <<  5)
+#define	 AFI_SM_INTR_INTA_DEASSERT			(1 <<  4)
+#define	 AFI_SM_INTR_INTD_ASSERT			(1 <<  3)
+#define	 AFI_SM_INTR_INTC_ASSERT			(1 <<  2)
+#define	 AFI_SM_INTR_INTB_ASSERT			(1 <<  1)
+#define	 AFI_SM_INTR_INTA_ASSERT			(1 <<  0)
+
+#define	AFI_AFI_INTR_ENABLE			0x0c8
+#define	 AFI_AFI_INTR_ENABLE_CODE(code)			(1 << (code))
+
+#define	AFI_PCIE_CONFIG				0x0f8
+#define	 AFI_PCIE_CONFIG_PCIE_DISABLE(x)		(1 << ((x) + 1))
+#define	 AFI_PCIE_CONFIG_PCIE_DISABLE_ALL		0x6
+#define	 AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_MASK	(0xf << 20)
+#define	 AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_XBAR2_1	(0x0 << 20)
+#define	 AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_XBAR4_1	(0x1 << 20)
+
+#define	AFI_FUSE				0x104
+#define	 AFI_FUSE_PCIE_T0_GEN2_DIS			(1 << 2)
+
+#define	AFI_PEX0_CTRL				0x110
+#define	AFI_PEX1_CTRL				0x118
+#define	AFI_PEX2_CTRL				0x128
+#define	 AFI_PEX_CTRL_OVERRIDE_EN			(1 << 4)
+#define	 AFI_PEX_CTRL_REFCLK_EN				(1 << 3)
+#define	 AFI_PEX_CTRL_CLKREQ_EN				(1 << 1)
+#define	 AFI_PEX_CTRL_RST_L				(1 << 0)
+
+#define	AFI_AXI_BAR6_SZ				0x134
+#define	AFI_AXI_BAR7_SZ				0x138
+#define	AFI_AXI_BAR8_SZ				0x13c
+#define	AFI_AXI_BAR6_START			0x140
+#define	AFI_AXI_BAR7_START			0x144
+#define	AFI_AXI_BAR8_START			0x148
+#define	AFI_FPCI_BAR6				0x14c
+#define	AFI_FPCI_BAR7				0x150
+#define	AFI_FPCI_BAR8				0x154
+#define	AFI_PLLE_CONTROL			0x160
+#define	 AFI_PLLE_CONTROL_BYPASS_PADS2PLLE_CONTROL	(1 << 9)
+#define	 AFI_PLLE_CONTROL_BYPASS_PCIE2PLLE_CONTROL	(1 << 8)
+#define	 AFI_PLLE_CONTROL_PADS2PLLE_CONTROL_EN		(1 << 1)
+#define	 AFI_PLLE_CONTROL_PCIE2PLLE_CONTROL_EN		(1 << 0)
+
+#define	AFI_PEXBIAS_CTRL			0x168
+
+/* FPCI Address space */
+#define	FPCI_MAP_IO			0xfdfc000000ULL
+#define	FPCI_MAP_TYPE0_CONFIG		0xfdfc000000ULL
+#define	FPCI_MAP_TYPE1_CONFIG		0xfdff000000ULL
+#define	FPCI_MAP_EXT_TYPE0_CONFIG	0xfe00000000ULL
+#define	FPCI_MAP_EXT_TYPE1_CONFIG	0xfe10000000ULL
+
+/* Configuration space */
+#define	RP_VEND_XP	0x00000F00
+#define	 RP_VEND_XP_DL_UP	(1 << 30)
+
+#define	RP_PRIV_MISC	0x00000FE0
+#define	 RP_PRIV_MISC_PRSNT_MAP_EP_PRSNT (0xE << 0)
+#define	 RP_PRIV_MISC_PRSNT_MAP_EP_ABSNT (0xF << 0)
+
+#define	RP_LINK_CONTROL_STATUS			0x00000090
+#define	 RP_LINK_CONTROL_STATUS_DL_LINK_ACTIVE	0x20000000
+#define	 RP_LINK_CONTROL_STATUS_LINKSTAT_MASK	0x3fff0000
+
+#define	TEGRA_PCIE_LINKUP_TIMEOUT	200
+
+#define	DEBUG
+#ifdef DEBUG
+#define	debugf(fmt, args...) do { printf(fmt,##args); } while (0)
+#else
+#define	debugf(fmt, args...)
+#endif
+
+/*
+ * Configuration space format:
+ *    [27:24] extended register
+ *    [23:16] bus
+ *    [15:11] slot (device)
+ *    [10: 8] function
+ *    [ 7: 0] register
+ */
+#define	PCI_CFG_EXT_REG(reg)	((((reg) >> 8) & 0x0f) << 24)
+#define	PCI_CFG_BUS(bus)	(((bus) & 0xff) << 16)
+#define	PCI_CFG_DEV(dev)	(((dev) & 0x1f) << 11)
+#define	PCI_CFG_FUN(fun)	(((fun) & 0x07) << 8)
+#define	PCI_CFG_BASE_REG(reg)	((reg)  & 0xff)
+
+#define	PADS_WR4(_sc, _r, _v)	bus_write_4((_sc)-pads_mem_res, (_r), (_v))
+#define	PADS_RD4(_sc, _r)	bus_read_4((_sc)->pads_mem_res, (_r))
+#define	AFI_WR4(_sc, _r, _v)	bus_write_4((_sc)->afi_mem_res, (_r), (_v))
+#define	AFI_RD4(_sc, _r)	bus_read_4((_sc)->afi_mem_res, (_r))
+
+static struct {
+	bus_size_t	axi_start;
+	bus_size_t	fpci_start;
+	bus_size_t	size;
+} bars[] = {
+    {AFI_AXI_BAR0_START, AFI_FPCI_BAR0, AFI_AXI_BAR0_SZ},	/* BAR 0 */
+    {AFI_AXI_BAR1_START, AFI_FPCI_BAR1, AFI_AXI_BAR1_SZ},	/* BAR 1 */
+    {AFI_AXI_BAR2_START, AFI_FPCI_BAR2, AFI_AXI_BAR2_SZ},	/* BAR 2 */
+    {AFI_AXI_BAR3_START, AFI_FPCI_BAR3, AFI_AXI_BAR3_SZ},	/* BAR 3 */
+    {AFI_AXI_BAR4_START, AFI_FPCI_BAR4, AFI_AXI_BAR4_SZ},	/* BAR 4 */
+    {AFI_AXI_BAR5_START, AFI_FPCI_BAR5, AFI_AXI_BAR5_SZ},	/* BAR 5 */
+    {AFI_AXI_BAR6_START, AFI_FPCI_BAR6, AFI_AXI_BAR6_SZ},	/* BAR 6 */
+    {AFI_AXI_BAR7_START, AFI_FPCI_BAR7, AFI_AXI_BAR7_SZ},	/* BAR 7 */
+    {AFI_AXI_BAR8_START, AFI_FPCI_BAR8, AFI_AXI_BAR8_SZ},	/* BAR 8 */
+    {AFI_MSI_AXI_BAR_ST, AFI_MSI_FPCI_BAR_ST, AFI_MSI_BAR_SZ},	/* MSI 9 */
+};
+
+/* Compatible devices. */
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-pcie",	1},
+	{NULL,		 		0},
+};
+
+struct tegra_pcib_port {
+	int		enabled;
+	int 		port_idx;		/* chip port index */
+	int		num_lanes;		/* number of lanes */
+	bus_size_t	afi_pex_ctrl;		/* offset of afi_pex_ctrl */
+
+	/* Config space properties. */
+	bus_addr_t	rp_base_addr;		/* PA of config window */
+	bus_size_t	rp_size;		/* size of config window */
+	bus_space_handle_t cfg_handle;		/* handle of config window */
+};
+
+#define	TEGRA_PCIB_MAX_PORTS	3
+struct tegra_pcib_softc {
+	device_t		dev;
+	struct mtx		mtx;
+	struct ofw_bus_iinfo	pci_iinfo;
+	struct rman		pref_mem_rman;
+	struct rman		mem_rman;
+	struct rman		io_rman;
+	struct resource		*pads_mem_res;
+	struct resource		*afi_mem_res;
+	struct resource		*cfg_mem_res;
+	struct resource 	*irq_res;
+	struct resource 	*msi_irq_res;
+	void			*intr_cookie;
+	void			*msi_intr_cookie;
+
+	struct tegra_pci_range	mem_range;
+	struct tegra_pci_range	pref_mem_range;
+	struct tegra_pci_range	io_range;
+
+	phy_t			phy;
+	clk_t			clk_pex;
+	clk_t			clk_afi;
+	clk_t			clk_pll_e;
+	clk_t			clk_cml;
+	hwreset_t			hwreset_pex;
+	hwreset_t			hwreset_afi;
+	hwreset_t			hwreset_pcie_x;
+	regulator_t		supply_avddio_pex;
+	regulator_t		supply_dvddio_pex;
+	regulator_t		supply_avdd_pex_pll;
+	regulator_t		supply_hvdd_pex;
+	regulator_t		supply_hvdd_pex_pll_e;
+	regulator_t		supply_vddio_pex_ctl;
+	regulator_t		supply_avdd_pll_erefe;
+
+	int			busnr;		/* host bridge bus number */
+	uint32_t		msi_bitmap;
+	bus_addr_t		cfg_base_addr;	/* base address of config */
+	bus_size_t		cfg_cur_offs; 	/* currently mapped window */
+	bus_space_handle_t 	cfg_handle;	/* handle of config window */
+	bus_space_tag_t 	bus_tag;	/* tag of config window */
+	int			lanes_cfg;
+	int			num_ports;
+	struct tegra_pcib_port *ports[TEGRA_PCIB_MAX_PORTS];
+};
+
+/* ------------------------------------------------------------------------- */
+/*
+ * Resource manager
+ */
+static int
+tegra_pcib_rman_init(struct tegra_pcib_softc *sc)
+{
+	int err;
+	char buf[64];
+
+	/* Memory management. */
+	sc->pref_mem_rman.rm_type = RMAN_ARRAY;
+	snprintf(buf, sizeof(buf), "%s prefetchable memory space",
+	    device_get_nameunit(sc->dev));
+	sc->pref_mem_rman.rm_descr = strdup(buf, M_DEVBUF);
+	err = rman_init(&sc->pref_mem_rman);
+	if (err)
+		return (err);
+
+	sc->mem_rman.rm_type = RMAN_ARRAY;
+	snprintf(buf, sizeof(buf), "%s non prefetchable memory space",
+	    device_get_nameunit(sc->dev));
+	sc->mem_rman.rm_descr = strdup(buf, M_DEVBUF);
+	err = rman_init(&sc->mem_rman);
+	if (err)
+		return (err);
+
+	sc->io_rman.rm_type = RMAN_ARRAY;
+	snprintf(buf, sizeof(buf), "%s I/O space",
+	    device_get_nameunit(sc->dev));
+	sc->io_rman.rm_descr = strdup(buf, M_DEVBUF);
+	err = rman_init(&sc->io_rman);
+	if (err) {
+		rman_fini(&sc->mem_rman);
+		return (err);
+	}
+
+	err = rman_manage_region(&sc->pref_mem_rman,
+	    sc->pref_mem_range.host_addr,
+	    sc->pref_mem_range.host_addr + sc->pref_mem_range.size - 1);
+	if (err)
+		goto error;
+	err = rman_manage_region(&sc->mem_rman,
+	    sc->mem_range.host_addr,
+	    sc->mem_range.host_addr + sc->mem_range.size - 1);
+	if (err)
+		goto error;
+	err = rman_manage_region(&sc->io_rman,
+	    sc->io_range.pci_addr,
+	    sc->io_range.pci_addr + sc->io_range.size - 1);
+	if (err)
+		goto error;
+	return (0);
+
+error:
+	rman_fini(&sc->pref_mem_rman);
+	rman_fini(&sc->mem_rman);
+	rman_fini(&sc->io_rman);
+	return (err);
+}
+
+static struct rman *
+tegra_pcib_rman(struct tegra_pcib_softc *sc, int type, u_int flags)
+{
+
+	switch (type) {
+	case SYS_RES_IOPORT:
+		return (&sc->io_rman);
+	case SYS_RES_MEMORY:
+		if (flags & RF_PREFETCHABLE)
+			return (&sc->pref_mem_rman);
+		else
+			return (&sc->mem_rman);
+	default:
+		break;
+	}
+
+	return (NULL);
+}
+
+static struct resource *
+tegra_pcib_alloc_resource(device_t dev, device_t child, int type, int *rid,
+    rman_res_t start, rman_res_t end, rman_res_t count, u_int flags)
+{
+	struct tegra_pcib_softc *sc;
+	struct rman *rm;
+	struct resource *res;
+
+	debugf("%s: enter %d start %lx end %lx count %lx\n", __func__,
+	    type, start, end, count);
+	sc = device_get_softc(dev);
+
+#if defined(NEW_PCIB) && defined(PCI_RES_BUS)
+	if (type ==  PCI_RES_BUS) {
+		  return (pci_domain_alloc_bus(0, child, rid, start, end, count,
+					       flags));
+	}
+#endif
+
+	rm = tegra_pcib_rman(sc, type, flags);
+
+	if (rm == NULL) {
+		res = BUS_ALLOC_RESOURCE(device_get_parent(dev), dev,
+		    type, rid, start, end, count, flags);
+
+		return (res);
+	}
+
+	if (bootverbose) {
+		device_printf(dev,
+		    "rman_reserve_resource: start=%#lx, end=%#lx, count=%#lx\n",
+		    start, end, count);
+	}
+
+	res = rman_reserve_resource(rm, start, end, count, flags, child);
+	if (res == NULL)
+		goto fail;
+	rman_set_rid(res, *rid);
+	if (flags & RF_ACTIVE) {
+		if (bus_activate_resource(child, type, *rid, res)) {
+			rman_release_resource(res);
+			goto fail;
+		}
+	}
+	return (res);
+
+fail:
+	if (bootverbose) {
+		device_printf(dev, "%s FAIL: type=%d, rid=%d, "
+		    "start=%016lx, end=%016lx, count=%016lx, flags=%x\n",
+		    __func__, type, *rid, start, end, count, flags);
+	}
+
+	return (NULL);
+}
+
+static int
+tegra_pcib_release_resource(device_t dev, device_t child, int type, int rid,
+    struct resource *res)
+{
+	struct tegra_pcib_softc *sc;
+	struct rman *rm;
+
+	sc = device_get_softc(dev);
+	debugf("%s: %d rid %x\n",  __func__, type, rid);
+
+#if defined(NEW_PCIB) && defined(PCI_RES_BUS)
+	if (type == PCI_RES_BUS)
+		return (pci_domain_release_bus(0, child, rid, res));
+#endif
+
+	rm = tegra_pcib_rman(sc, type, rman_get_flags(res));
+	if (rm != NULL) {
+		KASSERT(rman_is_region_manager(res, rm), ("rman mismatch"));
+		rman_release_resource(res);
+	}
+
+	return (bus_generic_release_resource(dev, child, type, rid, res));
+}
+
+static int
+tegra_pcib_adjust_resource(device_t dev, device_t child, int type,
+			    struct resource *res, u_long start, u_long end)
+{
+	struct tegra_pcib_softc *sc;
+	struct rman *rm;
+
+	sc = device_get_softc(dev);
+	debugf("%s: %d start %lx end %lx \n", __func__,
+	    type, start, end);
+
+#if defined(NEW_PCIB) && defined(PCI_RES_BUS)
+	if (type == PCI_RES_BUS)
+		return (pci_domain_adjust_bus(0, child, res, start, end));
+#endif
+
+	rm = tegra_pcib_rman(sc, type, rman_get_flags(res));
+	if (rm != NULL)
+		return (rman_adjust_resource(res, start, end));
+	return (bus_generic_adjust_resource(dev, child, type, res, start, end));
+}
+extern bus_space_tag_t fdtbus_bs_tag;
+static int
+tegra_pcib_pcie_activate_resource(device_t dev, device_t child, int type,
+    int rid, struct resource *r)
+{
+	struct tegra_pcib_softc *sc;
+	vm_offset_t start;
+	void *p;
+	int rv;
+
+	sc = device_get_softc(dev);
+	rv = rman_activate_resource(r);
+	if (rv != 0)
+		return (rv);
+	switch(type) {
+	case SYS_RES_IOPORT:
+		start = rman_get_start(r) + sc->io_range.host_addr;
+		break;
+	default:
+		start = rman_get_start(r);
+		rman_get_start(r);
+		break;
+	}
+
+	if (bootverbose)
+		printf("%s: start %zx, len %ld\n", __func__, start,
+			rman_get_size(r));
+
+	p = pmap_mapdev(start, (vm_size_t)rman_get_size(r));
+	rman_set_virtual(r, p);
+	rman_set_bustag(r, fdtbus_bs_tag);
+	rman_set_bushandle(r, (u_long)p);
+	return (0);
+}
+
+/* ------------------------------------------------------------------------- */
+/*
+ * IVARs
+ */
+static int
+tegra_pcib_read_ivar(device_t dev, device_t child, int which, uintptr_t *result)
+{
+	struct tegra_pcib_softc *sc = device_get_softc(dev);
+
+	switch (which) {
+	case PCIB_IVAR_BUS:
+		*result = sc->busnr;
+		return (0);
+	case PCIB_IVAR_DOMAIN:
+		*result = device_get_unit(dev);
+		return (0);
+	}
+
+	return (ENOENT);
+}
+
+static int
+tegra_pcib_write_ivar(device_t dev, device_t child, int which, uintptr_t value)
+{
+	struct tegra_pcib_softc *sc = device_get_softc(dev);
+
+	switch (which) {
+	case PCIB_IVAR_BUS:
+		sc->busnr = value;
+		return (0);
+	}
+
+	return (ENOENT);
+}
+
+static int
+tegra_pcib_maxslots(device_t dev)
+{
+	return (16);
+}
+
+
+static int
+tegra_pcib_route_interrupt(device_t bus, device_t dev, int pin)
+{
+	struct tegra_pcib_softc *sc;
+
+	sc = device_get_softc(bus);
+	device_printf(bus, "route pin %d for device %d.%d to %lu\n",
+		      pin, pci_get_slot(dev), pci_get_function(dev),
+		      rman_get_start(sc->irq_res));
+
+	return (rman_get_start(sc->irq_res));
+}
+
+static int
+tegra_pcbib_map_cfg(struct tegra_pcib_softc *sc, u_int bus, u_int slot,
+    u_int func, u_int reg)
+{
+	bus_size_t offs;
+	int rv;
+
+	offs = sc->cfg_base_addr;
+	offs |= PCI_CFG_BUS(bus) | PCI_CFG_DEV(slot) | PCI_CFG_FUN(func) |
+	    PCI_CFG_EXT_REG(reg);
+	if ((sc->cfg_handle != 0) && (sc->cfg_cur_offs == offs))
+		return (0);
+	if (sc->cfg_handle != 0)
+		bus_space_unmap(sc->bus_tag, sc->cfg_handle, 0x800);
+
+	rv = bus_space_map(sc->bus_tag, offs, 0x800, 0, &sc->cfg_handle);
+	if (rv != 0)
+		device_printf(sc->dev, "Cannot map config space\n");
+	else
+		sc->cfg_cur_offs = offs;
+	return (rv);
+}
+
+static uint32_t
+tegra_pcib_read_config(device_t dev, u_int bus, u_int slot, u_int func,
+    u_int reg, int bytes)
+{
+	struct tegra_pcib_softc *sc;
+	bus_space_handle_t hndl;
+	uint32_t off;
+	uint32_t val;
+	int rv, i;
+
+	sc = device_get_softc(dev);
+	if (bus == 0) {
+		if (func != 0)
+			return (0xFFFFFFFF);
+		for (i = 0; i < TEGRA_PCIB_MAX_PORTS; i++) {
+			if ((sc->ports[i] != NULL) &&
+			    (sc->ports[i]->port_idx == slot)) {
+				hndl = sc->ports[i]->cfg_handle;
+				off = reg & 0xFFF;
+				break;
+			}
+		}
+		if (i >= TEGRA_PCIB_MAX_PORTS)
+			return (0xFFFFFFFF);
+	} else {
+		rv = tegra_pcbib_map_cfg(sc, bus, slot, func, reg);
+		if (rv != 0)
+			return (0xFFFFFFFF);
+		hndl = sc->cfg_handle;
+		off = PCI_CFG_BASE_REG(reg);
+	}
+
+	val = bus_space_read_4(sc->bus_tag, hndl, off & ~3);
+	switch (bytes) {
+	case 4:
+		break;
+	case 2:
+		if (off & 3)
+			val >>= 16;
+		val &= 0xffff;
+		break;
+	case 1:
+		val >>= ((off & 3) << 3);
+		val &= 0xff;
+		break;
+	}
+	return val;
+}
+
+static void
+tegra_pcib_write_config(device_t dev, u_int bus, u_int slot, u_int func,
+    u_int reg, uint32_t val, int bytes)
+{
+	struct tegra_pcib_softc *sc;
+	bus_space_handle_t hndl;
+	uint32_t off;
+	uint32_t val2;
+	int rv, i;
+
+	sc = device_get_softc(dev);
+	if (bus == 0) {
+		if (func != 0)
+			return;
+		for (i = 0; i < TEGRA_PCIB_MAX_PORTS; i++) {
+			if ((sc->ports[i] != NULL) &&
+			    (sc->ports[i]->port_idx == slot)) {
+				hndl = sc->ports[i]->cfg_handle;
+				off = reg & 0xFFF;
+				break;
+			}
+		}
+		if (i >= TEGRA_PCIB_MAX_PORTS)
+			return;
+	} else {
+		rv = tegra_pcbib_map_cfg(sc, bus, slot, func, reg);
+		if (rv != 0)
+			return;
+		hndl = sc->cfg_handle;
+		off = PCI_CFG_BASE_REG(reg);
+	}
+
+	switch (bytes) {
+	case 4:
+		bus_space_write_4(sc->bus_tag, hndl, off, val);
+		break;
+	case 2:
+		val2 = bus_space_read_4(sc->bus_tag, hndl, off & ~3);
+		val2 &= ~(0xffff << ((off & 3) << 3));
+		val2 |= ((val & 0xffff) << ((off & 3) << 3));
+		bus_space_write_4(sc->bus_tag, hndl, off & ~3, val2);
+		break;
+	case 1:
+		val2 = bus_space_read_4(sc->bus_tag, hndl, off & ~3);
+		val2 &= ~(0xff << ((off & 3) << 3));
+		val2 |= ((val & 0xff) << ((off & 3) << 3));
+		bus_space_write_4(sc->bus_tag, hndl, off & ~3, val2);
+		break;
+	}
+}
+
+static int tegra_pci_intr(void *arg)
+{
+	struct tegra_pcib_softc *sc = arg;
+	uint32_t code, signature;
+
+	code = bus_read_4(sc->afi_mem_res, AFI_INTR_CODE) & AFI_INTR_CODE_MASK;
+	signature = bus_read_4(sc->afi_mem_res, AFI_INTR_SIGNATURE);
+	bus_write_4(sc->afi_mem_res, AFI_INTR_CODE, 0);
+	if (code == AFI_INTR_CODE_INT_CODE_SM_MSG)
+		return(FILTER_STRAY);
+
+	printf("tegra_pci_intr: code %x sig %x\n", code, signature);
+	return (FILTER_HANDLED);
+}
+
+#if defined(TEGRA_PCI_MSI)
+static int
+tegra_pcib_map_msi(device_t dev, device_t child, int irq, uint64_t *addr,
+    uint32_t *data)
+{
+	struct tegra_pcib_softc *sc;
+
+	sc = device_get_softc(dev);
+	irq = irq - MSI_IRQ;
+
+	/* validate parameters */
+	if (isclr(&sc->msi_bitmap, irq)) {
+		device_printf(dev, "invalid MSI 0x%x\n", irq);
+		return (EINVAL);
+	}
+
+	tegra_msi_data(irq, addr, data);
+
+	debugf("%s: irq: %d addr: %jx data: %x\n",
+	    __func__, irq, *addr, *data);
+
+	return (0);
+}
+
+static int
+tegra_pcib_alloc_msi(device_t dev, device_t child, int count,
+    int maxcount __unused, int *irqs)
+{
+	struct tegra_pcib_softc *sc;
+	u_int start = 0, i;
+
+	if (powerof2(count) == 0 || count > MSI_IRQ_NUM)
+		return (EINVAL);
+
+	sc = device_get_softc(dev);
+	mtx_lock(&sc->mtx);
+
+	for (start = 0; (start + count) < MSI_IRQ_NUM; start++) {
+		for (i = start; i < start + count; i++) {
+			if (isset(&sc->msi_bitmap, i))
+				break;
+		}
+		if (i == start + count)
+			break;
+	}
+
+	if ((start + count) == MSI_IRQ_NUM) {
+		mtx_unlock(&sc->mtx);
+		return (ENXIO);
+	}
+
+	for (i = start; i < start + count; i++) {
+		setbit(&sc->msi_bitmap, i);
+		irqs[i] = MSI_IRQ + i;
+	}
+	debugf("%s: start: %x count: %x\n", __func__, start, count);
+
+	mtx_unlock(&sc->mtx);
+	return (0);
+}
+
+static int
+tegra_pcib_release_msi(device_t dev, device_t child, int count, int *irqs)
+{
+	struct tegra_pcib_softc *sc;
+	u_int i;
+
+	sc = device_get_softc(dev);
+	mtx_lock(&sc->mtx);
+
+	for (i = 0; i < count; i++)
+		clrbit(&sc->msi_bitmap, irqs[i] - MSI_IRQ);
+
+	mtx_unlock(&sc->mtx);
+	return (0);
+}
+#endif
+
+static bus_size_t
+tegra_pcib_pex_ctrl(struct tegra_pcib_softc *sc, int port)
+{
+	if (port >= TEGRA_PCIB_MAX_PORTS)
+		panic("invalid port number: %d\n", port);
+
+	if (port == 0)
+		return (AFI_PEX0_CTRL);
+	else if (port == 1)
+		return (AFI_PEX1_CTRL);
+	else if (port == 2)
+		return (AFI_PEX2_CTRL);
+	else
+		panic("invalid port number: %d\n", port);
+}
+
+static int
+tegra_pcib_enable_fdt_resources(struct tegra_pcib_softc *sc)
+{
+	int rv;
+
+	rv = hwreset_assert(sc->hwreset_pcie_x);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot assert 'pcie_x' reset\n");
+		return (rv);
+	}
+	rv = hwreset_assert(sc->hwreset_afi);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot assert  'afi' reset\n");
+		return (rv);
+	}
+	rv = hwreset_assert(sc->hwreset_pex);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot assert  'pex' reset\n");
+		return (rv);
+	}
+
+	tegra_powergate_power_off(TEGRA_POWERGATE_PCX);
+
+	/* Power supplies. */
+	rv = regulator_enable(sc->supply_avddio_pex);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable 'avddio_pex' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_dvddio_pex);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable 'dvddio_pex' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_avdd_pex_pll);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable 'avdd-pex-pll' regulator\n");
+		return (rv);
+	}
+
+	rv = regulator_enable(sc->supply_hvdd_pex);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable 'hvdd-pex-supply' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_hvdd_pex_pll_e);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable 'hvdd-pex-pll-e-supply' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_vddio_pex_ctl);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable 'vddio-pex-ctl' regulator\n");
+		return (rv);
+	}
+	rv = regulator_enable(sc->supply_avdd_pll_erefe);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot enable 'avdd-pll-erefe-supply' regulator\n");
+		return (rv);
+	}
+
+	rv = tegra_powergate_sequence_power_up(TEGRA_POWERGATE_PCX,
+	    sc->clk_pex, sc->hwreset_pex);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'PCX' powergate\n");
+		return (rv);
+	}
+
+	rv = hwreset_deassert(sc->hwreset_afi);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot unreset 'afi' reset\n");
+		return (rv);
+	}
+
+	rv = clk_enable(sc->clk_afi);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'afi' clock\n");
+		return (rv);
+	}
+
+	rv = clk_enable(sc->clk_cml);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'cml' clock\n");
+		return (rv);
+	}
+
+	rv = clk_enable(sc->clk_pll_e);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'pll_e' clock\n");
+		return (rv);
+	}
+	return (0);
+}
+
+static struct tegra_pcib_port *
+tegra_pcib_parse_port(struct tegra_pcib_softc *sc, phandle_t node)
+{
+	struct tegra_pcib_port *port;
+	uint32_t tmp[5];
+	char tmpstr[6];
+	int rv;
+
+	port = malloc(sizeof(struct tegra_pcib_port), M_DEVBUF, M_WAITOK);
+
+	rv = OF_getprop(node, "status", tmpstr, sizeof(tmpstr));
+	if (rv <= 0 || strcmp(tmpstr, "okay") == 0 ||
+	   strcmp(tmpstr, "ok") == 0)
+		port->enabled = 1;
+	else
+		port->enabled = 0;
+
+	rv = OF_getencprop(node, "assigned-addresses", tmp, sizeof(tmp));
+	if (rv != sizeof(tmp)) {
+		device_printf(sc->dev, "Cannot parse assigned-address: %d\n",
+		    rv);
+		goto fail;
+	}
+	port->rp_base_addr = tmp[2];
+	port->rp_size = tmp[4];
+	port->port_idx = OFW_PCI_PHYS_HI_DEVICE(tmp[0]) - 1;
+	if (port->port_idx >= TEGRA_PCIB_MAX_PORTS) {
+		device_printf(sc->dev, "Invalid port index: %d\n",
+		    port->port_idx);
+		goto fail;
+	}
+	/* XXX - TODO:
+	 * Implement proper function for parsing pci "reg" property:
+	 *  - it have PCI bus format
+	 *  - its relative to matching "assigned-addresses"
+	 */
+	rv = OF_getencprop(node, "reg", tmp, sizeof(tmp));
+	if (rv != sizeof(tmp)) {
+		device_printf(sc->dev, "Cannot parse reg: %d\n", rv);
+		goto fail;
+	}
+	port->rp_base_addr += tmp[2];
+
+	rv = OF_getencprop(node, "nvidia,num-lanes", &port->num_lanes,
+	    sizeof(port->num_lanes));
+	if (rv != sizeof(port->num_lanes)) {
+		device_printf(sc->dev, "Cannot parse nvidia,num-lanes: %d\n",
+		    rv);
+		goto fail;
+	}
+	if (port->num_lanes > 4) {
+		device_printf(sc->dev, "Invalid nvidia,num-lanes: %d\n",
+		    port->num_lanes);
+		goto fail;
+	}
+
+	port->afi_pex_ctrl = tegra_pcib_pex_ctrl(sc, port->port_idx);
+	sc->lanes_cfg |= port->num_lanes << (4 * port->port_idx);
+
+	return (port);
+fail:
+	free(port, M_DEVBUF);
+	return (NULL);
+}
+
+
+static int
+tegra_pcib_parse_fdt_resources(struct tegra_pcib_softc *sc, phandle_t node)
+{
+	phandle_t child;
+	struct tegra_pcib_port *port;
+	int rv;
+
+	/* Power supplies. */
+	rv = regulator_get_by_ofw_property(sc->dev, "avddio-pex-supply",
+	    &sc->supply_avddio_pex);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot get 'avddio-pex' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "dvddio-pex-supply",
+	     &sc->supply_dvddio_pex);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot get 'dvddio-pex' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "avdd-pex-pll-supply",
+	     &sc->supply_avdd_pex_pll);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot get 'avdd-pex-pll' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "hvdd-pex-supply",
+	     &sc->supply_hvdd_pex);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot get 'hvdd-pex' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "hvdd-pex-pll-e-supply",
+	     &sc->supply_hvdd_pex_pll_e);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot get 'hvdd-pex-pll-e' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "vddio-pex-ctl-supply",
+	    &sc->supply_vddio_pex_ctl);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot get 'vddio-pex-ctl' regulator\n");
+		return (ENXIO);
+	}
+	rv = regulator_get_by_ofw_property(sc->dev, "avdd-pll-erefe-supply",
+	     &sc->supply_avdd_pll_erefe);
+	if (rv != 0) {
+		device_printf(sc->dev,
+		    "Cannot get 'avdd-pll-erefe' regulator\n");
+		return (ENXIO);
+	}
+
+	/* Resets. */
+	rv = hwreset_get_by_ofw_name(sc->dev, "pex", &sc->hwreset_pex);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'pex' reset\n");
+		return (ENXIO);
+	}
+	rv = hwreset_get_by_ofw_name(sc->dev, "afi", &sc->hwreset_afi);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'afi' reset\n");
+		return (ENXIO);
+	}
+	rv = hwreset_get_by_ofw_name(sc->dev, "pcie_x", &sc->hwreset_pcie_x);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'pcie_x' reset\n");
+		return (ENXIO);
+	}
+
+	/* Clocks. */
+	rv = clk_get_by_ofw_name(sc->dev, "pex", &sc->clk_pex);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'pex' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(sc->dev, "afi", &sc->clk_afi);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'afi' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(sc->dev, "pll_e", &sc->clk_pll_e);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'pll_e' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(sc->dev, "cml", &sc->clk_cml);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'cml' clock\n");
+		return (ENXIO);
+	}
+
+	/* Phy. */
+	rv = phy_get_by_ofw_name(sc->dev, "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)) {
+		port = tegra_pcib_parse_port(sc, child);
+		if (port == NULL) {
+			device_printf(sc->dev, "Cannot parse PCIe port node\n");
+			return (ENXIO);
+		}
+		sc->ports[sc->num_ports++] = port;
+	}
+
+	return (0);
+}
+
+static int
+tegra_pcib_decode_ranges(struct tegra_pcib_softc *sc,
+    struct tegra_pci_range *ranges, int nranges)
+{
+	int i;
+
+	for (i = 2; i < nranges; i++) {
+		if (ranges[i].space_code == OFW_PCI_PHYS_HI_SPACE_IO) {
+			if (sc->io_range.size != 0) {
+				device_printf(sc->dev,
+				    "Duplicated IO range found in DT\n");
+				return (ENXIO);
+			}
+			sc->io_range = ranges[i];
+		}
+		if ((ranges[i].space_code == OFW_PCI_PHYS_HI_SPACE_MEM32) &&
+		    !ranges[i].prefetchable) {
+			if (sc->mem_range.size != 0) {
+				device_printf(sc->dev,
+				    "Duplicated memory range found in DT\n");
+				return (ENXIO);
+			}
+			sc->mem_range = ranges[i];
+		}
+		if ((ranges[i].space_code == OFW_PCI_PHYS_HI_SPACE_MEM32) &&
+		    ranges[i].prefetchable) {
+			if (sc->pref_mem_range.size != 0) {
+				device_printf(sc->dev,
+				    "Duplicated memory range found in DT\n");
+				return (ENXIO);
+			}
+			sc->pref_mem_range = ranges[i];
+		}
+	}
+	if ((sc->io_range.size == 0) || (sc->mem_range.size == 0)
+	    || (sc->pref_mem_range.size == 0)) {
+		device_printf(sc->dev,
+		    " Not all required ranges are found in DT\n");
+		return (ENXIO);
+	}
+	return (0);
+}
+
+/*
+ * Hardware config.
+ */
+static int
+tegra_pcib_wait_for_link(struct tegra_pcib_softc *sc,
+    struct tegra_pcib_port *port)
+{
+	uint32_t reg;
+	int i;
+
+
+	/* Setup link detection. */
+	reg = tegra_pcib_read_config(sc->dev, 0, port->port_idx, 0,
+	    RP_PRIV_MISC, 4);
+	reg &= ~RP_PRIV_MISC_PRSNT_MAP_EP_ABSNT;
+	reg |= RP_PRIV_MISC_PRSNT_MAP_EP_PRSNT;
+	tegra_pcib_write_config(sc->dev, 0, port->port_idx, 0,
+	    RP_PRIV_MISC, reg, 4);
+
+	for (i = TEGRA_PCIE_LINKUP_TIMEOUT; i > 0; i--) {
+		reg = tegra_pcib_read_config(sc->dev, 0, port->port_idx, 0,
+		    RP_VEND_XP, 4);
+		if (reg & RP_VEND_XP_DL_UP)
+				break;
+
+	}
+	if (i <= 0)
+		return (ETIMEDOUT);
+
+	for (i = TEGRA_PCIE_LINKUP_TIMEOUT; i > 0; i--) {
+		reg = tegra_pcib_read_config(sc->dev, 0, port->port_idx, 0,
+		    RP_LINK_CONTROL_STATUS, 4);
+		if (reg & RP_LINK_CONTROL_STATUS_DL_LINK_ACTIVE)
+				break;
+
+	}
+	if (i <= 0)
+		return (ETIMEDOUT);
+	return (0);
+}
+
+static void
+tegra_pcib_port_enable(struct tegra_pcib_softc *sc, int port_num)
+{
+	struct tegra_pcib_port *port;
+	uint32_t reg;
+	int rv;
+
+	port = sc->ports[port_num];
+
+	/* Put port to reset. */
+	reg = AFI_RD4(sc, port->afi_pex_ctrl);
+	reg &= ~AFI_PEX_CTRL_RST_L;
+	AFI_WR4(sc, port->afi_pex_ctrl, reg);
+	AFI_RD4(sc, port->afi_pex_ctrl);
+	DELAY(10);
+
+	/* Enable clocks. */
+	reg |= AFI_PEX_CTRL_REFCLK_EN;
+	reg |= AFI_PEX_CTRL_CLKREQ_EN;
+	reg |= AFI_PEX_CTRL_OVERRIDE_EN;
+	AFI_WR4(sc, port->afi_pex_ctrl, reg);
+	AFI_RD4(sc, port->afi_pex_ctrl);
+	DELAY(100);
+
+	/* Release reset. */
+	reg |= AFI_PEX_CTRL_RST_L;
+	AFI_WR4(sc, port->afi_pex_ctrl, reg);
+
+	rv = tegra_pcib_wait_for_link(sc, port);
+	if (bootverbose)
+		device_printf(sc->dev, " port %d (%d lane%s): Link is %s\n",
+			 port->port_idx, port->num_lanes,
+			 port->num_lanes > 1 ? "s": "",
+			 rv == 0 ? "up": "down");
+}
+
+
+static void
+tegra_pcib_port_disable(struct tegra_pcib_softc *sc, uint32_t port_num)
+{
+	struct tegra_pcib_port *port;
+	uint32_t reg;
+
+	port = sc->ports[port_num];
+
+	/* Put port to reset. */
+	reg = AFI_RD4(sc, port->afi_pex_ctrl);
+	reg &= ~AFI_PEX_CTRL_RST_L;
+	AFI_WR4(sc, port->afi_pex_ctrl, reg);
+	AFI_RD4(sc, port->afi_pex_ctrl);
+	DELAY(10);
+
+	/* Disable clocks. */
+	reg &= ~AFI_PEX_CTRL_CLKREQ_EN;
+	reg &= ~AFI_PEX_CTRL_REFCLK_EN;
+	AFI_WR4(sc, port->afi_pex_ctrl, reg);
+
+	if (bootverbose)
+		device_printf(sc->dev, " port %d (%d lane%s): Disabled\n",
+			 port->port_idx, port->num_lanes,
+			 port->num_lanes > 1 ? "s": "");
+}
+
+static void
+tegra_pcib_set_bar(struct tegra_pcib_softc *sc, int bar, uint32_t axi,
+    uint64_t fpci, uint32_t size, int is_memory)
+{
+	uint32_t fpci_reg;
+	uint32_t axi_reg;
+	uint32_t size_reg;
+
+	axi_reg = axi & ~0xFFF;
+	size_reg = size >> 12;
+	fpci_reg = (uint32_t)(fpci >> 8) & ~0xF;
+	fpci_reg |= is_memory ? 0x1 : 0x0;
+	AFI_WR4(sc, bars[bar].axi_start, axi_reg);
+	AFI_WR4(sc, bars[bar].size, size_reg);
+	AFI_WR4(sc, bars[bar].fpci_start, fpci_reg);
+}
+
+static int
+tegra_pcib_enable(struct tegra_pcib_softc *sc, uint32_t port)
+{
+	int rv;
+	int i;
+	uint32_t reg;
+
+	rv = tegra_pcib_enable_fdt_resources(sc);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable FDT resources\n");
+		return (rv);
+	}
+	/* Enable PLLE control. */
+	reg = AFI_RD4(sc, AFI_PLLE_CONTROL);
+	reg &= ~AFI_PLLE_CONTROL_BYPASS_PADS2PLLE_CONTROL;
+	reg |= AFI_PLLE_CONTROL_PADS2PLLE_CONTROL_EN;
+	AFI_WR4(sc, AFI_PLLE_CONTROL, reg);
+
+	/* Set bias pad. */
+	AFI_WR4(sc, AFI_PEXBIAS_CTRL, 0);
+
+	/* Configure mode and ports. */
+	reg = AFI_RD4(sc, AFI_PCIE_CONFIG);
+	reg &= ~AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_MASK;
+	if (sc->lanes_cfg == 0x14) {
+		if (bootverbose)
+			device_printf(sc->dev,
+			    "Using x1,x4 configuration\n");
+		reg |= AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_XBAR4_1;
+	} else if (sc->lanes_cfg == 0x12) {
+		if (bootverbose)
+			device_printf(sc->dev,
+			    "Using x1,x2 configuration\n");
+		reg |= AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_XBAR2_1;
+	} else {
+		device_printf(sc->dev,
+		    "Unsupported lanes configuration: 0x%X\n", sc->lanes_cfg);
+	}
+	reg |= AFI_PCIE_CONFIG_PCIE_DISABLE_ALL;
+	for (i = 0; i < TEGRA_PCIB_MAX_PORTS; i++) {
+		if ((sc->ports[i] != NULL))
+			reg &=
+			 ~AFI_PCIE_CONFIG_PCIE_DISABLE(sc->ports[i]->port_idx);
+	}
+	AFI_WR4(sc, AFI_PCIE_CONFIG, reg);
+
+	/* Enable Gen2 support. */
+	reg = AFI_RD4(sc, AFI_FUSE);
+	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);
+	}
+
+	rv = hwreset_deassert(sc->hwreset_pcie_x);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot unreset  'pci_x' reset\n");
+		return (rv);
+	}
+
+	/* Enable config space. */
+	reg = AFI_RD4(sc, AFI_CONFIGURATION);
+	reg |= AFI_CONFIGURATION_EN_FPCI;
+	AFI_WR4(sc, AFI_CONFIGURATION, reg);
+
+	/* Enable AFI errors. */
+	reg = 0;
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_INI_SLVERR);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_INI_DECERR);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_TGT_SLVERR);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_TGT_DECERR);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_TGT_WRERR);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_SM_MSG);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_DFPCI_DECERR);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_AXI_DECERR);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_FPCI_TIMEOUT);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_PE_PRSNT_SENSE);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_PE_CLKREQ_SENSE);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_CLKCLAMP_SENSE);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_RDY4PD_SENSE);
+	reg |= AFI_AFI_INTR_ENABLE_CODE(AFI_INTR_CODE_INT_CODE_P2P_ERROR);
+	AFI_WR4(sc, AFI_AFI_INTR_ENABLE, reg);
+	AFI_WR4(sc, AFI_SM_INTR_ENABLE, 0xffffffff);
+
+	/* Enable INT, disable MSI. */
+	AFI_WR4(sc, AFI_INTR_MASK, AFI_INTR_MASK_INT_MASK);
+
+	/* Mask all FPCI errors. */
+	AFI_WR4(sc, AFI_FPCI_ERROR_MASKS, 0);
+
+	/* Setup AFI translation windows. */
+	/* BAR 0 - type 1 extended configuration. */
+	tegra_pcib_set_bar(sc, 0, rman_get_start(sc->cfg_mem_res),
+	   FPCI_MAP_EXT_TYPE1_CONFIG, rman_get_size(sc->cfg_mem_res), 0);
+
+	/* BAR 1 - downstream I/O. */
+	tegra_pcib_set_bar(sc, 1, sc->io_range.host_addr, FPCI_MAP_IO,
+	    sc->io_range.size, 0);
+
+	/* BAR 2 - downstream prefetchable memory 1:1. */
+	tegra_pcib_set_bar(sc, 2, sc->pref_mem_range.host_addr,
+	    sc->pref_mem_range.host_addr, sc->pref_mem_range.size, 1);
+
+	/* BAR 3 - downstream not prefetchable memory 1:1 .*/
+	tegra_pcib_set_bar(sc, 3, sc->mem_range.host_addr,
+	    sc->mem_range.host_addr, sc->mem_range.size, 1);
+
+	/* BAR 3-8 clear. */
+	tegra_pcib_set_bar(sc, 4, 0, 0, 0, 0);
+	tegra_pcib_set_bar(sc, 5, 0, 0, 0, 0);
+	tegra_pcib_set_bar(sc, 6, 0, 0, 0, 0);
+	tegra_pcib_set_bar(sc, 7, 0, 0, 0, 0);
+	tegra_pcib_set_bar(sc, 8, 0, 0, 0, 0);
+
+	/* MSI BAR - clear. */
+	tegra_pcib_set_bar(sc, 9, 0, 0, 0, 0);
+	return(0);
+}
+
+static int
+tegra_pcib_probe(device_t dev)
+{
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data != 0) {
+		device_set_desc(dev, "Nvidia Integrated PCI/PCI-E Controller");
+		return (BUS_PROBE_DEFAULT);
+	}
+	return (ENXIO);
+}
+
+static int
+tegra_pcib_attach(device_t dev)
+{
+	struct tegra_pcib_softc *sc;
+	phandle_t node;
+	uint32_t unit;
+	int rv;
+	int rid;
+	int nranges;
+	struct tegra_pci_range *ranges;
+	struct tegra_pcib_port *port;
+	int i;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	unit = fdt_get_unit(dev);
+	mtx_init(&sc->mtx, "msi_mtx", NULL, MTX_DEF);
+
+
+	node = ofw_bus_get_node(dev);
+
+	rv = tegra_pcib_parse_fdt_resources(sc, node);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get FDT resources\n");
+		return (rv);
+	}
+
+	nranges = tegra_pci_get_ranges(node, &ranges);
+	if (nranges != 5) {
+		device_printf(sc->dev, "Unexpected number of ranges: %d\n",
+		    nranges);
+		rv = ENXIO;
+		goto out;
+	}
+
+	/* Allocate bus_space resources. */
+	rid = 0;
+	sc->pads_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->pads_mem_res == NULL) {
+		device_printf(dev, "Cannot allocate PADS register\n");
+		rv = ENXIO;
+		goto out;
+	}
+	/*
+	 * XXX - FIXME
+	 * tag for config space is not filled when RF_ALLOCATED flag is used.
+	 */
+	sc->bus_tag = rman_get_bustag(sc->pads_mem_res);
+
+	rid = 1;
+	sc->afi_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->afi_mem_res == NULL) {
+		device_printf(dev, "Cannot allocate AFI register\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	rid = 2;
+	sc->cfg_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ALLOCATED);
+	if (sc->cfg_mem_res == NULL) {
+		device_printf(dev, "Cannot allocate config space memory\n");
+		rv = ENXIO;
+		goto out;
+	}
+	sc->cfg_base_addr = rman_get_start(sc->cfg_mem_res);
+
+
+	/* Map RP slots */
+	for (i = 0; i < TEGRA_PCIB_MAX_PORTS; i++) {
+		if (sc->ports[i] == NULL)
+			continue;
+		port = sc->ports[i];
+		rv = bus_space_map(sc->bus_tag, port->rp_base_addr,
+		    port->rp_size, 0, &port->cfg_handle);
+		if (rv != 0) {
+			device_printf(sc->dev, "Cannot allocate memory for "
+			    "port: %d\n", i);
+			rv = ENXIO;
+			goto out;
+		}
+	}
+
+	/*
+	 * Get PCI interrupt info.
+	 */
+	ofw_bus_setup_iinfo(node, &sc->pci_iinfo, sizeof(pcell_t));
+	rid = 0;
+	sc->irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	    RF_ACTIVE | RF_SHAREABLE);
+	if (sc->irq_res == NULL) {
+		device_printf(dev, "Cannot allocate IRQ resources\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	rid = 1;
+	sc->msi_irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	    RF_ACTIVE);
+	if (sc->irq_res == NULL) {
+		device_printf(dev, "Cannot allocate MSI IRQ resources\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	if (bus_setup_intr(dev, sc->irq_res, INTR_TYPE_BIO | INTR_MPSAFE,
+			   tegra_pci_intr, NULL, sc, &sc->intr_cookie)) {
+		device_printf(dev, "cannot setup interrupt handler\n");
+		rv = ENXIO;
+		goto out;
+	}
+
+	/* Memory management. */
+	rv = tegra_pcib_decode_ranges(sc, ranges, nranges);
+	if (rv != 0)
+		goto out;
+
+	rv = tegra_pcib_rman_init(sc);
+	if (rv != 0)
+		goto out;
+	free(ranges, M_DEVBUF);
+	ranges = NULL;
+
+	/*
+	 * Enable PCIE device.
+	 */
+	rv = tegra_pcib_enable(sc, unit);
+	if (rv != 0)
+		goto out;
+	for (i = 0; i < TEGRA_PCIB_MAX_PORTS; i++) {
+		if (sc->ports[i] == NULL)
+			continue;
+		if (sc->ports[i]->enabled)
+			tegra_pcib_port_enable(sc, i);
+		else
+			tegra_pcib_port_disable(sc, i);
+	}
+
+	device_add_child(dev, "pci", -1);
+
+	return (bus_generic_attach(dev));
+
+out:
+	if (ranges != NULL)
+		free(ranges, M_DEVBUF);
+
+	return (rv);
+}
+
+
+static device_method_t tegra_pcib_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,			tegra_pcib_probe),
+	DEVMETHOD(device_attach,		tegra_pcib_attach),
+
+	/* Bus interface */
+	DEVMETHOD(bus_read_ivar,		tegra_pcib_read_ivar),
+	DEVMETHOD(bus_write_ivar,		tegra_pcib_write_ivar),
+	DEVMETHOD(bus_alloc_resource,		tegra_pcib_alloc_resource),
+	DEVMETHOD(bus_adjust_resource,		tegra_pcib_adjust_resource),
+	DEVMETHOD(bus_release_resource,		tegra_pcib_release_resource),
+	DEVMETHOD(bus_activate_resource,	tegra_pcib_pcie_activate_resource),
+	DEVMETHOD(bus_deactivate_resource,	bus_generic_deactivate_resource),
+	DEVMETHOD(bus_setup_intr,		bus_generic_setup_intr),
+	DEVMETHOD(bus_teardown_intr,		bus_generic_teardown_intr),
+
+	/* pcib interface */
+	DEVMETHOD(pcib_maxslots,		tegra_pcib_maxslots),
+	DEVMETHOD(pcib_read_config,		tegra_pcib_read_config),
+	DEVMETHOD(pcib_write_config,		tegra_pcib_write_config),
+	DEVMETHOD(pcib_route_interrupt,		tegra_pcib_route_interrupt),
+
+#if defined(TEGRA_PCI_MSI)
+	DEVMETHOD(pcib_alloc_msi,		tegra_pcib_alloc_msi),
+	DEVMETHOD(pcib_release_msi,		tegra_pcib_release_msi),
+	DEVMETHOD(pcib_map_msi,			tegra_pcib_map_msi),
+#endif
+
+	/* OFW bus interface */
+	DEVMETHOD(ofw_bus_get_compat,		ofw_bus_gen_get_compat),
+	DEVMETHOD(ofw_bus_get_model,		ofw_bus_gen_get_model),
+	DEVMETHOD(ofw_bus_get_name,		ofw_bus_gen_get_name),
+	DEVMETHOD(ofw_bus_get_node,		ofw_bus_gen_get_node),
+	DEVMETHOD(ofw_bus_get_type,		ofw_bus_gen_get_type),
+
+	DEVMETHOD_END
+};
+
+static driver_t tegra_pcib_driver = {
+	"pcib",
+	tegra_pcib_methods,
+	sizeof(struct tegra_pcib_softc),
+};
+
+devclass_t pcib_devclass;
+
+DRIVER_MODULE(pcib, simplebus, tegra_pcib_driver, pcib_devclass, 0, 0);
\ No newline at end of file
diff --git a/sys/arm/nvidia/tegra_pinmux.c b/sys/arm/nvidia/tegra_pinmux.c
new file mode 100644
index 0000000..6123746
--- /dev/null
+++ b/sys/arm/nvidia/tegra_pinmux.c
@@ -0,0 +1,804 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * Pin multiplexer driver for Tegra SoCs.
+ */
+#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 <machine/bus.h>
+#include <machine/fdt.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/fdt/fdt_pinctrl.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+/* Pin multipexor register. */
+#define	TEGRA_MUX_FUNCTION_MASK  0x03
+#define	TEGRA_MUX_FUNCTION_SHIFT 0
+#define	TEGRA_MUX_PUPD_MASK  0x03
+#define	TEGRA_MUX_PUPD_SHIFT 2
+#define	TEGRA_MUX_TRISTATE_SHIFT 4
+#define	TEGRA_MUX_ENABLE_INPUT_SHIFT 5
+#define	TEGRA_MUX_OPEN_DRAIN_SHIFT 6
+#define	TEGRA_MUX_LOCK_SHIFT 7
+#define	TEGRA_MUX_IORESET_SHIFT 8
+#define	TEGRA_MUX_RCV_SEL_SHIFT 9
+
+
+/* Pin goup register. */
+#define	TEGRA_GRP_HSM_SHIFT 2
+#define	TEGRA_GRP_SCHMT_SHIFT 3
+#define	TEGRA_GRP_DRV_TYPE_SHIFT 6
+#define	TEGRA_GRP_DRV_TYPE_MASK 0x03
+#define	TEGRA_GRP_DRV_DRVDN_SLWR_SHIFT 28
+#define	TEGRA_GRP_DRV_DRVDN_SLWR_MASK 0x03
+#define	TEGRA_GRP_DRV_DRVUP_SLWF_SHIFT 30
+#define	TEGRA_GRP_DRV_DRVUP_SLWF_MASK 0x03
+
+struct pinmux_softc {
+	device_t	dev;
+	struct resource	*pad_mem_res;
+	struct resource	*mux_mem_res;
+	struct resource	*mipi_mem_res;
+};
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-pinmux",	1},
+	{NULL,				0},
+};
+
+enum prop_id {
+	PROP_ID_PULL,
+	PROP_ID_TRISTATE,
+	PROP_ID_ENABLE_INPUT,
+	PROP_ID_OPEN_DRAIN,
+	PROP_ID_LOCK,
+	PROP_ID_IORESET,
+	PROP_ID_RCV_SEL,
+	PROP_ID_HIGH_SPEED_MODE,
+	PROP_ID_SCHMITT,
+	PROP_ID_LOW_POWER_MODE,
+	PROP_ID_DRIVE_DOWN_STRENGTH,
+	PROP_ID_DRIVE_UP_STRENGTH,
+	PROP_ID_SLEW_RATE_FALLING,
+	PROP_ID_SLEW_RATE_RISING,
+	PROP_ID_DRIVE_TYPE,
+
+	PROP_ID_MAX_ID
+};
+
+/* Numeric based parameters. */
+static const struct prop_name {
+	const char *name;
+	enum prop_id id;
+} prop_names[] = {
+	{"nvidia,pull",			PROP_ID_PULL},
+	{"nvidia,tristate",		PROP_ID_TRISTATE},
+	{"nvidia,enable-input",		PROP_ID_ENABLE_INPUT},
+	{"nvidia,open-drain",		PROP_ID_OPEN_DRAIN},
+	{"nvidia,lock",			PROP_ID_LOCK},
+	{"nvidia,io-reset",		PROP_ID_IORESET},
+	{"nvidia,rcv-sel",		PROP_ID_RCV_SEL},
+	{"nvidia,high-speed-mode",	PROP_ID_HIGH_SPEED_MODE},
+	{"nvidia,schmitt",		PROP_ID_SCHMITT},
+	{"nvidia,low-power-mode",	PROP_ID_LOW_POWER_MODE},
+	{"nvidia,pull-down-strength",	PROP_ID_DRIVE_DOWN_STRENGTH},
+	{"nvidia,pull-up-strength",	PROP_ID_DRIVE_UP_STRENGTH},
+	{"nvidia,slew-rate-falling",	PROP_ID_SLEW_RATE_FALLING},
+	{"nvidia,slew-rate-rising",	PROP_ID_SLEW_RATE_RISING},
+	{"nvidia,drive-type",		PROP_ID_DRIVE_TYPE},
+};
+
+/*
+ * configuration for one pin group.
+ */
+struct pincfg {
+	char	*function;
+	int	params[PROP_ID_MAX_ID];
+};
+#define	GPIO_BANK_A	 0
+#define	GPIO_BANK_B	 1
+#define	GPIO_BANK_C	 2
+#define	GPIO_BANK_D	 3
+#define	GPIO_BANK_E	 4
+#define	GPIO_BANK_F	 5
+#define	GPIO_BANK_G	 6
+#define	GPIO_BANK_H	 7
+#define	GPIO_BANK_I	 8
+#define	GPIO_BANK_J	 9
+#define	GPIO_BANK_K	10
+#define	GPIO_BANK_L	11
+#define	GPIO_BANK_M	12
+#define	GPIO_BANK_N	13
+#define	GPIO_BANK_O	14
+#define	GPIO_BANK_P	15
+#define	GPIO_BANK_Q	16
+#define	GPIO_BANK_R	17
+#define	GPIO_BANK_S	18
+#define	GPIO_BANK_T	19
+#define	GPIO_BANK_U	20
+#define	GPIO_BANK_V	21
+#define	GPIO_BANK_W	22
+#define	GPIO_BANK_X	23
+#define	GPIO_BANK_Y	24
+#define	GPIO_BANK_Z	25
+#define	GPIO_BANK_AA	26
+#define	GPIO_BANK_BB	27
+#define	GPIO_BANK_CC	28
+#define	GPIO_BANK_DD	29
+#define	GPIO_BANK_EE	30
+#define	GPIO_BANK_FF	31
+#define	GPIO_NUM(b, p) (8 * (b) + (p))
+
+struct tegra_mux {
+	char *name;
+	bus_size_t reg;
+	char *functions[4];
+	int gpio_num;
+};
+
+#define	GMUX(r, gb, gi, nm, f1, f2, f3, f4)				\
+{									\
+	.name = #nm,							\
+	.reg = r,							\
+	.gpio_num = GPIO_NUM(GPIO_BANK_##gb, gi),			\
+	.functions = {#f1, #f2, #f3, #f4},				\
+}
+
+#define	FMUX(r, nm, f1, f2, f3, f4)					\
+{									\
+	.name = #nm,							\
+	.reg = r,							\
+	.gpio_num = -1,							\
+	.functions = {#f1, #f2, #f3, #f4},				\
+}
+
+static const struct tegra_mux pin_mux_tbl[] = {
+	GMUX(0x000,  O, 1, ulpi_data0_po1,         spi3,       hsi,        uarta,        ulpi),
+	GMUX(0x004,  O, 2, ulpi_data1_po2,         spi3,       hsi,        uarta,        ulpi),
+	GMUX(0x008,  O, 3, ulpi_data2_po3,         spi3,       hsi,        uarta,        ulpi),
+	GMUX(0x00C,  O, 4, ulpi_data3_po4,         spi3,       hsi,        uarta,        ulpi),
+	GMUX(0x010,  O, 5, ulpi_data4_po5,         spi2,       hsi,        uarta,        ulpi),
+	GMUX(0x014,  O, 6, ulpi_data5_po6,         spi2,       hsi,        uarta,        ulpi),
+	GMUX(0x018,  O, 7, ulpi_data6_po7,         spi2,       hsi,        uarta,        ulpi),
+	GMUX(0x01C,  O, 0, ulpi_data7_po0,         spi2,       hsi,        uarta,        ulpi),
+	GMUX(0x020,  P, 9, ulpi_clk_py0,           spi1,       spi5,       uartd,        ulpi),
+	GMUX(0x024,  P, 1, ulpi_dir_py1,           spi1,       spi5,       uartd,        ulpi),
+	GMUX(0x028,  P, 2, ulpi_nxt_py2,           spi1,       spi5,       uartd,        ulpi),
+	GMUX(0x02C,  P, 3, ulpi_stp_py3,           spi1,       spi5,       uartd,        ulpi),
+	GMUX(0x030,  P, 0, dap3_fs_pp0,            i2s2,       spi5,       displaya,     displayb),
+	GMUX(0x034,  P, 1, dap3_din_pp1,           i2s2,       spi5,       displaya,     displayb),
+	GMUX(0x038,  P, 2, dap3_dout_pp2,          i2s2,       spi5,       displaya,     rsvd4),
+	GMUX(0x03C,  P, 3, dap3_sclk_pp3,          i2s2,       spi5,       rsvd3,        displayb),
+	GMUX(0x040,  V, 0, pv0,                    rsvd1,      rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x044,  V, 1, pv1,                    rsvd1,      rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x048,  Z, 0, sdmmc1_clk_pz0,         sdmmc1,     clk12,      rsvd3,        rsvd4),
+	GMUX(0x04C,  Z, 1, sdmmc1_cmd_pz1,         sdmmc1,     spdif,      spi4,         uarta),
+	GMUX(0x050,  Y, 4, sdmmc1_dat3_py4,        sdmmc1,     spdif,      spi4,         uarta),
+	GMUX(0x054,  Y, 5, sdmmc1_dat2_py5,        sdmmc1,     pwm0,       spi4,         uarta),
+	GMUX(0x058,  Y, 6, sdmmc1_dat1_py6,        sdmmc1,     pwm1,       spi4,         uarta),
+	GMUX(0x05C,  Y, 7, sdmmc1_dat0_py7,        sdmmc1,     rsvd2,      spi4,         uarta),
+	GMUX(0x068,  W, 5, clk2_out_pw5,           extperiph2, rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x06C, CC, 5, clk2_req_pcc5,          dap,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x110,  N, 7, hdmi_int_pn7,           rsvd1,      rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x114,  V, 4, ddc_scl_pv4,            i2c4,       rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x118,  V, 5, ddc_sda_pv5,            i2c4,       rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x164,  V, 3, uart2_rxd_pc3,          irda,       spdif,      uarta,        spi4),
+	GMUX(0x168,  C, 2, uart2_txd_pc2,          irda,       spdif,      uarta,        spi4),
+	GMUX(0x16C,  J, 6, uart2_rts_n_pj6,        uarta,      uartb,      gmi,          spi4),
+	GMUX(0x170,  J, 5, uart2_cts_n_pj5,        uarta,      uartb,      gmi,          spi4),
+	GMUX(0x174,  W, 6, uart3_txd_pw6,          uartc,      rsvd2,      gmi,          spi4),
+	GMUX(0x178,  W, 7, uart3_rxd_pw7,          uartc,      rsvd2,      gmi,          spi4),
+	GMUX(0x17C,  S, 1, uart3_cts_n_pa1,        uartc,      sdmmc1,     dtv,          gmi),
+	GMUX(0x180,  C, 0, uart3_rts_n_pc0,        uartc,      pwm0,       dtv,          gmi),
+	GMUX(0x184,  U, 0, pu0,                    owr,        uarta,      gmi,          rsvd4),
+	GMUX(0x188,  U, 1, pu1,                    rsvd1,      uarta,      gmi,          rsvd4),
+	GMUX(0x18C,  U, 2, pu2,                    rsvd1,      uarta,      gmi,          rsvd4),
+	GMUX(0x190,  U, 3, pu3,                    pwm0,       uarta,      gmi,          displayb),
+	GMUX(0x194,  U, 4, pu4,                    pwm1,       uarta,      gmi,          displayb),
+	GMUX(0x198,  U, 5, pu5,                    pwm2,       uarta,      gmi,          displayb),
+	GMUX(0x19C,  U, 6, pu6,                    pwm3,       uarta,      rsvd3,        gmi),
+	GMUX(0x1A0,  C, 5, gen1_i2c_sda_pc5,       i2c1,       rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x1A4,  C, 4, gen1_i2c_scl_pc4,       i2c1,       rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x1A8,  P, 3, dap4_fs_pp4,            i2s3,       gmi,        dtv,          rsvd4),
+	GMUX(0x1AC,  P, 4, dap4_din_pp5,           i2s3,       gmi,        rsvd3,        rsvd4),
+	GMUX(0x1B0,  P, 5, dap4_dout_pp6,          i2s3,       gmi,        dtv,          rsvd4),
+	GMUX(0x1B4,  P, 7, dap4_sclk_pp7,          i2s3,       gmi,        rsvd3,        rsvd4),
+	GMUX(0x1B8,  P, 0, clk3_out_pee0,          extperiph3, rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x1BC, EE, 1, clk3_req_pee1,          dev3,       rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x1C0,  C, 7, pc7,                    rsvd1,      rsvd2,      gmi,          gmi_alt),
+	GMUX(0x1C4,  I, 5, pi5,                    sdmmc2,     rsvd2,      gmi,          rsvd4),
+	GMUX(0x1C8,  I, 7, pi7,                    rsvd1,      trace,      gmi,          dtv),
+	GMUX(0x1CC,  K, 0, pk0,                    rsvd1,      sdmmc3,     gmi,          soc),
+	GMUX(0x1D0,  K, 1, pk1,                    sdmmc2,     trace,      gmi,          rsvd4),
+	GMUX(0x1D4,  J, 0, pj0,                    rsvd1,      rsvd2,      gmi,          usb),
+	GMUX(0x1D8,  J, 2, pj2,                    rsvd1,      rsvd2,      gmi,          soc),
+	GMUX(0x1DC,  K, 3, pk3,                    sdmmc2,     trace,      gmi,          ccla),
+	GMUX(0x1E0,  K, 4, pk4,                    sdmmc2,     rsvd2,      gmi,          gmi_alt),
+	GMUX(0x1E4,  K, 2, pk2,                    rsvd1,      rsvd2,      gmi,          rsvd4),
+	GMUX(0x1E8,  I, 3, pi3,                    rsvd1,      rsvd2,      gmi,          spi4),
+	GMUX(0x1EC,  I, 6, pi6,                    rsvd1,      rsvd2,      gmi,          sdmmc2),
+	GMUX(0x1F0,  G, 0, pg0,                    rsvd1,      rsvd2,      gmi,          rsvd4),
+	GMUX(0x1F4,  G, 1, pg1,                    rsvd1,      rsvd2,      gmi,          rsvd4),
+	GMUX(0x1F8,  G, 2, pg2,                    rsvd1,      trace,      gmi,          rsvd4),
+	GMUX(0x1FC,  G, 3, pg3,                    rsvd1,      trace,      gmi,          rsvd4),
+	GMUX(0x200,  G, 4, pg4,                    rsvd1,      tmds,       gmi,          spi4),
+	GMUX(0x204,  G, 5, pg5,                    rsvd1,      rsvd2,      gmi,          spi4),
+	GMUX(0x208,  G, 6, pg6,                    rsvd1,      rsvd2,      gmi,          spi4),
+	GMUX(0x20C,  G, 7, pg7,                    rsvd1,      rsvd2,      gmi,          spi4),
+	GMUX(0x210,  H, 0, ph0,                    pwm0,       trace,      gmi,          dtv),
+	GMUX(0x214,  H, 1, ph1,                    pwm1,       tmds,       gmi,          displaya),
+	GMUX(0x218,  H, 2, ph2,                    pwm2,       tmds,       gmi,          cldvfs),
+	GMUX(0x21C,  H, 3, ph3,                    pwm3,       spi4,       gmi,          cldvfs),
+	GMUX(0x220,  H, 4, ph4,                    sdmmc2,     rsvd2,      gmi,          rsvd4),
+	GMUX(0x224,  H, 5, ph5,                    sdmmc2,     rsvd2,      gmi,          rsvd4),
+	GMUX(0x228,  H, 6, ph6,                    sdmmc2,     trace,      gmi,          dtv),
+	GMUX(0x22C,  H, 7, ph7,                    sdmmc2,     trace,      gmi,          dtv),
+	GMUX(0x230,  J, 7, pj7,                    uartd,      rsvd2,      gmi,          gmi_alt),
+	GMUX(0x234,  B, 0, pb0,                    uartd,      rsvd2,      gmi,          rsvd4),
+	GMUX(0x238,  B, 1, pb1,                    uartd,      rsvd2,      gmi,          rsvd4),
+	GMUX(0x23C,  K, 7, pk7,                    uartd,      rsvd2,      gmi,          rsvd4),
+	GMUX(0x240,  I, 0, pi0,                    rsvd1,      rsvd2,      gmi,          rsvd4),
+	GMUX(0x244,  I, 1, pi1,                    rsvd1,      rsvd2,      gmi,          rsvd4),
+	GMUX(0x248,  I, 2, pi2,                    sdmmc2,     trace,      gmi,          rsvd4),
+	GMUX(0x24C,  I, 4, pi4,                    spi4,       trace,      gmi,          displaya),
+	GMUX(0x250,  T, 5, gen2_i2c_scl_pt5,       i2c2,       rsvd2,      gmi,          rsvd4),
+	GMUX(0x254,  T, 6, gen2_i2c_sda_pt6,       i2c2,       rsvd2,      gmi,          rsvd4),
+	GMUX(0x258, CC, 4, sdmmc4_clk_pcc4,        sdmmc4,     rsvd2,      gmi,          rsvd4),
+	GMUX(0x25C,  T, 7, sdmmc4_cmd_pt7,         sdmmc4,     rsvd2,      gmi,          rsvd4),
+	GMUX(0x260, AA, 0, sdmmc4_dat0_paa0,       sdmmc4,     spi3,       gmi,          rsvd4),
+	GMUX(0x264, AA, 1, sdmmc4_dat1_paa1,       sdmmc4,     spi3,       gmi,          rsvd4),
+	GMUX(0x268, AA, 2, sdmmc4_dat2_paa2,       sdmmc4,     spi3,       gmi,          rsvd4),
+	GMUX(0x26C, AA, 3, sdmmc4_dat3_paa3,       sdmmc4,     spi3,       gmi,          rsvd4),
+	GMUX(0x270, AA, 4, sdmmc4_dat4_paa4,       sdmmc4,     spi3,       gmi,          rsvd4),
+	GMUX(0x274, AA, 5, sdmmc4_dat5_paa5,       sdmmc4,     spi3,       rsvd3,        rsvd4),
+	GMUX(0x278, AA, 6, sdmmc4_dat6_paa6,       sdmmc4,     spi3,       gmi,          rsvd4),
+	GMUX(0x27C, AA, 7, sdmmc4_dat7_paa7,       sdmmc4,     rsvd2,      gmi,          rsvd4),
+	GMUX(0x284, CC, 0, cam_mclk_pcc0,          vi,         vi_alt1,    vi_alt3,      sdmmc2),
+	GMUX(0x288, CC, 1, pcc1,                   i2s4,       rsvd2,      rsvd3,        sdmmc2),
+	GMUX(0x28C, BB, 0, pbb0,                   vgp6,       vimclk2,    sdmmc2,       vimclk2_alt),
+	GMUX(0x290, BB, 1, cam_i2c_scl_pbb1,       vgp1,       i2c3,       rsvd3,        sdmmc2),
+	GMUX(0x294, BB, 2, cam_i2c_sda_pbb2,       vgp2,       i2c3,       rsvd3,        sdmmc2),
+	GMUX(0x298, BB, 3, pbb3,                   vgp3,       displaya,   displayb,     sdmmc2),
+	GMUX(0x29C, BB, 4, pbb4,                   vgp4,       displaya,   displayb,     sdmmc2),
+	GMUX(0x2A0, BB, 5, pbb5,                   vgp5,       displaya,   rsvd3,        sdmmc2),
+	GMUX(0x2A4, BB, 6, pbb6,                   i2s4,       rsvd2,      displayb,     sdmmc2),
+	GMUX(0x2A8, BB, 7, pbb7,                   i2s4,       rsvd2,      rsvd3,        sdmmc2),
+	GMUX(0x2AC, CC, 2, pcc2,                   i2s4,       rsvd2,      sdmmc3,       sdmmc2),
+	FMUX(0x2B0,        jtag_rtck,              rtck,       rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x2B4,  Z, 6, pwr_i2c_scl_pz6,        i2cpwr,     rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x2B8,  Z, 7, pwr_i2c_sda_pz7,        i2cpwr,     rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x2BC,  R, 0, kb_row0_pr0,            kbc,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x2C0,  R, 1, kb_row1_pr1,            kbc,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x2C4,  R, 2, kb_row2_pr2,            kbc,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x2C8,  R, 3, kb_row3_pr3,            kbc,        displaya,   sys,          displayb),
+	GMUX(0x2CC,  R, 4, kb_row4_pr4,            kbc,        displaya,   rsvd3,        displayb),
+	GMUX(0x2D0,  R, 5, kb_row5_pr5,            kbc,        displaya,   rsvd3,        displayb),
+	GMUX(0x2D4,  R, 6, kb_row6_pr6,            kbc,        displaya,   displaya_alt, displayb),
+	GMUX(0x2D8,  R, 7, kb_row7_pr7,            kbc,        rsvd2,      cldvfs,       uarta),
+	GMUX(0x2DC,  S, 0, kb_row8_ps0,            kbc,        rsvd2,      cldvfs,       uarta),
+	GMUX(0x2E0,  S, 1, kb_row9_ps1,            kbc,        rsvd2,      rsvd3,        uarta),
+	GMUX(0x2E4,  S, 2, kb_row10_ps2,           kbc,        rsvd2,      rsvd3,        uarta),
+	GMUX(0x2E8,  S, 3, kb_row11_ps3,           kbc,        rsvd2,      rsvd3,        irda),
+	GMUX(0x2EC,  S, 4, kb_row12_ps4,           kbc,        rsvd2,      rsvd3,        irda),
+	GMUX(0x2F0,  S, 5, kb_row13_ps5,           kbc,        rsvd2,      spi2,         rsvd4),
+	GMUX(0x2F4,  S, 6, kb_row14_ps6,           kbc,        rsvd2,      spi2,         rsvd4),
+	GMUX(0x2F8,  S, 7, kb_row15_ps7,           kbc,        soc,        rsvd3,        rsvd4),
+	GMUX(0x2FC,  Q, 0, kb_col0_pq0,            kbc,        rsvd2,      spi2,         rsvd4),
+	GMUX(0x300,  Q, 1, kb_col1_pq1,            kbc,        rsvd2,      spi2,         rsvd4),
+	GMUX(0x304,  Q, 2, kb_col2_pq2,            kbc,        rsvd2,      spi2,         rsvd4),
+	GMUX(0x308,  Q, 3, kb_col3_pq3,            kbc,        displaya,   pwm2,         uarta),
+	GMUX(0x30C,  Q, 4, kb_col4_pq4,            kbc,        owr,        sdmmc3,       uarta),
+	GMUX(0x310,  Q, 5, kb_col5_pq5,            kbc,        rsvd2,      sdmmc3,       rsvd4),
+	GMUX(0x314,  Q, 6, kb_col6_pq6,            kbc,        rsvd2,      spi2,         uartd),
+	GMUX(0x318,  Q, 7, kb_col7_pq7,            kbc,        rsvd2,      spi2,         uartd),
+	GMUX(0x31C,  A, 0, clk_32k_out_pa0,        blink,      soc,        rsvd3,        rsvd4),
+	FMUX(0x324,        core_pwr_req,           pwron,      rsvd2,      rsvd3,        rsvd4),
+	FMUX(0x328,        cpu_pwr_req,            cpu,        rsvd2,      rsvd3,        rsvd4),
+	FMUX(0x32C,        pwr_int_n,              pmi,        rsvd2,      rsvd3,        rsvd4),
+	FMUX(0x330,        clk_32k_in,             clk,        rsvd2,      rsvd3,        rsvd4),
+	FMUX(0x334,        owr,                    owr,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x338,  N, 0, dap1_fs_pn0,            i2s0,       hda,        gmi,          rsvd4),
+	GMUX(0x33C,  N, 1, dap1_din_pn1,           i2s0,       hda,        gmi,          rsvd4),
+	GMUX(0x340,  N, 2, dap1_dout_pn2,          i2s0,       hda,        gmi,          sata),
+	GMUX(0x344,  N, 3, dap1_sclk_pn3,          i2s0,       hda,        gmi,          rsvd4),
+	GMUX(0x348, EE, 2, dap_mclk1_req_pee2,     dap,        dap1,       sata,         rsvd4),
+	GMUX(0x34C,  W, 4, dap_mclk1_pw4,          extperiph1, dap2,       rsvd3,        rsvd4),
+	GMUX(0x350,  K, 6, spdif_in_pk6,           spdif,      rsvd2,      rsvd3,        i2c3),
+	GMUX(0x354,  K, 5, spdif_out_pk5,          spdif,      rsvd2,      rsvd3,        i2c3),
+	GMUX(0x358,  A, 2, dap2_fs_pa2,            i2s1,       hda,        gmi,          rsvd4),
+	GMUX(0x35C,  A, 4, dap2_din_pa4,           i2s1,       hda,        gmi,          rsvd4),
+	GMUX(0x360,  A, 5, dap2_dout_pa5,          i2s1,       hda,        gmi,          rsvd4),
+	GMUX(0x364,  A, 3, dap2_sclk_pa3,          i2s1,       hda,        gmi,          rsvd4),
+	GMUX(0x368,  X, 0, dvfs_pwm_px0,           spi6,       cldvfs,     gmi,          rsvd4),
+	GMUX(0x36C,  X, 1, gpio_x1_aud_px1,        spi6,       rsvd2,      gmi,          rsvd4),
+	GMUX(0x370,  X, 3, gpio_x3_aud_px3,        spi6,       spi1,       gmi,          rsvd4),
+	GMUX(0x374,  X, 2, dvfs_clk_px2,           spi6,       cldvfs,     gmi,          rsvd4),
+	GMUX(0x378,  X, 4, gpio_x4_aud_px4,        gmi,        spi1,       spi2,         dap2),
+	GMUX(0x37C,  X, 5, gpio_x5_aud_px5,        gmi,        spi1,       spi2,         rsvd4),
+	GMUX(0x380,  X, 6, gpio_x6_aud_px6,        spi6,       spi1,       spi2,         gmi),
+	GMUX(0x384,  X, 7, gpio_x7_aud_px7,        rsvd1,      spi1,       spi2,         rsvd4),
+	GMUX(0x390,  A, 6, sdmmc3_clk_pa6,         sdmmc3,     rsvd2,      rsvd3,        spi3),
+	GMUX(0x394,  A, 7, sdmmc3_cmd_pa7,         sdmmc3,     pwm3,       uarta,        spi3),
+	GMUX(0x398,  B, 7, sdmmc3_dat0_pb7,        sdmmc3,     rsvd2,      rsvd3,        spi3),
+	GMUX(0x39C,  B, 6, sdmmc3_dat1_pb6,        sdmmc3,     pwm2,       uarta,        spi3),
+	GMUX(0x3A0,  B, 5, sdmmc3_dat2_pb5,        sdmmc3,     pwm1,       displaya,     spi3),
+	GMUX(0x3A4,  B, 4, sdmmc3_dat3_pb4,        sdmmc3,     pwm0,       displayb,     spi3),
+	GMUX(0x3BC, DD, 1, pex_l0_rst_n_pdd1,      pe0,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x3C0, DD, 2, pex_l0_clkreq_n_pdd2,   pe0,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x3C4, DD, 3, pex_wake_n_pdd3,        pe,         rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x3CC, DD, 5, pex_l1_rst_n_pdd5,      pe1,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x3D0, DD, 6, pex_l1_clkreq_n_pdd6,   pe1,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x3E0, EE, 3, hdmi_cec_pee3,          cec,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x3E4,  V, 3, sdmmc1_wp_n_pv3,        sdmmc1,     clk12,      spi4,         uarta),
+	GMUX(0x3E8,  V, 2, sdmmc3_cd_n_pv2,        sdmmc3,     owr,        rsvd3,        rsvd4),
+	GMUX(0x3EC,  W, 2, gpio_w2_aud_pw2,        spi6,       rsvd2,      spi2,         i2c1),
+	GMUX(0x3F0,  W, 3, gpio_w3_aud_pw3,        spi6,       spi1,       spi2,         i2c1),
+	GMUX(0x3F4,  N, 4, usb_vbus_en0_pn4,       usb,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x3F8,  N, 5, usb_vbus_en1_pn5,       usb,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x3FC, EE, 5, sdmmc3_clk_lb_in_pee5,  sdmmc3,     rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x400, EE, 4, sdmmc3_clk_lb_out_pee4, sdmmc3,     rsvd2,      rsvd3,        rsvd4),
+	FMUX(0x404,        gmi_clk_lb,             sdmmc2,     rsvd2,      gmi,          rsvd4),
+	FMUX(0x408,        reset_out_n,            rsvd1,      rsvd2,      rsvd3,        reset_out_n),
+	GMUX(0x40C,  T, 0, kb_row16_pt0,           kbc,        rsvd2,      rsvd3,        uartc),
+	GMUX(0x410,  T, 1, kb_row17_pt1,           kbc,        rsvd2,      rsvd3,        uartc),
+	GMUX(0x414, FF, 1, usb_vbus_en2_pff1,      usb,        rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x418, FF, 2, pff2,                   sata,       rsvd2,      rsvd3,        rsvd4),
+	GMUX(0x430, FF, 0, dp_hpd_pff0,            dp,         rsvd2,      rsvd3,        rsvd4),
+};
+
+struct tegra_grp {
+	char *name;
+	bus_size_t reg;
+	int drvdn_shift;
+	int drvdn_mask;
+	int drvup_shift;
+	int drvup_mask;
+};
+
+#define	GRP(r, nm, dn_s, dn_w, up_s, up_w)				\
+{									\
+	.name = #nm,							\
+	.reg = r - 0x868,						\
+	.drvdn_shift = dn_s,						\
+	.drvdn_mask = (1 << dn_w) - 1,					\
+	.drvup_shift = up_s,						\
+	.drvup_mask = (1 << dn_w) - 1,					\
+}
+
+/* Use register offsets from TRM */
+static const struct tegra_grp pin_grp_tbl[] = {
+	GRP(0x868, ao1,          12,  5,  20,  5),
+	GRP(0x86C, ao2,          12,  5,  20,  5),
+	GRP(0x870, at1,          12,  7,  20,  7),
+	GRP(0x874, at2,          12,  7,  20,  7),
+	GRP(0x878, at3,          12,  7,  20,  7),
+	GRP(0x87C, at4,          12,  7,  20,  7),
+	GRP(0x880, at5,          14,  5,  19,  5),
+	GRP(0x884, cdev1,        12,  5,  20,  5),
+	GRP(0x888, cdev2,        12,  5,  20,  5),
+	GRP(0x890, dap1,         12,  5,  20,  5),
+	GRP(0x894, dap2,         12,  5,  20,  5),
+	GRP(0x898, dap3,         12,  5,  20,  5),
+	GRP(0x89C, dap4,         12,  5,  20,  5),
+	GRP(0x8A0, dbg,          12,  5,  20,  5),
+	GRP(0x8B0, sdio3,        12,  7,  20,  7),
+	GRP(0x8B4, spi,          12,  5,  20,  5),
+	GRP(0x8B8, uaa,          12,  5,  20,  5),
+	GRP(0x8BC, uab,          12,  5,  20,  5),
+	GRP(0x8C0, uart2,        12,  5,  20,  5),
+	GRP(0x8C4, uart3,        12,  5,  20,  5),
+	GRP(0x8EC, sdio1,        12,  7,  20,  7),
+	GRP(0x8FC, ddc,          12,  5,  20,  5),
+	GRP(0x900, gma,          14,  5,  20,  5),
+	GRP(0x910, gme,          14,  5,  19,  5),
+	GRP(0x914, gmf,          14,  5,  19,  5),
+	GRP(0x918, gmg,          14,  5,  19,  5),
+	GRP(0x91C, gmh,          14,  5,  19,  5),
+	GRP(0x920, owr,          12,  5,  20,  5),
+	GRP(0x924, uda,          12,  5,  20,  5),
+	GRP(0x928, gpv,          12,  5,  20,  5),
+	GRP(0x92C, dev3,         12,  5,  20,  5),
+	GRP(0x938, cec,          12,  5,  20,  5),
+	GRP(0x994, at6,          12,  7,  20,  7),
+	GRP(0x998, dap5,         12,  5,  20,  5),
+	GRP(0x99C, usb_vbus_en,  12,  5,  20,  5),
+	GRP(0x9A8, ao3,          12,  5,  -1,  0),
+	GRP(0x9B0, ao0,          12,  5,  20,  5),
+	GRP(0x9B4, hv0,          12,  5,  -1,  0),
+	GRP(0x9C4, sdio4,        12,  5,  20,  5),
+	GRP(0x9C8, ao4,          12,  7,  20,  7),
+};
+
+static const struct tegra_grp *
+pinmux_search_grp(char *grp_name)
+{
+	int i;
+
+	for (i = 0; i < nitems(pin_grp_tbl); i++) {
+		if (strcmp(grp_name, pin_grp_tbl[i].name) == 0)
+			return 	(&pin_grp_tbl[i]);
+	}
+	return (NULL);
+}
+
+static const struct tegra_mux *
+pinmux_search_mux(char *pin_name)
+{
+	int i;
+
+	for (i = 0; i < nitems(pin_mux_tbl); i++) {
+		if (strcmp(pin_name, pin_mux_tbl[i].name) == 0)
+			return 	(&pin_mux_tbl[i]);
+	}
+	return (NULL);
+}
+
+static int
+pinmux_mux_function(const struct tegra_mux *mux, char *fnc_name)
+{
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		if (strcmp(fnc_name, mux->functions[i]) == 0)
+			return 	(i);
+	}
+	return (-1);
+}
+
+static int
+pinmux_config_mux(struct pinmux_softc *sc, char *pin_name,
+    const struct tegra_mux *mux, struct pincfg *cfg)
+{
+	int tmp;
+	uint32_t reg;
+
+	reg = bus_read_4(sc->mux_mem_res, mux->reg);
+
+	if (cfg->function != NULL) {
+		tmp = pinmux_mux_function(mux, cfg->function);
+		if (tmp == -1) {
+			device_printf(sc->dev,
+			    "Unknown function %s for pin %s\n", cfg->function,
+			    pin_name);
+			return (ENXIO);
+		}
+		reg &= ~(TEGRA_MUX_FUNCTION_MASK << TEGRA_MUX_FUNCTION_SHIFT);
+		reg |=  (tmp & TEGRA_MUX_FUNCTION_MASK) <<
+		    TEGRA_MUX_FUNCTION_SHIFT;
+	}
+	if (cfg->params[PROP_ID_PULL] != -1) {
+		reg &= ~(TEGRA_MUX_PUPD_MASK << TEGRA_MUX_PUPD_SHIFT);
+		reg |=  (cfg->params[PROP_ID_PULL] & TEGRA_MUX_PUPD_MASK) <<
+		    TEGRA_MUX_PUPD_SHIFT;
+	}
+	if (cfg->params[PROP_ID_TRISTATE] != -1) {
+		reg &= ~(1 << TEGRA_MUX_TRISTATE_SHIFT);
+		reg |=  (cfg->params[PROP_ID_TRISTATE] & 1) <<
+		    TEGRA_MUX_TRISTATE_SHIFT;
+	}
+	if (cfg->params[TEGRA_MUX_ENABLE_INPUT_SHIFT] != -1) {
+		reg &= ~(1 << TEGRA_MUX_ENABLE_INPUT_SHIFT);
+		reg |=  (cfg->params[TEGRA_MUX_ENABLE_INPUT_SHIFT] & 1) <<
+		    TEGRA_MUX_ENABLE_INPUT_SHIFT;
+	}
+	if (cfg->params[PROP_ID_ENABLE_INPUT] != -1) {
+		reg &= ~(1 << TEGRA_MUX_ENABLE_INPUT_SHIFT);
+		reg |=  (cfg->params[PROP_ID_ENABLE_INPUT] & 1) <<
+		    TEGRA_MUX_ENABLE_INPUT_SHIFT;
+	}
+	if (cfg->params[PROP_ID_ENABLE_INPUT] != -1) {
+		reg &= ~(1 << TEGRA_MUX_ENABLE_INPUT_SHIFT);
+		reg |=  (cfg->params[PROP_ID_OPEN_DRAIN] & 1) <<
+		    TEGRA_MUX_ENABLE_INPUT_SHIFT;
+	}
+	if (cfg->params[PROP_ID_LOCK] != -1) {
+		reg &= ~(1 << TEGRA_MUX_LOCK_SHIFT);
+		reg |=  (cfg->params[PROP_ID_LOCK] & 1) <<
+		    TEGRA_MUX_LOCK_SHIFT;
+	}
+	if (cfg->params[PROP_ID_IORESET] != -1) {
+		reg &= ~(1 << TEGRA_MUX_IORESET_SHIFT);
+		reg |=  (cfg->params[PROP_ID_IORESET] & 1) <<
+		    TEGRA_MUX_IORESET_SHIFT;
+	}
+	if (cfg->params[PROP_ID_RCV_SEL] != -1) {
+		reg &= ~(1 << TEGRA_MUX_RCV_SEL_SHIFT);
+		reg |=  (cfg->params[PROP_ID_RCV_SEL] & 1) <<
+		    TEGRA_MUX_RCV_SEL_SHIFT;
+	}
+	bus_write_4(sc->mux_mem_res, mux->reg, reg);
+	return (0);
+}
+
+static int
+pinmux_config_grp(struct pinmux_softc *sc, char *grp_name,
+    const struct tegra_grp *grp, struct pincfg *cfg)
+{
+	uint32_t reg;
+
+	reg = bus_read_4(sc->pad_mem_res, grp->reg);
+
+	if (cfg->params[PROP_ID_HIGH_SPEED_MODE] != -1) {
+		reg &= ~(1 << TEGRA_GRP_HSM_SHIFT);
+		reg |=  (cfg->params[PROP_ID_HIGH_SPEED_MODE] & 1) <<
+		    TEGRA_GRP_HSM_SHIFT;
+	}
+	if (cfg->params[PROP_ID_SCHMITT] != -1) {
+		reg &= ~(1 << TEGRA_GRP_SCHMT_SHIFT);
+		reg |=  (cfg->params[PROP_ID_SCHMITT] & 1) <<
+		    TEGRA_GRP_SCHMT_SHIFT;
+	}
+	if (cfg->params[PROP_ID_DRIVE_TYPE] != -1) {
+		reg &= ~(TEGRA_GRP_DRV_TYPE_MASK << TEGRA_GRP_DRV_TYPE_SHIFT);
+		reg |=  (cfg->params[PROP_ID_DRIVE_TYPE] &
+		    TEGRA_GRP_DRV_TYPE_MASK) << TEGRA_GRP_DRV_TYPE_SHIFT;
+	}
+	if (cfg->params[PROP_ID_SLEW_RATE_RISING] != -1) {
+		reg &= ~(TEGRA_GRP_DRV_DRVDN_SLWR_MASK <<
+		    TEGRA_GRP_DRV_DRVDN_SLWR_SHIFT);
+		reg |=  (cfg->params[PROP_ID_SLEW_RATE_RISING] &
+		    TEGRA_GRP_DRV_DRVDN_SLWR_MASK) <<
+		    TEGRA_GRP_DRV_DRVDN_SLWR_SHIFT;
+	}
+	if (cfg->params[PROP_ID_SLEW_RATE_FALLING] != -1) {
+		reg &= ~(TEGRA_GRP_DRV_DRVUP_SLWF_MASK <<
+		    TEGRA_GRP_DRV_DRVUP_SLWF_SHIFT);
+		reg |=  (cfg->params[PROP_ID_SLEW_RATE_FALLING] &
+		    TEGRA_GRP_DRV_DRVUP_SLWF_MASK) <<
+		    TEGRA_GRP_DRV_DRVUP_SLWF_SHIFT;
+	}
+	if ((cfg->params[PROP_ID_DRIVE_DOWN_STRENGTH] != -1) &&
+		 (grp->drvdn_mask != -1)) {
+		reg &= ~(grp->drvdn_shift << grp->drvdn_mask);
+		reg |=  (cfg->params[PROP_ID_DRIVE_DOWN_STRENGTH] &
+		    grp->drvdn_mask) << grp->drvdn_shift;
+	}
+	if ((cfg->params[PROP_ID_DRIVE_UP_STRENGTH] != -1) &&
+		 (grp->drvup_mask != -1)) {
+		reg &= ~(grp->drvup_shift << grp->drvup_mask);
+		reg |=  (cfg->params[PROP_ID_DRIVE_UP_STRENGTH] &
+		    grp->drvup_mask) << grp->drvup_shift;
+	}
+	bus_write_4(sc->pad_mem_res, grp->reg, reg);
+	return (0);
+}
+
+static int
+pinmux_config_node(struct pinmux_softc *sc, char *pin_name, struct pincfg *cfg)
+{
+	const struct tegra_mux *mux;
+	const struct tegra_grp *grp;
+	uint32_t reg;
+	int rv;
+
+	/* Handle MIPI special case first */
+	if (strcmp(pin_name, "dsi_b") == 0) {
+		if (cfg->function == NULL) {
+			/* nothing to set */
+			return (0);
+		}
+		reg = bus_read_4(sc->mipi_mem_res, 0); /* register 0x820 */
+		if (strcmp(cfg->function, "csi") == 0)
+			reg &= ~(1 << 1);
+		else if (strcmp(cfg->function, "dsi_b") == 0)
+			reg |= (1 << 1);
+		bus_write_4(sc->mipi_mem_res, 0, reg); /* register 0x820 */
+	}
+
+	/* Handle pin muxes */
+	mux = pinmux_search_mux(pin_name);
+	if (mux != NULL) {
+		if (mux->gpio_num != -1) {
+			/* XXXX TODO: Reserve gpio here */
+		}
+		rv = pinmux_config_mux(sc, pin_name, mux, cfg);
+		return (rv);
+	}
+
+	/* Handle pin groups */
+	grp = pinmux_search_grp(pin_name);
+	if (grp != NULL) {
+		rv = pinmux_config_grp(sc, pin_name, grp, cfg);
+		return (rv);
+	}
+
+	device_printf(sc->dev, "Unknown pin: %s\n", pin_name);
+	return (ENXIO);
+}
+
+static int
+pinmux_read_node(struct pinmux_softc *sc, phandle_t node, struct pincfg *cfg,
+    char **pins, int *lpins)
+{
+	int rv, i;
+
+	*lpins = OF_getprop_alloc(node, "nvidia,pins", 1, (void **)pins);
+	if (*lpins <= 0)
+		return (ENOENT);
+
+	/* Read function (mux) settings. */
+	rv = OF_getprop_alloc(node, "nvidia,function", 1,
+	    (void **)&cfg->function);
+	if (rv <= 0)
+		cfg->function = NULL;
+
+	/* Read numeric properties. */
+	for (i = 0; i < PROP_ID_MAX_ID; i++) {
+		rv = OF_getencprop(node, prop_names[i].name, &cfg->params[i],
+		    sizeof(cfg->params[i]));
+		if (rv <= 0)
+			cfg->params[i] = -1;
+	}
+	return (0);
+}
+
+static int
+pinmux_process_node(struct pinmux_softc *sc, phandle_t node)
+{
+	struct pincfg cfg;
+	char *pins, *pname;
+	int i, len, lpins, rv;
+
+	rv = pinmux_read_node(sc, node, &cfg, &pins, &lpins);
+	if (rv != 0)
+		return (rv);
+
+	len = 0;
+	pname = pins;
+	do {
+		i = strlen(pname) + 1;
+		rv = pinmux_config_node(sc, pname, &cfg);
+		if (rv != 0)
+			device_printf(sc->dev,
+			    "Cannot configure pin: %s: %d\n", pname, rv);
+
+		len += i;
+		pname += i;
+	} while (len < lpins);
+
+	if (pins != NULL)
+		free(pins, M_OFWPROP);
+	if (cfg.function != NULL)
+		free(cfg.function, M_OFWPROP);
+	return (rv);
+}
+
+static int pinmux_configure(device_t dev, phandle_t cfgxref)
+{
+	struct pinmux_softc *sc;
+	phandle_t node, cfgnode;
+	int rv;
+
+	sc = device_get_softc(dev);
+	cfgnode = OF_node_from_xref(cfgxref);
+
+
+	for (node = OF_child(cfgnode); node != 0; node = OF_peer(node)) {
+		if (!fdt_is_enabled(node))
+			continue;
+		rv = pinmux_process_node(sc, node);
+	}
+	return (0);
+}
+
+static int
+pinmux_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_search_compatible(dev, compat_data)->ocd_data)
+		return (ENXIO);
+
+	device_set_desc(dev, "Tegra pin configuration");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+pinmux_detach(device_t dev)
+{
+
+	/* This device is always present. */
+	return (EBUSY);
+}
+
+static int
+pinmux_attach(device_t dev)
+{
+	struct pinmux_softc * sc;
+	int rid;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	rid = 0;
+	sc->pad_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->pad_mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	rid = 1;
+	sc->mux_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mux_mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	rid = 2;
+	sc->mipi_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mipi_mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	/* Register as a pinctrl device and process default configuration */
+	fdt_pinctrl_register(dev, NULL);
+	fdt_pinctrl_configure_by_name(dev, "boot");
+
+	return (0);
+}
+
+
+static device_method_t tegra_pinmux_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,         pinmux_probe),
+	DEVMETHOD(device_attach,        pinmux_attach),
+	DEVMETHOD(device_detach,        pinmux_detach),
+
+	/* fdt_pinctrl interface */
+	DEVMETHOD(fdt_pinctrl_configure,pinmux_configure),
+
+	DEVMETHOD_END
+};
+
+static driver_t tegra_pinmux_driver = {
+	"tegra_pinmux",
+	tegra_pinmux_methods,
+	sizeof(struct pinmux_softc),
+};
+
+static devclass_t tegra_pinmux_devclass;
+
+EARLY_DRIVER_MODULE(tegra_pinmux, simplebus, tegra_pinmux_driver,
+    tegra_pinmux_devclass, 0, 0, 71);
diff --git a/sys/arm/nvidia/tegra_pmc.h b/sys/arm/nvidia/tegra_pmc.h
new file mode 100644
index 0000000..933c408
--- /dev/null
+++ b/sys/arm/nvidia/tegra_pmc.h
@@ -0,0 +1,115 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _TEGRA_PMC_H_
+#define	_TEGRA_PMC_H_
+
+enum tegra_suspend_mode {
+      TEGRA_SUSPEND_NONE = 0,
+      TEGRA_SUSPEND_LP2, 	/* CPU voltage off */
+      TEGRA_SUSPEND_LP1, 	/* CPU voltage off, DRAM self-refresh */
+      TEGRA_SUSPEND_LP0, 	/* CPU + core voltage off, DRAM self-refresh */
+};
+
+/* PARTIDs for powergate */
+enum tegra_powergate_id {
+	TEGRA_POWERGATE_CRAIL	= 0,
+	TEGRA_POWERGATE_TD	= 1,
+	TEGRA_POWERGATE_VE	= 2,
+	TEGRA_POWERGATE_PCX	= 3,
+	TEGRA_POWERGATE_VDE	= 4,
+	TEGRA_POWERGATE_L2C	= 5,
+	TEGRA_POWERGATE_MPE	= 6,
+	TEGRA_POWERGATE_HEG	= 7,
+	TEGRA_POWERGATE_SAX	= 8,
+	TEGRA_POWERGATE_CE1	= 9,
+	TEGRA_POWERGATE_CE2	= 10,
+	TEGRA_POWERGATE_CE3	= 11,
+	TEGRA_POWERGATE_CELP	= 12,
+	/* */
+	TEGRA_POWERGATE_CE0	= 14,
+	TEGRA_POWERGATE_C0NC	= 15,
+	TEGRA_POWERGATE_C1NC	= 16,
+	TEGRA_POWERGATE_SOR	= 17,
+	TEGRA_POWERGATE_DIS	= 18,
+	TEGRA_POWERGATE_DISB	= 19,
+	TEGRA_POWERGATE_XUSBA	= 20,
+	TEGRA_POWERGATE_XUSBB	= 21,
+	TEGRA_POWERGATE_XUSBC	= 22,
+	TEGRA_POWERGATE_VIC	= 23,
+	TEGRA_POWERGATE_IRAM	= 24,
+	/* */
+	TEGRA_POWERGATE_3D	= 32
+
+};
+
+/* PARTIDs for power rails */
+enum tegra_powerrail_id {
+	TEGRA_IO_RAIL_CSIA	= 0,
+	TEGRA_IO_RAIL_CSIB	= 1,
+	TEGRA_IO_RAIL_DSI	= 2,
+	TEGRA_IO_RAIL_MIPI_BIAS	= 3,
+	TEGRA_IO_RAIL_PEX_BIAS	= 4,
+	TEGRA_IO_RAIL_PEX_CLK1	= 5,
+	TEGRA_IO_RAIL_PEX_CLK2	= 6,
+	TEGRA_IO_RAIL_USB0	= 9,
+	TEGRA_IO_RAIL_USB1	= 10,
+	TEGRA_IO_RAIL_USB2	= 11,
+	TEGRA_IO_RAIL_USB_BIAS	= 12,
+	TEGRA_IO_RAIL_NAND	= 13,
+	TEGRA_IO_RAIL_UART	= 14,
+	TEGRA_IO_RAIL_BB	= 15,
+	TEGRA_IO_RAIL_AUDIO	= 17,
+	TEGRA_IO_RAIL_HSIC	= 19,
+	TEGRA_IO_RAIL_COMP	= 22,
+	TEGRA_IO_RAIL_HDMI	= 28,
+	TEGRA_IO_RAIL_PEX_CNTRL	= 32,
+	TEGRA_IO_RAIL_SDMMC1	= 33,
+	TEGRA_IO_RAIL_SDMMC3	= 34,
+	TEGRA_IO_RAIL_SDMMC4	= 35,
+	TEGRA_IO_RAIL_CAM	= 36,
+	TEGRA_IO_RAIL_RES	= 37,
+	TEGRA_IO_RAIL_HV	= 38,
+	TEGRA_IO_RAIL_DSIB	= 39,
+	TEGRA_IO_RAIL_DSIC	= 40,
+	TEGRA_IO_RAIL_DSID	= 41,
+	TEGRA_IO_RAIL_CSIE	= 44,
+	TEGRA_IO_RAIL_LVDS	= 57,
+	TEGRA_IO_RAIL_SYS_DDC	= 58,
+};
+
+int tegra_powergate_is_powered(enum tegra_powergate_id id);
+int tegra_powergate_power_on(enum tegra_powergate_id id);
+int tegra_powergate_power_off(enum tegra_powergate_id id);
+int tegra_powergate_remove_clamping(enum tegra_powergate_id id);
+int tegra_powergate_sequence_power_up(enum tegra_powergate_id id,
+    clk_t clk, hwreset_t rst);
+int tegra_io_rail_power_on(int tegra_powerrail_id);
+int tegra_io_rail_power_off(int tegra_powerrail_id);
+
+#endif /*_TEGRA_PMC_H_*/
\ No newline at end of file
diff --git a/sys/arm/nvidia/tegra_rtc.c b/sys/arm/nvidia/tegra_rtc.c
new file mode 100644
index 0000000..c15ded3
--- /dev/null
+++ b/sys/arm/nvidia/tegra_rtc.c
@@ -0,0 +1,303 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * RTC driver for Tegra SoCs.
+ */
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/clock.h>
+#include <sys/kernel.h>
+#include <sys/limits.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/module.h>
+#include <sys/resource.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+#include <sys/rman.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include "clock_if.h"
+
+#define	RTC_CONTROL				0x00
+#define	RTC_BUSY				0x04
+#define	 RTC_BUSY_STATUS				(1 << 0)
+#define	RTC_SECONDS				0x08
+#define	RTC_SHADOW_SECONDS			0x0c
+#define	RTC_MILLI_SECONDS			0x10
+#define	RTC_SECONDS_ALARM0			0x14
+#define	RTC_SECONDS_ALARM1			0x18
+#define	RTC_MILLI_SECONDS_ALARM			0x1c
+#define	RTC_SECONDS_COUNTDOWN_ALARM		0x20
+#define	RTC_MILLI_SECONDS_COUNTDOW_ALARM	0x24
+#define	RTC_INTR_MASK				0x28
+#define	 RTC_INTR_MSEC_CDN_ALARM			(1 << 4)
+#define	 RTC_INTR_SEC_CDN_ALARM				(1 << 3)
+#define	 RTC_INTR_MSEC_ALARM				(1 << 2)
+#define	 RTC_INTR_SEC_ALARM1				(1 << 1)
+#define	 RTC_INTR_SEC_ALARM0				(1 << 0)
+
+#define	RTC_INTR_STATUS				0x2c
+#define	RTC_INTR_SOURCE				0x30
+#define	RTC_INTR_SET				0x34
+#define	RTC_CORRECTION_FACTOR			0x38
+
+#define	WR4(_sc, _r, _v)	bus_write_4((_sc)->mem_res, (_r), (_v))
+#define	RD4(_sc, _r)		bus_read_4((_sc)->mem_res, (_r))
+
+#define	LOCK(_sc)		mtx_lock(&(_sc)->mtx)
+#define	UNLOCK(_sc)		mtx_unlock(&(_sc)->mtx)
+#define	SLEEP(_sc, timeout)						\
+	mtx_sleep(sc, &sc->mtx, 0, "rtcwait", timeout);
+#define	LOCK_INIT(_sc)							\
+	mtx_init(&_sc->mtx, device_get_nameunit(_sc->dev), "tegra_rtc", MTX_DEF)
+#define	LOCK_DESTROY(_sc)	mtx_destroy(&_sc->mtx)
+#define	ASSERT_LOCKED(_sc)	mtx_assert(&_sc->mtx, MA_OWNED)
+#define	ASSERT_UNLOCKED(_sc)	mtx_assert(&_sc->mtx, MA_NOTOWNED)
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-rtc",	1},
+	{NULL,			0}
+};
+
+struct tegra_rtc_softc {
+	device_t		dev;
+	struct mtx		mtx;
+
+	struct resource		*mem_res;
+	struct resource		*irq_res;
+	void			*irq_h;
+
+	clk_t			clk;
+	uint32_t		core_freq;
+};
+
+static void
+tegra_rtc_wait(struct tegra_rtc_softc *sc)
+{
+	int timeout;
+
+	for (timeout = 500; timeout >0; timeout--) {
+		if ((RD4(sc, RTC_BUSY) & RTC_BUSY_STATUS) == 0)
+			break;
+		DELAY(1);
+	}
+	if (timeout <= 0)
+		device_printf(sc->dev, "Device busy timeouted\n");
+
+}
+
+/*
+ * Get the time of day clock and return it in ts.
+ * Return 0 on success, an error number otherwise.
+ */
+static int
+tegra_rtc_gettime(device_t dev, struct timespec *ts)
+{
+	struct tegra_rtc_softc *sc;
+	struct timeval tv;
+	uint32_t msec, sec;
+
+	sc = device_get_softc(dev);
+
+	LOCK(sc);
+	msec = RD4(sc, RTC_MILLI_SECONDS);
+	sec = RD4(sc, RTC_SHADOW_SECONDS);
+	UNLOCK(sc);
+	tv.tv_sec = sec;
+	tv.tv_usec = msec * 1000;
+	TIMEVAL_TO_TIMESPEC(&tv, ts);
+	return (0);
+}
+
+
+static int
+tegra_rtc_settime(device_t dev, struct timespec *ts)
+{
+	struct tegra_rtc_softc *sc;
+	struct timeval tv;
+
+	sc = device_get_softc(dev);
+
+	LOCK(sc);
+	TIMESPEC_TO_TIMEVAL(&tv, ts);
+	tegra_rtc_wait(sc);
+	WR4(sc, RTC_SECONDS, tv.tv_sec);
+	UNLOCK(sc);
+
+	return (0);
+}
+
+
+static void
+tegra_rtc_intr(void *arg)
+{
+	struct tegra_rtc_softc *sc;
+	uint32_t status;
+
+	sc = (struct tegra_rtc_softc *)arg;
+	LOCK(sc);
+	status = RD4(sc, RTC_INTR_STATUS);
+	WR4(sc, RTC_INTR_STATUS, status);
+	UNLOCK(sc);
+}
+
+static int
+tegra_rtc_probe(device_t dev)
+{
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+tegra_rtc_attach(device_t dev)
+{
+	int rv, rid;
+	struct tegra_rtc_softc *sc;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	LOCK_INIT(sc);
+
+	/* Get the memory resource for the register mapping. */
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot map registers.\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	/* Allocate our IRQ resource. */
+	rid = 0;
+	sc->irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	    RF_ACTIVE);
+	if (sc->irq_res == NULL) {
+		device_printf(dev, "Cannot allocate interrupt.\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	/* OFW resources. */
+	rv = clk_get_by_ofw_index(dev, 0, &sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get i2c clock: %d\n", rv);
+		goto fail;
+	}
+	rv = clk_enable(sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable clock: %d\n", rv);
+		goto fail;
+	}
+
+	/* Init hardware. */
+	WR4(sc, RTC_SECONDS_ALARM0, 0);
+	WR4(sc, RTC_SECONDS_ALARM1, 0);
+	WR4(sc, RTC_INTR_STATUS, 0xFFFFFFFF);
+	WR4(sc, RTC_INTR_MASK, 0);
+
+	/* Setup  interrupt */
+	rv = bus_setup_intr(dev, sc->irq_res, INTR_TYPE_MISC | INTR_MPSAFE,
+	    NULL, tegra_rtc_intr, sc, &sc->irq_h);
+	if (rv) {
+		device_printf(dev, "Cannot setup interrupt.\n");
+		goto fail;
+	}
+
+	/*
+	 * Register as a time of day clock with 1-second resolution.
+	 *
+	 * XXXX Not yet, we don't have support for multiple RTCs
+	 */
+	/* clock_register(dev, 1000000); */
+
+	return (bus_generic_attach(dev));
+
+fail:
+	if (sc->clk != NULL)
+		clk_release(sc->clk);
+	if (sc->irq_h != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->irq_h);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+	LOCK_DESTROY(sc);
+
+	return (rv);
+}
+
+static int
+tegra_rtc_detach(device_t dev)
+{
+	struct tegra_rtc_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (sc->irq_h != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->irq_h);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+
+	LOCK_DESTROY(sc);
+	return (bus_generic_detach(dev));
+}
+
+static device_method_t tegra_rtc_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		tegra_rtc_probe),
+	DEVMETHOD(device_attach,	tegra_rtc_attach),
+	DEVMETHOD(device_detach,	tegra_rtc_detach),
+
+	/* clock interface */
+	DEVMETHOD(clock_gettime,	tegra_rtc_gettime),
+	DEVMETHOD(clock_settime,	tegra_rtc_settime),
+
+	DEVMETHOD_END
+};
+
+DEFINE_CLASS_0(tegra_rtc, tegra_rtc_driver, tegra_rtc_methods,
+    sizeof(struct tegra_rtc_softc));
+static devclass_t tegra_rtc_devclass;
+DRIVER_MODULE(tegra_rtc, simplebus, tegra_rtc_driver, tegra_rtc_devclass, 0, 0);
diff --git a/sys/arm/nvidia/tegra_sdhci.c b/sys/arm/nvidia/tegra_sdhci.c
new file mode 100644
index 0000000..e2ec063
--- /dev/null
+++ b/sys/arm/nvidia/tegra_sdhci.c
@@ -0,0 +1,465 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * SDHCI driver glue for NVIDIA Tegra family
+ *
+ */
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/types.h>
+#include <sys/bus.h>
+#include <sys/callout.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/module.h>
+#include <sys/mutex.h>
+#include <sys/resource.h>
+#include <sys/rman.h>
+#include <sys/sysctl.h>
+#include <sys/taskqueue.h>
+#include <sys/time.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+#include <machine/intr.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/gpio/gpiobusvar.h>
+#include <dev/mmc/bridge.h>
+#include <dev/mmc/mmcreg.h>
+#include <dev/mmc/mmcbrvar.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+#include <dev/sdhci/sdhci.h>
+
+#include "sdhci_if.h"
+
+/* Tegra SDHOST controller vendor register definitions */
+#define	SDMMC_VENDOR_CLOCK_CNTRL		0x100
+#define	 VENDOR_CLOCK_CNTRL_CLK_SHIFT			8
+#define	 VENDOR_CLOCK_CNTRL_CLK_MASK			0xFF
+#define	SDMMC_VENDOR_SYS_SW_CNTRL		0x104
+#define	SDMMC_VENDOR_CAP_OVERRIDES		0x10C
+#define	SDMMC_VENDOR_BOOT_CNTRL			0x110
+#define	SDMMC_VENDOR_BOOT_ACK_TIMEOUT		0x114
+#define	SDMMC_VENDOR_BOOT_DAT_TIMEOUT		0x118
+#define	SDMMC_VENDOR_DEBOUNCE_COUNT		0x11C
+#define	SDMMC_VENDOR_MISC_CNTRL			0x120
+#define	 VENDOR_MISC_CTRL_ENABLE_SDR104			0x8
+#define	 VENDOR_MISC_CTRL_ENABLE_SDR50			0x10
+#define	 VENDOR_MISC_CTRL_ENABLE_SDHCI_SPEC_300		0x20
+#define	 VENDOR_MISC_CTRL_ENABLE_DDR50			0x200
+#define	SDMMC_MAX_CURRENT_OVERRIDE		0x124
+#define	SDMMC_MAX_CURRENT_OVERRIDE_HI		0x128
+#define	SDMMC_VENDOR_CLK_GATE_HYSTERESIS_COUNT 	0x1D0
+#define	SDMMC_VENDOR_PHWRESET_VAL0		0x1D4
+#define	SDMMC_VENDOR_PHWRESET_VAL1		0x1D8
+#define	SDMMC_VENDOR_PHWRESET_VAL2		0x1DC
+#define	SDMMC_SDMEMCOMPPADCTRL_0		0x1E0
+#define	SDMMC_AUTO_CAL_CONFIG			0x1E4
+#define	SDMMC_AUTO_CAL_INTERVAL			0x1E8
+#define	SDMMC_AUTO_CAL_STATUS			0x1EC
+#define	SDMMC_SDMMC_MCCIF_FIFOCTRL		0x1F4
+#define	SDMMC_TIMEOUT_WCOAL_SDMMC		0x1F8
+
+/* Compatible devices. */
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-sdhci",	1},
+	{NULL,				0},
+};
+
+struct tegra_sdhci_softc {
+	device_t		dev;
+	struct resource *	mem_res;
+	struct resource *	irq_res;
+	void *			intr_cookie;
+	u_int			quirks;	/* Chip specific quirks */
+	u_int			caps;	/* If we override SDHCI_CAPABILITIES */
+	uint32_t		max_clk; /* Max possible freq */
+	clk_t			clk;
+	hwreset_t 		reset;
+	gpio_pin_t		gpio_cd;
+	gpio_pin_t		gpio_wp;
+	gpio_pin_t		gpio_power;
+
+	int			force_card_present;
+	struct sdhci_slot	slot;
+
+};
+
+static inline uint32_t
+RD4(struct tegra_sdhci_softc *sc, bus_size_t off)
+{
+
+	return (bus_read_4(sc->mem_res, off));
+}
+
+static uint8_t
+tegra_sdhci_read_1(device_t dev, struct sdhci_slot *slot, bus_size_t off)
+{
+	struct tegra_sdhci_softc *sc;
+
+	sc = device_get_softc(dev);
+	return (bus_read_1(sc->mem_res, off));
+}
+
+static uint16_t
+tegra_sdhci_read_2(device_t dev, struct sdhci_slot *slot, bus_size_t off)
+{
+	struct tegra_sdhci_softc *sc;
+
+	sc = device_get_softc(dev);
+	return (bus_read_2(sc->mem_res, off));
+}
+
+static uint32_t
+tegra_sdhci_read_4(device_t dev, struct sdhci_slot *slot, bus_size_t off)
+{
+	struct tegra_sdhci_softc *sc;
+	uint32_t val32;
+
+	sc = device_get_softc(dev);
+	val32 = bus_read_4(sc->mem_res, off);
+	/* Force the card-present state if necessary. */
+	if (off == SDHCI_PRESENT_STATE && sc->force_card_present)
+		val32 |= SDHCI_CARD_PRESENT;
+	return (val32);
+}
+
+static void
+tegra_sdhci_read_multi_4(device_t dev, struct sdhci_slot *slot, bus_size_t off,
+    uint32_t *data, bus_size_t count)
+{
+	struct tegra_sdhci_softc *sc;
+
+	sc = device_get_softc(dev);
+	bus_read_multi_4(sc->mem_res, off, data, count);
+}
+
+static void
+tegra_sdhci_write_1(device_t dev, struct sdhci_slot *slot, bus_size_t off,
+    uint8_t val)
+{
+	struct tegra_sdhci_softc *sc;
+
+	sc = device_get_softc(dev);
+	bus_write_1(sc->mem_res, off, val);
+}
+
+static void
+tegra_sdhci_write_2(device_t dev, struct sdhci_slot *slot, bus_size_t off,
+    uint16_t val)
+{
+	struct tegra_sdhci_softc *sc;
+
+	sc = device_get_softc(dev);
+	bus_write_2(sc->mem_res, off, val);
+}
+
+static void
+tegra_sdhci_write_4(device_t dev, struct sdhci_slot *slot, bus_size_t off,
+    uint32_t val)
+{
+	struct tegra_sdhci_softc *sc;
+
+	sc = device_get_softc(dev);
+	bus_write_4(sc->mem_res, off, val);
+}
+
+static void
+tegra_sdhci_write_multi_4(device_t dev, struct sdhci_slot *slot, bus_size_t off,
+    uint32_t *data, bus_size_t count)
+{
+	struct tegra_sdhci_softc *sc;
+
+	sc = device_get_softc(dev);
+	bus_write_multi_4(sc->mem_res, off, data, count);
+}
+
+static void
+tegra_sdhci_intr(void *arg)
+{
+	struct tegra_sdhci_softc *sc = arg;
+
+	sdhci_generic_intr(&sc->slot);
+	RD4(sc, SDHCI_INT_STATUS);
+}
+
+static int
+tegra_generic_get_ro(device_t brdev, device_t reqdev)
+{
+
+	return (0);
+}
+
+static int
+tegra_sdhci_probe(device_t dev)
+{
+	struct tegra_sdhci_softc *sc;
+	phandle_t node;
+	pcell_t cid;
+	const struct ofw_compat_data *cd;
+
+	sc = device_get_softc(dev);
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_is_compatible(dev, "nvidia,tegra124-sdhci")) {
+		device_set_desc(dev, "Tegra SDHCI controller");
+	} else
+		return (ENXIO);
+	cd = ofw_bus_search_compatible(dev, compat_data);
+	if (cd->ocd_data == 0)
+		return (ENXIO);
+
+	node = ofw_bus_get_node(dev);
+
+	/* Allow dts to patch quirks, slots, and max-frequency. */
+	if ((OF_getencprop(node, "quirks", &cid, sizeof(cid))) > 0)
+		sc->quirks = cid;
+	if ((OF_getencprop(node, "max-frequency", &cid, sizeof(cid))) > 0)
+		sc->max_clk = cid;
+
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+tegra_sdhci_attach(device_t dev)
+{
+	struct tegra_sdhci_softc *sc;
+	int rid, rv;
+	uint64_t freq;
+	phandle_t node, prop;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	node = ofw_bus_get_node(dev);
+
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (!sc->mem_res) {
+		device_printf(dev, "cannot allocate memory window\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	rid = 0;
+	sc->irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	    RF_ACTIVE);
+	if (!sc->irq_res) {
+		device_printf(dev, "cannot allocate interrupt\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	if (bus_setup_intr(dev, sc->irq_res, INTR_TYPE_BIO | INTR_MPSAFE,
+	    NULL, tegra_sdhci_intr, sc, &sc->intr_cookie)) {
+		device_printf(dev, "cannot setup interrupt handler\n");
+		rv = ENXIO;
+		goto fail;
+	}
+
+	rv = hwreset_get_by_ofw_name(sc->dev, "sdhci", &sc->reset);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'sdhci' reset\n");
+		goto fail;
+	}
+	rv = hwreset_deassert(sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot unreset 'sdhci' reset\n");
+		goto fail;
+	}
+
+	gpio_pin_get_by_ofw_property(sc->dev, "cd-gpios", &sc->gpio_cd);
+	gpio_pin_get_by_ofw_property(sc->dev, "power-gpios", &sc->gpio_power);
+	gpio_pin_get_by_ofw_property(sc->dev, "wp-gpios", &sc->gpio_wp);
+
+	rv = clk_get_by_ofw_index(dev, 0, &sc->clk);
+	if (rv != 0) {
+
+		device_printf(dev, "Cannot get clock\n");
+		goto fail;
+	}
+
+	rv = clk_get_by_ofw_index(dev, 0, &sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get clock\n");
+		goto fail;
+	}
+	rv = clk_enable(sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable clock\n");
+		goto fail;
+	}
+	rv = clk_set_freq(sc->clk, 48000000, CLK_SET_ROUND_DOWN);
+	if (rv != 0) {
+		device_printf(dev, "Cannot set clock\n");
+	}
+	rv = clk_get_freq(sc->clk, &freq);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get clock frequency\n");
+		goto fail;
+	}
+	if (bootverbose)
+		device_printf(dev, " Base MMC clock: %lld\n", freq);
+
+	/* Fill slot information. */
+	sc->max_clk = (int)freq;
+	sc->quirks |= SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
+	    SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
+	    SDHCI_QUIRK_MISSING_CAPS;
+
+	/* Limit real slot capabilities. */
+	sc->caps = RD4(sc, SDHCI_CAPABILITIES);
+	if (OF_getencprop(node, "bus-width", &prop, sizeof(prop)) > 0) {
+		sc->caps &= ~(MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA);
+		switch (prop) {
+		case 8:
+			sc->caps |= MMC_CAP_8_BIT_DATA;
+			/* FALLTHROUGH */
+		case 4:
+			sc->caps |= MMC_CAP_4_BIT_DATA;
+			break;
+		case 1:
+			break;
+		default:
+			device_printf(dev, "Bad bus-width value %u\n", prop);
+			break;
+		}
+	}
+	if (OF_hasprop(node, "non-removable"))
+		sc->force_card_present = 1;
+	/*
+	 * Clear clock field, so SDHCI driver uses supplied frequency.
+	 * in sc->slot.max_clk
+	 */
+	sc->caps &= ~SDHCI_CLOCK_V3_BASE_MASK;
+
+	sc->slot.quirks = sc->quirks;
+	sc->slot.max_clk = sc->max_clk;
+	sc->slot.caps = sc->caps;
+
+	rv = sdhci_init_slot(dev, &sc->slot, 0);
+	if (rv != 0) {
+		goto fail;
+	}
+
+	bus_generic_probe(dev);
+	bus_generic_attach(dev);
+
+	sdhci_start_slot(&sc->slot);
+
+	return (0);
+
+fail:
+	if (sc->gpio_cd != NULL)
+		gpio_pin_release(sc->gpio_cd);
+	if (sc->gpio_wp != NULL)
+		gpio_pin_release(sc->gpio_wp);
+	if (sc->gpio_power != NULL)
+		gpio_pin_release(sc->gpio_power);
+	if (sc->clk != NULL)
+		clk_release(sc->clk);
+	if (sc->reset != NULL)
+		hwreset_release(sc->reset);
+	if (sc->intr_cookie != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->intr_cookie);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+
+	return (rv);
+}
+
+static int
+tegra_sdhci_detach(device_t dev)
+{
+	struct tegra_sdhci_softc *sc = device_get_softc(dev);
+	struct sdhci_slot *slot = &sc->slot;
+
+	bus_generic_detach(dev);
+	clk_release(sc->clk);
+	bus_teardown_intr(dev, sc->irq_res, sc->intr_cookie);
+	bus_release_resource(dev, SYS_RES_IRQ, rman_get_rid(sc->irq_res),
+			     sc->irq_res);
+
+	sdhci_cleanup_slot(slot);
+	bus_release_resource(dev, SYS_RES_MEMORY,
+			     rman_get_rid(sc->mem_res),
+			     sc->mem_res);
+	return (0);
+}
+
+static device_method_t tegra_sdhci_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		tegra_sdhci_probe),
+	DEVMETHOD(device_attach,	tegra_sdhci_attach),
+	DEVMETHOD(device_detach,	tegra_sdhci_detach),
+
+	/* Bus interface */
+	DEVMETHOD(bus_read_ivar,	sdhci_generic_read_ivar),
+	DEVMETHOD(bus_write_ivar,	sdhci_generic_write_ivar),
+
+	/* MMC bridge interface */
+	DEVMETHOD(mmcbr_update_ios,	sdhci_generic_update_ios),
+	DEVMETHOD(mmcbr_request,	sdhci_generic_request),
+	DEVMETHOD(mmcbr_get_ro,		tegra_generic_get_ro),
+	DEVMETHOD(mmcbr_acquire_host,	sdhci_generic_acquire_host),
+	DEVMETHOD(mmcbr_release_host,	sdhci_generic_release_host),
+
+	/* SDHCI registers accessors */
+	DEVMETHOD(sdhci_read_1,		tegra_sdhci_read_1),
+	DEVMETHOD(sdhci_read_2,		tegra_sdhci_read_2),
+	DEVMETHOD(sdhci_read_4,		tegra_sdhci_read_4),
+	DEVMETHOD(sdhci_read_multi_4,	tegra_sdhci_read_multi_4),
+	DEVMETHOD(sdhci_write_1,	tegra_sdhci_write_1),
+	DEVMETHOD(sdhci_write_2,	tegra_sdhci_write_2),
+	DEVMETHOD(sdhci_write_4,	tegra_sdhci_write_4),
+	DEVMETHOD(sdhci_write_multi_4,	tegra_sdhci_write_multi_4),
+
+	{ 0, 0 }
+};
+
+static devclass_t tegra_sdhci_devclass;
+
+static driver_t tegra_sdhci_driver = {
+	"sdhci_tegra",
+	tegra_sdhci_methods,
+	sizeof(struct tegra_sdhci_softc),
+};
+
+DRIVER_MODULE(sdhci_tegra, simplebus, tegra_sdhci_driver, tegra_sdhci_devclass,
+    0, 0);
+MODULE_DEPEND(sdhci_tegra, sdhci, 1, 1, 1);
+DRIVER_MODULE(mmc, sdhci_tegra, mmc_driver, mmc_devclass, NULL, NULL);
diff --git a/sys/arm/nvidia/tegra_soctherm.c b/sys/arm/nvidia/tegra_soctherm.c
new file mode 100644
index 0000000..3102173
--- /dev/null
+++ b/sys/arm/nvidia/tegra_soctherm.c
@@ -0,0 +1,696 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+/*
+ * Thermometer and thermal zones driver for Tegra SoCs.
+ * Calibration data and algo are taken from Linux, because this part of SoC
+ * is undocumented in TRM.
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/gpio.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/sysctl.h>
+
+#include <machine/bus.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <arm/nvidia/tegra_efuse.h>
+#include <gnu/dts/include/dt-bindings/thermal/tegra124-soctherm.h>
+#include "tegra_soctherm_if.h"
+
+/* Per sensors registers - base is 0x0c0*/
+#define	TSENSOR_CONFIG0				0x000
+#define	 TSENSOR_CONFIG0_TALL(x)			(((x) & 0xFFFFF) << 8)
+#define	 TSENSOR_CONFIG0_STATUS_CLR			(1 << 5)
+#define	 TSENSOR_CONFIG0_TCALC_OVERFLOW			(1 << 4)
+#define	 TSENSOR_CONFIG0_OVERFLOW			(1 << 3)
+#define	 TSENSOR_CONFIG0_CPTR_OVERFLOW			(1 << 2)
+#define	 TSENSOR_CONFIG0_RO_SEL				(1 << 1)
+#define	 TSENSOR_CONFIG0_STOP				(1 << 0)
+
+#define	TSENSOR_CONFIG1				0x004
+#define	 TSENSOR_CONFIG1_TEMP_ENABLE			(1U << 31)
+#define	 TSENSOR_CONFIG1_TEN_COUNT(x)			(((x) & 0x3F) << 24)
+#define	 TSENSOR_CONFIG1_TIDDQ_EN(x)			(((x) & 0x3F) << 15)
+#define	 TSENSOR_CONFIG1_TSAMPLE(x)			(((x) & 0x3FF) << 0)
+
+#define	TSENSOR_CONFIG2				0x008
+#define	TSENSOR_CONFIG2_THERMA(x)			(((x) & 0xFFFF) << 16)
+#define	TSENSOR_CONFIG2_THERMB(x)			(((x) & 0xFFFF) << 0)
+
+#define	TSENSOR_STATUS0				0x00c
+#define	 TSENSOR_STATUS0_CAPTURE_VALID			(1U << 31)
+#define	 TSENSOR_STATUS0_CAPTURE(x)			(((x) >> 0) & 0xffff)
+
+#define	TSENSOR_STATUS1				0x010
+#define	 TSENSOR_STATUS1_TEMP_VALID			(1U << 31)
+#define	 TSENSOR_STATUS1_TEMP(x)			(((x) >> 0) & 0xffff)
+
+#define	TSENSOR_STATUS2				0x014
+#define	 TSENSOR_STATUS2_TEMP_MAX(x)			(((x) >> 16) & 0xffff)
+#define	 TSENSOR_STATUS2_TEMP_MIN(x)			(((x) >>  0) & 0xffff)
+
+/* Global registers */
+#define	TSENSOR_PDIV				0x1c0
+#define	 TSENSOR_PDIV_T124				0x8888
+#define	TSENSOR_HOTSPOT_OFF			0x1c4
+#define	 TSENSOR_HOTSPOT_OFF_T124			0x00060600
+#define	TSENSOR_TEMP1				0x1c8
+#define	TSENSOR_TEMP2				0x1cc
+
+/* Readbacks */
+#define	READBACK_VALUE_MASK				0xff00
+#define	READBACK_VALUE_SHIFT				8
+#define	READBACK_ADD_HALF				(1 << 7)
+#define	READBACK_NEGATE					(1 << 0)
+
+
+/* Fuses */
+#define	 FUSE_TSENSOR_CALIB_CP_TS_BASE_SHIFT		0
+#define	 FUSE_TSENSOR_CALIB_CP_TS_BASE_BITS		13
+#define	 FUSE_TSENSOR_CALIB_FT_TS_BASE_SHIFT		13
+#define	 FUSE_TSENSOR_CALIB_FT_TS_BASE_BITS		13
+
+#define	FUSE_TSENSOR8_CALIB			0x180
+#define	 FUSE_TSENSOR8_CALIB_CP_TS_BASE(x)		(((x) >>  0) & 0x3ff)
+#define	 FUSE_TSENSOR8_CALIB_FT_TS_BASE(x)		(((x) >> 10) & 0x7ff)
+
+#define	FUSE_SPARE_REALIGNMENT_REG		0x1fc
+#define	 FUSE_SPARE_REALIGNMENT_REG_SHIFT_CP_SHIFT 	0
+#define	 FUSE_SPARE_REALIGNMENT_REG_SHIFT_CP_BITS 	6
+#define	 FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT_SHIFT 	21
+#define	 FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT_BITS 	5
+#define	 FUSE_SPARE_REALIGNMENT_REG_SHIFT_CP(x) 	(((x) >>  0) & 0x3f)
+#define	 FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT(x) 	(((x) >> 21) & 0x1f)
+
+
+#define	NOMINAL_CALIB_FT_T124	105
+#define	NOMINAL_CALIB_CP_T124	25
+
+#define	WR4(_sc, _r, _v)	bus_write_4((_sc)->mem_res, (_r), (_v))
+#define	RD4(_sc, _r)		bus_read_4((_sc)->mem_res, (_r))
+
+static struct sysctl_ctx_list soctherm_sysctl_ctx;
+
+struct soctherm_shared_cal {
+	uint32_t		base_cp;
+	uint32_t		base_ft;
+	int32_t			actual_temp_cp;
+	int32_t			actual_temp_ft;
+};
+struct tsensor_cfg {
+	uint32_t		tall;
+	uint32_t		tsample;
+	uint32_t		tiddq_en;
+	uint32_t		ten_count;
+	uint32_t		pdiv;
+	uint32_t		tsample_ate;
+	uint32_t		pdiv_ate;
+};
+
+struct tsensor {
+	char 			*name;
+	int			id;
+	struct tsensor_cfg 	*cfg;
+	bus_addr_t		sensor_base;
+	bus_addr_t		calib_fuse;
+	int 			fuse_corr_alpha;
+	int			fuse_corr_beta;
+
+	int16_t			therm_a;
+	int16_t			therm_b;
+};
+
+struct soctherm_softc {
+	device_t		dev;
+	struct resource		*mem_res;
+	struct resource		*irq_res;
+	void			*irq_ih;
+
+	clk_t			tsensor_clk;
+	clk_t			soctherm_clk;
+	hwreset_t			reset;
+
+	int			ntsensors;
+	struct tsensor		*tsensors;
+};
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-soctherm",	1},
+	{NULL,				0},
+};
+
+static struct tsensor_cfg t124_tsensor_config = {
+	.tall = 16300,
+	.tsample = 120,
+	.tiddq_en = 1,
+	.ten_count = 1,
+	.pdiv = 8,
+	.tsample_ate = 480,
+	.pdiv_ate = 8
+};
+
+
+static struct tsensor t124_tsensors[] = {
+	{
+		.name = "cpu0",
+		.id = TEGRA124_SOCTHERM_SENSOR_CPU,
+		.cfg = &t124_tsensor_config,
+		.sensor_base = 0x0c0,
+		.calib_fuse = 0x098,
+		.fuse_corr_alpha = 1135400,
+		.fuse_corr_beta = -6266900,
+	},
+	{
+		.name = "cpu1",
+		.id = -1,
+		.cfg = &t124_tsensor_config,
+		.sensor_base = 0x0e0,
+		.calib_fuse = 0x084,
+		.fuse_corr_alpha = 1122220,
+		.fuse_corr_beta = -5700700,
+	},
+	{
+		.name = "cpu2",
+		.id = -1,
+		.cfg = &t124_tsensor_config,
+		.sensor_base = 0x100,
+		.calib_fuse = 0x088,
+		.fuse_corr_alpha = 1127000,
+		.fuse_corr_beta = -6768200,
+	},
+	{
+		.name = "cpu3",
+		.id = -1,
+		.cfg = &t124_tsensor_config,
+		.sensor_base = 0x120,
+		.calib_fuse = 0x12c,
+		.fuse_corr_alpha = 1110900,
+		.fuse_corr_beta = -6232000,
+	},
+	{
+		.name = "mem0",
+		.id = TEGRA124_SOCTHERM_SENSOR_MEM,
+		.cfg = &t124_tsensor_config,
+		.sensor_base = 0x140,
+		.calib_fuse = 0x158,
+		.fuse_corr_alpha = 1122300,
+		.fuse_corr_beta = -5936400,
+	},
+	{
+		.name = "mem1",
+		.id = -1,
+		.cfg = &t124_tsensor_config,
+		.sensor_base = 0x160,
+		.calib_fuse = 0x15c,
+		.fuse_corr_alpha = 1145700,
+		.fuse_corr_beta = -7124600,
+	},
+	{
+		.name = "gpu",
+		.id = TEGRA124_SOCTHERM_SENSOR_GPU,
+		.cfg = &t124_tsensor_config,
+		.sensor_base = 0x180,
+		.calib_fuse = 0x154,
+		.fuse_corr_alpha = 1120100,
+		.fuse_corr_beta = -6000500,
+	},
+	{
+		.name = "pllX",
+		.id = TEGRA124_SOCTHERM_SENSOR_PLLX,
+		.cfg = &t124_tsensor_config,
+		.sensor_base = 0x1a0,
+		.calib_fuse = 0x160,
+		.fuse_corr_alpha = 1106500,
+		.fuse_corr_beta = -6729300,
+	},
+};
+
+/* Extract signed integer bitfield from register */
+static int
+extract_signed(uint32_t reg, int shift, int bits)
+{
+	int32_t val;
+	uint32_t mask;
+
+	mask = (1 << bits) - 1;
+	val = ((reg >> shift) & mask) << (32 - bits);
+	val >>= 32 - bits;
+	return ((int32_t)val);
+}
+
+static inline int64_t div64_s64_precise(int64_t a, int64_t b)
+{
+	int64_t r, al;
+
+	al = a << 16;
+	r = (al * 2 + 1) / (2 * b);
+	return r >> 16;
+}
+
+static void
+get_shared_cal(struct soctherm_softc *sc, struct soctherm_shared_cal *cal)
+{
+	uint32_t val;
+	int calib_cp, calib_ft;
+
+	val = tegra_fuse_read_4(FUSE_TSENSOR8_CALIB);
+	cal->base_cp = FUSE_TSENSOR8_CALIB_CP_TS_BASE(val);
+	cal->base_ft = FUSE_TSENSOR8_CALIB_FT_TS_BASE(val);
+
+	val = tegra_fuse_read_4(FUSE_SPARE_REALIGNMENT_REG);
+	calib_ft = extract_signed(val,
+	    FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT_SHIFT,
+	    FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT_BITS);
+	calib_cp = extract_signed(val,
+	    FUSE_SPARE_REALIGNMENT_REG_SHIFT_CP_SHIFT,
+	    FUSE_SPARE_REALIGNMENT_REG_SHIFT_CP_BITS);
+
+	cal->actual_temp_cp = 2 * NOMINAL_CALIB_CP_T124 + calib_cp;
+	cal->actual_temp_ft = 2 * NOMINAL_CALIB_FT_T124 + calib_ft;
+#ifdef DEBUG
+	printf("%s: base_cp: %u, base_ft: %d,"
+	    " actual_temp_cp: %d, actual_temp_ft: %d\n",
+	    __func__, cal->base_cp, cal->base_ft,
+	    cal->actual_temp_cp, cal->actual_temp_ft);
+#endif
+}
+
+
+static void
+tsensor_calibration(struct tsensor *sensor, struct soctherm_shared_cal *shared)
+{
+	uint32_t val;
+	int mult, div, calib_cp, calib_ft;
+	int actual_tsensor_ft, actual_tsensor_cp, delta_sens, delta_temp;
+	int temp_a, temp_b;
+	int64_t tmp;
+
+	val =  tegra_fuse_read_4(sensor->calib_fuse);
+	calib_cp = extract_signed(val,
+	    FUSE_TSENSOR_CALIB_CP_TS_BASE_SHIFT,
+	    FUSE_TSENSOR_CALIB_CP_TS_BASE_BITS);
+	actual_tsensor_cp = shared->base_cp * 64 + calib_cp;
+
+	calib_ft = extract_signed(val,
+	    FUSE_TSENSOR_CALIB_FT_TS_BASE_SHIFT,
+	    FUSE_TSENSOR_CALIB_FT_TS_BASE_BITS);
+	actual_tsensor_ft = shared->base_ft * 32 + calib_ft;
+
+	delta_sens = actual_tsensor_ft - actual_tsensor_cp;
+	delta_temp = shared->actual_temp_ft - shared->actual_temp_cp;
+	mult = sensor->cfg->pdiv * sensor->cfg->tsample_ate;
+	div = sensor->cfg->tsample * sensor->cfg->pdiv_ate;
+
+
+	temp_a = div64_s64_precise((int64_t) delta_temp * (1LL << 13) * mult,
+				   (int64_t) delta_sens * div);
+
+	tmp = (int64_t)actual_tsensor_ft * shared->actual_temp_cp -
+	      (int64_t)actual_tsensor_cp * shared->actual_temp_ft;
+	temp_b = div64_s64_precise(tmp, (int64_t)delta_sens);
+
+	temp_a = div64_s64_precise((int64_t)temp_a * sensor->fuse_corr_alpha,
+				   1000000);
+	temp_b = div64_s64_precise((int64_t)temp_b * sensor->fuse_corr_alpha +
+				   sensor->fuse_corr_beta, 1000000);
+	sensor->therm_a = (int16_t)temp_a;
+	sensor->therm_b = (int16_t)temp_b;
+#ifdef DEBUG
+	printf("%s: sensor %s fuse: 0x%08X (0x%04X, 0x%04X)"
+	    " calib_cp: %d(0x%04X), calib_ft: %d(0x%04X)\n",
+	    __func__, sensor->name, val, val & 0x1FFF, (val >> 13) & 0x1FFF,
+	    calib_cp, calib_cp, calib_ft, calib_ft);
+	printf("therma: 0x%04X(%d), thermb: 0x%04X(%d)\n",
+	    (uint16_t)sensor->therm_a, temp_a,
+	    (uint16_t)sensor->therm_b, sensor->therm_b);
+#endif
+}
+
+static void
+soctherm_init_tsensor(struct soctherm_softc *sc, struct tsensor *sensor,
+    struct soctherm_shared_cal *shared_cal)
+{
+	uint32_t val;
+
+	tsensor_calibration(sensor, shared_cal);
+
+	val = RD4(sc, sensor->sensor_base + TSENSOR_CONFIG0);
+	val |= TSENSOR_CONFIG0_STOP;
+	val |= TSENSOR_CONFIG0_STATUS_CLR;
+	WR4(sc, sensor->sensor_base + TSENSOR_CONFIG0, val);
+
+	val = TSENSOR_CONFIG0_TALL(sensor->cfg->tall);
+	val |= TSENSOR_CONFIG0_STOP;
+	WR4(sc, sensor->sensor_base + TSENSOR_CONFIG0, val);
+
+	val = TSENSOR_CONFIG1_TSAMPLE(sensor->cfg->tsample - 1);
+	val |= TSENSOR_CONFIG1_TIDDQ_EN(sensor->cfg->tiddq_en);
+	val |= TSENSOR_CONFIG1_TEN_COUNT(sensor->cfg->ten_count);
+	val |= TSENSOR_CONFIG1_TEMP_ENABLE;
+	WR4(sc, sensor->sensor_base + TSENSOR_CONFIG1, val);
+
+	val = TSENSOR_CONFIG2_THERMA((uint16_t)sensor->therm_a) |
+	     TSENSOR_CONFIG2_THERMB((uint16_t)sensor->therm_b);
+	WR4(sc, sensor->sensor_base + TSENSOR_CONFIG2, val);
+
+	val = RD4(sc, sensor->sensor_base + TSENSOR_CONFIG0);
+	val &= ~TSENSOR_CONFIG0_STOP;
+	WR4(sc, sensor->sensor_base + TSENSOR_CONFIG0, val);
+#ifdef DEBUG
+	printf(" Sensor: %s  cfg:0x%08X, 0x%08X, 0x%08X,"
+	    " sts:0x%08X, 0x%08X, 0x%08X\n", sensor->name,
+	    RD4(sc, sensor->sensor_base + TSENSOR_CONFIG0),
+	    RD4(sc, sensor->sensor_base + TSENSOR_CONFIG1),
+	    RD4(sc, sensor->sensor_base + TSENSOR_CONFIG2),
+	    RD4(sc, sensor->sensor_base + TSENSOR_STATUS0),
+	    RD4(sc, sensor->sensor_base + TSENSOR_STATUS1),
+	    RD4(sc, sensor->sensor_base + TSENSOR_STATUS2)
+	    );
+#endif
+}
+
+static int
+soctherm_convert_raw(uint32_t val)
+{
+	int32_t t;
+
+	t = ((val & READBACK_VALUE_MASK) >> READBACK_VALUE_SHIFT) * 1000;
+	if (val & READBACK_ADD_HALF)
+		t += 500;
+	if (val & READBACK_NEGATE)
+		t *= -1;
+
+	return t;
+}
+
+static int
+soctherm_read_temp(struct soctherm_softc *sc, struct tsensor *sensor, int *temp)
+{
+	int timeout;
+	uint32_t val;
+
+
+	/* wait for valid sample */
+	for (timeout = 1000; timeout > 0; timeout--) {
+		val = RD4(sc, sensor->sensor_base + TSENSOR_STATUS1);
+		if ((val & TSENSOR_STATUS1_TEMP_VALID) != 0)
+			break;
+		DELAY(100);
+	}
+	if (timeout <= 0)
+		device_printf(sc->dev, "Sensor %s timeouted\n", sensor->name);
+	*temp = soctherm_convert_raw(val);
+#ifdef DEBUG
+	printf("%s: Raw: 0x%08X, temp: %d\n", __func__, val, *temp);
+	printf(" Sensor: %s  cfg:0x%08X, 0x%08X, 0x%08X,"
+	    " sts:0x%08X, 0x%08X, 0x%08X\n", sensor->name,
+	    RD4(sc, sensor->sensor_base + TSENSOR_CONFIG0),
+	    RD4(sc, sensor->sensor_base + TSENSOR_CONFIG1),
+	    RD4(sc, sensor->sensor_base + TSENSOR_CONFIG2),
+	    RD4(sc, sensor->sensor_base + TSENSOR_STATUS0),
+	    RD4(sc, sensor->sensor_base + TSENSOR_STATUS1),
+	    RD4(sc, sensor->sensor_base + TSENSOR_STATUS2)
+	    );
+#endif
+	return 0;
+}
+
+static int
+soctherm_get_temp(device_t dev, device_t cdev, uintptr_t id, int *val)
+{
+	struct soctherm_softc *sc;
+	int i;
+
+	sc = device_get_softc(dev);
+	/* The direct sensor map starts at 0x100 */
+	if (id >= 0x100) {
+		id -= 0x100;
+		if (id >= sc->ntsensors)
+			return (ERANGE);
+		return(soctherm_read_temp(sc, sc->tsensors + id, val));
+	}
+	/* Linux (DT) compatible thermal zones */
+	for (i = 0; i < sc->ntsensors; i++) {
+		if (sc->tsensors->id == id)
+			return(soctherm_read_temp(sc, sc->tsensors + id, val));
+	}
+	return (ERANGE);
+}
+
+static int
+soctherm_sysctl_temperature(SYSCTL_HANDLER_ARGS)
+{
+	struct soctherm_softc *sc;
+	int val;
+	int rv;
+	int id;
+
+	/* Write request */
+	if (req->newptr != NULL)
+		return (EINVAL);
+
+	sc = arg1;
+	id = arg2;
+
+	if (id >= sc->ntsensors)
+		return (ERANGE);
+	rv =  soctherm_read_temp(sc, sc->tsensors + id, &val);
+	if (rv != 0)
+		return (rv);
+
+	val = val / 100;
+	val +=  2731;
+	rv = sysctl_handle_int(oidp, &val, 0, req);
+	return (rv);
+}
+
+static int
+soctherm_init_sysctl(struct soctherm_softc *sc)
+{
+	int i;
+	struct sysctl_oid *oid, *tmp;
+
+	sysctl_ctx_init(&soctherm_sysctl_ctx);
+	/* create node for hw.temp */
+	oid = SYSCTL_ADD_NODE(&soctherm_sysctl_ctx,
+	    SYSCTL_STATIC_CHILDREN(_hw), OID_AUTO, "temperature",
+	    CTLFLAG_RD, NULL, "");
+	if (oid == NULL)
+		return (ENXIO);
+
+	/* Add sensors */
+	for (i = sc->ntsensors  - 1; i >= 0; i--) {
+		tmp = SYSCTL_ADD_PROC(&soctherm_sysctl_ctx,
+		    SYSCTL_CHILDREN(oid), OID_AUTO, sc->tsensors[i].name,
+		    CTLTYPE_INT | CTLFLAG_RD, sc, i,
+		    soctherm_sysctl_temperature, "IK", "SoC Temperature");
+		if (tmp == NULL)
+			return (ENXIO);
+	}
+
+	return (0);
+}
+
+static int
+soctherm_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	device_set_desc(dev, "Tegra temperature sensors");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+soctherm_attach(device_t dev)
+{
+	struct soctherm_softc *sc;
+	phandle_t node;
+	int i, rid, rv;
+	struct soctherm_shared_cal shared_calib;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	node = ofw_bus_get_node(sc->dev);
+
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		goto fail;
+	}
+
+	rid = 0;
+	sc->irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid, RF_ACTIVE);
+	if (sc->irq_res == NULL) {
+		device_printf(dev, "Cannot allocate IRQ resources\n");
+		goto fail;
+	}
+
+/*
+	if ((bus_setup_intr(dev, sc->irq_res, INTR_TYPE_MISC,
+	    soctherm_intr, NULL, sc, &sc->irq_ih))) {
+		device_printf(dev,
+		    "WARNING: unable to register interrupt handler\n");
+		goto fail;
+	}
+*/
+
+	/* OWF resources */
+	rv = hwreset_get_by_ofw_name(dev, "soctherm", &sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get fuse reset\n");
+		goto fail;
+	}
+	rv = clk_get_by_ofw_name(dev, "tsensor", &sc->tsensor_clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get 'tsensor' clock: %d\n", rv);
+		goto fail;
+	}
+	rv = clk_get_by_ofw_name(dev, "soctherm", &sc->soctherm_clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get 'soctherm' clock: %d\n", rv);
+		goto fail;
+	}
+
+	rv = hwreset_assert(sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot assert reset\n");
+		goto fail;
+	}
+	rv = clk_enable(sc->tsensor_clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable 'tsensor' clock: %d\n", rv);
+		goto fail;
+	}
+	rv = clk_enable(sc->soctherm_clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable 'soctherm' clock: %d\n", rv);
+		goto fail;
+	}
+	rv = hwreset_deassert(sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot clear reset\n");
+		goto fail;
+	}
+
+	/* Tegra 124 */
+	sc->tsensors = t124_tsensors;
+	sc->ntsensors = nitems(t124_tsensors);
+	get_shared_cal(sc, &shared_calib);
+
+	WR4(sc, TSENSOR_PDIV, TSENSOR_PDIV_T124);
+	WR4(sc, TSENSOR_HOTSPOT_OFF, TSENSOR_HOTSPOT_OFF_T124);
+
+	for (i = 0; i < sc->ntsensors; i++)
+		soctherm_init_tsensor(sc, sc->tsensors + i, &shared_calib);
+
+	rv = soctherm_init_sysctl(sc);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot initialize sysctls\n");
+		goto fail;
+	}
+
+	OF_device_register_xref(OF_xref_from_node(node), dev);
+	return (bus_generic_attach(dev));
+
+fail:
+	if (sc->irq_ih != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->irq_ih);
+	sysctl_ctx_free(&soctherm_sysctl_ctx);
+	if (sc->tsensor_clk != NULL)
+		clk_release(sc->tsensor_clk);
+	if (sc->soctherm_clk != NULL)
+		clk_release(sc->soctherm_clk);
+	if (sc->reset != NULL)
+		hwreset_release(sc->reset);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+
+	return (ENXIO);
+}
+
+static int
+soctherm_detach(device_t dev)
+{
+	struct soctherm_softc *sc;
+	sc = device_get_softc(dev);
+
+	if (sc->irq_ih != NULL)
+		bus_teardown_intr(dev, sc->irq_res, sc->irq_ih);
+	sysctl_ctx_free(&soctherm_sysctl_ctx);
+	if (sc->tsensor_clk != NULL)
+		clk_release(sc->tsensor_clk);
+	if (sc->soctherm_clk != NULL)
+		clk_release(sc->soctherm_clk);
+	if (sc->reset != NULL)
+		hwreset_release(sc->reset);
+	if (sc->irq_res != NULL)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->irq_res);
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->mem_res);
+
+	return (ENXIO);
+}
+
+static device_method_t tegra_soctherm_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,			soctherm_probe),
+	DEVMETHOD(device_attach,		soctherm_attach),
+	DEVMETHOD(device_detach,		soctherm_detach),
+
+	/* SOCTHERM interface */
+	DEVMETHOD(tegra_soctherm_get_temperature, soctherm_get_temp),
+
+	DEVMETHOD_END
+};
+
+static devclass_t tegra_soctherm_devclass;
+DEFINE_CLASS_0(tegra_soctherm, tegra_soctherm_driver, tegra_soctherm_methods,
+    sizeof(struct soctherm_softc));
+EARLY_DRIVER_MODULE(tegra_soctherm, simplebus, tegra_soctherm_driver,
+tegra_soctherm_devclass, 0, 0, 79);
diff --git a/sys/arm/nvidia/tegra_soctherm_if.m b/sys/arm/nvidia/tegra_soctherm_if.m
new file mode 100644
index 0000000..55ae044
--- /dev/null
+++ b/sys/arm/nvidia/tegra_soctherm_if.m
@@ -0,0 +1,42 @@
+#-
+# Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+# OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+# OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+# SUCH DAMAGE.
+#
+# $FreeBSD$
+#
+
+#include <machine/bus.h>
+
+INTERFACE tegra_soctherm;
+
+
+/**
+ * Read temperature
+ */
+METHOD int get_temperature{
+	device_t	dev;
+	device_t	consumer;
+	uintptr_t	id;
+	int 		*val;
+};
diff --git a/sys/arm/nvidia/tegra_uart.c b/sys/arm/nvidia/tegra_uart.c
new file mode 100644
index 0000000..88b750c
--- /dev/null
+++ b/sys/arm/nvidia/tegra_uart.c
@@ -0,0 +1,252 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+
+/*
+ * UART driver for Tegra SoCs.
+ */
+#include "opt_platform.h"
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/sysctl.h>
+#include <machine/bus.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+#include <dev/uart/uart.h>
+#include <dev/uart/uart_cpu.h>
+#include <dev/uart/uart_cpu_fdt.h>
+#include <dev/uart/uart_bus.h>
+#include <dev/uart/uart_dev_ns8250.h>
+#include <dev/ic/ns16550.h>
+
+#include "uart_if.h"
+
+/*
+ * High-level UART interface.
+ */
+struct tegra_softc {
+	struct ns8250_softc ns8250_base;
+	clk_t		clk;
+	hwreset_t		reset;
+};
+
+/*
+ * UART class interface.
+ */
+static int
+tegra_uart_attach(struct uart_softc *sc)
+{
+	int rv;
+	struct ns8250_softc *ns8250 = (struct ns8250_softc*)sc;
+	struct uart_bas *bas = &sc->sc_bas;
+
+	rv = ns8250_bus_attach(sc);
+	if (rv != 0)
+		return (rv);
+
+	ns8250->ier_rxbits = 0x1d;
+	ns8250->ier_mask = 0xc0;
+	ns8250->ier = uart_getreg(bas, REG_IER) & ns8250->ier_mask;
+	ns8250->ier = ns8250->ier_rxbits;
+	uart_setreg(bas, REG_IER, ns8250->ier);
+	uart_barrier(bas);
+	return (0);
+}
+
+static void
+tegra_uart_grab(struct uart_softc *sc)
+{
+	struct uart_bas *bas = &sc->sc_bas;
+	struct ns8250_softc *ns8250 = (struct ns8250_softc*)sc;
+	u_char ier;
+
+	/*
+	 * turn off all interrupts to enter polling mode. Leave the
+	 * saved mask alone. We'll restore whatever it was in ungrab.
+	 * All pending interrupt signals are reset when IER is set to 0.
+	 */
+	uart_lock(sc->sc_hwmtx);
+	ier = uart_getreg(bas, REG_IER);
+	uart_setreg(bas, REG_IER, ier & ns8250->ier_mask);
+	uart_setreg(bas, REG_FCR, 0);
+	uart_barrier(bas);
+	uart_unlock(sc->sc_hwmtx);
+}
+
+static void
+tegra_uart_ungrab(struct uart_softc *sc)
+{
+	struct ns8250_softc *ns8250 = (struct ns8250_softc*)sc;
+	struct uart_bas *bas = &sc->sc_bas;
+
+	/*
+	 * Restore previous interrupt mask
+	 */
+	uart_lock(sc->sc_hwmtx);
+	uart_setreg(bas, REG_FCR, ns8250->fcr);
+	uart_setreg(bas, REG_IER, ns8250->ier);
+	uart_barrier(bas);
+	uart_unlock(sc->sc_hwmtx);
+}
+
+static kobj_method_t tegra_methods[] = {
+	KOBJMETHOD(uart_probe,		ns8250_bus_probe),
+	KOBJMETHOD(uart_attach,		tegra_uart_attach),
+	KOBJMETHOD(uart_detach,		ns8250_bus_detach),
+	KOBJMETHOD(uart_flush,		ns8250_bus_flush),
+	KOBJMETHOD(uart_getsig,		ns8250_bus_getsig),
+	KOBJMETHOD(uart_ioctl,		ns8250_bus_ioctl),
+	KOBJMETHOD(uart_ipend,		ns8250_bus_ipend),
+	KOBJMETHOD(uart_param,		ns8250_bus_param),
+	KOBJMETHOD(uart_receive,	ns8250_bus_receive),
+	KOBJMETHOD(uart_setsig,		ns8250_bus_setsig),
+	KOBJMETHOD(uart_transmit,	ns8250_bus_transmit),
+	KOBJMETHOD(uart_grab,		tegra_uart_grab),
+	KOBJMETHOD(uart_ungrab,		tegra_uart_ungrab),
+	KOBJMETHOD_END
+};
+
+static struct uart_class tegra_uart_class = {
+	"tegra class",
+	tegra_methods,
+	sizeof(struct tegra_softc),
+	.uc_ops = &uart_ns8250_ops,
+	.uc_range = 8,
+	.uc_rclk = 0,
+};
+
+/* Compatible devices. */
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra124-uart", (uintptr_t)&tegra_uart_class},
+	{NULL,			(uintptr_t)NULL},
+};
+
+UART_FDT_CLASS(compat_data);
+
+/*
+ * UART Driver interface.
+ */
+static int
+uart_fdt_get_shift1(phandle_t node)
+{
+	pcell_t shift;
+
+	if ((OF_getencprop(node, "reg-shift", &shift, sizeof(shift))) <= 0)
+		shift = 2;
+	return ((int)shift);
+}
+
+static int
+tegra_uart_probe(device_t dev)
+{
+	struct tegra_softc *sc;
+	phandle_t node;
+	uint64_t freq;
+	int shift;
+	int rv;
+	const struct ofw_compat_data *cd;
+
+	sc = device_get_softc(dev);
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+	cd = ofw_bus_search_compatible(dev, compat_data);
+	if (cd->ocd_data == 0)
+		return (ENXIO);
+	sc->ns8250_base.base.sc_class = (struct uart_class *)cd->ocd_data;
+
+	rv = hwreset_get_by_ofw_name(dev, "serial", &sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get 'serial' reset\n");
+		return (ENXIO);
+	}
+	rv = hwreset_deassert(sc->reset);
+	if (rv != 0) {
+		device_printf(dev, "Cannot unreset 'serial' reset\n");
+		return (ENXIO);
+	}
+
+	node = ofw_bus_get_node(dev);
+	shift = uart_fdt_get_shift1(node);
+	rv = clk_get_by_ofw_index(dev, 0, &sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get UART clock: %d\n", rv);
+		return (ENXIO);
+	}
+	rv = clk_enable(sc->clk);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable UART clock: %d\n", rv);
+		return (ENXIO);
+	}
+	rv = clk_get_freq(sc->clk, &freq);
+	if (rv != 0) {
+		device_printf(dev, "Cannot enable UART clock: %d\n", rv);
+		return (ENXIO);
+	}
+	device_printf(dev, "got UART clock: %lld\n", freq);
+	return (uart_bus_probe(dev, shift, (int)freq, 0, 0));
+}
+
+static int
+tegra_uart_detach(device_t dev)
+{
+	struct tegra_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (sc->clk != NULL) {
+		clk_release(sc->clk);
+	}
+
+	return (uart_bus_detach(dev));
+}
+
+static device_method_t tegra_uart_bus_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		tegra_uart_probe),
+	DEVMETHOD(device_attach,	uart_bus_attach),
+	DEVMETHOD(device_detach,	tegra_uart_detach),
+	{ 0, 0 }
+};
+
+static driver_t tegra_uart_driver = {
+	uart_driver_name,
+	tegra_uart_bus_methods,
+	sizeof(struct tegra_softc),
+};
+
+DRIVER_MODULE(tegra_uart, simplebus,  tegra_uart_driver, uart_devclass,
+    0, 0);
\ No newline at end of file
diff --git a/sys/arm/nvidia/tegra_usbphy.c b/sys/arm/nvidia/tegra_usbphy.c
new file mode 100644
index 0000000..40c0714
--- /dev/null
+++ b/sys/arm/nvidia/tegra_usbphy.c
@@ -0,0 +1,839 @@
+/*-
+ * Copyright (c) 2016 Michal Meloun <mmel@FreeBSD.org>
+ * 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$");
+
+
+/*
+ * USB phy driver for Tegra SoCs.
+ */
+#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 <machine/bus.h>
+#include <machine/fdt.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+#include <dev/extres/phy/phy.h>
+#include <dev/extres/regulator/regulator.h>
+#include <dev/fdt/fdt_common.h>
+#include <dev/fdt/fdt_pinctrl.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include "phy_if.h"
+
+#define	CTRL_ICUSB_CTRL			0x15c
+#define	  ICUSB_CTR_IC_ENB1			(1 << 3)
+
+#define	CTRL_USB_USBMODE		0x1f8
+#define	  USB_USBMODE_MASK			(3 << 0)
+#define	  USB_USBMODE_HOST			(3 << 0)
+#define	  USB_USBMODE_DEVICE			(2 << 0)
+
+#define	CTRL_USB_HOSTPC1_DEVLC		0x1b4
+#define	 USB_HOSTPC1_DEVLC_PTS(x)		(((x) & 0x7) << 29)
+#define	 USB_HOSTPC1_DEVLC_STS			(1 << 28)
+#define	 USB_HOSTPC1_DEVLC_PHCD			(1 << 22)
+
+
+#define	IF_USB_SUSP_CTRL		0x400
+#define	 FAST_WAKEUP_RESP			(1 << 26)
+#define	 UTMIP_SUSPL1_SET			(1 << 25)
+#define	 USB_WAKEUP_DEBOUNCE_COUNT(x)		(((x) & 0x7) << 16)
+#define	 USB_SUSP_SET				(1 << 14)
+#define	 UTMIP_PHY_ENB				(1 << 12)
+#define	 UTMIP_RESET				(1 << 11)
+#define	 USB_SUSP_POL				(1 << 10)
+#define	 USB_PHY_CLK_VALID_INT_ENB		(1 << 9)
+#define	 USB_PHY_CLK_VALID_INT_STS		(1 << 8)
+#define	 USB_PHY_CLK_VALID			(1 << 7)
+#define	 USB_CLKEN				(1 << 6)
+#define	 USB_SUSP_CLR				(1 << 5)
+#define	 USB_WAKE_ON_DISCON_EN_DEV		(1 << 4)
+#define	 USB_WAKE_ON_CNNT_EN_DEV		(1 << 3)
+#define	 USB_WAKE_ON_RESUME_EN			(1 << 2)
+#define	 USB_WAKEUP_INT_ENB			(1 << 1)
+#define	 USB_WAKEUP_INT_STS			(1 << 0)
+
+#define	IF_USB_PHY_VBUS_SENSORS		0x404
+#define	 B_SESS_END_SW_VALUE			(1 << 4)
+#define	 B_SESS_END_SW_EN			(1 << 3)
+
+
+#define	UTMIP_XCVR_CFG0			0x808
+#define	 UTMIP_XCVR_HSSLEW_MSB(x)		((((x) & 0x1fc) >> 2) << 25)
+#define	 UTMIP_XCVR_SETUP_MSB(x)		((((x) & 0x70) >> 4) << 22)
+#define	 UTMIP_XCVR_LSBIAS_SEL			(1 << 21)
+#define	 UTMIP_XCVR_DISCON_METHOD		(1 << 20)
+#define	 UTMIP_FORCE_PDZI_POWERUP		(1 << 19)
+#define	 UTMIP_FORCE_PDZI_POWERDOWN		(1 << 18)
+#define	 UTMIP_FORCE_PD2_POWERUP		(1 << 17)
+#define	 UTMIP_FORCE_PD2_POWERDOWN		(1 << 16)
+#define	 UTMIP_FORCE_PD_POWERUP			(1 << 15)
+#define	 UTMIP_FORCE_PD_POWERDOWN		(1 << 14)
+#define	 UTMIP_XCVR_TERMEN			(1 << 13)
+#define	 UTMIP_XCVR_HSLOOPBACK			(1 << 12)
+#define	 UTMIP_XCVR_LSFSLEW(x)			(((x) & 0x3) << 10)
+#define	 UTMIP_XCVR_LSRSLEW(x)			(((x) & 0x3) << 8)
+#define	 UTMIP_XCVR_FSSLEW(x)			(((x) & 0x3) << 6)
+#define	 UTMIP_XCVR_HSSLEW(x)			(((x) & 0x3) << 4)
+#define	 UTMIP_XCVR_SETUP(x)			(((x) & 0xf) << 0)
+
+#define	UTMIP_BIAS_CFG0			0x80C
+#define	 UTMIP_IDDIG_C_VAL			(1 << 30)
+#define	 UTMIP_IDDIG_C_SEL			(1 << 29)
+#define	 UTMIP_IDDIG_B_VAL			(1 << 28)
+#define	 UTMIP_IDDIG_B_SEL			(1 << 27)
+#define	 UTMIP_IDDIG_A_VAL			(1 << 26)
+#define	 UTMIP_IDDIG_A_SEL			(1 << 25)
+#define	 UTMIP_HSDISCON_LEVEL_MSB(x)		((((x) & 0x4) >> 2) << 24)
+#define	 UTMIP_IDPD_VAL				(1 << 23)
+#define	 UTMIP_IDPD_SEL				(1 << 22)
+#define	 UTMIP_IDDIG_VAL			(1 << 21)
+#define	 UTMIP_IDDIG_SEL			(1 << 20)
+#define	 UTMIP_GPI_VAL				(1 << 19)
+#define	 UTMIP_GPI_SEL				(1 << 18)
+#define	 UTMIP_ACTIVE_TERM_OFFSET(x)		(((x) & 0x7) << 15)
+#define	 UTMIP_ACTIVE_PULLUP_OFFSET(x)		(((x) & 0x7) << 12)
+#define	 UTMIP_OTGPD				(1 << 11)
+#define	 UTMIP_BIASPD				(1 << 10)
+#define	 UTMIP_VBUS_LEVEL_LEVEL(x)		(((x) & 0x3) << 8)
+#define	 UTMIP_SESS_LEVEL_LEVEL(x)		(((x) & 0x3) << 6)
+#define	 UTMIP_HSCHIRP_LEVEL(x)			(((x) & 0x3) << 4)
+#define	 UTMIP_HSDISCON_LEVEL(x)		(((x) & 0x3) << 2)
+#define	 UTMIP_HSSQUELCH_LEVEL(x)		(((x) & 0x3) << 0)
+
+
+#define	UTMIP_HSRX_CFG0			0x810
+#define	 UTMIP_KEEP_PATT_ON_ACTIVE(x)		(((x) & 0x3) << 30)
+#define	 UTMIP_ALLOW_CONSEC_UPDN		(1 << 29)
+#define	 UTMIP_REALIGN_ON_NEW_PKT		(1 << 28)
+#define	 UTMIP_PCOUNT_UPDN_DIV(x)		(((x) & 0xf) << 24)
+#define	 UTMIP_SQUELCH_EOP_DLY(x)		(((x) & 0x7) << 21)
+#define	 UTMIP_NO_STRIPPING			(1 << 20)
+#define	 UTMIP_IDLE_WAIT(x)			(((x) & 0x1f) << 15)
+#define	 UTMIP_ELASTIC_LIMIT(x)			(((x) & 0x1f) << 10)
+#define	 UTMIP_ELASTIC_OVERRUN_DISABLE		(1 << 9)
+#define	 UTMIP_ELASTIC_UNDERRUN_DISABLE		(1 << 8)
+#define	 UTMIP_PASS_CHIRP			(1 << 7)
+#define	 UTMIP_PASS_FEEDBACK			(1 << 6)
+#define	 UTMIP_PCOUNT_INERTIA(x)		(((x) & 0x3) << 4)
+#define	 UTMIP_PHASE_ADJUST(x)			(((x) & 0x3) << 2)
+#define	 UTMIP_THREE_SYNCBITS			(1 << 1)
+#define	 UTMIP_USE4SYNC_TRAN			(1 << 0)
+
+#define	UTMIP_HSRX_CFG1			0x814
+#define	 UTMIP_HS_SYNC_START_DLY(x)		(((x) & 0x1F) << 1)
+#define	 UTMIP_HS_ALLOW_KEEP_ALIVE		(1 << 0)
+
+#define	UTMIP_TX_CFG0			0x820
+#define	 UTMIP_FS_PREAMBLE_J			(1 << 19)
+#define	 UTMIP_FS_POSTAMBLE_OUTPUT_ENABLE	(1 << 18)
+#define	 UTMIP_FS_PREAMBLE_OUTPUT_ENABLE	(1 << 17)
+#define	 UTMIP_FSLS_ALLOW_SOP_TX_STUFF_ERR	(1 << 16)
+#define	 UTMIP_HS_READY_WAIT_FOR_VALID		(1 << 15)
+#define	 UTMIP_HS_TX_IPG_DLY(x)			(((x) & 0x1f) << 10)
+#define	 UTMIP_HS_DISCON_EOP_ONLY		(1 << 9)
+#define	 UTMIP_HS_DISCON_DISABLE		(1 << 8)
+#define	 UTMIP_HS_POSTAMBLE_OUTPUT_ENABLE	(1 << 7)
+#define	 UTMIP_HS_PREAMBLE_OUTPUT_ENABLE	(1 << 6)
+#define	 UTMIP_SIE_RESUME_ON_LINESTATE		(1 << 5)
+#define	 UTMIP_SOF_ON_NO_STUFF			(1 << 4)
+#define	 UTMIP_SOF_ON_NO_ENCODE			(1 << 3)
+#define	 UTMIP_NO_STUFFING			(1 << 2)
+#define	 UTMIP_NO_ENCODING			(1 << 1)
+#define	 UTMIP_NO_SYNC_NO_EOP			(1 << 0)
+
+#define	UTMIP_MISC_CFG0			0x824
+#define	 UTMIP_DPDM_OBSERVE_SEL(x)		(((x) & 0xf) << 27)
+#define	 UTMIP_DPDM_OBSERVE			(1 << 26)
+#define	 UTMIP_KEEP_XCVR_PD_ON_SOFT_DISCON	(1 << 25)
+#define	 UTMIP_ALLOW_LS_ON_SOFT_DISCON		(1 << 24)
+#define	 UTMIP_FORCE_FS_DISABLE_ON_DEV_CHIRP	(1 << 23)
+#define	 UTMIP_SUSPEND_EXIT_ON_EDGE		(1 << 22)
+#define	 UTMIP_LS_TO_FS_SKIP_4MS		(1 << 21)
+#define	 UTMIP_INJECT_ERROR_TYPE(x)		(((x) & 0x3) << 19)
+#define	 UTMIP_FORCE_HS_CLOCK_ON		(1 << 18)
+#define	 UTMIP_DISABLE_HS_TERM			(1 << 17)
+#define	 UTMIP_FORCE_HS_TERM			(1 << 16)
+#define	 UTMIP_DISABLE_PULLUP_DP		(1 << 15)
+#define	 UTMIP_DISABLE_PULLUP_DM		(1 << 14)
+#define	 UTMIP_DISABLE_PULLDN_DP		(1 << 13)
+#define	 UTMIP_DISABLE_PULLDN_DM		(1 << 12)
+#define	 UTMIP_FORCE_PULLUP_DP			(1 << 11)
+#define	 UTMIP_FORCE_PULLUP_DM			(1 << 10)
+#define	 UTMIP_FORCE_PULLDN_DP			(1 << 9)
+#define	 UTMIP_FORCE_PULLDN_DM			(1 << 8)
+#define	 UTMIP_STABLE_COUNT(x)			(((x) & 0x7) << 5)
+#define	 UTMIP_STABLE_ALL			(1 << 4)
+#define	 UTMIP_NO_FREE_ON_SUSPEND		(1 << 3)
+#define	 UTMIP_NEVER_FREE_RUNNING_TERMS		(1 << 2)
+#define	 UTMIP_ALWAYS_FREE_RUNNING_TERMS	(1 << 1)
+#define	 UTMIP_COMB_TERMS			(1 << 0)
+
+#define	UTMIP_MISC_CFG1			0x828
+#define	 UTMIP_PHY_XTAL_CLOCKEN			(1 << 30)
+
+#define	UTMIP_DEBOUNCE_CFG0		0x82C
+#define	 UTMIP_BIAS_DEBOUNCE_B(x)		(((x) & 0xffff) << 16)
+#define	 UTMIP_BIAS_DEBOUNCE_A(x)		(((x) & 0xffff) << 0)
+
+#define	UTMIP_BAT_CHRG_CFG0		0x830
+#define	 UTMIP_CHRG_DEBOUNCE_TIMESCALE(x) 	(((x) & 0x1f) << 8)
+#define	 UTMIP_OP_I_SRC_ENG			(1 << 5)
+#define	 UTMIP_ON_SRC_ENG			(1 << 4)
+#define	 UTMIP_OP_SRC_ENG			(1 << 3)
+#define	 UTMIP_ON_SINK_ENG			(1 << 2)
+#define	 UTMIP_OP_SINK_ENG			(1 << 1)
+#define	 UTMIP_PD_CHRG				(1 << 0)
+
+#define	UTMIP_SPARE_CFG0		0x834
+#define	 FUSE_HS_IREF_CAP_CFG			(1 << 7)
+#define	 FUSE_HS_SQUELCH_LEVEL			(1 << 6)
+#define	 FUSE_SPARE				(1 << 5)
+#define	 FUSE_TERM_RANGE_ADJ_SEL		(1 << 4)
+#define	 FUSE_SETUP_SEL				(1 << 3)
+#define	 HS_RX_LATE_SQUELCH			(1 << 2)
+#define	 HS_RX_FLUSH_ALAP  			(1 << 1)
+#define	 HS_RX_IPG_ERROR_ENABLE 		(1 << 0)
+
+#define	UTMIP_XCVR_CFG1			0x838
+#define	 UTMIP_XCVR_RPU_RANGE_ADJ(x)		(((x) & 0x3) << 26)
+#define	 UTMIP_XCVR_HS_IREF_CAP(x)		(((x) & 0x3) << 24)
+#define	 UTMIP_XCVR_SPARE(x)			(((x) & 0x3) << 22)
+#define	 UTMIP_XCVR_TERM_RANGE_ADJ(x)		(((x) & 0xf) << 18)
+#define	 UTMIP_RCTRL_SW_SET			(1 << 17)
+#define	 UTMIP_RCTRL_SW_VAL(x)			(((x) & 0x1f) << 12)
+#define	 UTMIP_TCTRL_SW_SET			(1 << 11)
+#define	 UTMIP_TCTRL_SW_VAL(x)			(((x) & 0x1f) << 6)
+#define	 UTMIP_FORCE_PDDR_POWERUP		(1 << 5)
+#define	 UTMIP_FORCE_PDDR_POWERDOWN		(1 << 4)
+#define	 UTMIP_FORCE_PDCHRP_POWERUP		(1 << 3)
+#define	 UTMIP_FORCE_PDCHRP_POWERDOWN		(1 << 2)
+#define	 UTMIP_FORCE_PDDISC_POWERUP		(1 << 1)
+#define	 UTMIP_FORCE_PDDISC_POWERDOWN		(1 << 0)
+
+#define	UTMIP_BIAS_CFG1			0x83c
+#define	 UTMIP_BIAS_DEBOUNCE_TIMESCALE(x)	(((x) & 0x3f) << 8)
+#define	 UTMIP_BIAS_PDTRK_COUNT(x)		(((x) & 0x1f) << 3)
+#define	 UTMIP_VBUS_WAKEUP_POWERDOWN		(1 << 2)
+#define	 UTMIP_FORCE_PDTRK_POWERUP		(1 << 1)
+#define	 UTMIP_FORCE_PDTRK_POWERDOWN		(1 << 0)
+
+static int usbpby_enable_cnt;
+
+enum usb_ifc_type {
+	USB_IFC_TYPE_UNKNOWN = 0,
+	USB_IFC_TYPE_UTMI,
+	USB_IFC_TYPE_ULPI
+};
+
+enum usb_dr_mode {
+	USB_DR_MODE_UNKNOWN = 0,
+	USB_DR_MODE_DEVICE,
+	USB_DR_MODE_HOST,
+	USB_DR_MODE_OTG
+};
+
+struct usbphy_softc {
+	device_t		dev;
+	struct resource		*mem_res;
+	struct resource		*pads_res;
+	clk_t			clk_reg;
+	clk_t			clk_pads;
+	clk_t			clk_pllu;
+	regulator_t		supply_vbus;
+	hwreset_t		reset_usb;
+	hwreset_t		reset_pads;
+	enum usb_ifc_type	ifc_type;
+	enum usb_dr_mode	dr_mode;
+	bool			have_utmi_regs;
+
+	/* UTMI params */
+	int			hssync_start_delay;
+	int			elastic_limit;
+	int			idle_wait_delay;
+	int			term_range_adj;
+	int			xcvr_lsfslew;
+	int			xcvr_lsrslew;
+	int			xcvr_hsslew;
+	int			hssquelch_level;
+	int			hsdiscon_level;
+	int			xcvr_setup;
+	int			xcvr_setup_use_fuses;
+};
+
+static struct ofw_compat_data compat_data[] = {
+	{"nvidia,tegra30-usb-phy",	1},
+	{NULL,				0},
+};
+
+#define	RD4(sc, offs)							\
+	 bus_read_4(sc->mem_res, offs)
+
+#define	WR4(sc, offs, val)						\
+	 bus_write_4(sc->mem_res, offs, val)
+
+static int
+reg_wait(struct usbphy_softc *sc, uint32_t reg, uint32_t mask, uint32_t val)
+{
+	int i;
+
+	for (i = 0; i < 1000; i++) {
+		if ((RD4(sc, reg) & mask) == val)
+			return (0);
+		DELAY(10);
+	}
+	return (ETIMEDOUT);
+}
+
+static int
+usbphy_utmi_phy_clk(struct usbphy_softc *sc, bool enable)
+{
+	uint32_t val;
+	int rv;
+
+	val = RD4(sc, CTRL_USB_HOSTPC1_DEVLC);
+	if (enable)
+		val &= ~USB_HOSTPC1_DEVLC_PHCD;
+	else
+		val |= USB_HOSTPC1_DEVLC_PHCD;
+	WR4(sc, CTRL_USB_HOSTPC1_DEVLC, val);
+
+	rv = reg_wait(sc, IF_USB_SUSP_CTRL, USB_PHY_CLK_VALID,
+	    enable ? USB_PHY_CLK_VALID: 0);
+	if (rv != 0) {
+		device_printf(sc->dev, "USB phy clock timeout.\n");
+		return (ETIMEDOUT);
+	}
+	return (0);
+}
+
+static int
+usbphy_utmi_enable(struct usbphy_softc *sc)
+{
+	int rv;
+	uint32_t val;
+
+	/* Reset phy */
+	val = RD4(sc, IF_USB_SUSP_CTRL);
+	val |= UTMIP_RESET;
+	WR4(sc, IF_USB_SUSP_CTRL, val);
+
+
+	val = RD4(sc, UTMIP_TX_CFG0);
+	val |= UTMIP_FS_PREAMBLE_J;
+	WR4(sc, UTMIP_TX_CFG0, val);
+
+	val = RD4(sc, UTMIP_HSRX_CFG0);
+	val &= ~UTMIP_IDLE_WAIT(~0);
+	val &= ~UTMIP_ELASTIC_LIMIT(~0);
+	val |= UTMIP_IDLE_WAIT(sc->idle_wait_delay);
+	val |= UTMIP_ELASTIC_LIMIT(sc->elastic_limit);
+	WR4(sc, UTMIP_HSRX_CFG0, val);
+
+	val = RD4(sc, UTMIP_HSRX_CFG1);
+	val &= ~UTMIP_HS_SYNC_START_DLY(~0);
+	val |= UTMIP_HS_SYNC_START_DLY(sc->hssync_start_delay);
+	WR4(sc, UTMIP_HSRX_CFG1, val);
+
+	val = RD4(sc, UTMIP_DEBOUNCE_CFG0);
+	val &= ~UTMIP_BIAS_DEBOUNCE_A(~0);
+	val |= UTMIP_BIAS_DEBOUNCE_A(0x7530);  /* For 12MHz */
+	WR4(sc, UTMIP_DEBOUNCE_CFG0, val);
+
+	val = RD4(sc, UTMIP_MISC_CFG0);
+	val &= ~UTMIP_SUSPEND_EXIT_ON_EDGE;
+	WR4(sc, UTMIP_MISC_CFG0, val);
+
+	if (sc->dr_mode == USB_DR_MODE_DEVICE) {
+		val = RD4(sc,IF_USB_SUSP_CTRL);
+		val &= ~USB_WAKE_ON_CNNT_EN_DEV;
+		val &= ~USB_WAKE_ON_DISCON_EN_DEV;
+		WR4(sc, IF_USB_SUSP_CTRL, val);
+
+		val = RD4(sc, UTMIP_BAT_CHRG_CFG0);
+		val &= ~UTMIP_PD_CHRG;
+		WR4(sc, UTMIP_BAT_CHRG_CFG0, val);
+	} else {
+		val = RD4(sc, UTMIP_BAT_CHRG_CFG0);
+		val |= UTMIP_PD_CHRG;
+		WR4(sc, UTMIP_BAT_CHRG_CFG0, val);
+	}
+
+	usbpby_enable_cnt++;
+	if (usbpby_enable_cnt == 1) {
+		rv = hwreset_deassert(sc->reset_pads);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			     "Cannot unreset 'utmi-pads' reset\n");
+			return (rv);
+		}
+		rv = clk_enable(sc->clk_pads);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			    "Cannot enable 'utmi-pads' clock\n");
+			return (rv);
+		}
+
+		val = bus_read_4(sc->pads_res, UTMIP_BIAS_CFG0);
+		val &= ~UTMIP_OTGPD;
+		val &= ~UTMIP_BIASPD;
+		val &= ~UTMIP_HSSQUELCH_LEVEL(~0);
+		val &= ~UTMIP_HSDISCON_LEVEL(~0);
+		val &= ~UTMIP_HSDISCON_LEVEL_MSB(~0);
+		val |= UTMIP_HSSQUELCH_LEVEL(sc->hssquelch_level);
+		val |= UTMIP_HSDISCON_LEVEL(sc->hsdiscon_level);
+		val |= UTMIP_HSDISCON_LEVEL_MSB(sc->hsdiscon_level);
+		bus_write_4(sc->pads_res, UTMIP_BIAS_CFG0, val);
+
+		rv = clk_disable(sc->clk_pads);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			    "Cannot disable 'utmi-pads' clock\n");
+			return (rv);
+		}
+	}
+
+	val = RD4(sc, UTMIP_XCVR_CFG0);
+	val &= ~UTMIP_FORCE_PD_POWERDOWN;
+	val &= ~UTMIP_FORCE_PD2_POWERDOWN ;
+	val &= ~UTMIP_FORCE_PDZI_POWERDOWN;
+	val &= ~UTMIP_XCVR_LSBIAS_SEL;
+	val &= ~UTMIP_XCVR_LSFSLEW(~0);
+	val &= ~UTMIP_XCVR_LSRSLEW(~0);
+	val &= ~UTMIP_XCVR_HSSLEW(~0);
+	val &= ~UTMIP_XCVR_HSSLEW_MSB(~0);
+	val |= UTMIP_XCVR_LSFSLEW(sc->xcvr_lsfslew);
+	val |= UTMIP_XCVR_LSRSLEW(sc->xcvr_lsrslew);
+	val |= UTMIP_XCVR_HSSLEW(sc->xcvr_hsslew);
+	val |= UTMIP_XCVR_HSSLEW_MSB(sc->xcvr_hsslew);
+	if (!sc->xcvr_setup_use_fuses) {
+		val &= ~UTMIP_XCVR_SETUP(~0);
+		val &= ~UTMIP_XCVR_SETUP_MSB(~0);
+		val |= UTMIP_XCVR_SETUP(sc->xcvr_setup);
+		val |= UTMIP_XCVR_SETUP_MSB(sc->xcvr_setup);
+	}
+	WR4(sc, UTMIP_XCVR_CFG0, val);
+
+	val = RD4(sc, UTMIP_XCVR_CFG1);
+	val &= ~UTMIP_FORCE_PDDISC_POWERDOWN;
+	val &= ~UTMIP_FORCE_PDCHRP_POWERDOWN;
+	val &= ~UTMIP_FORCE_PDDR_POWERDOWN;
+	val &= ~UTMIP_XCVR_TERM_RANGE_ADJ(~0);
+	val |= UTMIP_XCVR_TERM_RANGE_ADJ(sc->term_range_adj);
+	WR4(sc, UTMIP_XCVR_CFG1, val);
+
+
+	val = RD4(sc, UTMIP_BIAS_CFG1);
+	val &= ~UTMIP_BIAS_PDTRK_COUNT(~0);
+	val |= UTMIP_BIAS_PDTRK_COUNT(0x5);
+	WR4(sc, UTMIP_BIAS_CFG1, val);
+
+	val = RD4(sc, UTMIP_SPARE_CFG0);
+	if (sc->xcvr_setup_use_fuses)
+		val |= FUSE_SETUP_SEL;
+	else
+		val &= ~FUSE_SETUP_SEL;
+	WR4(sc, UTMIP_SPARE_CFG0, val);
+
+	val = RD4(sc, IF_USB_SUSP_CTRL);
+	val |= UTMIP_PHY_ENB;
+	WR4(sc, IF_USB_SUSP_CTRL, val);
+
+	val = RD4(sc, IF_USB_SUSP_CTRL);
+	val &= ~UTMIP_RESET;
+	WR4(sc, IF_USB_SUSP_CTRL, val);
+
+	usbphy_utmi_phy_clk(sc, true);
+
+	val = RD4(sc, CTRL_USB_USBMODE);
+	val &= ~USB_USBMODE_MASK;
+	if (sc->dr_mode == USB_DR_MODE_HOST)
+		val |= USB_USBMODE_HOST;
+	else
+		val |= USB_USBMODE_DEVICE;
+	WR4(sc, CTRL_USB_USBMODE, val);
+
+	val = RD4(sc, CTRL_USB_HOSTPC1_DEVLC);
+	val &= ~USB_HOSTPC1_DEVLC_PTS(~0);
+	val |= USB_HOSTPC1_DEVLC_PTS(0);
+	WR4(sc, CTRL_USB_HOSTPC1_DEVLC, val);
+
+	return (0);
+}
+
+static int
+usbphy_utmi_disable(struct usbphy_softc *sc)
+{
+	int rv;
+	uint32_t val;
+
+	usbphy_utmi_phy_clk(sc, false);
+
+	if (sc->dr_mode == USB_DR_MODE_DEVICE) {
+		val = RD4(sc, IF_USB_SUSP_CTRL);
+		val &= ~USB_WAKEUP_DEBOUNCE_COUNT(~0);
+		val |= USB_WAKE_ON_CNNT_EN_DEV;
+		val |= USB_WAKEUP_DEBOUNCE_COUNT(5);
+		WR4(sc, IF_USB_SUSP_CTRL, val);
+	}
+
+	val = RD4(sc, IF_USB_SUSP_CTRL);
+	val |= UTMIP_RESET;
+	WR4(sc, IF_USB_SUSP_CTRL, val);
+
+	val = RD4(sc, UTMIP_BAT_CHRG_CFG0);
+	val |= UTMIP_PD_CHRG;
+	WR4(sc, UTMIP_BAT_CHRG_CFG0, val);
+
+	val = RD4(sc, UTMIP_XCVR_CFG0);
+	val |= UTMIP_FORCE_PD_POWERDOWN;
+	val |= UTMIP_FORCE_PD2_POWERDOWN;
+	val |= UTMIP_FORCE_PDZI_POWERDOWN;
+	WR4(sc, UTMIP_XCVR_CFG0, val);
+
+	val = RD4(sc, UTMIP_XCVR_CFG1);
+	val |= UTMIP_FORCE_PDDISC_POWERDOWN;
+	val |= UTMIP_FORCE_PDCHRP_POWERDOWN;
+	val |= UTMIP_FORCE_PDDR_POWERDOWN;
+	WR4(sc, UTMIP_XCVR_CFG1, val);
+
+	usbpby_enable_cnt--;
+	if (usbpby_enable_cnt <= 0) {
+		rv = clk_enable(sc->clk_pads);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			    "Cannot enable 'utmi-pads' clock\n");
+			return (rv);
+		}
+		val =bus_read_4(sc->pads_res, UTMIP_BIAS_CFG0);
+		val |= UTMIP_OTGPD;
+		val |= UTMIP_BIASPD;
+		bus_write_4(sc->pads_res, UTMIP_BIAS_CFG0, val);
+
+		rv = clk_disable(sc->clk_pads);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			    "Cannot disable 'utmi-pads' clock\n");
+			return (rv);
+		}
+	}
+	return (0);
+}
+
+static int
+usbphy_phy_enable(device_t dev, int id, bool enable)
+{
+	struct usbphy_softc *sc;
+	int rv = 0;
+
+	sc = device_get_softc(dev);
+
+	if (sc->ifc_type != USB_IFC_TYPE_UTMI) {
+			device_printf(sc->dev,
+			    "Only UTMI interface is supported.\n");
+			return (ENXIO);
+	}
+	if (enable)
+		rv = usbphy_utmi_enable(sc);
+	else
+		rv = usbphy_utmi_disable(sc);
+
+	return (rv);
+}
+
+static enum usb_ifc_type
+usb_get_ifc_mode(device_t dev, phandle_t node, char *name)
+{
+	char *tmpstr;
+	int rv;
+	enum usb_ifc_type ret;
+
+	rv = OF_getprop_alloc(node, name, 1, (void **)&tmpstr);
+	if (rv <= 0)
+		return (USB_IFC_TYPE_UNKNOWN);
+
+	ret = USB_IFC_TYPE_UNKNOWN;
+	if (strcmp(tmpstr, "utmi") == 0)
+		ret = USB_IFC_TYPE_UTMI;
+	else if (strcmp(tmpstr, "ulpi") == 0)
+		ret = USB_IFC_TYPE_ULPI;
+	else
+		device_printf(dev, "Unsupported phy type: %s\n", tmpstr);
+	free(tmpstr, M_OFWPROP);
+	return (ret);
+}
+
+static enum usb_dr_mode
+usb_get_dr_mode(device_t dev, phandle_t node, char *name)
+{
+	char *tmpstr;
+	int rv;
+	enum usb_dr_mode ret;
+
+	rv = OF_getprop_alloc(node, name, 1, (void **)&tmpstr);
+	if (rv <= 0)
+		return (USB_DR_MODE_UNKNOWN);
+
+	ret = USB_DR_MODE_UNKNOWN;
+	if (strcmp(tmpstr, "device") == 0)
+		ret = USB_DR_MODE_DEVICE;
+	else if (strcmp(tmpstr, "host") == 0)
+		ret = USB_DR_MODE_HOST;
+	else if (strcmp(tmpstr, "otg") == 0)
+		ret = USB_DR_MODE_OTG;
+	else
+		device_printf(dev, "Unknown dr mode: %s\n", tmpstr);
+	free(tmpstr, M_OFWPROP);
+	return (ret);
+}
+
+static int
+usbphy_utmi_read_params(struct usbphy_softc *sc, phandle_t node)
+{
+	int rv;
+
+	rv = OF_getencprop(node, "nvidia,hssync-start-delay",
+	    &sc->hssync_start_delay, sizeof (sc->hssync_start_delay));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getencprop(node, "nvidia,elastic-limit",
+	    &sc->elastic_limit, sizeof (sc->elastic_limit));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getencprop(node, "nvidia,idle-wait-delay",
+	    &sc->idle_wait_delay, sizeof (sc->idle_wait_delay));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getencprop(node, "nvidia,term-range-adj",
+	    &sc->term_range_adj, sizeof (sc->term_range_adj));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getencprop(node, "nvidia,xcvr-lsfslew",
+	    &sc->xcvr_lsfslew, sizeof (sc->xcvr_lsfslew));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getencprop(node, "nvidia,xcvr-lsrslew",
+	    &sc->xcvr_lsrslew, sizeof (sc->xcvr_lsrslew));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getencprop(node, "nvidia,xcvr-hsslew",
+	    &sc->xcvr_hsslew, sizeof (sc->xcvr_hsslew));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getencprop(node, "nvidia,hssquelch-level",
+	    &sc->hssquelch_level, sizeof (sc->hssquelch_level));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getencprop(node, "nvidia,hsdiscon-level",
+	    &sc->hsdiscon_level, sizeof (sc->hsdiscon_level));
+	if (rv <= 0)
+		return (ENXIO);
+
+	rv = OF_getproplen(node, "nvidia,xcvr-setup-use-fuses");
+	if (rv >= 1) {
+		sc->xcvr_setup_use_fuses = 1;
+	} else {
+		rv = OF_getencprop(node, "nvidia,xcvr-setup",
+		    &sc->xcvr_setup, sizeof (sc->xcvr_setup));
+		if (rv <= 0)
+			return (ENXIO);
+	}
+
+	return (0);
+}
+
+static int
+usbphy_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_search_compatible(dev, compat_data)->ocd_data)
+		return (ENXIO);
+
+	device_set_desc(dev, "Tegra USB phy");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+usbphy_attach(device_t dev)
+{
+	struct usbphy_softc * sc;
+	int rid, rv;
+	phandle_t node;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE | RF_SHAREABLE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	rid = 1;
+	sc->pads_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE | RF_SHAREABLE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Cannot allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	node = ofw_bus_get_node(dev);
+
+	rv = hwreset_get_by_ofw_name(sc->dev, "usb", &sc->reset_usb);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get 'usb' reset\n");
+		return (ENXIO);
+	}
+	rv = hwreset_get_by_ofw_name(sc->dev, "utmi-pads", &sc->reset_pads);
+	if (rv != 0) {
+		device_printf(dev, "Cannot get 'utmi-pads' reset\n");
+		return (ENXIO);
+	}
+
+	rv = clk_get_by_ofw_name(sc->dev, "reg", &sc->clk_reg);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'reg' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(sc->dev, "pll_u", &sc->clk_pllu);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'pll_u' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_get_by_ofw_name(sc->dev, "utmi-pads", &sc->clk_pads);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot get 'utmi-pads' clock\n");
+		return (ENXIO);
+	}
+
+	rv = hwreset_deassert(sc->reset_usb);
+	if (rv != 0) {
+		device_printf(dev, "Cannot unreset 'usb' reset\n");
+		return (ENXIO);
+	}
+
+	rv = clk_enable(sc->clk_pllu);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'pllu' clock\n");
+		return (ENXIO);
+	}
+	rv = clk_enable(sc->clk_reg);
+	if (rv != 0) {
+		device_printf(sc->dev, "Cannot enable 'reg' clock\n");
+		return (ENXIO);
+	}
+	if (OF_hasprop(node, "nvidia,has-utmi-pad-registers"))
+		sc->have_utmi_regs = true;
+
+	sc->dr_mode = usb_get_dr_mode(dev, node, "dr_mode");
+	if (sc->dr_mode == USB_DR_MODE_UNKNOWN)
+		sc->dr_mode = USB_DR_MODE_HOST;
+
+	sc->ifc_type = usb_get_ifc_mode(dev, node, "phy_type");
+
+	/* We supports only utmi phy mode for now .... */
+	if (sc->ifc_type != USB_IFC_TYPE_UTMI) {
+		device_printf(dev, "Unsupported phy type\n");
+		return (ENXIO);
+	}
+	rv = usbphy_utmi_read_params(sc, node);
+	if (rv < 0)
+		return rv;
+
+	if (OF_hasprop(node, "vbus-supply")) {
+		rv = regulator_get_by_ofw_property(sc->dev, "vbus-supply",
+		    &sc->supply_vbus);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			   "Cannot get \"vbus\" regulator\n");
+			return (ENXIO);
+		}
+		rv = regulator_enable(sc->supply_vbus);
+		if (rv != 0) {
+			device_printf(sc->dev,
+			    "Cannot enable  \"vbus\" regulator\n");
+			return (rv);
+		}
+	}
+
+	phy_register_provider(dev);
+	return (0);
+}
+
+static int
+usbphy_detach(device_t dev)
+{
+
+	/* This device is always present. */
+	return (EBUSY);
+}
+
+static device_method_t tegra_usbphy_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		usbphy_probe),
+	DEVMETHOD(device_attach,	usbphy_attach),
+	DEVMETHOD(device_detach,	usbphy_detach),
+
+	/* phy interface */
+	DEVMETHOD(phy_enable,		usbphy_phy_enable),
+
+	DEVMETHOD_END
+};
+
+static driver_t tegra_usbphy_driver = {
+	"tegra_usbphy",
+	tegra_usbphy_methods,
+	sizeof(struct usbphy_softc),
+};
+
+static devclass_t tegra_usbphy_devclass;
+
+EARLY_DRIVER_MODULE(tegra_usbphy, simplebus, tegra_usbphy_driver,
+    tegra_usbphy_devclass, 0, 0, 79);