MFC: r266470, r273546, r276017, r277932, r279153, r279778, r279780, r278797,

     r278861, r280283, r280284, r280294, r280452, r280558, r280571, r281863,
     r282049, r282357, r282440, r282441, r282358, r282359, r283550, r283918,
     r290171, r290667, r290381, r290533, r290666, r292483, r295659, r297545,
     r298305, r298383, r298428, r306489, r306557, r307067, r307068, r307087,
     r307088, r307089, r307091, r307092, r307093, r307098, r307115, r307154,
     r307240, r307241, r315967, r316476

Unbreak BCM2835/RPI-B support by bringing it in line with stable/11 and
head:

- Optimise reading of pending interrupt registers.

- Fix a bug where some DTS layouts could cause the premature ending of the
  search (i.e. without returning any result) and you would end up with a
  random MAC address.

- Reduce the diff between head and arm_intrng with the bcm2835 interrupt
  controller.

- Allow the retrieving of the reserved pins state.

- Add support to the bcm2835 mailbox driver to work before interrupts are
  enabled. This will be needed to enable the power on devices early on in the
  boot process.

- Add support for enabling the USB on the Raspberry Pi boards when it hasn't
  been done by U-Boot. This allows the USB to work when we load the kernel
  directly.

- Call config_intrhook_disestablish on failure of the bcm2835 fb and fbd intr
  hooks. With this we can get through the boot even if these functions fail.

- Add the structures needed to get/set the power state. These can be used
  when, for example, we boot without U-Boot and wish to enable USB, or to
  suspend an unneeded device.

- Add a mask to match only the relative base address of BSC controllers.

- Move the code to set the device power to the bcm2835 mailbox driver so it
  can be reused by other drivers.

- Add the SOC_BCM2835 and SOC_BCM2836 options for the arm kernel and add the
  former to std.bcm2835.

- Add a helper function to read clock frequencies from videocore and use this
  to get the default frequency of the sdhci device.

- Add partial support for the Raspberry Pi 2.

- Remove a debug #error from the bcm2835 sdhci driver.

- Fetch the SDHCI frequency from videocore (our prefered source) and only if
  it fails, fetch the clock-frequency from DTB. If both methods fail, use the
  hardcoded default.

- Pass the supplied buffer length instead of a fixed size.

- Add the routines to query and setup the framebuffer state using the
  BCM2835_MBOX_CHAN_PROP channel.  The old channel (BCM2835_MBOX_CHAN_FB)
  seems deprecated on recent firmware versions and is causing a freeze on
  RPi 2.

- Fix DMA on RPi 2. BCM2836 has a different base address for peripherals.

- Enable DMA for sdhci on RPi 2 (BCM2836).

- Add a missing wakeup when releasing ownership of the SPI hardware.

- Fix framebuffer compatibility with new RPi firmware.

- Refactor bcm2835_cpufreq to use bcm2835_mbox_property API.

- Fix the sc(4) framebuffer driver on RPi 2.

- Fix the vt(4) framebuffer driver on RPi 2.

- Remove unused mutex and softc variables.

- Refactor mailbox property API to make it usable for /dev/vcio driver.

- Replace semaphore-base locking with sleep/wait synchronization.

- Fix infinite loop if response from VideoCore never received.

- Set have_message in interrupt to handle "response before READ" case.

- Serialize access to property channel when using bcm2835_mbox_property.

- Force framebuffer virtual viewport to be the same as physical.

- Use proper type of tag in bcm2835_mbox_fb_init.

- bcm2835_cpufreq: Only attach driver if we correcly match on the machine
  compatible string.

- Add dev.fb.X.resync sysctl to resync ARM framebuffer with VideoCore.

- Do not use DMA channels used by GPU.

- Define local-intc for BCM2836 platform (RPI2) and make BCM2835 intc
  a child of it.

- Fix build for Pi kernels with syscons enabled.

- Use VM_MEMATTR_WRITE_COMBINING memattr for mmap(2) on framebuffer.

- Make intc driver compatible with upstream DTS.

- Make Rapsberry Pi watchdog driver compatible with upstream DTS.

- Make sure intc is attached before interrupt consumers.

- Make framebuffer driver compatible with upstream DT.

- Add one more heuristic to determine MAC address of the SMSC device.

- Add compatibility strings from upstream DT.

- Fix spelling mistake, BCM2835_PASWORD -> BCM2835_PASSWORD.

Approved by:	re (kib)

28 files changed, 1791 insertions, 1080 deletions

diff --git a/sys/arm/broadcom/bcm2835/bcm2835_bsc.c b/sys/arm/broadcom/bcm2835/bcm2835_bsc.c
index 3e1afcd..e149d5d 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_bsc.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_bsc.c
@@ -52,6 +52,13 @@ __FBSDID("$FreeBSD$");
 
 #include "iicbus_if.h"
 
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-bsc",	1},
+	{"brcm,bcm2708-i2c",		1},
+	{"brcm,bcm2835-i2c",		1},
+	{NULL,				0}
+};
+
 static void bcm_bsc_intr(void *);
 static int bcm_bsc_detach(device_t);
 
@@ -214,7 +221,7 @@ bcm_bsc_probe(device_t dev)
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-bsc"))
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
 		return (ENXIO);
 
 	device_set_desc(dev, "BCM2708/2835 BSC controller");
@@ -247,7 +254,7 @@ bcm_bsc_attach(device_t dev)
 	/* Check the unit we are attaching by its base address. */
 	start = rman_get_start(sc->sc_mem_res);
 	for (i = 0; i < nitems(bcm_bsc_pins); i++) {
-		if (bcm_bsc_pins[i].start == start)
+		if (bcm_bsc_pins[i].start == (start & BCM_BSC_BASE_MASK))
 			break;
 	}
 	if (i == nitems(bcm_bsc_pins)) {
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_bscvar.h b/sys/arm/broadcom/bcm2835/bcm2835_bscvar.h
index 5068356..6b31dc3 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_bscvar.h
+++ b/sys/arm/broadcom/bcm2835/bcm2835_bscvar.h
@@ -35,9 +35,10 @@ struct {
 	uint32_t	scl;
 	unsigned long	start;
 } bcm_bsc_pins[] = {
-	{ 0, 1, 0x20205000 },	/* BSC0 GPIO pins and base address. */
-	{ 2, 3, 0x20804000 }	/* BSC1 GPIO pins and base address. */
+	{ 0, 1, 0x205000 },	/* BSC0 GPIO pins and base address. */
+	{ 2, 3, 0x804000 }	/* BSC1 GPIO pins and base address. */
 };
+#define	BCM_BSC_BASE_MASK	0x00ffffff
 
 struct bcm_bsc_softc {
 	device_t		sc_dev;
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_common.c b/sys/arm/broadcom/bcm2835/bcm2835_common.c
index 5c3c258..ef4fe38 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_common.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_common.c
@@ -58,14 +58,22 @@ fdt_intc_decode_ic(phandle_t node, pcell_t *intr, int *interrupt, int *trig,
     int *pol)
 {
 
-	if (!fdt_is_compatible(node, "broadcom,bcm2835-armctrl-ic"))
-		return (ENXIO);
-
-	*interrupt = fdt32_to_cpu(intr[0]);
-	*trig = INTR_TRIGGER_CONFORM;
-	*pol = INTR_POLARITY_CONFORM;
-
-	return (0);
+	if (fdt_is_compatible(node, "broadcom,bcm2835-armctrl-ic") ||
+	    fdt_is_compatible(node, "brcm,bcm2836-armctrl-ic")) {
+		*interrupt = fdt32_to_cpu(intr[0]);
+		*trig = INTR_TRIGGER_CONFORM;
+		*pol = INTR_POLARITY_CONFORM;
+		return (0);
+	}
+#ifdef SOC_BCM2836
+	if (fdt_is_compatible(node, "brcm,bcm2836-l1-intc")) {
+		*interrupt = fdt32_to_cpu(intr[0]) + 72;
+		*trig = INTR_TRIGGER_CONFORM;
+		*pol = INTR_POLARITY_CONFORM;
+		return (0);
+	}
+#endif
+	return (ENXIO);
 }
 
 
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_cpufreq.c b/sys/arm/broadcom/bcm2835/bcm2835_cpufreq.c
index 2ea5b49..2b17ea8 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_cpufreq.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_cpufreq.c
@@ -44,6 +44,11 @@ __FBSDID("$FreeBSD$");
 #include <machine/cpu.h>
 #include <machine/intr.h>
 
+#include <dev/fdt/fdt_common.h>
+
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
 #include <arm/broadcom/bcm2835/bcm2835_mbox.h>
 #include <arm/broadcom/bcm2835/bcm2835_mbox_prop.h>
 #include <arm/broadcom/bcm2835/bcm2835_vcbus.h>
@@ -108,17 +113,17 @@ struct bcm2835_cpufreq_softc {
 	int		voltage_sdram_p;
 	int		turbo_mode;
 
-	/* mbox buffer (physical address) */
-	bus_dma_tag_t	dma_tag;
-	bus_dmamap_t	dma_map;
-	bus_size_t	dma_size;
-	void		*dma_buf;
-	bus_addr_t	dma_phys;
-
 	/* initial hook for waiting mbox intr */
 	struct intr_config_hook	init_hook;
 };
 
+static struct ofw_compat_data compat_data[] = {
+	{ "broadcom,bcm2835-vc",	1 },
+	{ "broadcom,bcm2708-vc",	1 },
+	{ "brcm,bcm2709",	1 },
+	{ NULL, 0 }
+};
+
 static int cpufreq_verbose = 0;
 TUNABLE_INT("hw.bcm2835.cpufreq.verbose", &cpufreq_verbose);
 static int cpufreq_lowest_freq = DEFAULT_LOWEST_FREQ;
@@ -144,84 +149,10 @@ bcm2835_dump(const void *data, int len)
 #endif
 
 static int
-bcm2835_mbox_call_prop(struct bcm2835_cpufreq_softc *sc)
-{
-	struct bcm2835_mbox_hdr *msg = (struct bcm2835_mbox_hdr *)sc->dma_buf;
-	struct bcm2835_mbox_tag_hdr *tag, *last;
-	uint8_t *up;
-	device_t mbox;
-	size_t hdr_size;
-	int idx;
-	int err;
-
-	/*
-	 * For multiple calls, locking is not here. The caller must have
-	 * VC semaphore.
-	 */
-
-	/* get mbox device */
-	mbox = devclass_get_device(devclass_find("mbox"), 0);
-	if (mbox == NULL) {
-		device_printf(sc->dev, "can't find mbox\n");
-		return (-1);
-	}
-
-	/* go mailbox property */
-#ifdef PROP_DEBUG
-	bcm2835_dump(msg, 64);
-#endif
-	bus_dmamap_sync(sc->dma_tag, sc->dma_map,
-	    BUS_DMASYNC_PREWRITE | BUS_DMASYNC_PREREAD);
-	MBOX_WRITE(mbox, BCM2835_MBOX_CHAN_PROP, (uint32_t)sc->dma_phys);
-	MBOX_READ(mbox, BCM2835_MBOX_CHAN_PROP, &err);
-	bus_dmamap_sync(sc->dma_tag, sc->dma_map, BUS_DMASYNC_POSTREAD);
-#ifdef PROP_DEBUG
-	bcm2835_dump(msg, 64);
-#endif
-
-	/* check response code */
-	if (msg->code != BCM2835_MBOX_CODE_RESP_SUCCESS) {
-		device_printf(sc->dev, "mbox response error\n");
-		return (-1);
-	}
-
-	/* tag = first tag */
-	up = (uint8_t *)msg;
-	hdr_size = sizeof(struct bcm2835_mbox_hdr);
-	tag = (struct bcm2835_mbox_tag_hdr *)(up + hdr_size);
-	/* last = end of buffer specified by header */
-	last = (struct bcm2835_mbox_tag_hdr *)(up + msg->buf_size);
-
-	/* loop unitl end tag (=0x0) */
-	hdr_size = sizeof(struct bcm2835_mbox_tag_hdr);
-	for (idx = 0; tag->tag != 0; idx++) {
-		if ((tag->val_len & BCM2835_MBOX_TAG_VAL_LEN_RESPONSE) == 0) {
-			device_printf(sc->dev, "tag%d response error\n", idx);
-			return (-1);
-		}
-		/* clear response bit */
-		tag->val_len &= ~BCM2835_MBOX_TAG_VAL_LEN_RESPONSE;
-
-		/* get next tag */
-		up = (uint8_t *)tag;
-		tag = (struct bcm2835_mbox_tag_hdr *)(up + hdr_size +
-		    tag->val_buf_size);
-
-		/* check buffer size of header */
-		if (tag > last) {
-			device_printf(sc->dev, "mbox buffer size error\n");
-			return (-1);
-		}
-	}
-
-	return (0);
-}
-
-static int
 bcm2835_cpufreq_get_clock_rate(struct bcm2835_cpufreq_softc *sc,
     uint32_t clock_id)
 {
-	struct msg_get_clock_rate *msg;
+	struct msg_get_clock_rate msg;
 	int rate;
 	int err;
 
@@ -239,26 +170,18 @@ bcm2835_cpufreq_get_clock_rate(struct bcm2835_cpufreq_softc *sc,
 	 *       u32: rate (in Hz)
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_get_clock_rate *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_GET_CLOCK_RATE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.clock_id = clock_id;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_CLOCK_RATE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.clock_id = clock_id;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't get clock rate (id=%u)\n",
 		    clock_id);
@@ -266,7 +189,7 @@ bcm2835_cpufreq_get_clock_rate(struct bcm2835_cpufreq_softc *sc,
 	}
 
 	/* result (Hz) */
-	rate = (int)msg->body.resp.rate_hz;
+	rate = (int)msg.body.resp.rate_hz;
 	DPRINTF("clock = %d(Hz)\n", rate);
 	return (rate);
 }
@@ -275,7 +198,7 @@ static int
 bcm2835_cpufreq_get_max_clock_rate(struct bcm2835_cpufreq_softc *sc,
     uint32_t clock_id)
 {
-	struct msg_get_max_clock_rate *msg;
+	struct msg_get_max_clock_rate msg;
 	int rate;
 	int err;
 
@@ -293,26 +216,18 @@ bcm2835_cpufreq_get_max_clock_rate(struct bcm2835_cpufreq_softc *sc,
 	 *       u32: rate (in Hz)
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_get_max_clock_rate *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_GET_MAX_CLOCK_RATE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.clock_id = clock_id;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_MAX_CLOCK_RATE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.clock_id = clock_id;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't get max clock rate (id=%u)\n",
 		    clock_id);
@@ -320,7 +235,7 @@ bcm2835_cpufreq_get_max_clock_rate(struct bcm2835_cpufreq_softc *sc,
 	}
 
 	/* result (Hz) */
-	rate = (int)msg->body.resp.rate_hz;
+	rate = (int)msg.body.resp.rate_hz;
 	DPRINTF("clock = %d(Hz)\n", rate);
 	return (rate);
 }
@@ -329,7 +244,7 @@ static int
 bcm2835_cpufreq_get_min_clock_rate(struct bcm2835_cpufreq_softc *sc,
     uint32_t clock_id)
 {
-	struct msg_get_min_clock_rate *msg;
+	struct msg_get_min_clock_rate msg;
 	int rate;
 	int err;
 
@@ -347,26 +262,18 @@ bcm2835_cpufreq_get_min_clock_rate(struct bcm2835_cpufreq_softc *sc,
 	 *       u32: rate (in Hz)
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_get_min_clock_rate *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_GET_MIN_CLOCK_RATE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.clock_id = clock_id;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_MIN_CLOCK_RATE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.clock_id = clock_id;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't get min clock rate (id=%u)\n",
 		    clock_id);
@@ -374,7 +281,7 @@ bcm2835_cpufreq_get_min_clock_rate(struct bcm2835_cpufreq_softc *sc,
 	}
 
 	/* result (Hz) */
-	rate = (int)msg->body.resp.rate_hz;
+	rate = (int)msg.body.resp.rate_hz;
 	DPRINTF("clock = %d(Hz)\n", rate);
 	return (rate);
 }
@@ -383,7 +290,7 @@ static int
 bcm2835_cpufreq_set_clock_rate(struct bcm2835_cpufreq_softc *sc,
     uint32_t clock_id, uint32_t rate_hz)
 {
-	struct msg_set_clock_rate *msg;
+	struct msg_set_clock_rate msg;
 	int rate;
 	int err;
 
@@ -402,27 +309,19 @@ bcm2835_cpufreq_set_clock_rate(struct bcm2835_cpufreq_softc *sc,
 	 *       u32: rate (in Hz)
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_set_clock_rate *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_SET_CLOCK_RATE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.clock_id = clock_id;
-	msg->body.req.rate_hz = rate_hz;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_SET_CLOCK_RATE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.clock_id = clock_id;
+	msg.body.req.rate_hz = rate_hz;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't set clock rate (id=%u)\n",
 		    clock_id);
@@ -440,18 +339,18 @@ bcm2835_cpufreq_set_clock_rate(struct bcm2835_cpufreq_softc *sc,
 		 */
 
 		/* setup single tag buffer */
-		memset(msg, 0, sizeof(*msg));
-		msg->hdr.buf_size = sizeof(*msg);
-		msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-		msg->tag_hdr.tag = BCM2835_MBOX_TAG_SET_CLOCK_RATE;
-		msg->tag_hdr.val_buf_size = sizeof(msg->body);
-		msg->tag_hdr.val_len = sizeof(msg->body.req);
-		msg->body.req.clock_id = clock_id;
-		msg->body.req.rate_hz = rate_hz;
-		msg->end_tag = 0;
+		memset(&msg, 0, sizeof(msg));
+		msg.hdr.buf_size = sizeof(msg);
+		msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+		msg.tag_hdr.tag = BCM2835_MBOX_TAG_SET_CLOCK_RATE;
+		msg.tag_hdr.val_buf_size = sizeof(msg.body);
+		msg.tag_hdr.val_len = sizeof(msg.body.req);
+		msg.body.req.clock_id = clock_id;
+		msg.body.req.rate_hz = rate_hz;
+		msg.end_tag = 0;
 
 		/* call mailbox property */
-		err = bcm2835_mbox_call_prop(sc);
+		err = bcm2835_mbox_property(&msg, sizeof(msg));
 		if (err) {
 			device_printf(sc->dev,
 			    "can't set clock rate (id=%u)\n", clock_id);
@@ -460,7 +359,7 @@ bcm2835_cpufreq_set_clock_rate(struct bcm2835_cpufreq_softc *sc,
 	}
 
 	/* result (Hz) */
-	rate = (int)msg->body.resp.rate_hz;
+	rate = (int)msg.body.resp.rate_hz;
 	DPRINTF("clock = %d(Hz)\n", rate);
 	return (rate);
 }
@@ -468,7 +367,7 @@ bcm2835_cpufreq_set_clock_rate(struct bcm2835_cpufreq_softc *sc,
 static int
 bcm2835_cpufreq_get_turbo(struct bcm2835_cpufreq_softc *sc)
 {
-	struct msg_get_turbo *msg;
+	struct msg_get_turbo msg;
 	int level;
 	int err;
 
@@ -486,33 +385,25 @@ bcm2835_cpufreq_get_turbo(struct bcm2835_cpufreq_softc *sc)
 	 *       u32: level
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_get_turbo *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_GET_TURBO;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.id = 0;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_TURBO;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.id = 0;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't get turbo\n");
 		return (MSG_ERROR);
 	}
 
 	/* result 0=non-turbo, 1=turbo */
-	level = (int)msg->body.resp.level;
+	level = (int)msg.body.resp.level;
 	DPRINTF("level = %d\n", level);
 	return (level);
 }
@@ -520,7 +411,7 @@ bcm2835_cpufreq_get_turbo(struct bcm2835_cpufreq_softc *sc)
 static int
 bcm2835_cpufreq_set_turbo(struct bcm2835_cpufreq_softc *sc, uint32_t level)
 {
-	struct msg_set_turbo *msg;
+	struct msg_set_turbo msg;
 	int value;
 	int err;
 
@@ -539,38 +430,30 @@ bcm2835_cpufreq_set_turbo(struct bcm2835_cpufreq_softc *sc, uint32_t level)
 	 *       u32: level
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_set_turbo *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* replace unknown value to OFF */
 	if (level != BCM2835_MBOX_TURBO_ON && level != BCM2835_MBOX_TURBO_OFF)
 		level = BCM2835_MBOX_TURBO_OFF;
 
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_SET_TURBO;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.id = 0;
-	msg->body.req.level = level;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_SET_TURBO;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.id = 0;
+	msg.body.req.level = level;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't set turbo\n");
 		return (MSG_ERROR);
 	}
 
 	/* result 0=non-turbo, 1=turbo */
-	value = (int)msg->body.resp.level;
+	value = (int)msg.body.resp.level;
 	DPRINTF("level = %d\n", value);
 	return (value);
 }
@@ -579,7 +462,7 @@ static int
 bcm2835_cpufreq_get_voltage(struct bcm2835_cpufreq_softc *sc,
     uint32_t voltage_id)
 {
-	struct msg_get_voltage *msg;
+	struct msg_get_voltage msg;
 	int value;
 	int err;
 
@@ -597,33 +480,25 @@ bcm2835_cpufreq_get_voltage(struct bcm2835_cpufreq_softc *sc,
 	 *       u32: value (offset from 1.2V in units of 0.025V)
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_get_voltage *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_GET_VOLTAGE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.voltage_id = voltage_id;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_VOLTAGE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.voltage_id = voltage_id;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't get voltage\n");
 		return (MSG_ERROR);
 	}
 
 	/* result (offset from 1.2V) */
-	value = (int)msg->body.resp.value;
+	value = (int)msg.body.resp.value;
 	DPRINTF("value = %d\n", value);
 	return (value);
 }
@@ -632,7 +507,7 @@ static int
 bcm2835_cpufreq_get_max_voltage(struct bcm2835_cpufreq_softc *sc,
     uint32_t voltage_id)
 {
-	struct msg_get_max_voltage *msg;
+	struct msg_get_max_voltage msg;
 	int value;
 	int err;
 
@@ -650,33 +525,25 @@ bcm2835_cpufreq_get_max_voltage(struct bcm2835_cpufreq_softc *sc,
 	 *       u32: value (offset from 1.2V in units of 0.025V)
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_get_max_voltage *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_GET_MAX_VOLTAGE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.voltage_id = voltage_id;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_MAX_VOLTAGE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.voltage_id = voltage_id;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't get max voltage\n");
 		return (MSG_ERROR);
 	}
 
 	/* result (offset from 1.2V) */
-	value = (int)msg->body.resp.value;
+	value = (int)msg.body.resp.value;
 	DPRINTF("value = %d\n", value);
 	return (value);
 }
@@ -684,7 +551,7 @@ static int
 bcm2835_cpufreq_get_min_voltage(struct bcm2835_cpufreq_softc *sc,
     uint32_t voltage_id)
 {
-	struct msg_get_min_voltage *msg;
+	struct msg_get_min_voltage msg;
 	int value;
 	int err;
 
@@ -702,33 +569,25 @@ bcm2835_cpufreq_get_min_voltage(struct bcm2835_cpufreq_softc *sc,
 	 *       u32: value (offset from 1.2V in units of 0.025V)
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_get_min_voltage *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_GET_MIN_VOLTAGE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.voltage_id = voltage_id;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_MIN_VOLTAGE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.voltage_id = voltage_id;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't get min voltage\n");
 		return (MSG_ERROR);
 	}
 
 	/* result (offset from 1.2V) */
-	value = (int)msg->body.resp.value;
+	value = (int)msg.body.resp.value;
 	DPRINTF("value = %d\n", value);
 	return (value);
 }
@@ -737,7 +596,7 @@ static int
 bcm2835_cpufreq_set_voltage(struct bcm2835_cpufreq_softc *sc,
     uint32_t voltage_id, int32_t value)
 {
-	struct msg_set_voltage *msg;
+	struct msg_set_voltage msg;
 	int err;
 
 	/*
@@ -766,34 +625,26 @@ bcm2835_cpufreq_set_voltage(struct bcm2835_cpufreq_softc *sc,
 		return (MSG_ERROR);
 	}
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_set_voltage *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_SET_VOLTAGE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.voltage_id = voltage_id;
-	msg->body.req.value = (uint32_t)value;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_SET_VOLTAGE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.voltage_id = voltage_id;
+	msg.body.req.value = (uint32_t)value;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't set voltage\n");
 		return (MSG_ERROR);
 	}
 
 	/* result (offset from 1.2V) */
-	value = (int)msg->body.resp.value;
+	value = (int)msg.body.resp.value;
 	DPRINTF("value = %d\n", value);
 	return (value);
 }
@@ -801,7 +652,7 @@ bcm2835_cpufreq_set_voltage(struct bcm2835_cpufreq_softc *sc,
 static int
 bcm2835_cpufreq_get_temperature(struct bcm2835_cpufreq_softc *sc)
 {
-	struct msg_get_temperature *msg;
+	struct msg_get_temperature msg;
 	int value;
 	int err;
 
@@ -819,33 +670,25 @@ bcm2835_cpufreq_get_temperature(struct bcm2835_cpufreq_softc *sc)
 	 *       u32: value
 	 */
 
-	/* using DMA buffer for VC */
-	msg = (struct msg_get_temperature *)sc->dma_buf;
-	if (sizeof(*msg) > sc->dma_size) {
-		device_printf(sc->dev, "DMA size overflow (%zu>%lu)\n",
-		    sizeof(*msg), sc->dma_size);
-		return (MSG_ERROR);
-	}
-
 	/* setup single tag buffer */
-	memset(msg, 0, sizeof(*msg));
-	msg->hdr.buf_size = sizeof(*msg);
-	msg->hdr.code = BCM2835_MBOX_CODE_REQ;
-	msg->tag_hdr.tag = BCM2835_MBOX_TAG_GET_TEMPERATURE;
-	msg->tag_hdr.val_buf_size = sizeof(msg->body);
-	msg->tag_hdr.val_len = sizeof(msg->body.req);
-	msg->body.req.temperature_id = 0;
-	msg->end_tag = 0;
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_TEMPERATURE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.temperature_id = 0;
+	msg.end_tag = 0;
 
 	/* call mailbox property */
-	err = bcm2835_mbox_call_prop(sc);
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
 	if (err) {
 		device_printf(sc->dev, "can't get temperature\n");
 		return (MSG_ERROR);
 	}
 
 	/* result (temperature of degree C) */
-	value = (int)msg->body.resp.value;
+	value = (int)msg.body.resp.value;
 	DPRINTF("value = %d\n", value);
 	return (value);
 }
@@ -1406,6 +1249,16 @@ bcm2835_cpufreq_init(void *arg)
 static void
 bcm2835_cpufreq_identify(driver_t *driver, device_t parent)
 {
+	const struct ofw_compat_data *compat;
+	phandle_t root;
+
+	root = OF_finddevice("/");
+	for (compat = compat_data; compat->ocd_str != NULL; compat++)
+		if (fdt_is_compatible(root, compat->ocd_str))
+			break;
+
+	if (compat->ocd_data == 0)
+		return;
 
 	DPRINTF("driver=%p, parent=%p\n", driver, parent);
 	if (device_find_child(parent, "bcm2835_cpufreq", -1) != NULL)
@@ -1422,23 +1275,11 @@ bcm2835_cpufreq_probe(device_t dev)
 	return (0);
 }
 
-static void
-bcm2835_cpufreq_cb(void *arg, bus_dma_segment_t *segs, int nseg, int err)
-{
-	bus_addr_t *addr;
-
-	if (err)
-		return;
-	addr = (bus_addr_t *)arg;
-	*addr = PHYS_TO_VCBUS(segs[0].ds_addr);
-}
-
 static int
 bcm2835_cpufreq_attach(device_t dev)
 {
 	struct bcm2835_cpufreq_softc *sc;
 	struct sysctl_oid *oid;
-	int err;
 
 	/* set self dev */
 	sc = device_get_softc(dev);
@@ -1454,41 +1295,6 @@ bcm2835_cpufreq_attach(device_t dev)
 	sc->max_voltage_core = 0;
 	sc->min_voltage_core = 0;
 
-	/* create VC mbox buffer */
-	sc->dma_size = PAGE_SIZE;
-	err = bus_dma_tag_create(
-	    bus_get_dma_tag(sc->dev),
-	    PAGE_SIZE, 0,		/* alignment, boundary */
-	    BUS_SPACE_MAXADDR_32BIT,	/* lowaddr */
-	    BUS_SPACE_MAXADDR,		/* highaddr */
-	    NULL, NULL,			/* filter, filterarg */
-	    sc->dma_size, 1,		/* maxsize, nsegments */
-	    sc->dma_size, 0,		/* maxsegsize, flags */
-	    NULL, NULL,			/* lockfunc, lockarg */
-	    &sc->dma_tag);
-	if (err) {
-		device_printf(dev, "can't create DMA tag\n");
-		return (ENXIO);
-	}
-
-	err = bus_dmamem_alloc(sc->dma_tag, (void **)&sc->dma_buf, 0,
-	    &sc->dma_map);
-	if (err) {
-		bus_dma_tag_destroy(sc->dma_tag);
-		device_printf(dev, "can't allocate dmamem\n");
-		return (ENXIO);
-	}
-
-	err = bus_dmamap_load(sc->dma_tag, sc->dma_map, sc->dma_buf,
-	    sc->dma_size, bcm2835_cpufreq_cb, &sc->dma_phys, 0);
-	if (err) {
-		bus_dmamem_free(sc->dma_tag, sc->dma_buf, sc->dma_map);
-		bus_dma_tag_destroy(sc->dma_tag);
-		device_printf(dev, "can't load DMA map\n");
-		return (ENXIO);
-	}
-	/* OK, ready to use VC buffer */
-
 	/* setup sysctl at first device */
 	if (device_get_unit(dev) == 0) {
 		sysctl_ctx_init(&bcm2835_sysctl_ctx);
@@ -1558,9 +1364,6 @@ bcm2835_cpufreq_attach(device_t dev)
 	sc->init_hook.ich_arg = sc;
 
 	if (config_intrhook_establish(&sc->init_hook) != 0) {
-		bus_dmamap_unload(sc->dma_tag, sc->dma_map);
-		bus_dmamem_free(sc->dma_tag, sc->dma_buf, sc->dma_map);
-		bus_dma_tag_destroy(sc->dma_tag);
 		device_printf(dev, "config_intrhook_establish failed\n");
 		return (ENOMEM);
 	}
@@ -1580,13 +1383,6 @@ bcm2835_cpufreq_detach(device_t dev)
 
 	sema_destroy(&vc_sema);
 
-	if (sc->dma_phys != 0)
-		bus_dmamap_unload(sc->dma_tag, sc->dma_map);
-	if (sc->dma_buf != NULL)
-		bus_dmamem_free(sc->dma_tag, sc->dma_buf, sc->dma_map);
-	if (sc->dma_tag != NULL)
-		bus_dma_tag_destroy(sc->dma_tag);
-
 	return (cpufreq_unregister(dev));
 }
 
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_dma.c b/sys/arm/broadcom/bcm2835/bcm2835_dma.c
index 6a2967a..897e480 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_dma.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_dma.c
@@ -104,6 +104,15 @@ __FBSDID("$FreeBSD$");
 /* relative offset from BCM_VC_DMA0_BASE (p.39) */
 #define	BCM_DMA_CH(n)		(0x100*(n))
 
+/* channels used by GPU */
+#define	BCM_DMA_CH_BULK		0
+#define	BCM_DMA_CH_FAST1	2
+#define	BCM_DMA_CH_FAST2	3
+
+#define	BCM_DMA_CH_GPU_MASK	((1 << BCM_DMA_CH_BULK) |	\
+				 (1 << BCM_DMA_CH_FAST1) |	\
+				 (1 << BCM_DMA_CH_FAST2))
+
 /* DMA Control Block - 256bit aligned (p.40) */
 struct bcm_dma_cb {
 	uint32_t info;		/* Transfer Information */
@@ -143,6 +152,13 @@ struct bcm_dma_softc {
 };
 
 static struct bcm_dma_softc *bcm_dma_sc = NULL;
+static uint32_t bcm_dma_channel_mask;
+
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-dma",	1},
+	{"brcm,bcm2835-dma",		1},
+	{NULL,				0}
+};
 
 static void
 bcm_dmamap_cb(void *arg, bus_dma_segment_t *segs,
@@ -205,16 +221,32 @@ static int
 bcm_dma_init(device_t dev)
 {
 	struct bcm_dma_softc *sc = device_get_softc(dev);
-	uint32_t mask;
+	uint32_t reg;
 	struct bcm_dma_ch *ch;
 	void *cb_virt;
 	vm_paddr_t cb_phys;
 	int err;
 	int i;
 
-	/* disable and clear interrupt status */
-	bus_write_4(sc->sc_mem, BCM_DMA_ENABLE, 0);
-	bus_write_4(sc->sc_mem, BCM_DMA_INT_STATUS, 0);
+	/*
+	 * Only channels set in bcm_dma_channel_mask can be controlled by us.
+	 * The others are out of our control as well as the corresponding bits
+	 * in both BCM_DMA_ENABLE and BCM_DMA_INT_STATUS global registers. As
+	 * these registers are RW ones, there is no safe way how to write only
+	 * the bits which can be controlled by us.
+	 *
+	 * Fortunately, after reset, all channels are enabled in BCM_DMA_ENABLE
+	 * register and all statuses are cleared in BCM_DMA_INT_STATUS one.
+	 * Not touching these registers is a trade off between correct
+	 * initialization which does not count on anything and not messing up
+	 * something we have no control over.
+	 */
+	reg = bus_read_4(sc->sc_mem, BCM_DMA_ENABLE);
+	if ((reg & bcm_dma_channel_mask) != bcm_dma_channel_mask)
+		device_printf(dev, "channels are not enabled\n");
+	reg = bus_read_4(sc->sc_mem, BCM_DMA_INT_STATUS);
+	if ((reg & bcm_dma_channel_mask) != 0)
+		device_printf(dev, "statuses are not cleared\n");
 
 	/* Allocate DMA chunks control blocks */
 	/* p.40 of spec - control block should be 32-bit aligned */
@@ -227,7 +259,7 @@ bcm_dma_init(device_t dev)
 	    &sc->sc_dma_tag);
 
 	if (err) {
-		device_printf(dev, "failed allocate DMA tag");
+		device_printf(dev, "failed allocate DMA tag\n");
 		return (err);
 	}
 
@@ -235,6 +267,13 @@ bcm_dma_init(device_t dev)
 	for (i = 0; i < BCM_DMA_CH_MAX; i++) {
 		ch = &sc->sc_dma_ch[i];
 
+		bzero(ch, sizeof(struct bcm_dma_ch));
+		ch->ch = i;
+		ch->flags = BCM_DMA_CH_UNMAP;
+
+		if ((bcm_dma_channel_mask & (1 << i)) == 0)
+			continue;
+
 		err = bus_dmamem_alloc(sc->sc_dma_tag, &cb_virt,
 		    BUS_DMA_WAITOK | BUS_DMA_COHERENT | BUS_DMA_ZERO,
 		    &ch->dma_map);
@@ -263,33 +302,15 @@ bcm_dma_init(device_t dev)
 			break;
 		}
 
-		bzero(ch, sizeof(struct bcm_dma_ch));
-		ch->ch = i;
 		ch->cb = cb_virt;
 		ch->vc_cb = cb_phys;
-		ch->intr_func = NULL;
-		ch->intr_arg = NULL;
-		ch->flags = BCM_DMA_CH_UNMAP;
-
+		ch->flags = BCM_DMA_CH_FREE;
 		ch->cb->info = INFO_WAIT_RESP;
 
 		/* reset DMA engine */
-		bcm_dma_reset(dev, i);
+		bus_write_4(sc->sc_mem, BCM_DMA_CS(i), CS_RESET);
 	}
 
-	/* now use DMA2/DMA3 only */
-	sc->sc_dma_ch[2].flags = BCM_DMA_CH_FREE;
-	sc->sc_dma_ch[3].flags = BCM_DMA_CH_FREE;
-
-	/* enable DMAs */
-	mask = 0;
-
-	for (i = 0; i < BCM_DMA_CH_MAX; i++)
-		if (sc->sc_dma_ch[i].flags & BCM_DMA_CH_FREE)
-			mask |= (1 << i);
-
-	bus_write_4(sc->sc_mem, BCM_DMA_ENABLE, mask);
-
 	return (0);
 }
 
@@ -599,8 +620,11 @@ bcm_dma_intr(void *arg)
 	/* my interrupt? */
 	cs = bus_read_4(sc->sc_mem, BCM_DMA_CS(ch->ch));
 
-	if (!(cs & (CS_INT | CS_ERR)))
+	if (!(cs & (CS_INT | CS_ERR))) {
+		device_printf(sc->sc_dev,
+		    "unexpected DMA intr CH=%d, CS=%x\n", ch->ch, cs);
 		return;
+	}
 
 	/* running? */
 	if (!(ch->flags & BCM_DMA_CH_USED)) {
@@ -640,7 +664,7 @@ bcm_dma_probe(device_t dev)
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-dma"))
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
 		return (ENXIO);
 
 	device_set_desc(dev, "BCM2835 DMA Controller");
@@ -651,6 +675,7 @@ static int
 bcm_dma_attach(device_t dev)
 {
 	struct bcm_dma_softc *sc = device_get_softc(dev);
+	phandle_t node;
 	int rid, err = 0;
 	int i;
 
@@ -664,6 +689,19 @@ bcm_dma_attach(device_t dev)
 		sc->sc_intrhand[i] = NULL;
 	}
 
+	/* Get DMA channel mask. */
+	node = ofw_bus_get_node(sc->sc_dev);
+	if (OF_getencprop(node, "brcm,dma-channel-mask", &bcm_dma_channel_mask,
+	    sizeof(bcm_dma_channel_mask)) == -1 &&
+	    OF_getencprop(node, "broadcom,channels", &bcm_dma_channel_mask,
+	    sizeof(bcm_dma_channel_mask)) == -1) {
+		device_printf(dev, "could not get channel mask property\n");
+		return (ENXIO);
+	}
+
+	/* Mask out channels used by GPU. */
+	bcm_dma_channel_mask &= ~BCM_DMA_CH_GPU_MASK;
+
 	/* DMA0 - DMA14 */
 	rid = 0;
 	sc->sc_mem = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid, RF_ACTIVE);
@@ -674,6 +712,9 @@ bcm_dma_attach(device_t dev)
 
 	/* IRQ DMA0 - DMA11 XXX NOT USE DMA12(spurious?) */
 	for (rid = 0; rid < BCM_DMA_CH_MAX; rid++) {
+		if ((bcm_dma_channel_mask & (1 << rid)) == 0)
+			continue;
+
 		sc->sc_irq[rid] = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
 						       RF_ACTIVE);
 		if (sc->sc_irq[rid] == NULL) {
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_dma.h b/sys/arm/broadcom/bcm2835/bcm2835_dma.h
index 785cf2c..c39344a 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_dma.h
+++ b/sys/arm/broadcom/bcm2835/bcm2835_dma.h
@@ -37,8 +37,6 @@
 /* request CH for any nubmer */
 #define	BCM_DMA_CH_INVALID	(-1)
 #define	BCM_DMA_CH_ANY		(-1)
-#define	BCM_DMA_CH_FAST1	(2)
-#define	BCM_DMA_CH_FAST2	(3)
 
 /* Peripheral DREQ Signals (4.2.1.3) */
 #define	BCM_DMA_DREQ_NONE	0
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_fb.c b/sys/arm/broadcom/bcm2835/bcm2835_fb.c
index 88156ed..58caac2 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_fb.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_fb.c
@@ -29,46 +29,27 @@ __FBSDID("$FreeBSD$");
 
 #include <sys/param.h>
 #include <sys/systm.h>
-#include <sys/bio.h>
 #include <sys/bus.h>
-#include <sys/conf.h>
-#include <sys/endian.h>
+#include <sys/consio.h>
+#include <sys/fbio.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
-#include <sys/kthread.h>
-#include <sys/lock.h>
 #include <sys/malloc.h>
 #include <sys/module.h>
-#include <sys/mutex.h>
-#include <sys/queue.h>
-#include <sys/resource.h>
-#include <sys/rman.h>
-#include <sys/time.h>
-#include <sys/timetc.h>
-#include <sys/fbio.h>
-#include <sys/consio.h>
-
-#include <sys/kdb.h>
 
-#include <machine/bus.h>
-#include <machine/cpu.h>
-#include <machine/cpufunc.h>
-#include <machine/resource.h>
-#include <machine/intr.h>
+#include <vm/vm.h>
+#include <vm/pmap.h>
 
+#include <dev/fb/fbreg.h>
 #include <dev/fdt/fdt_common.h>
 #include <dev/ofw/ofw_bus.h>
 #include <dev/ofw/ofw_bus_subr.h>
-
-#include <dev/fb/fbreg.h>
 #include <dev/syscons/syscons.h>
 
-#include <arm/broadcom/bcm2835/bcm2835_mbox.h>
-#include <arm/broadcom/bcm2835/bcm2835_vcbus.h>
+#include <arm/broadcom/bcm2835/bcm2835_mbox_prop.h>
 
 #include "mbox_if.h"
 
-#define	BCMFB_FONT_HEIGHT	16
-
 struct argb {
 	uint8_t		a;
 	uint8_t		r;
@@ -101,40 +82,15 @@ static u_char mouse_pointer[16] = {
         0x0c, 0x0c, 0x06, 0x06, 0x00, 0x00, 0x00, 0x00
 };
 
-#define FB_WIDTH		640
-#define FB_HEIGHT		480
-#define FB_DEPTH		24
-
-struct bcm_fb_config {
-	uint32_t	xres;
-	uint32_t	yres;
-	uint32_t	vxres;
-	uint32_t	vyres;
-	uint32_t	pitch;
-	uint32_t	bpp;
-	uint32_t	xoffset;
-	uint32_t	yoffset;
-	/* Filled by videocore */
-	uint32_t	base;
-	uint32_t	screen_size;
-};
+#define	BCMFB_FONT_HEIGHT	16
+#define	BCMFB_FONT_WIDTH	8
+#define	FB_WIDTH		640
+#define	FB_HEIGHT		480
+#define	FB_DEPTH		24
 
 struct bcmsc_softc {
-	device_t		dev;
-	struct cdev *		cdev;
-	struct mtx		mtx;
-	bus_dma_tag_t		dma_tag;
-	bus_dmamap_t		dma_map;
-	struct bcm_fb_config*	fb_config;
-	bus_addr_t		fb_config_phys;
-	struct intr_config_hook	init_hook;
-
-};
-
-struct video_adapter_softc {
 	/* Videoadpater part */
 	video_adapter_t	va;
-	int		console;
 
 	intptr_t	fb_addr;
 	intptr_t	fb_paddr;
@@ -149,200 +105,85 @@ struct video_adapter_softc {
 	unsigned int	ymargin;
 
 	unsigned char	*font;
+	int		fbswap;
 	int		initialized;
 };
 
-static struct bcmsc_softc *bcmsc_softc;
-static struct video_adapter_softc va_softc;
+static struct bcmsc_softc bcmsc;
 
-#define	bcm_fb_lock(_sc)	mtx_lock(&(_sc)->mtx)
-#define	bcm_fb_unlock(_sc)	mtx_unlock(&(_sc)->mtx)
-#define	bcm_fb_lock_assert(sc)	mtx_assert(&(_sc)->mtx, MA_OWNED)
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-fb",		1},
+	{"brcm,bcm2708-fb",		1},
+	{NULL,				0}
+};
 
 static int bcm_fb_probe(device_t);
 static int bcm_fb_attach(device_t);
-static void bcm_fb_dmamap_cb(void *arg, bus_dma_segment_t *segs, int nseg, int err);
 static void bcmfb_update_margins(video_adapter_t *adp);
 static int bcmfb_configure(int);
 
-static void
-bcm_fb_init(void *arg)
-{
-	struct bcmsc_softc *sc = arg;
-	struct video_adapter_softc *va_sc = &va_softc;
-	int err;
-	volatile struct bcm_fb_config*	fb_config = sc->fb_config;
-	phandle_t node;
-	pcell_t cell;
-	device_t mbox;
-
-	node = ofw_bus_get_node(sc->dev);
-
-	fb_config->xres = 0;
-	fb_config->yres = 0;
-	fb_config->bpp = 0;
-
-	if ((OF_getprop(node, "broadcom,width", &cell, sizeof(cell))) > 0)
-		fb_config->xres = (int)fdt32_to_cpu(cell);
-	if (fb_config->xres == 0)
-		fb_config->xres = FB_WIDTH;
-
-	if ((OF_getprop(node, "broadcom,height", &cell, sizeof(cell))) > 0)
-		fb_config->yres = (uint32_t)fdt32_to_cpu(cell);
-	if (fb_config->yres == 0)
-		fb_config->yres = FB_HEIGHT;
-
-	if ((OF_getprop(node, "broadcom,depth", &cell, sizeof(cell))) > 0)
-		fb_config->bpp = (uint32_t)fdt32_to_cpu(cell);
-	if (fb_config->bpp == 0)
-		fb_config->bpp = FB_DEPTH;
-
-	fb_config->vxres = 0;
-	fb_config->vyres = 0;
-	fb_config->xoffset = 0;
-	fb_config->yoffset = 0;
-	fb_config->base = 0;
-	fb_config->pitch = 0;
-	fb_config->screen_size = 0;
-
-	bus_dmamap_sync(sc->dma_tag, sc->dma_map,
-		BUS_DMASYNC_PREWRITE | BUS_DMASYNC_PREREAD);
-
-	mbox = devclass_get_device(devclass_find("mbox"), 0);
-	if (mbox) {
-		MBOX_WRITE(mbox, BCM2835_MBOX_CHAN_FB, sc->fb_config_phys);
-		MBOX_READ(mbox, BCM2835_MBOX_CHAN_FB, &err);
-	}
-	bus_dmamap_sync(sc->dma_tag, sc->dma_map,
-		BUS_DMASYNC_POSTREAD);
-
-	if (fb_config->base != 0) {
-		device_printf(sc->dev, "%dx%d(%dx%d@%d,%d) %dbpp\n", 
-			fb_config->xres, fb_config->yres,
-			fb_config->vxres, fb_config->vyres,
-			fb_config->xoffset, fb_config->yoffset,
-			fb_config->bpp);
-
-
-		device_printf(sc->dev, "pitch %d, base 0x%08x, screen_size %d\n", 
-			fb_config->pitch, fb_config->base,
-			fb_config->screen_size);
-
-		va_sc->fb_addr = (intptr_t)pmap_mapdev(fb_config->base, fb_config->screen_size);
-		va_sc->fb_paddr = fb_config->base;
-		va_sc->fb_size = fb_config->screen_size;
-		va_sc->depth = fb_config->bpp;
-		va_sc->stride = fb_config->pitch;
-
-		va_sc->width = fb_config->xres;
-		va_sc->height = fb_config->yres;
-		bcmfb_update_margins(&va_sc->va);
-	}
-	else {
-		device_printf(sc->dev, "Failed to set framebuffer info\n");
-		return;
-	}
-
-	config_intrhook_disestablish(&sc->init_hook);
-}
-
 static int
 bcm_fb_probe(device_t dev)
 {
-	int error = 0;
+	int error;
 
-	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-fb"))
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
 		return (ENXIO);
 
 	device_set_desc(dev, "BCM2835 framebuffer device");
-
 	error = sc_probe_unit(device_get_unit(dev), 
 	    device_get_flags(dev) | SC_AUTODETECT_KBD);
 	if (error != 0)
 		return (error);
 
-
 	return (BUS_PROBE_DEFAULT);
 }
 
 static int
 bcm_fb_attach(device_t dev)
 {
-	struct bcmsc_softc *sc = device_get_softc(dev);
-	int dma_size = sizeof(struct bcm_fb_config);
-	int err;
-
-	if (bcmsc_softc)
-		return (ENXIO);
+	struct bcm2835_fb_config fb;
+	struct bcmsc_softc *sc;
 
-	bcmsc_softc = sc;
-
-	sc->dev = dev;
-	mtx_init(&sc->mtx, "bcm2835fb", "fb", MTX_DEF);
-
-	err = bus_dma_tag_create(
-	    bus_get_dma_tag(sc->dev),
-	    PAGE_SIZE, 0,		/* alignment, boundary */
-	    BUS_SPACE_MAXADDR_32BIT,	/* lowaddr */
-	    BUS_SPACE_MAXADDR,		/* highaddr */
-	    NULL, NULL,			/* filter, filterarg */
-	    dma_size, 1,		/* maxsize, nsegments */
-	    dma_size, 0,		/* maxsegsize, flags */
-	    NULL, NULL,			/* lockfunc, lockarg */
-	    &sc->dma_tag);
-
-	err = bus_dmamem_alloc(sc->dma_tag, (void **)&sc->fb_config,
-	    0, &sc->dma_map);
-	if (err) {
-		device_printf(dev, "cannot allocate framebuffer\n");
-		goto fail;
-	}
-
-	err = bus_dmamap_load(sc->dma_tag, sc->dma_map, sc->fb_config,
-	    dma_size, bcm_fb_dmamap_cb, &sc->fb_config_phys, BUS_DMA_NOWAIT);
-
-	if (err) {
-		device_printf(dev, "cannot load DMA map\n");
-		goto fail;
-	}
+	sc = (struct bcmsc_softc *)vid_get_adapter(vid_find_adapter(
+	    "bcmfb", 0));
+	if (sc != NULL)
+		device_set_softc(dev, sc);
+	else
+		sc = device_get_softc(dev);
 
-	err = (sc_attach_unit(device_get_unit(dev),
-	    device_get_flags(dev) | SC_AUTODETECT_KBD));
+	memset(&fb, 0, sizeof(fb));
+	if (bcm2835_mbox_fb_get_w_h(&fb) != 0)
+		return (ENXIO);
+	fb.bpp = FB_DEPTH;
+	fb.vxres = fb.xres;
+	fb.vyres = fb.yres;
+	fb.xoffset = fb.yoffset = 0;
+	if (bcm2835_mbox_fb_init(&fb) != 0)
+		return (ENXIO);
 
-	if (err) {
+	sc->fb_addr = (intptr_t)pmap_mapdev(fb.base, fb.size);
+	sc->fb_paddr = fb.base;
+	sc->fb_size = fb.size;
+	sc->depth = fb.bpp;
+	sc->stride = fb.pitch;
+	sc->width = fb.xres;
+	sc->height = fb.yres;
+	bcmfb_update_margins(&sc->va);
+
+	if (sc_attach_unit(device_get_unit(dev),
+	    device_get_flags(dev) | SC_AUTODETECT_KBD) != 0) {
 		device_printf(dev, "failed to attach syscons\n");
-		goto fail;
+		return (ENXIO);
 	}
 
-	/* 
-	 * We have to wait until interrupts are enabled. 
-	 * Mailbox relies on it to get data from VideoCore
-	 */
-        sc->init_hook.ich_func = bcm_fb_init;
-        sc->init_hook.ich_arg = sc;
-
-        if (config_intrhook_establish(&sc->init_hook) != 0) {
-		device_printf(dev, "failed to establish intrhook\n");
-                return (ENOMEM);
-	}
+	device_printf(dev, "%dx%d(%dx%d@%d,%d) %dbpp\n", fb.xres, fb.yres,
+	    fb.vxres, fb.vyres, fb.xoffset, fb.yoffset, fb.bpp);
+	device_printf(dev,
+	    "fbswap: %d, pitch %d, base 0x%08x, screen_size %d\n",
+	    sc->fbswap, fb.pitch, fb.base, fb.size);
 
 	return (0);
-
-fail:
-	return (ENXIO);
-}
-
-
-static void
-bcm_fb_dmamap_cb(void *arg, bus_dma_segment_t *segs, int nseg, int err)
-{
-	bus_addr_t *addr;
-
-	if (err)
-		return;
-
-	addr = (bus_addr_t*)arg;
-	*addr = PHYS_TO_VCBUS(segs[0].ds_addr);
 }
 
 static device_method_t bcm_fb_methods[] = {
@@ -362,6 +203,7 @@ static driver_t bcm_fb_driver = {
 };
 
 DRIVER_MODULE(bcm2835fb, ofwbus, bcm_fb_driver, bcm_fb_devclass, 0, 0);
+DRIVER_MODULE(bcm2835fb, simplebus, bcm_fb_driver, bcm_fb_devclass, 0, 0);
 
 /*
  * Video driver routines and glue.
@@ -505,13 +347,13 @@ bcmrend_set_cursor(scr_stat* scp, int base, int height, int blink)
 static void
 bcmrend_draw_cursor(scr_stat* scp, int off, int blink, int on, int flip)
 {
-	video_adapter_t* adp = scp->sc->adp;
-	struct video_adapter_softc *sc;
-	int row, col;
+	int bytes, col, i, j, row;
+	struct bcmsc_softc *sc;
 	uint8_t *addr;
-	int i, j, bytes;
+	video_adapter_t *adp;
 
-	sc = (struct video_adapter_softc *)adp;
+	adp = scp->sc->adp;
+	sc = (struct bcmsc_softc *)adp;
 
 	if (scp->curs_attr.height <= 0)
 		return;
@@ -530,8 +372,7 @@ bcmrend_draw_cursor(scr_stat* scp, int off, int blink, int on, int flip)
 	    + (row + sc->ymargin)*(sc->stride)
 	    + (sc->depth/8) * (col + sc->xmargin);
 
-	bytes = sc->depth/8;
-
+	bytes = sc->depth / 8;
 	/* our cursor consists of simply inverting the char under it */
 	for (i = 0; i < adp->va_info.vi_cheight; i++) {
 		for (j = 0; j < adp->va_info.vi_cwidth; j++) {
@@ -578,56 +419,80 @@ extern u_char dflt_font_16[];
 static void
 bcmfb_update_margins(video_adapter_t *adp)
 {
-	struct video_adapter_softc *sc;
+	struct bcmsc_softc *sc;
 	video_info_t *vi;
 
-	sc = (struct video_adapter_softc *)adp;
+	sc = (struct bcmsc_softc *)adp;
 	vi = &adp->va_info;
 
 	sc->xmargin = (sc->width - (vi->vi_width * vi->vi_cwidth)) / 2;
-	sc->ymargin = (sc->height - (vi->vi_height * vi->vi_cheight))/2;
+	sc->ymargin = (sc->height - (vi->vi_height * vi->vi_cheight)) / 2;
 }
 
 static int
 bcmfb_configure(int flags)
 {
-	struct video_adapter_softc *va_sc;
-
-	va_sc = &va_softc;
-	phandle_t display, root;
+	char bootargs[2048], *n, *p, *v;
 	pcell_t cell;
+	phandle_t chosen, display, root;
+	struct bcmsc_softc *sc;
 
-	if (va_sc->initialized)
+	sc = &bcmsc;
+	if (sc->initialized)
 		return (0);
 
-	va_sc->width = 0;
-	va_sc->height = 0;
+	sc->width = 0;
+	sc->height = 0;
 
 	/*
 	 * It seems there is no way to let syscons framework know
 	 * that framebuffer resolution has changed. So just try
-	 * to fetch data from FDT and go with defaults if failed
+	 * to fetch data from FDT bootargs, FDT display data and
+	 * finally go with defaults if everything else has failed.
 	 */
+	chosen = OF_finddevice("/chosen");
+	if (chosen != 0 &&
+	    OF_getprop(chosen, "bootargs", &bootargs, sizeof(bootargs)) > 0) {
+		p = bootargs;
+		while ((v = strsep(&p, " ")) != NULL) {
+			if (*v == '\0')
+				continue;
+			n = strsep(&v, "=");
+			if (strcmp(n, "bcm2708_fb.fbwidth") == 0 && v != NULL)
+				sc->width = (unsigned int)strtol(v, NULL, 0);
+			else if (strcmp(n, "bcm2708_fb.fbheight") == 0 &&
+			    v != NULL)
+				sc->height = (unsigned int)strtol(v, NULL, 0);
+			else if (strcmp(n, "bcm2708_fb.fbswap") == 0 &&
+			    v != NULL)
+				if (*v == '1')
+					sc->fbswap = 1;
+		}
+	}
+
 	root = OF_finddevice("/");
 	if ((root != 0) && 
 	    (display = fdt_find_compatible(root, "broadcom,bcm2835-fb", 1))) {
-		if ((OF_getprop(display, "broadcom,width", 
-		    &cell, sizeof(cell))) > 0)
-			va_sc->width = (int)fdt32_to_cpu(cell);
+		if (sc->width == 0) {
+			if ((OF_getprop(display, "broadcom,width", 
+			    &cell, sizeof(cell))) > 0)
+				sc->width = (int)fdt32_to_cpu(cell);
+		}
 
-		if ((OF_getprop(display, "broadcom,height", 
-		    &cell, sizeof(cell))) > 0)
-			va_sc->height = (int)fdt32_to_cpu(cell);
+		if (sc->height == 0) {
+			if ((OF_getprop(display, "broadcom,height", 
+			    &cell, sizeof(cell))) > 0)
+				sc->height = (int)fdt32_to_cpu(cell);
+		}
 	}
 
-	if (va_sc->width == 0)
-		va_sc->width = FB_WIDTH;
-	if (va_sc->height == 0)
-		va_sc->height = FB_HEIGHT;
-
-	bcmfb_init(0, &va_sc->va, 0);
+	if (sc->width == 0)
+		sc->width = FB_WIDTH;
+	if (sc->height == 0)
+		sc->height = FB_HEIGHT;
 
-	va_sc->initialized = 1;
+	bcmfb_init(0, &sc->va, 0);
+	sc->initialized = 1;
 
 	return (0);
 }
@@ -642,20 +507,19 @@ bcmfb_probe(int unit, video_adapter_t **adp, void *arg, int flags)
 static int
 bcmfb_init(int unit, video_adapter_t *adp, int flags)
 {
-	struct video_adapter_softc *sc;
+	struct bcmsc_softc *sc;
 	video_info_t *vi;
 
-	sc = (struct video_adapter_softc *)adp;
+	sc = (struct bcmsc_softc *)adp;
 	vi = &adp->va_info;
 
 	vid_init_struct(adp, "bcmfb", -1, unit);
 
 	sc->font = dflt_font_16;
 	vi->vi_cheight = BCMFB_FONT_HEIGHT;
-	vi->vi_cwidth = 8;
-
-	vi->vi_width = sc->width/8;
-	vi->vi_height = sc->height/vi->vi_cheight;
+	vi->vi_cwidth = BCMFB_FONT_WIDTH;
+	vi->vi_width = sc->width / vi->vi_cwidth;
+	vi->vi_height = sc->height / vi->vi_cheight;
 
 	/*
 	 * Clamp width/height to syscons maximums
@@ -666,8 +530,7 @@ bcmfb_init(int unit, video_adapter_t *adp, int flags)
 		vi->vi_height = ROW;
 
 	sc->xmargin = (sc->width - (vi->vi_width * vi->vi_cwidth)) / 2;
-	sc->ymargin = (sc->height - (vi->vi_height * vi->vi_cheight))/2;
-
+	sc->ymargin = (sc->height - (vi->vi_height * vi->vi_cheight)) / 2;
 
 	adp->va_window = (vm_offset_t) bcmfb_static_window;
 	adp->va_flags |= V_ADP_FONT /* | V_ADP_COLOR | V_ADP_MODECHANGE */;
@@ -707,8 +570,9 @@ static int
 bcmfb_load_font(video_adapter_t *adp, int page, int size, int width,
     u_char *data, int c, int count)
 {
-	struct video_adapter_softc *sc = (struct video_adapter_softc *)adp;
+	struct bcmsc_softc *sc;
 
+	sc = (struct bcmsc_softc *)adp;
 	sc->font = data;
 
 	return (0);
@@ -781,9 +645,9 @@ static int
 bcmfb_blank_display(video_adapter_t *adp, int mode)
 {
 
-	struct video_adapter_softc *sc;
+	struct bcmsc_softc *sc;
 
-	sc = (struct video_adapter_softc *)adp;
+	sc = (struct bcmsc_softc *)adp;
 	if (sc && sc->fb_addr)
 		memset((void*)sc->fb_addr, 0, sc->fb_size);
 
@@ -794,9 +658,9 @@ static int
 bcmfb_mmap(video_adapter_t *adp, vm_ooffset_t offset, vm_paddr_t *paddr,
     int prot, vm_memattr_t *memattr)
 {
-	struct video_adapter_softc *sc;
+	struct bcmsc_softc *sc;
 
-	sc = (struct video_adapter_softc *)adp;
+	sc = (struct bcmsc_softc *)adp;
 
 	/*
 	 * This might be a legacy VGA mem request: if so, just point it at the
@@ -813,10 +677,10 @@ bcmfb_mmap(video_adapter_t *adp, vm_ooffset_t offset, vm_paddr_t *paddr,
 static int
 bcmfb_ioctl(video_adapter_t *adp, u_long cmd, caddr_t data)
 {
-	struct video_adapter_softc *sc;
+	struct bcmsc_softc *sc;
 	struct fbtype *fb;
 
-	sc = (struct video_adapter_softc *)adp;
+	sc = (struct bcmsc_softc *)adp;
 
 	switch (cmd) {
 	case FBIOGTYPE:
@@ -898,16 +762,13 @@ bcmfb_putp(video_adapter_t *adp, vm_offset_t off, uint32_t p, uint32_t a,
 static int
 bcmfb_putc(video_adapter_t *adp, vm_offset_t off, uint8_t c, uint8_t a)
 {
-	struct video_adapter_softc *sc;
-	int row;
-	int col;
-	int i, j, k;
-	uint8_t *addr;
+	int bytes, col, i, j, k, row;
+	struct bcmsc_softc *sc;
 	u_char *p;
-	uint8_t fg, bg, color;
+	uint8_t *addr, fg, bg, color;
 	uint16_t rgb;
 
-	sc = (struct video_adapter_softc *)adp;
+	sc = (struct bcmsc_softc *)adp;
 
 	if (sc->fb_addr == 0)
 		return (0);
@@ -922,6 +783,7 @@ bcmfb_putc(video_adapter_t *adp, vm_offset_t off, uint8_t c, uint8_t a)
 	fg = a & 0xf ;
 	bg = (a >> 4) & 0xf;
 
+	bytes = sc->depth / 8;
 	for (i = 0; i < BCMFB_FONT_HEIGHT; i++) {
 		for (j = 0, k = 7; j < 8; j++, k--) {
 			if ((p[i] & (1 << k)) == 0)
@@ -931,22 +793,32 @@ bcmfb_putc(video_adapter_t *adp, vm_offset_t off, uint8_t c, uint8_t a)
 
 			switch (sc->depth) {
 			case 32:
-				addr[4*j+0] = bcmfb_palette[color].r;
-				addr[4*j+1] = bcmfb_palette[color].g;
-				addr[4*j+2] = bcmfb_palette[color].b;
-				addr[4*j+3] = bcmfb_palette[color].a;
-				break;
 			case 24:
-				addr[3*j] = bcmfb_palette[color].r;
-				addr[3*j+1] = bcmfb_palette[color].g;
-				addr[3*j+2] = bcmfb_palette[color].b;
+				if (sc->fbswap) {
+					addr[bytes * j + 0] =
+					    bcmfb_palette[color].b;
+					addr[bytes * j + 1] =
+					    bcmfb_palette[color].g;
+					addr[bytes * j + 2] =
+					    bcmfb_palette[color].r;
+				} else {
+					addr[bytes * j + 0] =
+					    bcmfb_palette[color].r;
+					addr[bytes * j + 1] =
+					    bcmfb_palette[color].g;
+					addr[bytes * j + 2] =
+					    bcmfb_palette[color].b;
+				}
+				if (sc->depth == 32)
+					addr[bytes * j + 3] =
+					    bcmfb_palette[color].a;
 				break;
 			case 16:
 				rgb = (bcmfb_palette[color].r >> 3) << 11;
 				rgb |= (bcmfb_palette[color].g >> 2) << 5;
 				rgb |= (bcmfb_palette[color].b >> 3);
-				addr[2*j] = rgb & 0xff;
-				addr[2*j + 1] = (rgb >> 8) & 0xff;
+				addr[bytes * j] = rgb & 0xff;
+				addr[bytes * j + 1] = (rgb >> 8) & 0xff;
 			default:
 				/* Not supported yet */
 				break;
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_fbd.c b/sys/arm/broadcom/bcm2835/bcm2835_fbd.c
index ed625bc..04cc83a 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_fbd.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_fbd.c
@@ -35,30 +35,13 @@ __FBSDID("$FreeBSD$");
 #include <sys/systm.h>
 #include <sys/bio.h>
 #include <sys/bus.h>
-#include <sys/conf.h>
-#include <sys/endian.h>
+#include <sys/fbio.h>
 #include <sys/kernel.h>
-#include <sys/kthread.h>
-#include <sys/lock.h>
 #include <sys/malloc.h>
 #include <sys/module.h>
-#include <sys/mutex.h>
-#include <sys/queue.h>
-#include <sys/resource.h>
-#include <sys/rman.h>
-#include <sys/time.h>
-#include <sys/timetc.h>
-#include <sys/fbio.h>
-#include <sys/consio.h>
 
-#include <sys/kdb.h>
-
-#include <machine/bus.h>
-#include <machine/cpu.h>
-#include <machine/cpufunc.h>
-#include <machine/fdt.h>
-#include <machine/resource.h>
-#include <machine/intr.h>
+#include <vm/vm.h>
+#include <vm/pmap.h>
 
 #include <dev/fdt/fdt_common.h>
 #include <dev/ofw/ofw_bus.h>
@@ -66,216 +49,199 @@ __FBSDID("$FreeBSD$");
 
 #include <dev/fb/fbreg.h>
 #include <dev/vt/vt.h>
+#include <dev/vt/colors/vt_termcolors.h>
 
-#include <arm/broadcom/bcm2835/bcm2835_mbox.h>
-#include <arm/broadcom/bcm2835/bcm2835_vcbus.h>
+#include <arm/broadcom/bcm2835/bcm2835_mbox_prop.h>
 
 #include "fb_if.h"
 #include "mbox_if.h"
 
-#define FB_WIDTH		640
-#define FB_HEIGHT		480
-#define FB_DEPTH		24
-
-struct bcm_fb_config {
-	uint32_t	xres;
-	uint32_t	yres;
-	uint32_t	vxres;
-	uint32_t	vyres;
-	uint32_t	pitch;
-	uint32_t	bpp;
-	uint32_t	xoffset;
-	uint32_t	yoffset;
-	/* Filled by videocore */
-	uint32_t	base;
-	uint32_t	screen_size;
-};
+#define	FB_DEPTH		24
 
 struct bcmsc_softc {
-	device_t		dev;
-	struct fb_info 		*info;
-	bus_dma_tag_t		dma_tag;
-	bus_dmamap_t		dma_map;
-	struct bcm_fb_config*	fb_config;
-	bus_addr_t		fb_config_phys;
-	struct intr_config_hook	init_hook;
+	struct fb_info 			info;
+	int				fbswap;
+	struct bcm2835_fb_config	fb;
+	device_t			dev;
+};
+
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-fb",		1},
+	{"brcm,bcm2708-fb",		1},
+	{NULL,				0}
 };
 
 static int bcm_fb_probe(device_t);
 static int bcm_fb_attach(device_t);
-static void bcm_fb_dmamap_cb(void *arg, bus_dma_segment_t *segs, int nseg,
-    int err);
 
-static void
-bcm_fb_init(void *arg)
+static int
+bcm_fb_init(struct bcmsc_softc *sc, struct bcm2835_fb_config *fb)
 {
-	volatile struct bcm_fb_config *fb_config;
-	struct bcmsc_softc *sc;
-	struct fb_info *info;
-	phandle_t node;
-	pcell_t cell;
-	device_t mbox;
-	device_t fbd;
-	int err = 0;
-
-	sc = arg;
-	fb_config = sc->fb_config;
-	node = ofw_bus_get_node(sc->dev);
-
-	fb_config->xres = 0;
-	fb_config->yres = 0;
-	fb_config->bpp = 0;
-	fb_config->vxres = 0;
-	fb_config->vyres = 0;
-	fb_config->xoffset = 0;
-	fb_config->yoffset = 0;
-	fb_config->base = 0;
-	fb_config->pitch = 0;
-	fb_config->screen_size = 0;
-
-	if ((OF_getprop(node, "broadcom,width", &cell, sizeof(cell))) > 0)
-		fb_config->xres = (int)fdt32_to_cpu(cell);
-	if (fb_config->xres == 0)
-		fb_config->xres = FB_WIDTH;
-
-	if ((OF_getprop(node, "broadcom,height", &cell, sizeof(cell))) > 0)
-		fb_config->yres = (uint32_t)fdt32_to_cpu(cell);
-	if (fb_config->yres == 0)
-		fb_config->yres = FB_HEIGHT;
-
-	if ((OF_getprop(node, "broadcom,depth", &cell, sizeof(cell))) > 0)
-		fb_config->bpp = (uint32_t)fdt32_to_cpu(cell);
-	if (fb_config->bpp == 0)
-		fb_config->bpp = FB_DEPTH;
-
-	bus_dmamap_sync(sc->dma_tag, sc->dma_map,
-		BUS_DMASYNC_PREWRITE | BUS_DMASYNC_PREREAD);
-
-	mbox = devclass_get_device(devclass_find("mbox"), 0);
-	if (mbox) {
-		MBOX_WRITE(mbox, BCM2835_MBOX_CHAN_FB, sc->fb_config_phys);
-		MBOX_READ(mbox, BCM2835_MBOX_CHAN_FB, &err);
-	}
-	bus_dmamap_sync(sc->dma_tag, sc->dma_map,
-		BUS_DMASYNC_POSTREAD);
-
-	if (fb_config->base != 0) {
-		device_printf(sc->dev, "%dx%d(%dx%d@%d,%d) %dbpp\n",
-			fb_config->xres, fb_config->yres,
-			fb_config->vxres, fb_config->vyres,
-			fb_config->xoffset, fb_config->yoffset,
-			fb_config->bpp);
-
-		device_printf(sc->dev, "pitch %d, base 0x%08x, screen_size %d\n",
-			fb_config->pitch, fb_config->base,
-			fb_config->screen_size);
-
-		info = malloc(sizeof(struct fb_info), M_DEVBUF,
-		    M_WAITOK | M_ZERO);
-		info->fb_name = device_get_nameunit(sc->dev);
-		info->fb_vbase = (intptr_t)pmap_mapdev(fb_config->base,
-		    fb_config->screen_size);
-		info->fb_pbase = fb_config->base;
-		info->fb_size = fb_config->screen_size;
-		info->fb_bpp = info->fb_depth = fb_config->bpp;
-		info->fb_stride = fb_config->pitch;
-		info->fb_width = fb_config->xres;
-		info->fb_height = fb_config->yres;
-
-		sc->info = info;
-
-		fbd = device_add_child(sc->dev, "fbd",
-		    device_get_unit(sc->dev));
-		if (fbd == NULL) {
-			device_printf(sc->dev, "Failed to add fbd child\n");
-			return;
-		}
-		if (device_probe_and_attach(fbd) != 0) {
-			device_printf(sc->dev, "Failed to attach fbd device\n");
-			return;
-		}
-	} else {
-		device_printf(sc->dev, "Failed to set framebuffer info\n");
-		return;
+	int err;
+
+	err = 0;
+
+	memset(fb, 0, sizeof(*fb));
+	if (bcm2835_mbox_fb_get_w_h(fb) != 0)
+		return (ENXIO);
+	fb->bpp = FB_DEPTH;
+
+	fb->vxres = fb->xres;
+	fb->vyres = fb->yres;
+	fb->xoffset = fb->yoffset = 0;
+
+	if ((err = bcm2835_mbox_fb_init(fb)) != 0) {
+		device_printf(sc->dev, "bcm2835_mbox_fb_init failed, err=%d\n", err);
+		return (ENXIO);
 	}
 
-	config_intrhook_disestablish(&sc->init_hook);
+	return (0);
 }
 
 static int
-bcm_fb_probe(device_t dev)
+bcm_fb_setup_fbd(struct bcmsc_softc *sc)
 {
-	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-fb"))
+	struct bcm2835_fb_config fb;
+	device_t fbd;
+	int err;
+
+	err = bcm_fb_init(sc, &fb);
+	if (err)
+		return (err);
+
+	memset(&sc->info, 0, sizeof(sc->info));
+	sc->info.fb_name = device_get_nameunit(sc->dev);
+
+	sc->info.fb_vbase = (intptr_t)pmap_mapdev(fb.base, fb.size);
+	sc->info.fb_pbase = fb.base;
+	sc->info.fb_size = fb.size;
+	sc->info.fb_bpp = sc->info.fb_depth = fb.bpp;
+	sc->info.fb_stride = fb.pitch;
+	sc->info.fb_width = fb.xres;
+	sc->info.fb_height = fb.yres;
+#ifdef VM_MEMATTR_WRITE_COMBINING
+	sc->info.fb_flags = FB_FLAG_MEMATTR;
+	sc->info.fb_memattr = VM_MEMATTR_WRITE_COMBINING;
+#endif
+
+	if (sc->fbswap) {
+		switch (sc->info.fb_bpp) {
+		case 24:
+			vt_generate_cons_palette(sc->info.fb_cmap,
+			    COLOR_FORMAT_RGB, 0xff, 0, 0xff, 8, 0xff, 16);
+			sc->info.fb_cmsize = 16;
+			break;
+		case 32:
+			vt_generate_cons_palette(sc->info.fb_cmap,
+			    COLOR_FORMAT_RGB, 0xff, 16, 0xff, 8, 0xff, 0);
+			sc->info.fb_cmsize = 16;
+			break;
+		}
+	}
+
+	fbd = device_add_child(sc->dev, "fbd", device_get_unit(sc->dev));
+	if (fbd == NULL) {
+		device_printf(sc->dev, "Failed to add fbd child\n");
+		pmap_unmapdev(sc->info.fb_vbase, sc->info.fb_size);
+		return (ENXIO);
+	} else if (device_probe_and_attach(fbd) != 0) {
+		device_printf(sc->dev, "Failed to attach fbd device\n");
+		device_delete_child(sc->dev, fbd);
+		pmap_unmapdev(sc->info.fb_vbase, sc->info.fb_size);
 		return (ENXIO);
+	}
 
-	device_set_desc(dev, "BCM2835 VT framebuffer driver");
+	device_printf(sc->dev, "%dx%d(%dx%d@%d,%d) %dbpp\n", fb.xres, fb.yres,
+	    fb.vxres, fb.vyres, fb.xoffset, fb.yoffset, fb.bpp);
+	device_printf(sc->dev,
+	    "fbswap: %d, pitch %d, base 0x%08x, screen_size %d\n",
+	    sc->fbswap, fb.pitch, fb.base, fb.size);
 
-	return (BUS_PROBE_DEFAULT);
+	return (0);
 }
 
 static int
-bcm_fb_attach(device_t dev)
+bcm_fb_resync_sysctl(SYSCTL_HANDLER_ARGS)
 {
-	struct bcmsc_softc *sc = device_get_softc(dev);
-	int dma_size = sizeof(struct bcm_fb_config);
+	struct bcmsc_softc *sc = arg1;
+	struct bcm2835_fb_config fb;
+	int val;
 	int err;
 
-	sc->dev = dev;
+	val = 0;
+	err = sysctl_handle_int(oidp, &val, 0, req);
+	if (err || !req->newptr) /* error || read request */
+		return (err);
 
-	err = bus_dma_tag_create(
-	    bus_get_dma_tag(sc->dev),
-	    PAGE_SIZE, 0,		/* alignment, boundary */
-	    BUS_SPACE_MAXADDR_32BIT,	/* lowaddr */
-	    BUS_SPACE_MAXADDR,		/* highaddr */
-	    NULL, NULL,			/* filter, filterarg */
-	    dma_size, 1,		/* maxsize, nsegments */
-	    dma_size, 0,		/* maxsegsize, flags */
-	    NULL, NULL,			/* lockfunc, lockarg */
-	    &sc->dma_tag);
-
-	err = bus_dmamem_alloc(sc->dma_tag, (void **)&sc->fb_config, 0,
-	    &sc->dma_map);
-	if (err) {
-		device_printf(dev, "cannot allocate framebuffer\n");
-		goto fail;
-	}
+	bcm_fb_init(sc, &fb);
 
-	err = bus_dmamap_load(sc->dma_tag, sc->dma_map, sc->fb_config,
-	    dma_size, bcm_fb_dmamap_cb, &sc->fb_config_phys, BUS_DMA_NOWAIT);
+	return (0);
+}
 
-	if (err) {
-		device_printf(dev, "cannot load DMA map\n");
-		goto fail;
-	}
+static void
+bcm_fb_sysctl_init(struct bcmsc_softc *sc)
+{
+	struct sysctl_ctx_list *ctx;
+	struct sysctl_oid *tree_node;
+	struct sysctl_oid_list *tree;
 
 	/*
-	 * We have to wait until interrupts are enabled.
-	 * Mailbox relies on it to get data from VideoCore
+	 * Add system sysctl tree/handlers.
 	 */
-        sc->init_hook.ich_func = bcm_fb_init;
-        sc->init_hook.ich_arg = sc;
+	ctx = device_get_sysctl_ctx(sc->dev);
+	tree_node = device_get_sysctl_tree(sc->dev);
+	tree = SYSCTL_CHILDREN(tree_node);
+	SYSCTL_ADD_PROC(ctx, tree, OID_AUTO, "resync",
+	    CTLFLAG_RW | CTLTYPE_UINT, sc, sizeof(*sc),
+	    bcm_fb_resync_sysctl, "IU", "Set to resync framebuffer with VC");
+}
 
-        if (config_intrhook_establish(&sc->init_hook) != 0) {
-		device_printf(dev, "failed to establish intrhook\n");
-                return (ENOMEM);
-	}
+static int
+bcm_fb_probe(device_t dev)
+{
 
-	return (0);
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	device_set_desc(dev, "BCM2835 VT framebuffer driver");
 
-fail:
-	return (ENXIO);
+	return (BUS_PROBE_DEFAULT);
 }
 
-static void
-bcm_fb_dmamap_cb(void *arg, bus_dma_segment_t *segs, int nseg, int err)
+static int
+bcm_fb_attach(device_t dev)
 {
-	bus_addr_t *addr;
+	char bootargs[2048], *n, *p, *v;
+	int err;
+	phandle_t chosen;
+	struct bcmsc_softc *sc;
 
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	/* Newer firmware versions needs an inverted color palette. */
+	sc->fbswap = 0;
+	chosen = OF_finddevice("/chosen");
+	if (chosen != 0 &&
+	    OF_getprop(chosen, "bootargs", &bootargs, sizeof(bootargs)) > 0) {
+		p = bootargs;
+		while ((v = strsep(&p, " ")) != NULL) {
+			if (*v == '\0')
+				continue;
+			n = strsep(&v, "=");
+			if (strcmp(n, "bcm2708_fb.fbswap") == 0 && v != NULL)
+				if (*v == '1')
+					sc->fbswap = 1;
+                }
+        }
+
+	bcm_fb_sysctl_init(sc);
+
+	err = bcm_fb_setup_fbd(sc);
 	if (err)
-		return;
+		return (err);
 
-	addr = (bus_addr_t*)arg;
-	*addr = PHYS_TO_VCBUS(segs[0].ds_addr);
+	return (0);
 }
 
 static struct fb_info *
@@ -285,7 +251,7 @@ bcm_fb_helper_getinfo(device_t dev)
 
 	sc = device_get_softc(dev);
 
-	return (sc->info);
+	return (&sc->info);
 }
 
 static device_method_t bcm_fb_methods[] = {
@@ -308,3 +274,4 @@ static driver_t bcm_fb_driver = {
 };
 
 DRIVER_MODULE(bcm2835fb, ofwbus, bcm_fb_driver, bcm_fb_devclass, 0, 0);
+DRIVER_MODULE(bcm2835fb, simplebus, bcm_fb_driver, bcm_fb_devclass, 0, 0);
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_gpio.c b/sys/arm/broadcom/bcm2835/bcm2835_gpio.c
index 48f45de..f971854 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_gpio.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_gpio.c
@@ -108,6 +108,12 @@ enum bcm_gpio_pud {
 #define	BCM_GPIO_READ(_sc, _off)		\
     bus_space_read_4(_sc->sc_bst, _sc->sc_bsh, _off)
 
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-gpio",	1},
+	{"brcm,bcm2835-gpio",		1},
+	{NULL,				0}
+};
+
 static int
 bcm_gpio_pin_is_ro(struct bcm_gpio_softc *sc, int pin)
 {
@@ -496,39 +502,6 @@ bcm_gpio_pin_toggle(device_t dev, uint32_t pin)
 }
 
 static int
-bcm_gpio_get_ro_pins(struct bcm_gpio_softc *sc)
-{
-	int i, len;
-	pcell_t pins[BCM_GPIO_PINS];
-	phandle_t gpio;
-
-	/* Find the gpio node to start. */
-	gpio = ofw_bus_get_node(sc->sc_dev);
-
-	len = OF_getproplen(gpio, "broadcom,read-only");
-	if (len < 0 || len > sizeof(pins))
-		return (-1);
-
-	if (OF_getprop(gpio, "broadcom,read-only", &pins, len) < 0)
-		return (-1);
-
-	sc->sc_ro_npins = len / sizeof(pcell_t);
-
-	device_printf(sc->sc_dev, "read-only pins: ");
-	for (i = 0; i < sc->sc_ro_npins; i++) {
-		sc->sc_ro_pins[i] = fdt32_to_cpu(pins[i]);
-		if (i > 0)
-			printf(",");
-		printf("%d", sc->sc_ro_pins[i]);
-	}
-	if (i > 0)
-		printf(".");
-	printf("\n");
-
-	return (0);
-}
-
-static int
 bcm_gpio_func_proc(SYSCTL_HANDLER_ARGS)
 {
 	char buf[16];
@@ -547,7 +520,9 @@ bcm_gpio_func_proc(SYSCTL_HANDLER_ARGS)
 	error = sysctl_handle_string(oidp, buf, sizeof(buf), req);
 	if (error != 0 || req->newptr == NULL)
 		return (error);
-
+	/* Ignore changes on read-only pins. */
+	if (bcm_gpio_pin_is_ro(sc, sc_sysctl->pin))
+		return (0);
 	/* Parse the user supplied string and check for a valid pin function. */
 	if (bcm_gpio_str_func(buf, &nfunc) != 0)
 		return (EINVAL);
@@ -597,63 +572,82 @@ bcm_gpio_sysctl_init(struct bcm_gpio_softc *sc)
 }
 
 static int
-bcm_gpio_get_reserved_pins(struct bcm_gpio_softc *sc)
+bcm_gpio_get_ro_pins(struct bcm_gpio_softc *sc, phandle_t node,
+	const char *propname, const char *label)
 {
-	int i, j, len, npins;
-	pcell_t pins[BCM_GPIO_PINS];
-	phandle_t gpio, node, reserved;
-	char name[32];
+	int i, need_comma, npins, range_start, range_stop;
+	pcell_t *pins;
 
-	/* Get read-only pins. */
-	if (bcm_gpio_get_ro_pins(sc) != 0)
+	/* Get the property data. */
+	npins = OF_getencprop_alloc(node, propname, sizeof(*pins),
+	    (void **)&pins);
+	if (npins < 0)
 		return (-1);
+	if (npins == 0) {
+		free(pins, M_OFWPROP);
+		return (0);
+	}
+	for (i = 0; i < npins; i++)
+		sc->sc_ro_pins[i + sc->sc_ro_npins] = pins[i];
+	sc->sc_ro_npins += npins;
+	need_comma = 0;
+	device_printf(sc->sc_dev, "%s pins: ", label);
+	range_start = range_stop = pins[0];
+	for (i = 1; i < npins; i++) {
+		if (pins[i] != range_stop + 1) {
+			if (need_comma)
+				printf(",");
+			if (range_start != range_stop)
+				printf("%d-%d", range_start, range_stop);
+			else
+				printf("%d", range_start);
+			range_start = range_stop = pins[i];
+			need_comma = 1;
+		} else
+			range_stop++;
+	}
+	if (need_comma)
+		printf(",");
+	if (range_start != range_stop)
+		printf("%d-%d.\n", range_start, range_stop);
+	else
+		printf("%d.\n", range_start);
+	free(pins, M_OFWPROP);
+
+	return (0);
+}
+
+static int
+bcm_gpio_get_reserved_pins(struct bcm_gpio_softc *sc)
+{
+	char *name;
+	phandle_t gpio, node, reserved;
+	ssize_t len;
 
-	/* Find the gpio/reserved pins node to start. */
+	/* Get read-only pins if they're provided */
 	gpio = ofw_bus_get_node(sc->sc_dev);
-	node = OF_child(gpio);
-	
-	/*
-	 * Find reserved node
-	 */
+	if (bcm_gpio_get_ro_pins(sc, gpio, "broadcom,read-only",
+	    "read-only") != 0)
+		return (0);
+	/* Traverse the GPIO subnodes to find the reserved pins node. */
 	reserved = 0;
+	node = OF_child(gpio);
 	while ((node != 0) && (reserved == 0)) {
-		len = OF_getprop(node, "name", name,
-		    sizeof(name) - 1);
-		name[len] = 0;
+		len = OF_getprop_alloc(node, "name", 1, (void **)&name);
+		if (len == -1)
+			return (-1);
 		if (strcmp(name, "reserved") == 0)
 			reserved = node;
+		free(name, M_OFWPROP);
 		node = OF_peer(node);
 	}
-
 	if (reserved == 0)
 		return (-1);
-
 	/* Get the reserved pins. */
-	len = OF_getproplen(reserved, "broadcom,pins");
-	if (len < 0 || len > sizeof(pins))
-		return (-1);
-
-	if (OF_getprop(reserved, "broadcom,pins", &pins, len) < 0)
+	if (bcm_gpio_get_ro_pins(sc, reserved, "broadcom,pins",
+	    "reserved") != 0)
 		return (-1);
 
-	npins = len / sizeof(pcell_t);
-
-	j = 0;
-	device_printf(sc->sc_dev, "reserved pins: ");
-	for (i = 0; i < npins; i++) {
-		if (i > 0)
-			printf(",");
-		printf("%d", fdt32_to_cpu(pins[i]));
-		/* Some pins maybe already on the list of read-only pins. */
-		if (bcm_gpio_pin_is_ro(sc, fdt32_to_cpu(pins[i])))
-			continue;
-		sc->sc_ro_pins[j++ + sc->sc_ro_npins] = fdt32_to_cpu(pins[i]);
-	}
-	sc->sc_ro_npins += j;
-	if (i > 0)
-		printf(".");
-	printf("\n");
-
 	return (0);
 }
 
@@ -664,7 +658,7 @@ bcm_gpio_probe(device_t dev)
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-gpio"))
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
 		return (ENXIO);
 
 	device_set_desc(dev, "BCM2708/2835 GPIO controller");
@@ -719,8 +713,6 @@ bcm_gpio_attach(device_t dev)
 
 	/* Initialize the software controlled pins. */
 	for (i = 0, j = 0; j < BCM_GPIO_PINS; j++) {
-		if (bcm_gpio_pin_is_ro(sc, j))
-			continue;
 		snprintf(sc->sc_gpio_pins[i].gp_name, GPIOMAXNAME,
 		    "pin %d", j);
 		func = bcm_gpio_get_function(sc, j);
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_intr.c b/sys/arm/broadcom/bcm2835/bcm2835_intr.c
index 4a94c69..23b66f3 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_intr.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_intr.c
@@ -45,6 +45,10 @@ __FBSDID("$FreeBSD$");
 #include <dev/ofw/ofw_bus.h>
 #include <dev/ofw/ofw_bus_subr.h>
 
+#ifdef SOC_BCM2836
+#include <arm/broadcom/bcm2835/bcm2836.h>
+#endif
+
 #define	INTC_PENDING_BASIC	0x00
 #define	INTC_PENDING_BANK1	0x04
 #define	INTC_PENDING_BANK2	0x08
@@ -60,10 +64,13 @@ __FBSDID("$FreeBSD$");
 #define	BANK1_END	(BANK1_START + 32 - 1)
 #define	BANK2_START	(BANK1_START + 32)
 #define	BANK2_END	(BANK2_START + 32 - 1)
+#define	BANK3_START	(BANK2_START + 32)
+#define	BANK3_END	(BANK3_START + 32 - 1)
 
 #define	IS_IRQ_BASIC(n)	(((n) >= 0) && ((n) < BANK1_START))
 #define	IS_IRQ_BANK1(n)	(((n) >= BANK1_START) && ((n) <= BANK1_END))
 #define	IS_IRQ_BANK2(n)	(((n) >= BANK2_START) && ((n) <= BANK2_END))
+#define	ID_IRQ_BCM2836(n) (((n) >= BANK3_START) && ((n) <= BANK3_END))
 #define	IRQ_BANK1(n)	((n) - BANK1_START)
 #define	IRQ_BANK2(n)	((n) - BANK2_START)
 
@@ -82,10 +89,10 @@ struct bcm_intc_softc {
 
 static struct bcm_intc_softc *bcm_intc_sc = NULL;
 
-#define	intc_read_4(reg)		\
-    bus_space_read_4(bcm_intc_sc->intc_bst, bcm_intc_sc->intc_bsh, reg)
-#define	intc_write_4(reg, val)		\
-    bus_space_write_4(bcm_intc_sc->intc_bst, bcm_intc_sc->intc_bsh, reg, val)
+#define	intc_read_4(_sc, reg)		\
+    bus_space_read_4((_sc)->intc_bst, (_sc)->intc_bsh, (reg))
+#define	intc_write_4(_sc, reg, val)		\
+    bus_space_write_4((_sc)->intc_bst, (_sc)->intc_bsh, (reg), (val))
 
 static int
 bcm_intc_probe(device_t dev)
@@ -94,7 +101,8 @@ bcm_intc_probe(device_t dev)
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-armctrl-ic"))
+	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-armctrl-ic") &&
+	    !ofw_bus_is_compatible(dev, "brcm,bcm2836-armctrl-ic"))
 		return (ENXIO);
 	device_set_desc(dev, "BCM2835 Interrupt Controller");
 	return (BUS_PROBE_DEFAULT);
@@ -139,54 +147,76 @@ static driver_t bcm_intc_driver = {
 
 static devclass_t bcm_intc_devclass;
 
-DRIVER_MODULE(intc, simplebus, bcm_intc_driver, bcm_intc_devclass, 0, 0);
+EARLY_DRIVER_MODULE(intc, simplebus, bcm_intc_driver, bcm_intc_devclass,
+    0, 0, BUS_PASS_INTERRUPT + BUS_PASS_ORDER_LATE);
 
 int
 arm_get_next_irq(int last_irq)
 {
+	struct bcm_intc_softc *sc = bcm_intc_sc;
 	uint32_t pending;
 	int32_t irq = last_irq + 1;
+#ifdef SOC_BCM2836
+	int ret;
+#endif
 
 	/* Sanity check */
 	if (irq < 0)
 		irq = 0;
-	
+
+#ifdef SOC_BCM2836
+	if ((ret = bcm2836_get_next_irq(irq)) >= 0)
+		return (ret + BANK3_START);
+#endif
+
 	/* TODO: should we mask last_irq? */
-	pending = intc_read_4(INTC_PENDING_BASIC);
-	while (irq < BANK1_START) {
-		if (pending & (1 << irq))
-			return irq;
-		irq++;
+	if (irq < BANK1_START) {
+		pending = intc_read_4(sc, INTC_PENDING_BASIC);
+		if ((pending & 0xFF) == 0) {
+			irq  = BANK1_START;	/* skip to next bank */
+		} else do {
+			if (pending & (1 << irq))
+				return irq;
+			irq++;
+		} while (irq < BANK1_START);
 	}
-
-	pending = intc_read_4(INTC_PENDING_BANK1);
-	while (irq < BANK2_START) {
-		if (pending & (1 << IRQ_BANK1(irq)))
-			return irq;
-		irq++;
+	if (irq < BANK2_START) {
+		pending = intc_read_4(sc, INTC_PENDING_BANK1);
+		if (pending == 0) {
+			irq  = BANK2_START;	/* skip to next bank */
+		} else do {
+			if (pending & (1 << IRQ_BANK1(irq)))
+				return irq;
+			irq++;
+		} while (irq < BANK2_START);
 	}
-
-	pending = intc_read_4(INTC_PENDING_BANK2);
-	while (irq <= BANK2_END) {
-		if (pending & (1 << IRQ_BANK2(irq)))
-			return irq;
-		irq++;
+	if (irq < BANK3_START) {
+		pending = intc_read_4(sc, INTC_PENDING_BANK2);
+		if (pending != 0) do {
+			if (pending & (1 << IRQ_BANK2(irq)))
+				return irq;
+			irq++;
+		} while (irq < BANK3_START);
 	}
-
 	return (-1);
 }
 
 void
 arm_mask_irq(uintptr_t nb)
 {
+	struct bcm_intc_softc *sc = bcm_intc_sc;
 	dprintf("%s: %d\n", __func__, nb);
 
 	if (IS_IRQ_BASIC(nb))
-		intc_write_4(INTC_DISABLE_BASIC, (1 << nb));
+		intc_write_4(sc, INTC_DISABLE_BASIC, (1 << nb));
 	else if (IS_IRQ_BANK1(nb))
-		intc_write_4(INTC_DISABLE_BANK1, (1 << IRQ_BANK1(nb)));
+		intc_write_4(sc, INTC_DISABLE_BANK1, (1 << IRQ_BANK1(nb)));
 	else if (IS_IRQ_BANK2(nb))
-		intc_write_4(INTC_DISABLE_BANK2, (1 << IRQ_BANK2(nb)));
+		intc_write_4(sc, INTC_DISABLE_BANK2, (1 << IRQ_BANK2(nb)));
+#ifdef SOC_BCM2836
+	else if (ID_IRQ_BCM2836(nb))
+		bcm2836_mask_irq(nb - BANK3_START);
+#endif
 	else
 		printf("arm_mask_irq: Invalid IRQ number: %d\n", nb);
 }
@@ -194,14 +224,19 @@ arm_mask_irq(uintptr_t nb)
 void
 arm_unmask_irq(uintptr_t nb)
 {
+	struct bcm_intc_softc *sc = bcm_intc_sc;
 	dprintf("%s: %d\n", __func__, nb);
 
 	if (IS_IRQ_BASIC(nb))
-		intc_write_4(INTC_ENABLE_BASIC, (1 << nb));
+		intc_write_4(sc, INTC_ENABLE_BASIC, (1 << nb));
 	else if (IS_IRQ_BANK1(nb))
-		intc_write_4(INTC_ENABLE_BANK1, (1 << IRQ_BANK1(nb)));
+		intc_write_4(sc, INTC_ENABLE_BANK1, (1 << IRQ_BANK1(nb)));
 	else if (IS_IRQ_BANK2(nb))
-		intc_write_4(INTC_ENABLE_BANK2, (1 << IRQ_BANK2(nb)));
+		intc_write_4(sc, INTC_ENABLE_BANK2, (1 << IRQ_BANK2(nb)));
+#ifdef SOC_BCM2836
+	else if (ID_IRQ_BCM2836(nb))
+		bcm2836_unmask_irq(nb - BANK3_START);
+#endif
 	else
 		printf("arm_mask_irq: Invalid IRQ number: %d\n", nb);
 }
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_machdep.c b/sys/arm/broadcom/bcm2835/bcm2835_machdep.c
index 0f13a79..db47e56 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_machdep.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_machdep.c
@@ -96,6 +96,7 @@ initarm_late_init(void)
 	}
 }
 
+#ifdef SOC_BCM2835
 /*
  * Set up static device mappings.
  * All on-chip peripherals exist in a 16MB range starting at 0x20000000.
@@ -108,6 +109,17 @@ initarm_devmap_init(void)
 	arm_devmap_add_entry(0x20000000, 0x01000000);
 	return (0);
 }
+#endif
+
+#ifdef SOC_BCM2836
+static int
+initarm_devmap_init(void)
+{
+
+	arm_devmap_add_entry(0x3f000000, 0x01000000);
+	return (0);
+}
+#endif
 
 struct arm32_dma_range *
 bus_dma_get_range(void)
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_mbox.c b/sys/arm/broadcom/bcm2835/bcm2835_mbox.c
index 3372b61..2fc0a9e 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_mbox.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_mbox.c
@@ -34,14 +34,16 @@ __FBSDID("$FreeBSD$");
 #include <sys/lock.h>
 #include <sys/module.h>
 #include <sys/mutex.h>
+#include <sys/sx.h>
 #include <sys/rman.h>
-#include <sys/sema.h>
 #include <machine/bus.h>
 
 #include <dev/ofw/ofw_bus.h>
 #include <dev/ofw/ofw_bus_subr.h>
 
 #include <arm/broadcom/bcm2835/bcm2835_mbox.h>
+#include <arm/broadcom/bcm2835/bcm2835_mbox_prop.h>
+#include <arm/broadcom/bcm2835/bcm2835_vcbus.h>
 
 #include "mbox_if.h"
 
@@ -81,7 +83,8 @@ struct bcm_mbox_softc {
 	bus_space_tag_t		bst;
 	bus_space_handle_t	bsh;
 	int			msg[BCM2835_MBOX_CHANS];
-	struct sema		sema[BCM2835_MBOX_CHANS];
+	int			have_message[BCM2835_MBOX_CHANS];
+	struct sx		property_chan_lock;
 };
 
 #define	mbox_read_4(sc, reg)		\
@@ -89,27 +92,49 @@ struct bcm_mbox_softc {
 #define	mbox_write_4(sc, reg, val)		\
     bus_space_write_4((sc)->bst, (sc)->bsh, reg, val)
 
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-mbox",	1},
+	{"brcm,bcm2835-mbox",		1},
+	{NULL,				0}
+};
+
+static int
+bcm_mbox_read_msg(struct bcm_mbox_softc *sc, int *ochan)
+{
+	uint32_t data;
+	uint32_t msg;
+	int chan;
+
+	msg = mbox_read_4(sc, REG_READ);
+	dprintf("bcm_mbox_intr: raw data %08x\n", msg);
+	chan = MBOX_CHAN(msg);
+	data = MBOX_DATA(msg);
+	if (sc->msg[chan]) {
+		printf("bcm_mbox_intr: channel %d oveflow\n", chan);
+		return (1);
+	}
+	dprintf("bcm_mbox_intr: chan %d, data %08x\n", chan, data);
+	sc->msg[chan] = msg;
+
+	if (ochan != NULL)
+		*ochan = chan;
+
+	return (0);
+}
+
 static void
 bcm_mbox_intr(void *arg)
 {
 	struct bcm_mbox_softc *sc = arg;
 	int chan;
-	uint32_t data;
-	uint32_t msg;
 
-	while (!(mbox_read_4(sc, REG_STATUS) & STATUS_EMPTY)) {
-		msg = mbox_read_4(sc, REG_READ);
-		dprintf("bcm_mbox_intr: raw data %08x\n", msg);
-		chan = MBOX_CHAN(msg);
-		data = MBOX_DATA(msg);
-		if (sc->msg[chan]) {
-			printf("bcm_mbox_intr: channel %d oveflow\n", chan);
-			continue;
+	MBOX_LOCK(sc);
+	while (!(mbox_read_4(sc, REG_STATUS) & STATUS_EMPTY))
+		if (bcm_mbox_read_msg(sc, &chan) == 0) {
+			sc->have_message[chan] = 1;
+			wakeup(&sc->have_message[chan]);
 		}
-		dprintf("bcm_mbox_intr: chan %d, data %08x\n", chan, data);
-		sc->msg[chan] = msg;
-		sema_post(&sc->sema[chan]);
-	}
+	MBOX_UNLOCK(sc);
 }
 
 static int
@@ -119,12 +144,12 @@ bcm_mbox_probe(device_t dev)
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	if (ofw_bus_is_compatible(dev, "broadcom,bcm2835-mbox")) {
-		device_set_desc(dev, "BCM2835 VideoCore Mailbox");
-		return(BUS_PROBE_DEFAULT);
-	}
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	device_set_desc(dev, "BCM2835 VideoCore Mailbox");
 
-	return (ENXIO);
+	return (BUS_PROBE_DEFAULT);
 }
 
 static int
@@ -161,9 +186,11 @@ bcm_mbox_attach(device_t dev)
 	mtx_init(&sc->lock, "vcio mbox", NULL, MTX_DEF);
 	for (i = 0; i < BCM2835_MBOX_CHANS; i++) {
 		sc->msg[i] = 0;
-		sema_init(&sc->sema[i], 0, "mbox");
+		sc->have_message[i] = 0;
 	}
 
+	sx_init(&sc->property_chan_lock, "mboxprop");
+
 	/* Read all pending messages */
 	while ((mbox_read_4(sc, REG_STATUS) & STATUS_EMPTY) == 0)
 		(void)mbox_read_4(sc, REG_READ);
@@ -184,6 +211,7 @@ bcm_mbox_write(device_t dev, int chan, uint32_t data)
 
 	dprintf("bcm_mbox_write: chan %d, data %08x\n", chan, data);
 	MBOX_LOCK(sc);
+	sc->have_message[chan] = 0;
 	while ((mbox_read_4(sc, REG_STATUS) & STATUS_FULL) && --limit)
 		DELAY(5);
 	if (limit == 0) {
@@ -201,14 +229,31 @@ static int
 bcm_mbox_read(device_t dev, int chan, uint32_t *data)
 {
 	struct bcm_mbox_softc *sc = device_get_softc(dev);
+	int err, read_chan;
 
 	dprintf("bcm_mbox_read: chan %d\n", chan);
+
+	err = 0;
 	MBOX_LOCK(sc);
-	while (sema_trywait(&sc->sema[chan]) == 0) {
-		/* do not unlock sc while waiting for the mbox */
-		if (sema_timedwait(&sc->sema[chan], 10*hz) == 0)
-			break;
-		printf("timeout sema for chan %d\n", chan);
+	if (!cold) {
+		if (sc->have_message[chan] == 0) {
+			if (mtx_sleep(&sc->have_message[chan], &sc->lock, 0,
+			    "mbox", 10*hz) != 0) {
+				device_printf(dev, "timeout waiting for message on chan %d\n", chan);
+				err = ETIMEDOUT;
+			}
+		}
+	} else {
+		do {
+			/* Wait for a message */
+			while ((mbox_read_4(sc, REG_STATUS) & STATUS_EMPTY))
+				;
+			/* Read the message */
+			if (bcm_mbox_read_msg(sc, &read_chan) != 0) {
+				err = EINVAL;
+				goto out;
+			}
+		} while (read_chan != chan);
 	}
 	/*
 	 *  get data from intr handler, the same channel is never coming
@@ -216,10 +261,12 @@ bcm_mbox_read(device_t dev, int chan, uint32_t *data)
 	 */
 	*data = MBOX_DATA(sc->msg[chan]);
 	sc->msg[chan] = 0;
+	sc->have_message[chan] = 0;
+out:
 	MBOX_UNLOCK(sc);
 	dprintf("bcm_mbox_read: chan %d, data %08x\n", chan, *data);
 
-	return (0);
+	return (err);
 }
 
 static device_method_t bcm_mbox_methods[] = {
@@ -241,3 +288,249 @@ static driver_t bcm_mbox_driver = {
 static devclass_t bcm_mbox_devclass;
 
 DRIVER_MODULE(mbox, simplebus, bcm_mbox_driver, bcm_mbox_devclass, 0, 0);
+
+static void
+bcm2835_mbox_dma_cb(void *arg, bus_dma_segment_t *segs, int nseg, int err)
+{
+	bus_addr_t *addr;
+
+	if (err)
+		return;
+	addr = (bus_addr_t *)arg;
+	*addr = PHYS_TO_VCBUS(segs[0].ds_addr);
+}
+
+static void *
+bcm2835_mbox_init_dma(device_t dev, size_t len, bus_dma_tag_t *tag,
+    bus_dmamap_t *map, bus_addr_t *phys)
+{
+	void *buf;
+	int err;
+
+	err = bus_dma_tag_create(bus_get_dma_tag(dev), 16, 0,
+	    BUS_SPACE_MAXADDR_32BIT, BUS_SPACE_MAXADDR, NULL, NULL,
+	    len, 1, len, 0, NULL, NULL, tag);
+	if (err != 0) {
+		device_printf(dev, "can't create DMA tag\n");
+		return (NULL);
+	}
+
+	err = bus_dmamem_alloc(*tag, &buf, 0, map);
+	if (err != 0) {
+		bus_dma_tag_destroy(*tag);
+		device_printf(dev, "can't allocate dmamem\n");
+		return (NULL);
+	}
+
+	err = bus_dmamap_load(*tag, *map, buf, len, bcm2835_mbox_dma_cb,
+	    phys, 0);
+	if (err != 0) {
+		bus_dmamem_free(*tag, buf, *map);
+		bus_dma_tag_destroy(*tag);
+		device_printf(dev, "can't load DMA map\n");
+		return (NULL);
+	}
+
+	return (buf);
+}
+
+static int
+bcm2835_mbox_err(device_t dev, bus_addr_t msg_phys, uint32_t resp_phys,
+	struct bcm2835_mbox_hdr *msg, size_t len)
+{
+	int idx;
+	struct bcm2835_mbox_tag_hdr *tag;
+	uint8_t *last;
+
+	if ((uint32_t)msg_phys != resp_phys) {
+		device_printf(dev, "response channel mismatch\n");
+		return (EIO);
+	}
+	if (msg->code != BCM2835_MBOX_CODE_RESP_SUCCESS) {
+		device_printf(dev, "mbox response error\n");
+		return (EIO);
+	}
+
+	/* Loop until the end tag. */
+	tag = (struct bcm2835_mbox_tag_hdr *)(msg + 1);
+	last = (uint8_t *)msg + len;
+	for (idx = 0; tag->tag != 0; idx++) {
+		if ((tag->val_len & BCM2835_MBOX_TAG_VAL_LEN_RESPONSE) == 0) {
+			device_printf(dev, "tag %d response error\n", idx);
+			return (EIO);
+		}
+		/* Clear the response bit. */
+		tag->val_len &= ~BCM2835_MBOX_TAG_VAL_LEN_RESPONSE;
+
+		/* Next tag. */
+		tag = (struct bcm2835_mbox_tag_hdr *)((uint8_t *)tag +
+		    sizeof(*tag) + tag->val_buf_size);
+
+		if ((uint8_t *)tag > last) {
+			device_printf(dev, "mbox buffer size error\n");
+			return (EIO);
+		}
+	}
+
+	return (0);
+}
+
+int
+bcm2835_mbox_property(void *msg, size_t msg_size)
+{
+	struct bcm_mbox_softc *sc;
+	struct msg_set_power_state *buf;
+	bus_dma_tag_t msg_tag;
+	bus_dmamap_t msg_map;
+	bus_addr_t msg_phys;
+	uint32_t reg;
+	device_t mbox;
+	int err;
+
+	/* get mbox device */
+	mbox = devclass_get_device(devclass_find("mbox"), 0);
+	if (mbox == NULL)
+		return (ENXIO);
+
+	sc = device_get_softc(mbox);
+	sx_xlock(&sc->property_chan_lock);
+
+	/* Allocate memory for the message */
+	buf = bcm2835_mbox_init_dma(mbox, msg_size, &msg_tag, &msg_map,
+	    &msg_phys);
+	if (buf == NULL) {
+		err = ENOMEM;
+		goto out;
+	}
+
+	memcpy(buf, msg, msg_size);
+
+	bus_dmamap_sync(msg_tag, msg_map,
+	    BUS_DMASYNC_PREWRITE);
+
+	MBOX_WRITE(mbox, BCM2835_MBOX_CHAN_PROP, (uint32_t)msg_phys);
+	MBOX_READ(mbox, BCM2835_MBOX_CHAN_PROP, &reg);
+
+	bus_dmamap_sync(msg_tag, msg_map,
+	    BUS_DMASYNC_PREREAD);
+
+	memcpy(msg, buf, msg_size);
+
+	err = bcm2835_mbox_err(mbox, msg_phys, reg,
+	    (struct bcm2835_mbox_hdr *)msg, msg_size);
+
+	bus_dmamap_unload(msg_tag, msg_map);
+	bus_dmamem_free(msg_tag, buf, msg_map);
+	bus_dma_tag_destroy(msg_tag);
+out:
+	sx_xunlock(&sc->property_chan_lock);
+	return (err);
+}
+
+int
+bcm2835_mbox_set_power_state(uint32_t device_id, boolean_t on)
+{
+	struct msg_set_power_state msg;
+	int err;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_SET_POWER_STATE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.device_id = device_id;
+	msg.body.req.state = (on ? BCM2835_MBOX_POWER_ON : 0) |
+	    BCM2835_MBOX_POWER_WAIT;
+	msg.end_tag = 0;
+
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
+
+	return (err);
+}
+
+int
+bcm2835_mbox_get_clock_rate(uint32_t clock_id, uint32_t *hz)
+{
+	struct msg_get_clock_rate msg;
+	int err;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	msg.tag_hdr.tag = BCM2835_MBOX_TAG_GET_CLOCK_RATE;
+	msg.tag_hdr.val_buf_size = sizeof(msg.body);
+	msg.tag_hdr.val_len = sizeof(msg.body.req);
+	msg.body.req.clock_id = clock_id;
+	msg.end_tag = 0;
+
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
+	*hz = msg.body.resp.rate_hz;
+
+	return (err);
+}
+
+int
+bcm2835_mbox_fb_get_w_h(struct bcm2835_fb_config *fb)
+{
+	int err;
+	struct msg_fb_get_w_h msg;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	BCM2835_MBOX_INIT_TAG(&msg.physical_w_h, GET_PHYSICAL_W_H);
+	msg.physical_w_h.tag_hdr.val_len = 0;
+	msg.end_tag = 0;
+
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
+	if (err == 0) {
+		fb->xres = msg.physical_w_h.body.resp.width;
+		fb->yres = msg.physical_w_h.body.resp.height;
+	}
+
+	return (err);
+}
+
+int
+bcm2835_mbox_fb_init(struct bcm2835_fb_config *fb)
+{
+	int err;
+	struct msg_fb_setup msg;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.hdr.buf_size = sizeof(msg);
+	msg.hdr.code = BCM2835_MBOX_CODE_REQ;
+	BCM2835_MBOX_INIT_TAG(&msg.physical_w_h, SET_PHYSICAL_W_H);
+	msg.physical_w_h.body.req.width = fb->xres;
+	msg.physical_w_h.body.req.height = fb->yres;
+	BCM2835_MBOX_INIT_TAG(&msg.virtual_w_h, SET_VIRTUAL_W_H);
+	msg.virtual_w_h.body.req.width = fb->vxres;
+	msg.virtual_w_h.body.req.height = fb->vyres;
+	BCM2835_MBOX_INIT_TAG(&msg.offset, SET_VIRTUAL_OFFSET);
+	msg.offset.body.req.x = fb->xoffset;
+	msg.offset.body.req.y = fb->yoffset;
+	BCM2835_MBOX_INIT_TAG(&msg.depth, SET_DEPTH);
+	msg.depth.body.req.bpp = fb->bpp;
+	BCM2835_MBOX_INIT_TAG(&msg.alpha, SET_ALPHA_MODE);
+	msg.alpha.body.req.alpha = BCM2835_MBOX_ALPHA_MODE_IGNORED;
+	BCM2835_MBOX_INIT_TAG(&msg.buffer, ALLOCATE_BUFFER);
+	msg.buffer.body.req.alignment = PAGE_SIZE;
+	BCM2835_MBOX_INIT_TAG(&msg.pitch, GET_PITCH);
+	msg.end_tag = 0;
+
+	err = bcm2835_mbox_property(&msg, sizeof(msg));
+	if (err == 0) {
+		fb->xres = msg.physical_w_h.body.resp.width;
+		fb->yres = msg.physical_w_h.body.resp.height;
+		fb->vxres = msg.virtual_w_h.body.resp.width;
+		fb->vyres = msg.virtual_w_h.body.resp.height;
+		fb->xoffset = msg.offset.body.resp.x;
+		fb->yoffset = msg.offset.body.resp.y;
+		fb->pitch = msg.pitch.body.resp.pitch;
+		fb->base = VCBUS_TO_PHYS(msg.buffer.body.resp.fb_address);
+		fb->size = msg.buffer.body.resp.fb_size;
+	}
+
+	return (err);
+}
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_mbox_prop.h b/sys/arm/broadcom/bcm2835/bcm2835_mbox_prop.h
index a2e212e..bc80aa6 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_mbox_prop.h
+++ b/sys/arm/broadcom/bcm2835/bcm2835_mbox_prop.h
@@ -52,6 +52,62 @@ struct bcm2835_mbox_tag_hdr {
 	uint32_t	val_len;
 };
 
+#define	BCM2835_MBOX_INIT_TAG(tag_, tagid_) do {		\
+	(tag_)->tag_hdr.tag = BCM2835_MBOX_TAG_##tagid_;	\
+	(tag_)->tag_hdr.val_buf_size = sizeof((tag_)->body);	\
+	(tag_)->tag_hdr.val_len = sizeof((tag_)->body.req);	\
+} while (0)
+
+#define BCM2835_MBOX_POWER_ID_EMMC		0x00000000
+#define BCM2835_MBOX_POWER_ID_UART0		0x00000001
+#define BCM2835_MBOX_POWER_ID_UART1		0x00000002
+#define BCM2835_MBOX_POWER_ID_USB_HCD		0x00000003
+#define BCM2835_MBOX_POWER_ID_I2C0		0x00000004
+#define BCM2835_MBOX_POWER_ID_I2C1		0x00000005
+#define BCM2835_MBOX_POWER_ID_I2C2		0x00000006
+#define BCM2835_MBOX_POWER_ID_SPI		0x00000007
+#define BCM2835_MBOX_POWER_ID_CCP2TX		0x00000008
+
+#define BCM2835_MBOX_POWER_ON			(1 << 0)
+#define BCM2835_MBOX_POWER_WAIT			(1 << 1)
+
+#define BCM2835_MBOX_TAG_GET_POWER_STATE	0x00020001
+#define BCM2835_MBOX_TAG_SET_POWER_STATE	0x00028001
+
+struct msg_get_power_state {
+	struct bcm2835_mbox_hdr hdr;
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+			uint32_t device_id;
+		} req;
+		struct {
+			uint32_t device_id;
+			uint32_t state;
+		} resp;
+	} body;
+	uint32_t end_tag;
+};
+
+struct msg_set_power_state {
+	struct bcm2835_mbox_hdr hdr;
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+			uint32_t device_id;
+			uint32_t state;
+		} req;
+		struct {
+			uint32_t device_id;
+			uint32_t state;
+		} resp;
+	} body;
+	uint32_t end_tag;
+};
+
+/* Sets the power state for a given device */
+int bcm2835_mbox_set_power_state(uint32_t, boolean_t);
+
 #define BCM2835_MBOX_CLOCK_ID_EMMC		0x00000001
 #define BCM2835_MBOX_CLOCK_ID_UART		0x00000002
 #define BCM2835_MBOX_CLOCK_ID_ARM		0x00000003
@@ -129,6 +185,8 @@ struct msg_get_min_clock_rate {
 	uint32_t end_tag;
 };
 
+int bcm2835_mbox_get_clock_rate(uint32_t, uint32_t *);
+
 #define BCM2835_MBOX_TURBO_ON			1
 #define BCM2835_MBOX_TURBO_OFF			0
 
@@ -270,4 +328,151 @@ struct msg_get_max_temperature {
 	uint32_t end_tag;
 };
 
+#define	BCM2835_MBOX_TAG_GET_PHYSICAL_W_H	0x00040003
+#define	BCM2835_MBOX_TAG_SET_PHYSICAL_W_H	0x00048003
+#define	BCM2835_MBOX_TAG_GET_VIRTUAL_W_H	0x00040004
+#define	BCM2835_MBOX_TAG_SET_VIRTUAL_W_H	0x00048004
+
+struct bcm2835_mbox_tag_fb_w_h {
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+			uint32_t width;
+			uint32_t height;
+		} req;
+		struct {
+			uint32_t width;
+			uint32_t height;
+		} resp;
+	} body;
+};
+
+#define	BCM2835_MBOX_TAG_GET_DEPTH		0x00040005
+#define	BCM2835_MBOX_TAG_SET_DEPTH		0x00048005
+
+struct bcm2835_mbox_tag_depth {
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+			uint32_t bpp;
+		} req;
+		struct {
+			uint32_t bpp;
+		} resp;
+	} body;
+};
+
+#define	BCM2835_MBOX_TAG_GET_ALPHA_MODE		0x00040007
+#define	BCM2835_MBOX_TAG_SET_ALPHA_MODE		0x00048007
+
+#define	BCM2835_MBOX_ALPHA_MODE_0_OPAQUE	0
+#define	BCM2835_MBOX_ALPHA_MODE_0_TRANSPARENT	1
+#define	BCM2835_MBOX_ALPHA_MODE_IGNORED		2
+
+struct bcm2835_mbox_tag_alpha_mode {
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+			uint32_t alpha;
+		} req;
+		struct {
+			uint32_t alpha;
+		} resp;
+	} body;
+};
+
+#define	BCM2835_MBOX_TAG_GET_VIRTUAL_OFFSET	0x00040009
+#define	BCM2835_MBOX_TAG_SET_VIRTUAL_OFFSET	0x00048009
+
+struct bcm2835_mbox_tag_virtual_offset {
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+			uint32_t x;
+			uint32_t y;
+		} req;
+		struct {
+			uint32_t x;
+			uint32_t y;
+		} resp;
+	} body;
+};
+
+#define	BCM2835_MBOX_TAG_GET_PITCH		0x00040008
+
+struct bcm2835_mbox_tag_pitch {
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+		} req;
+		struct {
+			uint32_t pitch;
+		} resp;
+	} body;
+};
+
+#define	BCM2835_MBOX_TAG_ALLOCATE_BUFFER	0x00040001
+
+struct bcm2835_mbox_tag_allocate_buffer {
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+			uint32_t alignment;
+		} req;
+		struct {
+			uint32_t fb_address;
+			uint32_t fb_size;
+		} resp;
+	} body;
+};
+
+#define	BCM2835_MBOX_TAG_RELEASE_BUFFER		0x00048001
+
+struct bcm2835_mbox_tag_release_buffer {
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+		} req;
+		struct {
+		} resp;
+	} body;
+};
+
+struct bcm2835_fb_config {
+	uint32_t xres;
+	uint32_t yres;
+	uint32_t vxres;
+	uint32_t vyres;
+	uint32_t xoffset;
+	uint32_t yoffset;
+	uint32_t bpp;
+	uint32_t pitch;
+	uint32_t base;
+	uint32_t size;
+};
+
+struct msg_fb_get_w_h {
+	struct bcm2835_mbox_hdr hdr;
+	struct bcm2835_mbox_tag_fb_w_h physical_w_h;
+	uint32_t end_tag;
+};
+
+int bcm2835_mbox_fb_get_w_h(struct bcm2835_fb_config *);
+
+struct msg_fb_setup {
+	struct bcm2835_mbox_hdr hdr;
+	struct bcm2835_mbox_tag_fb_w_h physical_w_h;
+	struct bcm2835_mbox_tag_fb_w_h virtual_w_h;
+	struct bcm2835_mbox_tag_virtual_offset offset;
+	struct bcm2835_mbox_tag_depth depth;
+	struct bcm2835_mbox_tag_alpha_mode alpha;
+	struct bcm2835_mbox_tag_allocate_buffer buffer;
+	struct bcm2835_mbox_tag_pitch pitch;
+	uint32_t end_tag;
+};
+
+int bcm2835_mbox_fb_init(struct bcm2835_fb_config *);
+
+int bcm2835_mbox_property(void *, size_t);
+
 #endif /* _BCM2835_MBOX_PROP_H_ */
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_sdhci.c b/sys/arm/broadcom/bcm2835/bcm2835_sdhci.c
index b2b8df3..f4c531b 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_sdhci.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_sdhci.c
@@ -54,6 +54,7 @@ __FBSDID("$FreeBSD$");
 #include "sdhci_if.h"
 
 #include "bcm2835_dma.h"
+#include <arm/broadcom/bcm2835/bcm2835_mbox_prop.h>
 #include "bcm2835_vcbus.h"
 
 #define	BCM2835_DEFAULT_SDHCI_FREQ	50
@@ -71,28 +72,24 @@ __FBSDID("$FreeBSD$");
 static int bcm2835_sdhci_hs = 1;
 static int bcm2835_sdhci_pio_mode = 0;
 
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-sdhci",	1},
+	{"brcm,bcm2835-mmc",		1},
+	{NULL,				0}
+};
+
 TUNABLE_INT("hw.bcm2835.sdhci.hs", &bcm2835_sdhci_hs);
 TUNABLE_INT("hw.bcm2835.sdhci.pio_mode", &bcm2835_sdhci_pio_mode);
 
 struct bcm_sdhci_softc {
 	device_t		sc_dev;
-	struct mtx		sc_mtx;
 	struct resource *	sc_mem_res;
 	struct resource *	sc_irq_res;
 	bus_space_tag_t		sc_bst;
 	bus_space_handle_t	sc_bsh;
 	void *			sc_intrhand;
 	struct mmc_request *	sc_req;
-	struct mmc_data *	sc_data;
-	uint32_t		sc_flags;
-#define	LPC_SD_FLAGS_IGNORECRC		(1 << 0)
-	int			sc_xfer_direction;
-#define	DIRECTION_READ		0
-#define	DIRECTION_WRITE		1
-	int			sc_xfer_done;
-	int			sc_bus_busy;
 	struct sdhci_slot	sc_slot;
-	int			sc_dma_inuse;
 	int			sc_dma_ch;
 	bus_dma_tag_t		sc_dma_tag;
 	bus_dmamap_t		sc_dma_map;
@@ -113,11 +110,6 @@ static void bcm_sdhci_intr(void *);
 static int bcm_sdhci_get_ro(device_t, device_t);
 static void bcm_sdhci_dma_intr(int ch, void *arg);
 
-#define	bcm_sdhci_lock(_sc)						\
-    mtx_lock(&_sc->sc_mtx);
-#define	bcm_sdhci_unlock(_sc)						\
-    mtx_unlock(&_sc->sc_mtx);
-
 static void
 bcm_sdhci_dmacb(void *arg, bus_dma_segment_t *segs, int nseg, int err)
 {
@@ -141,10 +133,11 @@ bcm_sdhci_probe(device_t dev)
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-sdhci"))
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
 		return (ENXIO);
 
 	device_set_desc(dev, "Broadcom 2708 SDHCI controller");
+
 	return (BUS_PROBE_DEFAULT);
 }
 
@@ -155,20 +148,37 @@ bcm_sdhci_attach(device_t dev)
 	int rid, err;
 	phandle_t node;
 	pcell_t cell;
-	int default_freq;
+	u_int default_freq;
 
 	sc->sc_dev = dev;
 	sc->sc_req = NULL;
-	err = 0;
 
-	default_freq = BCM2835_DEFAULT_SDHCI_FREQ;
-	node = ofw_bus_get_node(sc->sc_dev);
-	if ((OF_getprop(node, "clock-frequency", &cell, sizeof(cell))) > 0)
-		default_freq = (int)fdt32_to_cpu(cell)/1000000;
+	err = bcm2835_mbox_set_power_state(BCM2835_MBOX_POWER_ID_EMMC,
+	    TRUE);
+	if (err != 0) {
+		if (bootverbose)
+			device_printf(dev, "Unable to enable the power\n");
+		return (err);
+	}
 
-	dprintf("SDHCI frequency: %dMHz\n", default_freq);
+	default_freq = 0;
+	err = bcm2835_mbox_get_clock_rate(BCM2835_MBOX_CLOCK_ID_EMMC,
+	    &default_freq);
+	if (err == 0) {
+		/* Convert to MHz */
+		default_freq /= 1000000;
+	}
+	if (default_freq == 0) {
+		node = ofw_bus_get_node(sc->sc_dev);
+		if ((OF_getencprop(node, "clock-frequency", &cell,
+		    sizeof(cell))) > 0)
+			default_freq = cell / 1000000;
+	}
+	if (default_freq == 0)
+		default_freq = BCM2835_DEFAULT_SDHCI_FREQ;
 
-	mtx_init(&sc->sc_mtx, "bcm sdhci", "sdhci", MTX_DEF);
+	if (bootverbose)
+		device_printf(dev, "SDHCI frequency: %dMHz\n", default_freq);
 
 	rid = 0;
 	sc->sc_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
@@ -212,11 +222,7 @@ bcm_sdhci_attach(device_t dev)
  
 	sdhci_init_slot(dev, &sc->sc_slot, 0);
 
-	sc->sc_dma_ch = bcm_dma_allocate(BCM_DMA_CH_FAST1);
-	if (sc->sc_dma_ch == BCM_DMA_CH_INVALID)
-		sc->sc_dma_ch = bcm_dma_allocate(BCM_DMA_CH_FAST2);
-	if (sc->sc_dma_ch == BCM_DMA_CH_INVALID)
-		sc->sc_dma_ch = bcm_dma_allocate(BCM_DMA_CH_ANY);
+	sc->sc_dma_ch = bcm_dma_allocate(BCM_DMA_CH_ANY);
 	if (sc->sc_dma_ch == BCM_DMA_CH_INVALID)
 		goto fail;
 
@@ -258,7 +264,6 @@ fail:
 		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->sc_irq_res);
 	if (sc->sc_mem_res)
 		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->sc_mem_res);
-	mtx_destroy(&sc->sc_mtx);
 
 	return (err);
 }
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_spi.c b/sys/arm/broadcom/bcm2835/bcm2835_spi.c
index c9f6d33..6416333 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_spi.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_spi.c
@@ -59,6 +59,12 @@ __FBSDID("$FreeBSD$");
 
 #include "spibus_if.h"
 
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-spi",	1},
+	{"brcm,bcm2835-spi",		1},
+	{NULL,				0}
+};
+
 static void bcm_spi_intr(void *);
 
 #ifdef	BCM_SPI_DEBUG
@@ -234,7 +240,7 @@ bcm_spi_probe(device_t dev)
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	if (!ofw_bus_is_compatible(dev, "broadcom,bcm2835-spi"))
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
 		return (ENXIO);
 
 	device_set_desc(dev, "BCM2708/2835 SPI controller");
@@ -428,6 +434,15 @@ bcm_spi_transfer(device_t dev, device_t child, struct spi_command *cmd)
 	KASSERT(cmd->tx_data_sz == cmd->rx_data_sz, 
 	    ("TX/RX data sizes should be equal"));
 
+	/* Get the proper chip select for this child. */
+	spibus_get_cs(child, &cs);
+	if (cs < 0 || cs > 2) {
+		device_printf(dev,
+		    "Invalid chip select %d requested by %s\n", cs,
+		    device_get_nameunit(child));
+		return (EINVAL);
+	}
+
 	BCM_SPI_LOCK(sc);
 
 	/* If the controller is in use wait until it is available. */
@@ -442,16 +457,6 @@ bcm_spi_transfer(device_t dev, device_t child, struct spi_command *cmd)
 	    SPI_CS_CLEAR_RXFIFO | SPI_CS_CLEAR_TXFIFO,
 	    SPI_CS_CLEAR_RXFIFO | SPI_CS_CLEAR_TXFIFO);
 
-	/* Get the proper chip select for this child. */
-	spibus_get_cs(child, &cs);
-	if (cs < 0 || cs > 2) {
-		device_printf(dev,
-		    "Invalid chip select %d requested by %s\n", cs,
-		    device_get_nameunit(child));
-		BCM_SPI_UNLOCK(sc);
-		return (EINVAL);
-	}
-
 	/* Save a pointer to the SPI command. */
 	sc->sc_cmd = cmd;
 	sc->sc_read = 0;
@@ -472,8 +477,10 @@ bcm_spi_transfer(device_t dev, device_t child, struct spi_command *cmd)
 	/* Make sure the SPI engine and interrupts are disabled. */
 	bcm_spi_modifyreg(sc, SPI_CS, SPI_CS_TA | SPI_CS_INTR | SPI_CS_INTD, 0);
 
-	/* Clear the controller flags. */
+	/* Release the controller and wakeup the next thread waiting for it. */
 	sc->sc_flags = 0;
+	wakeup_one(dev);
+	BCM_SPI_UNLOCK(sc);
 
 	/*
 	 * Check for transfer timeout.  The SPI controller doesn't
@@ -484,8 +491,6 @@ bcm_spi_transfer(device_t dev, device_t child, struct spi_command *cmd)
 		err = EIO;
 	}
 
-	BCM_SPI_UNLOCK(sc);
-
 	return (err);
 }
 
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_vcbus.h b/sys/arm/broadcom/bcm2835/bcm2835_vcbus.h
index cb871f2..e1d44b3 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_vcbus.h
+++ b/sys/arm/broadcom/bcm2835/bcm2835_vcbus.h
@@ -37,14 +37,20 @@
 #define	BCM2835_VCBUS_IO_BASE		0x7E000000
 #define	BCM2835_VCBUS_SDRAM_UNCACHED	0xC0000000
 
+#ifdef SOC_BCM2836
+#define	BCM2835_ARM_IO_BASE		0x3f000000
+#define	BCM2835_VCBUS_SDRAM_BASE	BCM2835_VCBUS_SDRAM_UNCACHED
+#else
 #define	BCM2835_ARM_IO_BASE		0x20000000
-#define	BCM2835_ARM_IO_SIZE		0x02000000
+#define	BCM2835_VCBUS_SDRAM_BASE	BCM2835_VCBUS_SDRAM_CACHED
+#endif
+#define	BCM2835_ARM_IO_SIZE		0x01000000
 
 /*
  * Convert physical address to VC bus address. Should be used 
  * when submitting address over mailbox interface 
  */
-#define	PHYS_TO_VCBUS(pa)	((pa) + BCM2835_VCBUS_SDRAM_CACHED)
+#define	PHYS_TO_VCBUS(pa)	((pa) + BCM2835_VCBUS_SDRAM_BASE)
 
 /* Check whether pa bellong top IO window */
 #define BCM2835_ARM_IS_IO(pa)	(((pa) >= BCM2835_ARM_IO_BASE) && \
@@ -61,6 +67,6 @@
  * when address is returned by VC over mailbox interface. e.g.
  * framebuffer base
  */
-#define	VCBUS_TO_PHYS(vca)	((vca) - BCM2835_VCBUS_SDRAM_CACHED)
+#define	VCBUS_TO_PHYS(vca)	((vca) & ~(BCM2835_VCBUS_SDRAM_BASE))
 
 #endif /* _BCM2835_VCBUS_H_ */
diff --git a/sys/arm/broadcom/bcm2835/bcm2835_wdog.c b/sys/arm/broadcom/bcm2835/bcm2835_wdog.c
index 58cb2c3..e0ee93f 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_wdog.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_wdog.c
@@ -46,7 +46,7 @@ __FBSDID("$FreeBSD$");
 
 #include <arm/broadcom/bcm2835/bcm2835_wdog.h>
 
-#define	BCM2835_PASWORD		0x5a
+#define	BCM2835_PASSWORD	0x5a
 
 #define BCM2835_WDOG_RESET	0
 #define BCM2835_PASSWORD_MASK	0xff000000
@@ -54,8 +54,8 @@ __FBSDID("$FreeBSD$");
 #define BCM2835_WDOG_TIME_MASK	0x000fffff
 #define BCM2835_WDOG_TIME_SHIFT	0
 
-#define	READ(_sc, _r) bus_space_read_4((_sc)->bst, (_sc)->bsh, (_r))
-#define	WRITE(_sc, _r, _v) bus_space_write_4((_sc)->bst, (_sc)->bsh, (_r), (_v))
+#define	READ(_sc, _r) bus_space_read_4((_sc)->bst, (_sc)->bsh, (_r) + (_sc)->regs_offset)
+#define	WRITE(_sc, _r, _v) bus_space_write_4((_sc)->bst, (_sc)->bsh, (_r) + (_sc)->regs_offset, (_v))
 
 #define BCM2835_RSTC_WRCFG_CLR		0xffffffcf
 #define BCM2835_RSTC_WRCFG_SET		0x00000030
@@ -77,6 +77,17 @@ struct bcmwd_softc {
 	int			wdog_period;
 	char			wdog_passwd;
 	struct mtx		mtx;
+	int			regs_offset;
+};
+
+#define	BSD_DTB		1
+#define	UPSTREAM_DTB	2
+#define	UPSTREAM_DTB_REGS_OFFSET	0x1c
+
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-wdt",	BSD_DTB},
+	{"brcm,bcm2835-pm-wdt",		UPSTREAM_DTB},
+	{NULL,				0}
 };
 
 static void bcmwd_watchdog_fn(void *private, u_int cmd, int *error);
@@ -88,12 +99,12 @@ bcmwd_probe(device_t dev)
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	if (ofw_bus_is_compatible(dev, "broadcom,bcm2835-wdt")) {
-		device_set_desc(dev, "BCM2708/2835 Watchdog");
-		return (BUS_PROBE_DEFAULT);
-	}
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0)
+		return (ENXIO);
+
+	device_set_desc(dev, "BCM2708/2835 Watchdog");
 
-	return (ENXIO);
+	return (BUS_PROBE_DEFAULT);
 }
 
 static int
@@ -107,7 +118,7 @@ bcmwd_attach(device_t dev)
 
 	sc = device_get_softc(dev);
 	sc->wdog_period = 7;
-	sc->wdog_passwd = BCM2835_PASWORD;
+	sc->wdog_passwd = BCM2835_PASSWORD;
 	sc->wdog_armed = 0;
 	sc->dev = dev;
 
@@ -121,6 +132,11 @@ bcmwd_attach(device_t dev)
 	sc->bst = rman_get_bustag(sc->res);
 	sc->bsh = rman_get_bushandle(sc->res);
 
+	/* compensate base address difference */
+	if (ofw_bus_search_compatible(dev, compat_data)->ocd_data
+	   == UPSTREAM_DTB)
+		sc->regs_offset = UPSTREAM_DTB_REGS_OFFSET;
+
 	bcmwd_lsc = sc;
 	mtx_init(&sc->mtx, "BCM2835 Watchdog", "bcmwd", MTX_DEF);
 	EVENTHANDLER_REGISTER(watchdog_list, bcmwd_watchdog_fn, sc, 0);
@@ -150,27 +166,27 @@ bcmwd_watchdog_fn(void *private, u_int cmd, int *error)
 			device_printf(sc->dev,
 			    "Can't arm, timeout must be between 1-15 seconds\n");
 			WRITE(sc, BCM2835_RSTC_REG, 
-			    (BCM2835_PASWORD << BCM2835_PASSWORD_SHIFT) |
+			    (BCM2835_PASSWORD << BCM2835_PASSWORD_SHIFT) |
 			    BCM2835_RSTC_RESET);
 			mtx_unlock(&sc->mtx);
 			return;
 		}
 
 		ticks = (sec << 16) & BCM2835_WDOG_TIME_MASK;
-		reg = (BCM2835_PASWORD << BCM2835_PASSWORD_SHIFT) | ticks;
+		reg = (BCM2835_PASSWORD << BCM2835_PASSWORD_SHIFT) | ticks;
 		WRITE(sc, BCM2835_WDOG_REG, reg);
 
 		reg = READ(sc, BCM2835_RSTC_REG);
 		reg &= BCM2835_RSTC_WRCFG_CLR;
 		reg |= BCM2835_RSTC_WRCFG_FULL_RESET;
-		reg |= (BCM2835_PASWORD << BCM2835_PASSWORD_SHIFT);
+		reg |= (BCM2835_PASSWORD << BCM2835_PASSWORD_SHIFT);
 		WRITE(sc, BCM2835_RSTC_REG, reg);
 
 		*error = 0;
 	}
 	else
 		WRITE(sc, BCM2835_RSTC_REG, 
-		    (BCM2835_PASWORD << BCM2835_PASSWORD_SHIFT) |
+		    (BCM2835_PASSWORD << BCM2835_PASSWORD_SHIFT) |
 		    BCM2835_RSTC_RESET);
 
 	mtx_unlock(&sc->mtx);
@@ -184,11 +200,11 @@ bcmwd_watchdog_reset()
 		return;
 
 	WRITE(bcmwd_lsc, BCM2835_WDOG_REG,
-	    (BCM2835_PASWORD << BCM2835_PASSWORD_SHIFT) | 10);
+	    (BCM2835_PASSWORD << BCM2835_PASSWORD_SHIFT) | 10);
 
 	WRITE(bcmwd_lsc, BCM2835_RSTC_REG,
 	    (READ(bcmwd_lsc, BCM2835_RSTC_REG) & BCM2835_RSTC_WRCFG_CLR) |
-		(BCM2835_PASWORD << BCM2835_PASSWORD_SHIFT) |
+		(BCM2835_PASSWORD << BCM2835_PASSWORD_SHIFT) |
 		BCM2835_RSTC_WRCFG_FULL_RESET);
 }
 
diff --git a/sys/arm/broadcom/bcm2835/bcm2836.c b/sys/arm/broadcom/bcm2835/bcm2836.c
new file mode 100644
index 0000000..3e89eae
--- /dev/null
+++ b/sys/arm/broadcom/bcm2835/bcm2836.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright 2015 Andrew Turner.
+ * 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/module.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+
+#include <dev/ofw/ofw_bus_subr.h>
+#include <dev/ofw/ofw_bus.h>
+
+#include <arm/broadcom/bcm2835/bcm2836.h>
+
+#define	ARM_LOCAL_BASE	0x40000000
+#define	ARM_LOCAL_SIZE	0x00001000
+
+#define	ARM_LOCAL_CONTROL		0x00
+#define	ARM_LOCAL_PRESCALER		0x08
+#define	 PRESCALER_19_2			0x80000000 /* 19.2 MHz */
+#define	ARM_LOCAL_INT_TIMER(n)		(0x40 + (n) * 4)
+#define	ARM_LOCAL_INT_MAILBOX(n)	(0x50 + (n) * 4)
+#define	ARM_LOCAL_INT_PENDING(n)	(0x60 + (n) * 4)
+#define	 INT_PENDING_MASK		0x0f
+
+/*
+ * A driver for features of the bcm2836.
+ */
+
+struct bcm2836_softc {
+	device_t	 sc_dev;
+	struct resource *sc_mem;
+};
+
+static device_identify_t bcm2836_identify;
+static device_probe_t bcm2836_probe;
+static device_attach_t bcm2836_attach;
+
+struct bcm2836_softc *softc;
+
+static void
+bcm2836_identify(driver_t *driver, device_t parent)
+{
+
+	if (BUS_ADD_CHILD(parent, 0, "bcm2836", -1) == NULL)
+		device_printf(parent, "add child failed\n");
+}
+
+static int
+bcm2836_probe(device_t dev)
+{
+
+	if (softc != NULL)
+		return (ENXIO);
+
+	device_set_desc(dev, "Broadcom bcm2836");
+
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+bcm2836_attach(device_t dev)
+{
+	int i, rid;
+
+	softc = device_get_softc(dev);
+	softc->sc_dev = dev;
+
+	rid = 0;
+	softc->sc_mem = bus_alloc_resource(dev, SYS_RES_MEMORY, &rid,
+	    ARM_LOCAL_BASE, ARM_LOCAL_BASE + ARM_LOCAL_SIZE, ARM_LOCAL_SIZE,
+	    RF_ACTIVE);
+	if (softc->sc_mem == NULL) {
+		device_printf(dev, "could not allocate memory resource\n");
+		return (ENXIO);
+	}
+
+	bus_write_4(softc->sc_mem, ARM_LOCAL_CONTROL, 0);
+	bus_write_4(softc->sc_mem, ARM_LOCAL_PRESCALER, PRESCALER_19_2);
+
+	for (i = 0; i < 4; i++)
+		bus_write_4(softc->sc_mem, ARM_LOCAL_INT_TIMER(i), 0);
+
+	for (i = 0; i < 4; i++)
+		bus_write_4(softc->sc_mem, ARM_LOCAL_INT_MAILBOX(i), 1);
+
+	return (0);
+}
+
+int
+bcm2836_get_next_irq(int last_irq)
+{
+	uint32_t reg;
+	int cpu;
+	int irq;
+
+	cpu = PCPU_GET(cpuid);
+
+	reg = bus_read_4(softc->sc_mem, ARM_LOCAL_INT_PENDING(cpu));
+	reg &= INT_PENDING_MASK;
+	if (reg == 0)
+		return (-1);
+
+	irq = ffs(reg) - 1;
+
+	return (irq);
+}
+
+void
+bcm2836_mask_irq(uintptr_t irq)
+{
+	uint32_t reg;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		reg = bus_read_4(softc->sc_mem, ARM_LOCAL_INT_TIMER(i));
+		reg &= ~(1 << irq);
+		bus_write_4(softc->sc_mem, ARM_LOCAL_INT_TIMER(i), reg);
+	}
+}
+
+void
+bcm2836_unmask_irq(uintptr_t irq)
+{
+	uint32_t reg;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		reg = bus_read_4(softc->sc_mem, ARM_LOCAL_INT_TIMER(i));
+		reg |= (1 << irq);
+		bus_write_4(softc->sc_mem, ARM_LOCAL_INT_TIMER(i), reg);
+	}
+}
+
+static device_method_t bcm2836_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_identify,	bcm2836_identify),
+	DEVMETHOD(device_probe,		bcm2836_probe),
+	DEVMETHOD(device_attach,	bcm2836_attach),
+
+	DEVMETHOD_END
+};
+
+static devclass_t bcm2836_devclass;
+
+static driver_t bcm2836_driver = {
+	"bcm2836",
+	bcm2836_methods,
+	sizeof(struct bcm2836_softc),
+};
+
+EARLY_DRIVER_MODULE(bcm2836, nexus, bcm2836_driver, bcm2836_devclass, 0, 0,
+    BUS_PASS_INTERRUPT + BUS_PASS_ORDER_MIDDLE);
diff --git a/sys/arm/broadcom/bcm2835/bcm2836.h b/sys/arm/broadcom/bcm2835/bcm2836.h
new file mode 100644
index 0000000..217dae1
--- /dev/null
+++ b/sys/arm/broadcom/bcm2835/bcm2836.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright 2015 Andrew Turner.
+ * 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 _BCM2815_BCM2836_H
+#define	_BCM2815_BCM2836_H
+
+int bcm2836_get_next_irq(int);
+void bcm2836_mask_irq(uintptr_t);
+void bcm2836_unmask_irq(uintptr_t);
+
+#endif
diff --git a/sys/arm/broadcom/bcm2835/bcm283x_dwc_fdt.c b/sys/arm/broadcom/bcm2835/bcm283x_dwc_fdt.c
new file mode 100644
index 0000000..2fc25af
--- /dev/null
+++ b/sys/arm/broadcom/bcm2835/bcm283x_dwc_fdt.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright 2015 Andrew Turner.
+ * 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/kernel.h>
+#include <sys/bus.h>
+#include <sys/callout.h>
+#include <sys/condvar.h>
+#include <sys/module.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/dwc_otg.h>
+#include <dev/usb/controller/dwc_otg_fdt.h>
+
+#include <arm/broadcom/bcm2835/bcm2835_mbox_prop.h>
+
+static struct ofw_compat_data compat_data[] = {
+	{"broadcom,bcm2835-usb",	1},
+	{"brcm,bcm2708-usb",		1},
+	{NULL,				0}
+};
+
+static device_probe_t bcm283x_dwc_otg_probe;
+static device_attach_t bcm283x_dwc_otg_attach;
+
+static int
+bcm283x_dwc_otg_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, "DWC OTG 2.0 integrated USB controller (bcm283x)");
+
+	return (BUS_PROBE_VENDOR);
+}
+
+static int
+bcm283x_dwc_otg_attach(device_t dev)
+{
+	int err;
+
+	err = bcm2835_mbox_set_power_state(BCM2835_MBOX_POWER_ID_USB_HCD, TRUE);
+	if (err)
+		device_printf(dev, "failed to set power state, err=%d\n", err);
+
+	return (dwc_otg_attach(dev));
+}
+
+static device_method_t bcm283x_dwc_otg_methods[] = {
+	/* bus interface */
+	DEVMETHOD(device_probe, bcm283x_dwc_otg_probe),
+	DEVMETHOD(device_attach, bcm283x_dwc_otg_attach),
+
+	DEVMETHOD_END
+};
+
+static devclass_t bcm283x_dwc_otg_devclass;
+
+DEFINE_CLASS_1(bcm283x_dwcotg, bcm283x_dwc_otg_driver, bcm283x_dwc_otg_methods,
+    sizeof(struct dwc_otg_fdt_softc), dwc_otg_driver);
+DRIVER_MODULE(bcm283x_dwcotg, simplebus, bcm283x_dwc_otg_driver,
+    bcm283x_dwc_otg_devclass, 0, 0);
+MODULE_DEPEND(bcm283x_dwcotg, usb, 1, 1, 1);
diff --git a/sys/arm/broadcom/bcm2835/files.bcm2835 b/sys/arm/broadcom/bcm2835/files.bcm2835
index e4ebc3b..8368cab 100644
--- a/sys/arm/broadcom/bcm2835/files.bcm2835
+++ b/sys/arm/broadcom/bcm2835/files.bcm2835
@@ -16,6 +16,8 @@ arm/broadcom/bcm2835/bcm2835_systimer.c		standard
 arm/broadcom/bcm2835/bcm2835_wdog.c		standard
 dev/usb/controller/dwc_otg_fdt.c		optional dwcotg
 
+arm/broadcom/bcm2835/bcm283x_dwc_fdt.c		optional dwcotg fdt
+
 arm/arm/bus_space_base.c			standard
 arm/arm/bus_space_generic.c                     standard
 arm/arm/bus_space_asm_generic.S                 standard
diff --git a/sys/arm/broadcom/bcm2835/files.bcm2836 b/sys/arm/broadcom/bcm2835/files.bcm2836
new file mode 100644
index 0000000..f3c1f70
--- /dev/null
+++ b/sys/arm/broadcom/bcm2835/files.bcm2836
@@ -0,0 +1,6 @@
+# $FreeBSD$
+
+arm/arm/cpufunc_asm_armv7.S			standard
+arm/arm/generic_timer.c				standard
+
+arm/broadcom/bcm2835/bcm2836.c			standard
diff --git a/sys/arm/broadcom/bcm2835/std.bcm2835 b/sys/arm/broadcom/bcm2835/std.bcm2835
index 026f5b2..affa807 100644
--- a/sys/arm/broadcom/bcm2835/std.bcm2835
+++ b/sys/arm/broadcom/bcm2835/std.bcm2835
@@ -3,6 +3,7 @@
 machine	arm armv6
 cpu	CPU_ARM1176
 makeoptions	CONF_CFLAGS="-mcpu=arm1176jzf-s -Wa,-mcpu=arm1176jzf-s"
+options 	SOC_BCM2835
 
 files	"../broadcom/bcm2835/files.bcm2835"
 
diff --git a/sys/arm/broadcom/bcm2835/std.bcm2836 b/sys/arm/broadcom/bcm2835/std.bcm2836
new file mode 100644
index 0000000..874083f
--- /dev/null
+++ b/sys/arm/broadcom/bcm2835/std.bcm2836
@@ -0,0 +1,10 @@
+# $FreeBSD$
+
+machine 	arm armv6
+cpu		CPU_CORTEXA
+makeoptions	CONF_CFLAGS="-march=armv7a"
+options 	SOC_BCM2836
+
+files	"../broadcom/bcm2835/files.bcm2836"
+files	"../broadcom/bcm2835/files.bcm283x"
+
diff --git a/sys/conf/options.arm b/sys/conf/options.arm
index aa13b2b..522426e 100644
--- a/sys/conf/options.arm
+++ b/sys/conf/options.arm
@@ -38,6 +38,8 @@ SOCDEV_PA		opt_global.h
 SOCDEV_VA		opt_global.h
 PV_STATS		opt_pmap.h
 QEMU_WORKAROUNDS	opt_global.h
+SOC_BCM2835		opt_global.h
+SOC_BCM2836		opt_global.h
 SOC_MV_ARMADAXP		opt_global.h
 SOC_MV_DISCOVERY	opt_global.h
 SOC_MV_DOVE		opt_global.h
diff --git a/sys/dev/usb/controller/dwc_otg_fdt.c b/sys/dev/usb/controller/dwc_otg_fdt.c
index c862bdd..512487c 100644
--- a/sys/dev/usb/controller/dwc_otg_fdt.c
+++ b/sys/dev/usb/controller/dwc_otg_fdt.c
@@ -63,15 +63,11 @@ __FBSDID("$FreeBSD$");
 #include <dev/usb/usb_bus.h>
 
 #include <dev/usb/controller/dwc_otg.h>
+#include <dev/usb/controller/dwc_otg_fdt.h>
 
 static device_probe_t dwc_otg_probe;
-static device_attach_t dwc_otg_attach;
 static device_detach_t dwc_otg_detach;
 
-struct dwc_otg_super_softc {
-	struct dwc_otg_softc sc_otg;	/* must be first */
-};
-
 static int
 dwc_otg_probe(device_t dev)
 {
@@ -84,13 +80,13 @@ dwc_otg_probe(device_t dev)
 
 	device_set_desc(dev, "DWC OTG 2.0 integrated USB controller");
 
-	return (0);
+	return (BUS_PROBE_DEFAULT);
 }
 
-static int
+int
 dwc_otg_attach(device_t dev)
 {
-	struct dwc_otg_super_softc *sc = device_get_softc(dev);
+	struct dwc_otg_fdt_softc *sc = device_get_softc(dev);
 	int err;
 	int rid;
 
@@ -117,7 +113,12 @@ dwc_otg_attach(device_t dev)
 	sc->sc_otg.sc_io_hdl = rman_get_bushandle(sc->sc_otg.sc_io_res);
 	sc->sc_otg.sc_io_size = rman_get_size(sc->sc_otg.sc_io_res);
 
-	rid = 0;
+
+	/*
+	 * brcm,bcm2708-usb FDT provides two interrupts,
+	 * we need only second one (VC_USB)
+	 */
+	rid = ofw_bus_is_compatible(dev, "brcm,bcm2708-usb") ? 1 : 0;
 	sc->sc_otg.sc_irq_res =
 	    bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid, RF_ACTIVE);
 	if (sc->sc_otg.sc_irq_res == NULL)
@@ -153,7 +154,7 @@ error:
 static int
 dwc_otg_detach(device_t dev)
 {
-	struct dwc_otg_super_softc *sc = device_get_softc(dev);
+	struct dwc_otg_fdt_softc *sc = device_get_softc(dev);
 	int err;
 
 	/* during module unload there are lots of children leftover */
@@ -198,10 +199,10 @@ static device_method_t dwc_otg_methods[] = {
 	DEVMETHOD_END
 };
 
-static driver_t dwc_otg_driver = {
+driver_t dwc_otg_driver = {
 	.name = "dwcotg",
 	.methods = dwc_otg_methods,
-	.size = sizeof(struct dwc_otg_super_softc),
+	.size = sizeof(struct dwc_otg_fdt_softc),
 };
 
 static devclass_t dwc_otg_devclass;
diff --git a/sys/dev/usb/controller/dwc_otg_fdt.h b/sys/dev/usb/controller/dwc_otg_fdt.h
new file mode 100644
index 0000000..9e01118
--- /dev/null
+++ b/sys/dev/usb/controller/dwc_otg_fdt.h
@@ -0,0 +1,39 @@
+/*-
+ * Copyright (c) 2012 Hans Petter Selasky. 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 _DWC_OTG_FDT_H_
+#define	_DWC_OTG_FDT_H_
+
+struct dwc_otg_fdt_softc {
+	struct dwc_otg_softc sc_otg;	/* must be first */
+};
+
+extern driver_t dwc_otg_driver;
+
+device_attach_t dwc_otg_attach;
+
+#endif
diff --git a/sys/dev/usb/net/if_smsc.c b/sys/dev/usb/net/if_smsc.c
index cb628c9..7cac613 100644
--- a/sys/dev/usb/net/if_smsc.c
+++ b/sys/dev/usb/net/if_smsc.c
@@ -1537,6 +1537,9 @@ smsc_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 }
 
 #ifdef FDT
+/*
+ * This is FreeBSD-specific compatibility strings for RPi/RPi2
+ */
 static phandle_t
 smsc_fdt_find_eth_node(phandle_t start)
 {
@@ -1548,11 +1551,68 @@ smsc_fdt_find_eth_node(phandle_t start)
 		    fdt_is_compatible(node, "usb,device"))
 			return (node);
 		child = smsc_fdt_find_eth_node(node);
-		if (child != 0)
+		if (child != -1)
 			return (child);
 	}
 
-	return (0);
+	return (-1);
+}
+
+/*
+ * Check if node's path is <*>/usb/hub/ethernet
+ */
+static int
+smsc_fdt_is_usb_eth(phandle_t node)
+{
+	char name[16];
+	int len;
+
+	memset(name, 0, sizeof(name));
+	len = OF_getprop(node, "name", name, sizeof(name));
+	if (len <= 0)
+		return (0);
+
+	if (strcmp(name, "ethernet"))
+		return (0);
+
+	node = OF_parent(node);
+	if (node == -1)
+		return (0);
+	len = OF_getprop(node, "name", name, sizeof(name));
+	if (len <= 0)
+		return (0);
+
+	if (strcmp(name, "hub"))
+		return (0);
+
+	node = OF_parent(node);
+	if (node == -1)
+		return (0);
+	len = OF_getprop(node, "name", name, sizeof(name));
+	if (len <= 0)
+		return (0);
+
+	if (strcmp(name, "usb"))
+		return (0);
+
+	return (1);
+}
+
+static phandle_t
+smsc_fdt_find_eth_node_by_path(phandle_t start)
+{
+	phandle_t child, node;
+
+	/* Traverse through entire tree to find usb ethernet nodes. */
+	for (node = OF_child(start); node != 0; node = OF_peer(node)) {
+		if (smsc_fdt_is_usb_eth(node))
+			return (node);
+		child = smsc_fdt_find_eth_node_by_path(node);
+		if (child != -1)
+			return (child);
+	}
+
+	return (-1);
 }
 
 /**
@@ -1568,8 +1628,14 @@ smsc_fdt_find_mac(unsigned char *mac)
 
 	root = OF_finddevice("/");
 	node = smsc_fdt_find_eth_node(root);
-	if (node != 0) {
+	/*
+	 * If it's not FreeBSD FDT blob for RPi, try more
+	 *     generic .../usb/hub/ethernet
+	 */
+	if (node == -1)
+		node = smsc_fdt_find_eth_node_by_path(root);
 
+	if (node != -1) {
 		/* Check if there is property */
 		if ((len = OF_getproplen(node, "local-mac-address")) > 0) {
 			if (len != ETHER_ADDR_LEN)