Add support for QEMU's version of Versatile Platform Board

14 files changed, 3347 insertions, 0 deletions

diff --git a/sys/arm/conf/VERSATILEPB b/sys/arm/conf/VERSATILEPB
new file mode 100644
index 0000000..9c87eff
--- /dev/null
+++ b/sys/arm/conf/VERSATILEPB
@@ -0,0 +1,101 @@
+# RPI-B -- Custom configuration for the Raspberry Pi
+#
+# For more information on this file, please read the handbook section on
+# Kernel Configuration Files:
+#
+#    http://www.FreeBSD.org/doc/en_US.ISO8859-1/books/handbook/kernelconfig-config.html
+#
+# The handbook is also available locally in /usr/share/doc/handbook
+# if you've installed the doc distribution, otherwise always see the
+# FreeBSD World Wide Web server (http://www.FreeBSD.org/) for the
+# latest information.
+#
+# An exhaustive list of options and more detailed explanations of the
+# device lines is also present in the ../../conf/NOTES and NOTES files. 
+# If you are in doubt as to the purpose or necessity of a line, check first 
+# in NOTES.
+#
+# $FreeBSD$
+
+ident		VERSATILEPB
+machine		arm	armv6
+cpu 		CPU_ARM11
+
+files		"../versatile/files.versatile"
+makeoptions	MODULES_OVERRIDE=""
+
+options 	KERNVIRTADDR=0xc0100000
+makeoptions	KERNVIRTADDR=0xc0100000
+options 	KERNPHYSADDR=0x00100000
+makeoptions	KERNPHYSADDR=0x00100000
+options 	PHYSADDR=0x00000000
+options 	STARTUP_PAGETABLE_ADDR=0x01000000
+options		FREEBSD_BOOT_LOADER
+options		LINUX_BOOT_ABI
+
+makeoptions	DEBUG=-g		#Build kernel with gdb(1) debug symbols
+options 	HZ=100
+
+options 	SCHED_4BSD		#4BSD scheduler
+options 	INET			#InterNETworking
+options 	FFS			#Berkeley Fast Filesystem
+options 	SOFTUPDATES		#Enable FFS soft updates support
+options 	UFS_ACL			#Support for access control lists
+options 	UFS_DIRHASH		#Improve performance on big directories
+device		snp
+
+options 	PSEUDOFS		#Pseudo-filesystem framework
+options 	COMPAT_43		#Compatible with BSD 4.3 [KEEP THIS!]
+options 	SCSI_DELAY=5000		#Delay (in ms) before probing SCSI
+options 	KTRACE			#ktrace(1) support
+options 	SYSVSHM			#SYSV-style shared memory
+options 	SYSVMSG			#SYSV-style message queues
+options 	SYSVSEM			#SYSV-style semaphores
+options 	_KPOSIX_PRIORITY_SCHEDULING #Posix P1003_1B real-time extensions
+options 	KBD_INSTALL_CDEV	# install a CDEV entry in /dev
+options         ROOTDEVNAME=\"ufs:da0s2a\"
+
+options 	PREEMPTION
+
+device		bpf
+device		loop
+device		mii
+device		mii_bitbang
+device		smc
+device		smcphy
+device		ether
+device		uart
+device		pl011
+device		pl190
+
+device		pty
+
+device		pci
+
+# SCSI Controllers
+device		sym		# NCR/Symbios/LSI Logic 53C8XX/53C1010/53C1510D
+
+# ATA/SCSI peripherals
+device		scbus		# SCSI bus (required for ATA/SCSI)
+device		da		# Direct Access (disks)
+device		pass		# Passthrough device (direct ATA/SCSI access)
+
+# NOTE: serial console is disabled if syscons enabled
+# Comment following lines for headless setup
+device		sc
+device		kbdmux
+options         SC_DFLT_FONT    # compile font in
+makeoptions     SC_DFLT_FONT=cp437
+
+options 	KDB
+options 	DDB			#Enable the kernel debugger
+options 	INVARIANTS		#Enable calls of extra sanity checking
+options 	INVARIANT_SUPPORT	#Extra sanity checks of internal structures, required by INVARIANTS
+
+device		md
+device		random		# Entropy device
+
+# Flattened Device Tree
+options         FDT
+options         FDT_DTB_STATIC
+makeoptions     FDT_DTS_FILE=versatilepb.dts
diff --git a/sys/arm/versatile/bus_space.c b/sys/arm/versatile/bus_space.c
new file mode 100644
index 0000000..4cce820
--- /dev/null
+++ b/sys/arm/versatile/bus_space.c
@@ -0,0 +1,113 @@
+/*-
+ * Copyright (C) 2012 FreeBSD Foundation
+ * 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.
+ * 3. Neither the name of MARVELL nor the names of contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+
+#include <machine/bus.h>
+
+/* Prototypes for all the bus_space structure functions */
+bs_protos(generic);
+bs_protos(generic_armv4);
+
+struct bus_space _base_tag = {
+	/* cookie */
+	.bs_cookie	= (void *) 0,
+	
+	/* mapping/unmapping */
+	.bs_map		= generic_bs_map,
+	.bs_unmap	= generic_bs_unmap,
+	.bs_subregion	= generic_bs_subregion,
+	
+	/* allocation/deallocation */
+	.bs_alloc	= generic_bs_alloc,
+	.bs_free	= generic_bs_free,
+	
+	/* barrier */
+	.bs_barrier	= generic_bs_barrier,
+	
+	/* read (single) */
+	.bs_r_1		= generic_bs_r_1,
+	.bs_r_2		= generic_armv4_bs_r_2,
+	.bs_r_4		= generic_bs_r_4,
+	.bs_r_8		= NULL,
+	
+	/* read multiple */
+	.bs_rm_1	= generic_bs_rm_1,
+	.bs_rm_2	= generic_armv4_bs_rm_2,
+	.bs_rm_4	= generic_bs_rm_4,
+	.bs_rm_8	= NULL,
+	
+	/* read region */
+	.bs_rr_1	= generic_bs_rr_1,
+	.bs_rr_2	= generic_armv4_bs_rr_2,
+	.bs_rr_4	= generic_bs_rr_4,
+	.bs_rr_8	= NULL,
+	
+	/* write (single) */
+	.bs_w_1		= generic_bs_w_1,
+	.bs_w_2		= generic_armv4_bs_w_2,
+	.bs_w_4		= generic_bs_w_4,
+	.bs_w_8		= NULL,
+	
+	/* write multiple */
+	.bs_wm_1	= generic_bs_wm_1,
+	.bs_wm_2	= generic_armv4_bs_wm_2,
+	.bs_wm_4	= generic_bs_wm_4,
+	.bs_wm_8	= NULL,
+	
+	/* write region */
+	.bs_wr_1	= generic_bs_wr_1,
+	.bs_wr_2	= generic_armv4_bs_wr_2,
+	.bs_wr_4	= generic_bs_wr_4,
+	.bs_wr_8	= NULL,
+	
+	/* set multiple */
+	/* XXX not implemented */
+	
+	/* set region */
+	.bs_sr_1	= NULL,
+	.bs_sr_2	= generic_armv4_bs_sr_2,
+	.bs_sr_4	= generic_bs_sr_4,
+	.bs_sr_8	= NULL,
+	
+	/* copy */
+	.bs_c_1		= NULL,
+	.bs_c_2		= generic_armv4_bs_c_2,
+	.bs_c_4		= NULL,
+	.bs_c_8		= NULL,
+};
+
+bus_space_tag_t fdtbus_bs_tag = &_base_tag;
diff --git a/sys/arm/versatile/common.c b/sys/arm/versatile/common.c
new file mode 100644
index 0000000..766db9d
--- /dev/null
+++ b/sys/arm/versatile/common.c
@@ -0,0 +1,75 @@
+/*-
+ * Copyright (C) 2008-2011 MARVELL INTERNATIONAL LTD.
+ * All rights reserved.
+ *
+ * Developed by Semihalf.
+ *
+ * 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.
+ * 3. Neither the name of MARVELL nor the names of contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include "opt_global.h"
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+#include <sys/kdb.h>
+#include <sys/reboot.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+
+#include <machine/bus.h>
+#include <machine/fdt.h>
+#include <machine/vmparam.h>
+
+struct fdt_fixup_entry fdt_fixup_table[] = {
+	{ NULL, NULL }
+};
+
+static int
+fdt_intc_decode_ic(phandle_t node, pcell_t *intr, int *interrupt, int *trig,
+    int *pol)
+{
+
+	if (!fdt_is_compatible(node, "arm,versatile-vic"))
+		return (ENXIO);
+
+	*interrupt = fdt32_to_cpu(intr[0]);
+	*trig = INTR_TRIGGER_CONFORM;
+	*pol = INTR_POLARITY_CONFORM;
+
+	return (0);
+}
+
+
+fdt_pic_decode_t fdt_pic_table[] = {
+	&fdt_intc_decode_ic,
+	NULL
+};
diff --git a/sys/arm/versatile/files.versatile b/sys/arm/versatile/files.versatile
new file mode 100644
index 0000000..0a00298
--- /dev/null
+++ b/sys/arm/versatile/files.versatile
@@ -0,0 +1,22 @@
+# $FreeBSD$
+
+arm/arm/bus_space_asm_generic.S			standard
+arm/arm/bus_space_generic.c                     standard
+arm/arm/cpufunc_asm_arm11.S                     standard
+arm/arm/cpufunc_asm_armv5.S                     standard
+arm/arm/cpufunc_asm_armv6.S                     standard
+arm/arm/irq_dispatch.S                          standard
+
+arm/versatile/bus_space.c			standard
+arm/versatile/common.c				standard
+arm/versatile/pl050.c				optional sc
+arm/versatile/sp804.c				standard
+arm/versatile/versatile_machdep.c		standard
+arm/versatile/versatile_clcd.c			optional sc
+arm/versatile/versatile_pci.c			optional pci
+arm/versatile/versatile_pci_bus_space.c		optional pci
+arm/versatile/versatile_sic.c			standard
+arm/versatile/versatile_timer.c			standard
+arm/versatile/if_smc_fdt.c			optional smc
+
+kern/kern_clocksource.c                         standard
diff --git a/sys/arm/versatile/if_smc_fdt.c b/sys/arm/versatile/if_smc_fdt.c
new file mode 100644
index 0000000..f61f18a
--- /dev/null
+++ b/sys/arm/versatile/if_smc_fdt.c
@@ -0,0 +1,131 @@
+/*-
+ * Copyright (c) 2008 Benno Rice
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/kernel.h>
+#include <sys/socket.h>
+
+#include <sys/module.h>
+#include <sys/bus.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+
+#include <net/ethernet.h>
+#include <net/if.h>
+#include <net/if_arp.h>
+#include <net/if_media.h>
+
+#include <dev/smc/if_smcvar.h>
+
+#include <dev/mii/mii.h>
+#include <dev/mii/miivar.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include "miibus_if.h"
+
+static int		smc_fdt_probe(device_t);
+static int		smc_fdt_attach(device_t);
+static int		smc_fdt_detach(device_t);
+
+static int
+smc_fdt_probe(device_t dev)
+{
+	struct	smc_softc *sc;
+
+	if (ofw_bus_is_compatible(dev, "smsc,lan91c111")) {
+		sc = device_get_softc(dev);
+		sc->smc_usemem = 1;
+
+		if (smc_probe(dev) != 0) {
+			return (ENXIO);
+		}
+
+		return (0);
+	}
+
+	return (ENXIO);
+}
+
+static int
+smc_fdt_attach(device_t dev)
+{
+	int	err;
+ 	struct	smc_softc *sc;
+
+	sc = device_get_softc(dev);
+
+	err = smc_attach(dev);
+	if (err) {
+		return (err);
+	}
+
+	return (0);
+}
+
+static int
+smc_fdt_detach(device_t dev)
+{
+
+	smc_detach(dev);
+
+	return (0);
+}
+
+static device_method_t smc_fdt_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		smc_fdt_probe),
+	DEVMETHOD(device_attach,	smc_fdt_attach),
+	DEVMETHOD(device_detach,	smc_fdt_detach),
+
+	/* MII interface */
+	DEVMETHOD(miibus_readreg,	smc_miibus_readreg),
+	DEVMETHOD(miibus_writereg,	smc_miibus_writereg),
+	DEVMETHOD(miibus_statchg,	smc_miibus_statchg),
+
+	{ 0, 0 }
+};
+
+static driver_t smc_fdt_driver = {
+	"smc",
+	smc_fdt_methods,
+	sizeof(struct smc_softc),
+};
+
+extern devclass_t smc_devclass;
+
+DRIVER_MODULE(smc, simplebus, smc_fdt_driver, smc_devclass, 0, 0);
+DRIVER_MODULE(miibus, smc, miibus_driver, miibus_devclass, 0, 0);
+MODULE_DEPEND(smc, fdt, 1, 1, 1);
+MODULE_DEPEND(smc, ether, 1, 1, 1);
+MODULE_DEPEND(smc, miibus, 1, 1, 1);
diff --git a/sys/arm/versatile/pl050.c b/sys/arm/versatile/pl050.c
new file mode 100644
index 0000000..f6662b1
--- /dev/null
+++ b/sys/arm/versatile/pl050.c
@@ -0,0 +1,713 @@
+/*
+ * Copyright (c) 2012 Oleksandr Tymoshenko <gonzo@freebsd.org>
+ * All rights reserved.
+ *
+ * Based on dev/usb/input/ukbd.c  
+ *
+ * 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/malloc.h>
+#include <sys/rman.h>
+#include <sys/proc.h>
+#include <sys/sched.h>
+#include <sys/kdb.h>
+
+#include <machine/bus.h>
+#include <machine/cpu.h>
+#include <machine/frame.h>
+#include <machine/intr.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <sys/ioccom.h>
+#include <sys/filio.h>
+#include <sys/tty.h>
+#include <sys/kbio.h>
+
+#include <dev/kbd/kbdreg.h>
+
+#include <machine/bus.h>
+#include <machine/fdt.h>
+
+#include <dev/kbd/kbdtables.h>
+
+#define	KMI_LOCK()	mtx_lock(&Giant)
+#define	KMI_UNLOCK()	mtx_unlock(&Giant)
+
+#ifdef	INVARIANTS
+/*
+ * Assert that the lock is held in all contexts
+ * where the code can be executed.
+ */
+#define	KMI_LOCK_ASSERT()	mtx_assert(&Giant, MA_OWNED)
+/*
+ * Assert that the lock is held in the contexts
+ * where it really has to be so.
+ */
+#define	KMI_CTX_LOCK_ASSERT()			 	\
+	do {						\
+		if (!kdb_active && panicstr == NULL)	\
+			mtx_assert(&Giant, MA_OWNED);	\
+	} while (0)
+#else
+#define KMI_LOCK_ASSERT()	(void)0
+#define KMI_CTX_LOCK_ASSERT()	(void)0
+#endif
+
+#define	KMICR		0x00
+#define		KMICR_TYPE_NONPS2	(1 << 5)
+#define		KMICR_RXINTREN		(1 << 4)
+#define		KMICR_TXINTREN		(1 << 3)
+#define		KMICR_EN		(1 << 2)
+#define		KMICR_FKMID		(1 << 1)
+#define		KMICR_FKMIC		(1 << 0)
+#define	KMISTAT		0x04
+#define		KMISTAT_TXEMPTY		(1 << 6)
+#define		KMISTAT_TXBUSY		(1 << 5)
+#define		KMISTAT_RXFULL		(1 << 4)
+#define		KMISTAT_RXBUSY		(1 << 3)
+#define		KMISTAT_RXPARITY	(1 << 2)
+#define		KMISTAT_KMIC		(1 << 1)
+#define		KMISTAT_KMID		(1 << 0)
+#define	KMIDATA		0x08
+#define	KMICLKDIV	0x0C
+#define	KMIIR		0x10
+#define		KMIIR_TXINTR		(1 << 1)
+#define		KMIIR_RXINTR		(1 << 0)
+
+#define	KMI_DRIVER_NAME          "kmi"
+#define	KMI_NFKEY        (sizeof(fkey_tab)/sizeof(fkey_tab[0]))	/* units */
+
+struct kmi_softc {
+	keyboard_t sc_kbd;
+	keymap_t sc_keymap;
+	accentmap_t sc_accmap;
+	fkeytab_t sc_fkeymap[KMI_NFKEY];
+
+	struct resource*	sc_mem_res;
+	struct resource*	sc_irq_res;
+	void*			sc_intr_hl;
+
+	int			sc_mode;		/* input mode (K_XLATE,K_RAW,K_CODE) */
+	int			sc_state;		/* shift/lock key state */
+	int			sc_accents;		/* accent key index (> 0) */
+	uint32_t		sc_flags;		/* flags */
+#define	KMI_FLAG_COMPOSE	0x00000001
+#define	KMI_FLAG_POLLING	0x00000002
+
+	struct			thread *sc_poll_thread;
+};
+
+/* Read/Write macros for Timer used as timecounter */
+#define pl050_kmi_read_4(sc, reg)		\
+	bus_read_4((sc)->sc_mem_res, (reg))
+
+#define pl050_kmi_write_4(sc, reg, val)	\
+	bus_write_4((sc)->sc_mem_res, (reg), (val))
+
+/* prototypes */
+static void	kmi_set_leds(struct kmi_softc *, uint8_t);
+static int	kmi_set_typematic(keyboard_t *, int);
+static uint32_t	kmi_read_char(keyboard_t *, int);
+static void	kmi_clear_state(keyboard_t *);
+static int	kmi_ioctl(keyboard_t *, u_long, caddr_t);
+static int	kmi_enable(keyboard_t *);
+static int	kmi_disable(keyboard_t *);
+
+/* early keyboard probe, not supported */
+static int
+kmi_configure(int flags)
+{
+	return (0);
+}
+
+/* detect a keyboard, not used */
+static int
+kmi_probe(int unit, void *arg, int flags)
+{
+	return (ENXIO);
+}
+
+/* reset and initialize the device, not used */
+static int
+kmi_init(int unit, keyboard_t **kbdp, void *arg, int flags)
+{
+	return (ENXIO);
+}
+
+/* test the interface to the device, not used */
+static int
+kmi_test_if(keyboard_t *kbd)
+{
+	return (0);
+}
+
+/* finish using this keyboard, not used */
+static int
+kmi_term(keyboard_t *kbd)
+{
+	return (ENXIO);
+}
+
+/* keyboard interrupt routine, not used */
+static int
+kmi_intr(keyboard_t *kbd, void *arg)
+{
+
+	return (0);
+}
+
+/* lock the access to the keyboard, not used */
+static int
+kmi_lock(keyboard_t *kbd, int lock)
+{
+	return (1);
+}
+
+/*
+ * Enable the access to the device; until this function is called,
+ * the client cannot read from the keyboard.
+ */
+static int
+kmi_enable(keyboard_t *kbd)
+{
+
+	KMI_LOCK();
+	KBD_ACTIVATE(kbd);
+	KMI_UNLOCK();
+
+	return (0);
+}
+
+/* disallow the access to the device */
+static int
+kmi_disable(keyboard_t *kbd)
+{
+
+	KMI_LOCK();
+	KBD_DEACTIVATE(kbd);
+	KMI_UNLOCK();
+
+	return (0);
+}
+
+/* check if data is waiting */
+static int
+kmi_check(keyboard_t *kbd)
+{
+	struct kmi_softc *sc = kbd->kb_data;
+	uint32_t reg;
+
+	KMI_CTX_LOCK_ASSERT();
+
+	if (!KBD_IS_ACTIVE(kbd))
+		return (0);
+
+	reg = pl050_kmi_read_4(sc, KMIIR);
+	return (reg & KMIIR_RXINTR);
+}
+
+/* check if char is waiting */
+static int
+kmi_check_char_locked(keyboard_t *kbd)
+{
+	KMI_CTX_LOCK_ASSERT();
+
+	if (!KBD_IS_ACTIVE(kbd))
+		return (0);
+
+	return (kmi_check(kbd));
+}
+
+static int
+kmi_check_char(keyboard_t *kbd)
+{
+	int result;
+
+	KMI_LOCK();
+	result = kmi_check_char_locked(kbd);
+	KMI_UNLOCK();
+
+	return (result);
+}
+
+/* read one byte from the keyboard if it's allowed */
+/* Currently unused. */
+static int
+kmi_read(keyboard_t *kbd, int wait)
+{
+	KMI_CTX_LOCK_ASSERT();
+
+	if (!KBD_IS_ACTIVE(kbd))
+		return (-1);
+
+	++(kbd->kb_count);
+	printf("Implement ME: %s\n", __func__);
+	return (0);
+}
+
+/* read char from the keyboard */
+static uint32_t
+kmi_read_char_locked(keyboard_t *kbd, int wait)
+{
+	struct kmi_softc *sc = kbd->kb_data;
+	uint32_t reg, data;
+
+	KMI_CTX_LOCK_ASSERT();
+
+	if (!KBD_IS_ACTIVE(kbd))
+		return (NOKEY);
+
+	reg = pl050_kmi_read_4(sc, KMIIR);
+	if (reg & KMIIR_RXINTR) {
+		data = pl050_kmi_read_4(sc, KMIDATA);
+		return (data);
+	}
+
+	++kbd->kb_count;
+	return (NOKEY);
+}
+
+/* Currently wait is always false. */
+static uint32_t
+kmi_read_char(keyboard_t *kbd, int wait)
+{
+	uint32_t keycode;
+
+	KMI_LOCK();
+	keycode = kmi_read_char_locked(kbd, wait);
+	KMI_UNLOCK();
+
+	return (keycode);
+}
+
+/* some useful control functions */
+static int
+kmi_ioctl_locked(keyboard_t *kbd, u_long cmd, caddr_t arg)
+{
+	struct kmi_softc *sc = kbd->kb_data;
+	int i;
+#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
+    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
+	int ival;
+
+#endif
+
+	KMI_LOCK_ASSERT();
+
+	switch (cmd) {
+	case KDGKBMODE:		/* get keyboard mode */
+		*(int *)arg = sc->sc_mode;
+		break;
+#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
+    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
+	case _IO('K', 7):
+		ival = IOCPARM_IVAL(arg);
+		arg = (caddr_t)&ival;
+		/* FALLTHROUGH */
+#endif
+	case KDSKBMODE:		/* set keyboard mode */
+		switch (*(int *)arg) {
+		case K_XLATE:
+			if (sc->sc_mode != K_XLATE) {
+				/* make lock key state and LED state match */
+				sc->sc_state &= ~LOCK_MASK;
+				sc->sc_state |= KBD_LED_VAL(kbd);
+			}
+			/* FALLTHROUGH */
+		case K_RAW:
+		case K_CODE:
+			if (sc->sc_mode != *(int *)arg) {
+				if ((sc->sc_flags & KMI_FLAG_POLLING) == 0)
+					kmi_clear_state(kbd);
+				sc->sc_mode = *(int *)arg;
+			}
+			break;
+		default:
+			return (EINVAL);
+		}
+		break;
+
+	case KDGETLED:			/* get keyboard LED */
+		*(int *)arg = KBD_LED_VAL(kbd);
+		break;
+#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
+    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
+	case _IO('K', 66):
+		ival = IOCPARM_IVAL(arg);
+		arg = (caddr_t)&ival;
+		/* FALLTHROUGH */
+#endif
+	case KDSETLED:			/* set keyboard LED */
+		/* NOTE: lock key state in "sc_state" won't be changed */
+		if (*(int *)arg & ~LOCK_MASK)
+			return (EINVAL);
+
+		i = *(int *)arg;
+
+		/* replace CAPS LED with ALTGR LED for ALTGR keyboards */
+		if (sc->sc_mode == K_XLATE &&
+		    kbd->kb_keymap->n_keys > ALTGR_OFFSET) {
+			if (i & ALKED)
+				i |= CLKED;
+			else
+				i &= ~CLKED;
+		}
+		if (KBD_HAS_DEVICE(kbd))
+			kmi_set_leds(sc, i);
+
+		KBD_LED_VAL(kbd) = *(int *)arg;
+		break;
+	case KDGKBSTATE:		/* get lock key state */
+		*(int *)arg = sc->sc_state & LOCK_MASK;
+		break;
+#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
+    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
+	case _IO('K', 20):
+		ival = IOCPARM_IVAL(arg);
+		arg = (caddr_t)&ival;
+		/* FALLTHROUGH */
+#endif
+	case KDSKBSTATE:		/* set lock key state */
+		if (*(int *)arg & ~LOCK_MASK) {
+			return (EINVAL);
+		}
+		sc->sc_state &= ~LOCK_MASK;
+		sc->sc_state |= *(int *)arg;
+
+		/* set LEDs and quit */
+		return (kmi_ioctl(kbd, KDSETLED, arg));
+
+	case KDSETREPEAT:		/* set keyboard repeat rate (new
+					 * interface) */
+		if (!KBD_HAS_DEVICE(kbd)) {
+			return (0);
+		}
+		if (((int *)arg)[1] < 0) {
+			return (EINVAL);
+		}
+		if (((int *)arg)[0] < 0) {
+			return (EINVAL);
+		}
+		if (((int *)arg)[0] < 200)	/* fastest possible value */
+			kbd->kb_delay1 = 200;
+		else
+			kbd->kb_delay1 = ((int *)arg)[0];
+		kbd->kb_delay2 = ((int *)arg)[1];
+		return (0);
+
+#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
+    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
+	case _IO('K', 67):
+		ival = IOCPARM_IVAL(arg);
+		arg = (caddr_t)&ival;
+		/* FALLTHROUGH */
+#endif
+	case KDSETRAD:			/* set keyboard repeat rate (old
+					 * interface) */
+		return (kmi_set_typematic(kbd, *(int *)arg));
+
+	case PIO_KEYMAP:		/* set keyboard translation table */
+	case OPIO_KEYMAP:		/* set keyboard translation table
+					 * (compat) */
+	case PIO_KEYMAPENT:		/* set keyboard translation table
+					 * entry */
+	case PIO_DEADKEYMAP:		/* set accent key translation table */
+		sc->sc_accents = 0;
+		/* FALLTHROUGH */
+	default:
+		return (genkbd_commonioctl(kbd, cmd, arg));
+	}
+
+	return (0);
+}
+
+static int
+kmi_ioctl(keyboard_t *kbd, u_long cmd, caddr_t arg)
+{
+	int result;
+
+	/*
+	 * XXX KDGKBSTATE, KDSKBSTATE and KDSETLED can be called from any
+	 * context where printf(9) can be called, which among other things
+	 * includes interrupt filters and threads with any kinds of locks
+	 * already held.  For this reason it would be dangerous to acquire
+	 * the Giant here unconditionally.  On the other hand we have to
+	 * have it to handle the ioctl.
+	 * So we make our best effort to auto-detect whether we can grab
+	 * the Giant or not.  Blame syscons(4) for this.
+	 */
+	switch (cmd) {
+	case KDGKBSTATE:
+	case KDSKBSTATE:
+	case KDSETLED:
+		if (!mtx_owned(&Giant) && !SCHEDULER_STOPPED())
+			return (EDEADLK);	/* best I could come up with */
+		/* FALLTHROUGH */
+	default:
+		KMI_LOCK();
+		result = kmi_ioctl_locked(kbd, cmd, arg);
+		KMI_UNLOCK();
+		return (result);
+	}
+}
+
+
+/* clear the internal state of the keyboard */
+static void
+kmi_clear_state(keyboard_t *kbd)
+{
+	struct kmi_softc *sc = kbd->kb_data;
+
+	KMI_CTX_LOCK_ASSERT();
+
+	sc->sc_flags &= ~(KMI_FLAG_COMPOSE | KMI_FLAG_POLLING);
+	sc->sc_state &= LOCK_MASK;	/* preserve locking key state */
+	sc->sc_accents = 0;
+}
+
+/* save the internal state, not used */
+static int
+kmi_get_state(keyboard_t *kbd, void *buf, size_t len)
+{
+	return (len == 0) ? 1 : -1;
+}
+
+/* set the internal state, not used */
+static int
+kmi_set_state(keyboard_t *kbd, void *buf, size_t len)
+{
+	return (EINVAL);
+}
+
+static int
+kmi_poll(keyboard_t *kbd, int on)
+{
+	struct kmi_softc *sc = kbd->kb_data;
+
+	KMI_LOCK();
+	if (on) {
+		sc->sc_flags |= KMI_FLAG_POLLING;
+		sc->sc_poll_thread = curthread;
+	} else {
+		sc->sc_flags &= ~KMI_FLAG_POLLING;
+	}
+	KMI_UNLOCK();
+
+	return (0);
+}
+
+/* local functions */
+
+static void
+kmi_set_leds(struct kmi_softc *sc, uint8_t leds)
+{
+
+	KMI_LOCK_ASSERT();
+
+	/* start transfer, if not already started */
+	printf("Implement me: %s\n", __func__);
+}
+
+static int
+kmi_set_typematic(keyboard_t *kbd, int code)
+{
+	static const int delays[] = {250, 500, 750, 1000};
+	static const int rates[] = {34, 38, 42, 46, 50, 55, 59, 63,
+		68, 76, 84, 92, 100, 110, 118, 126,
+		136, 152, 168, 184, 200, 220, 236, 252,
+	272, 304, 336, 368, 400, 440, 472, 504};
+
+	if (code & ~0x7f) {
+		return (EINVAL);
+	}
+	kbd->kb_delay1 = delays[(code >> 5) & 3];
+	kbd->kb_delay2 = rates[code & 0x1f];
+	return (0);
+}
+
+static keyboard_switch_t kmisw = {
+	.probe = &kmi_probe,
+	.init = &kmi_init,
+	.term = &kmi_term,
+	.intr = &kmi_intr,
+	.test_if = &kmi_test_if,
+	.enable = &kmi_enable,
+	.disable = &kmi_disable,
+	.read = &kmi_read,
+	.check = &kmi_check,
+	.read_char = &kmi_read_char,
+	.check_char = &kmi_check_char,
+	.ioctl = &kmi_ioctl,
+	.lock = &kmi_lock,
+	.clear_state = &kmi_clear_state,
+	.get_state = &kmi_get_state,
+	.set_state = &kmi_set_state,
+	.get_fkeystr = &genkbd_get_fkeystr,
+	.poll = &kmi_poll,
+	.diag = &genkbd_diag,
+};
+
+KEYBOARD_DRIVER(kmi, kmisw, kmi_configure);
+
+static void
+pl050_kmi_intr(void *arg)
+{
+	struct kmi_softc *sc = arg;
+	uint32_t c;
+
+	KMI_CTX_LOCK_ASSERT();
+
+	if ((sc->sc_flags & KMI_FLAG_POLLING) != 0)
+		return;
+
+	if (KBD_IS_ACTIVE(&sc->sc_kbd) &&
+	    KBD_IS_BUSY(&sc->sc_kbd)) {
+		/* let the callback function process the input */
+		(sc->sc_kbd.kb_callback.kc_func) (&sc->sc_kbd, KBDIO_KEYINPUT,
+		    sc->sc_kbd.kb_callback.kc_arg);
+	} else {
+		/* read and discard the input, no one is waiting for it */
+		do {
+			c = kmi_read_char_locked(&sc->sc_kbd, 0);
+		} while (c != NOKEY);
+	}
+
+}
+
+static int
+pl050_kmi_probe(device_t dev)
+{
+
+	if (ofw_bus_is_compatible(dev, "arm,pl050")) {
+		device_set_desc(dev, "PL050 Keyboard/Mouse Interface");
+		return (BUS_PROBE_DEFAULT);
+	}
+
+	return (ENXIO);
+}
+
+static int
+pl050_kmi_attach(device_t dev)
+{
+	struct kmi_softc *sc = device_get_softc(dev);
+	keyboard_t *kbd;
+	int rid;
+	int i;
+
+	kbd = &sc->sc_kbd;
+	rid = 0;
+
+	sc->sc_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid, RF_ACTIVE);
+	if (sc->sc_mem_res == NULL) {
+		device_printf(dev, "could not allocate memory resource\n");
+		return (ENXIO);
+	}
+
+	/* Request the IRQ resources */
+	sc->sc_irq_res =  bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid, RF_ACTIVE);
+	if (sc->sc_irq_res == NULL) {
+		device_printf(dev, "Error: could not allocate irq resources\n");
+		return (ENXIO);
+	}
+
+	/* Setup and enable the timer */
+	if (bus_setup_intr(dev, sc->sc_irq_res, INTR_TYPE_CLK,
+			NULL, pl050_kmi_intr, sc,
+			&sc->sc_intr_hl) != 0) {
+		bus_release_resource(dev, SYS_RES_IRQ, rid,
+			sc->sc_irq_res);
+		device_printf(dev, "Unable to setup the clock irq handler.\n");
+		return (ENXIO);
+	}
+
+	/* TODO: clock & divisor */
+
+	pl050_kmi_write_4(sc, KMICR, KMICR_EN | KMICR_RXINTREN);
+
+	kbd_init_struct(kbd, KMI_DRIVER_NAME, KB_OTHER, 
+			device_get_unit(dev), 0, 0, 0);
+	kbd->kb_data = (void *)sc;
+
+	sc->sc_keymap = key_map;
+	sc->sc_accmap = accent_map;
+	for (i = 0; i < KMI_NFKEY; i++) {
+		sc->sc_fkeymap[i] = fkey_tab[i];
+	}
+
+	kbd_set_maps(kbd, &sc->sc_keymap, &sc->sc_accmap,
+	    sc->sc_fkeymap, KMI_NFKEY);
+
+	KBD_FOUND_DEVICE(kbd);
+	kmi_clear_state(kbd);
+	KBD_PROBE_DONE(kbd);
+
+	KBD_INIT_DONE(kbd);
+
+	if (kbd_register(kbd) < 0) {
+		goto detach;
+	}
+	KBD_CONFIG_DONE(kbd);
+
+#ifdef KBD_INSTALL_CDEV
+	if (kbd_attach(kbd)) {
+		goto detach;
+	}
+#endif
+
+	if (bootverbose) {
+		genkbd_diag(kbd, bootverbose);
+	}
+	return (0);
+
+detach:
+	return (ENXIO);
+
+}
+
+static device_method_t pl050_kmi_methods[] = {
+	DEVMETHOD(device_probe,		pl050_kmi_probe),
+	DEVMETHOD(device_attach,	pl050_kmi_attach),
+	{ 0, 0 }
+};
+
+static driver_t pl050_kmi_driver = {
+	"kmi",
+	pl050_kmi_methods,
+	sizeof(struct kmi_softc),
+};
+
+static devclass_t pl050_kmi_devclass;
+
+DRIVER_MODULE(pl050_kmi, simplebus, pl050_kmi_driver, pl050_kmi_devclass, 0, 0);
diff --git a/sys/arm/versatile/sp804.c b/sys/arm/versatile/sp804.c
new file mode 100644
index 0000000..82a1889
--- /dev/null
+++ b/sys/arm/versatile/sp804.c
@@ -0,0 +1,352 @@
+/*
+ * Copyright (c) 2012 Oleksandr Tymoshenko <gonzo@freebsd.org>
+ * Copyright (c) 2012 Damjan Marion <dmarion@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/timeet.h>
+#include <sys/timetc.h>
+#include <sys/watchdog.h>
+#include <machine/bus.h>
+#include <machine/cpu.h>
+#include <machine/frame.h>
+#include <machine/intr.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <machine/bus.h>
+#include <machine/fdt.h>
+
+#define	SP804_TIMER1_LOAD	0x00
+#define	SP804_TIMER1_VALUE	0x04
+#define	SP804_TIMER1_CONTROL	0x08
+#define		TIMER_CONTROL_EN	(1 << 7)
+#define		TIMER_CONTROL_FREERUN	(0 << 6)
+#define		TIMER_CONTROL_PERIODIC	(1 << 6)
+#define		TIMER_CONTROL_INTREN	(1 << 5)
+#define		TIMER_CONTROL_DIV1	(0 << 2)
+#define		TIMER_CONTROL_DIV16	(1 << 2)
+#define		TIMER_CONTROL_DIV256	(2 << 2)
+#define		TIMER_CONTROL_32BIT	(1 << 1)
+#define		TIMER_CONTROL_ONESHOT	(1 << 0)
+#define	SP804_TIMER1_INTCLR	0x0C
+#define	SP804_TIMER1_RIS	0x10
+#define	SP804_TIMER1_MIS	0x14
+#define	SP804_TIMER1_BGLOAD	0x18
+#define	SP804_TIMER2_LOAD	0x20
+#define	SP804_TIMER2_VALUE	0x24
+#define	SP804_TIMER2_CONTROL	0x28
+#define	SP804_TIMER2_INTCLR	0x2C
+#define	SP804_TIMER2_RIS	0x30
+#define	SP804_TIMER2_MIS	0x34
+#define	SP804_TIMER2_BGLOAD	0x38
+
+#define	SP804_PERIPH_ID0	0xFE0
+#define	SP804_PERIPH_ID1	0xFE4
+#define	SP804_PERIPH_ID2	0xFE8
+#define	SP804_PERIPH_ID3	0xFEC
+#define	SP804_PRIMECELL_ID0	0xFF0
+#define	SP804_PRIMECELL_ID1	0xFF4
+#define	SP804_PRIMECELL_ID2	0xFF8
+#define	SP804_PRIMECELL_ID3	0xFFC
+
+#define	DEFAULT_FREQUENCY	1000000
+/*
+ * QEMU seems to have problem with full frequency
+ */
+#define	DEFAULT_DIVISOR		16
+#define	DEFAULT_CONTROL_DIV	TIMER_CONTROL_DIV16
+
+struct sp804_timer_softc {
+	struct resource*	mem_res;
+	struct resource*	irq_res;
+	void*			intr_hl;
+	uint32_t		sysclk_freq;
+	bus_space_tag_t		bst;
+	bus_space_handle_t	bsh;
+	struct timecounter	tc;
+	bool			et_enabled;
+	struct eventtimer	et;
+};
+
+/* Read/Write macros for Timer used as timecounter */
+#define sp804_timer_tc_read_4(reg)		\
+	bus_space_read_4(sc->bst, sc->bsh, reg)
+
+#define sp804_timer_tc_write_4(reg, val)	\
+	bus_space_write_4(sc->bst, sc->bsh, reg, val)
+
+static unsigned sp804_timer_tc_get_timecount(struct timecounter *);
+
+static unsigned
+sp804_timer_tc_get_timecount(struct timecounter *tc)
+{
+	struct sp804_timer_softc *sc = tc->tc_priv;
+	return 0xffffffff - sp804_timer_tc_read_4(SP804_TIMER1_VALUE);
+}
+
+static int
+sp804_timer_start(struct eventtimer *et, struct bintime *first,
+              struct bintime *period)
+{
+	struct sp804_timer_softc *sc = et->et_priv;
+	uint32_t count, reg;
+
+	if (first != NULL) {
+		sc->et_enabled = 1;
+
+		count = (sc->et.et_frequency * (first->frac >> 32)) >> 32;
+		if (first->sec != 0)
+			count += sc->et.et_frequency * first->sec;
+
+		sp804_timer_tc_write_4(SP804_TIMER2_LOAD, count);
+		reg = TIMER_CONTROL_32BIT | TIMER_CONTROL_INTREN |
+		    TIMER_CONTROL_PERIODIC | DEFAULT_CONTROL_DIV |
+		    TIMER_CONTROL_EN;
+		sp804_timer_tc_write_4(SP804_TIMER2_CONTROL, reg);
+
+		return (0);
+	} 
+
+	if (period != NULL) {
+		panic("period");
+	}
+
+	return (EINVAL);
+}
+
+static int
+sp804_timer_stop(struct eventtimer *et)
+{
+	struct sp804_timer_softc *sc = et->et_priv;
+	uint32_t reg;
+
+	sc->et_enabled = 0;
+	reg = sp804_timer_tc_read_4(SP804_TIMER2_CONTROL);
+	reg &= ~(TIMER_CONTROL_EN);
+	sp804_timer_tc_write_4(SP804_TIMER2_CONTROL, reg);
+
+	return (0);
+}
+
+static int
+sp804_timer_intr(void *arg)
+{
+	struct sp804_timer_softc *sc = arg;
+	static uint32_t prev = 0;
+	uint32_t x = 0;
+
+	x = sp804_timer_tc_read_4(SP804_TIMER1_VALUE);
+
+	prev =x ;
+	sp804_timer_tc_write_4(SP804_TIMER2_INTCLR, 1);
+	if (sc->et_enabled) {
+		if (sc->et.et_active) {
+			sc->et.et_event_cb(&sc->et, sc->et.et_arg);
+		}
+	}
+
+	return (FILTER_HANDLED);
+}
+
+static int
+sp804_timer_probe(device_t dev)
+{
+
+	if (ofw_bus_is_compatible(dev, "arm,sp804")) {
+		device_set_desc(dev, "SP804 System Timer");
+		return (BUS_PROBE_DEFAULT);
+	}
+
+	return (ENXIO);
+}
+
+static int
+sp804_timer_attach(device_t dev)
+{
+	struct sp804_timer_softc *sc = device_get_softc(dev);
+	int rid = 0;
+	int i;
+	uint32_t id, reg;
+
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid, RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "could not allocate memory resource\n");
+		return (ENXIO);
+	}
+
+	sc->bst = rman_get_bustag(sc->mem_res);
+	sc->bsh = rman_get_bushandle(sc->mem_res);
+
+	/* Request the IRQ resources */
+	sc->irq_res =  bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid, RF_ACTIVE);
+	if (sc->irq_res == NULL) {
+		device_printf(dev, "Error: could not allocate irq resources\n");
+		return (ENXIO);
+	}
+
+	/* TODO: get frequency from FDT */
+	sc->sysclk_freq = DEFAULT_FREQUENCY;
+
+	/* Setup and enable the timer */
+	if (bus_setup_intr(dev, sc->irq_res, INTR_TYPE_CLK,
+			sp804_timer_intr, NULL, sc,
+			&sc->intr_hl) != 0) {
+		bus_release_resource(dev, SYS_RES_IRQ, rid,
+			sc->irq_res);
+		device_printf(dev, "Unable to setup the clock irq handler.\n");
+		return (ENXIO);
+	}
+
+	sp804_timer_tc_write_4(SP804_TIMER1_CONTROL, 0);
+	sp804_timer_tc_write_4(SP804_TIMER2_CONTROL, 0);
+
+	/*
+	 * Timer 1, timecounter
+	 */
+	sc->tc.tc_frequency = DEFAULT_FREQUENCY;
+	sc->tc.tc_name = "SP804 Timecouter";
+	sc->tc.tc_get_timecount = sp804_timer_tc_get_timecount;
+	sc->tc.tc_poll_pps = NULL;
+	sc->tc.tc_counter_mask = ~0u;
+	sc->tc.tc_quality = 1000;
+	sc->tc.tc_priv = sc;
+
+	sp804_timer_tc_write_4(SP804_TIMER1_VALUE, 0xffffffff);
+	sp804_timer_tc_write_4(SP804_TIMER1_LOAD, 0xffffffff);
+	reg = TIMER_CONTROL_PERIODIC | TIMER_CONTROL_32BIT;
+	sp804_timer_tc_write_4(SP804_TIMER1_CONTROL, reg);
+	reg |= TIMER_CONTROL_EN;
+	sp804_timer_tc_write_4(SP804_TIMER1_CONTROL, reg);
+	tc_init(&sc->tc);
+
+	/* 
+	 * Timer 2, event timer
+	 */
+	sc->et_enabled = 0;
+	sc->et.et_name = malloc(64, M_DEVBUF, M_NOWAIT | M_ZERO);
+	sprintf(sc->et.et_name, "SP804 Event Timer %d",
+		device_get_unit(dev));
+	sc->et.et_flags = ET_FLAGS_PERIODIC | ET_FLAGS_ONESHOT;
+	sc->et.et_quality = 1000;
+	sc->et.et_frequency = sc->sysclk_freq / DEFAULT_DIVISOR;
+	sc->et.et_min_period.sec = 0;
+	sc->et.et_min_period.frac =
+	    ((0x00000002LLU << 32) / sc->et.et_frequency) << 32;
+	sc->et.et_max_period.sec = 0xfffffff0U / sc->et.et_frequency;
+	sc->et.et_max_period.frac =
+	    ((0xfffffffeLLU << 32) / sc->et.et_frequency) << 32;
+	sc->et.et_start = sp804_timer_start;
+	sc->et.et_stop = sp804_timer_stop;
+	sc->et.et_priv = sc;
+	et_register(&sc->et);
+
+	id = 0;
+	for (i = 3; i >= 0; i--) {
+		id = (id << 8) | 
+		     (sp804_timer_tc_read_4(SP804_PERIPH_ID0 + i*4) & 0xff);
+	}
+
+	device_printf(dev, "peripheral ID: %08x\n", id);
+
+	id = 0;
+	for (i = 3; i >= 0; i--) {
+		id = (id << 8) | 
+		     (sp804_timer_tc_read_4(SP804_PRIMECELL_ID0 + i*4) & 0xff);
+	}
+
+	device_printf(dev, "PrimeCell ID: %08x\n", id);
+
+	return (0);
+}
+
+static device_method_t sp804_timer_methods[] = {
+	DEVMETHOD(device_probe,		sp804_timer_probe),
+	DEVMETHOD(device_attach,	sp804_timer_attach),
+	{ 0, 0 }
+};
+
+static driver_t sp804_timer_driver = {
+	"timer",
+	sp804_timer_methods,
+	sizeof(struct sp804_timer_softc),
+};
+
+static devclass_t sp804_timer_devclass;
+
+DRIVER_MODULE(sp804_timer, simplebus, sp804_timer_driver, sp804_timer_devclass, 0, 0);
+
+void
+DELAY(int usec)
+{
+	int32_t counts;
+	uint32_t first, last;
+	device_t timer_dev;
+	struct sp804_timer_softc *sc;
+
+	timer_dev = devclass_get_device(sp804_timer_devclass, 0);
+
+	if (timer_dev == NULL) {
+		/*
+		 * Timer is not initialized yet
+		 */
+		for (; usec > 0; usec--)
+			for (counts = 200; counts > 0; counts--)
+				/* Prevent gcc from optimizing  out the loop */
+				cpufunc_nullop();
+		return;
+	}
+
+       	sc = device_get_softc(timer_dev);
+
+	/* Get the number of times to count */
+	counts = usec * ((sc->tc.tc_frequency / 1000000) + 1);
+
+	first = sp804_timer_tc_get_timecount(&sc->tc);
+
+	while (counts > 0) {
+		last = sp804_timer_tc_get_timecount(&sc->tc);
+		if (last == first)
+			continue;
+		if (last>first) {
+			counts -= (int32_t)(last - first);
+		} else {
+			counts -= (int32_t)((0xFFFFFFFF - first) + last);
+		}
+		first = last;
+	}
+}
diff --git a/sys/arm/versatile/versatile_clcd.c b/sys/arm/versatile/versatile_clcd.c
new file mode 100644
index 0000000..9b1ceff
--- /dev/null
+++ b/sys/arm/versatile/versatile_clcd.c
@@ -0,0 +1,828 @@
+/*
+ * Copyright (c) 2012 Oleksandr Tymoshenko <gonzo@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/fbio.h>
+#include <sys/consio.h>
+#include <sys/kdb.h>
+
+#include <machine/bus.h>
+#include <machine/cpu.h>
+#include <machine/frame.h>
+#include <machine/intr.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <dev/fb/fbreg.h>
+#include <dev/syscons/syscons.h>
+
+#include <machine/bus.h>
+#include <machine/fdt.h>
+
+#define	PL110_VENDOR_ARM926PXP	1
+
+#define	MEM_SYS		0
+#define	MEM_CLCD	1
+#define MEM_REGIONS	2
+
+#define	SYS_CLCD		0x00
+#define		SYS_CLCD_CLCDID_SHIFT	0x08
+#define		SYS_CLCD_CLCDID_MASK	0x1f
+#define		SYS_CLCD_PWR3V5VSWITCH	(1 << 4)
+#define		SYS_CLCD_VDDPOSSWITCH 	(1 << 3)
+#define		SYS_CLCD_NLCDIOON  	(1 << 2)
+#define		SYS_CLCD_LCD_MODE_MASK	0x03
+
+#define	CLCD_MODE_RGB888	0x0
+#define	CLCD_MODE_RGB555	0x01
+#define	CLCD_MODE_RBG565	0x02
+#define	CLCD_MODE_RGB565	0x03
+
+#define	CLCDC_TIMING0		0x00
+#define	CLCDC_TIMING1		0x04
+#define	CLCDC_TIMING2		0x08
+#define	CLCDC_TIMING3		0x0C
+#define	CLCDC_TIMING3		0x0C
+#define	CLCDC_UPBASE		0x10
+#define	CLCDC_LPBASE		0x14
+#ifdef PL110_VENDOR_ARM926PXP
+#define	CLCDC_CONTROL		0x18
+#define	CLCDC_IMSC		0x1C
+#else
+#define	CLCDC_IMSC		0x18
+#define	CLCDC_CONTROL		0x1C
+#endif
+#define		CONTROL_WATERMARK	(1 << 16)
+#define		CONTROL_VCOMP_VS	(0 << 12)
+#define		CONTROL_VCOMP_BP	(1 << 12)
+#define		CONTROL_VCOMP_SAV	(2 << 12)
+#define		CONTROL_VCOMP_FP	(3 << 12)
+#define		CONTROL_PWR		(1 << 11)
+#define		CONTROL_BEPO		(1 << 10)
+#define		CONTROL_BEBO		(1 << 9)
+#define		CONTROL_BGR		(1 << 8)
+#define		CONTROL_DUAL		(1 << 7)
+#define		CONTROL_MONO8		(1 << 6)
+#define		CONTROL_TFT		(1 << 5)
+#define		CONTROL_BW		(1 << 4)
+#define		CONTROL_BPP1		(0x00 << 1)
+#define		CONTROL_BPP2		(0x01 << 1)
+#define		CONTROL_BPP4		(0x02 << 1)
+#define		CONTROL_BPP8		(0x03 << 1)
+#define		CONTROL_BPP16		(0x04 << 1)
+#define		CONTROL_BPP24		(0x05 << 1)
+#define		CONTROL_EN	(1 << 0)
+#define	CLCDC_RIS		0x20
+#define	CLCDC_MIS		0x24
+#define		INTR_MBERR		(1 << 4)
+#define		INTR_VCOMP		(1 << 3)
+#define		INTR_LNB		(1 << 2)
+#define		INTR_FUF		(1 << 1)
+#define	CLCDC_ICR		0x28
+
+#ifdef DEBUG
+#define dprintf(fmt, args...) do { printf("%s(): ", __func__);   \
+    printf(fmt,##args); } while (0)
+#else
+#define dprintf(fmt, args...)
+#endif
+
+#define	versatile_clcdc_sys_read_4(sc, reg)	\
+	bus_read_4((sc)->mem_res[MEM_SYS], (reg))
+#define	versatile_clcdc_sys_write_4(sc, reg, val)	\
+	bus_write_4((sc)->mem_res[MEM_SYS], (reg), (val))
+
+#define	versatile_clcdc_read_4(sc, reg)	\
+	bus_read_4((sc)->mem_res[MEM_CLCD], (reg))
+#define	versatile_clcdc_write_4(sc, reg, val)	\
+	bus_write_4((sc)->mem_res[MEM_CLCD], (reg), (val))
+
+struct versatile_clcdc_softc {
+	struct resource*	mem_res[MEM_REGIONS];
+
+	struct mtx		mtx;
+
+	int			width;
+	int			height;
+	int			mode;
+
+	bus_dma_tag_t		dma_tag;
+	bus_dmamap_t		dma_map;
+	bus_addr_t		fb_phys;
+	uint8_t			*fb_base;
+
+};
+
+struct video_adapter_softc {
+	/* Videoadpater part */
+	video_adapter_t	va;
+	int		console;
+
+	intptr_t	fb_addr;
+	unsigned int	fb_size;
+
+	unsigned int	height;
+	unsigned int	width;
+	unsigned int	depth;
+	unsigned int	stride;
+
+	unsigned int	xmargin;
+	unsigned int	ymargin;
+
+	unsigned char	*font;
+	int		initialized;
+};
+
+struct argb {
+	uint8_t		a;
+	uint8_t		r;
+	uint8_t		g;
+	uint8_t		b;
+};
+
+static struct argb versatilefb_palette[16] = {
+	{0x00, 0x00, 0x00, 0x00},
+	{0x00, 0x00, 0x00, 0xaa},
+	{0x00, 0x00, 0xaa, 0x00},
+	{0x00, 0x00, 0xaa, 0xaa},
+	{0x00, 0xaa, 0x00, 0x00},
+	{0x00, 0xaa, 0x00, 0xaa},
+	{0x00, 0xaa, 0x55, 0x00},
+	{0x00, 0xaa, 0xaa, 0xaa},
+	{0x00, 0x55, 0x55, 0x55},
+	{0x00, 0x55, 0x55, 0xff},
+	{0x00, 0x55, 0xff, 0x55},
+	{0x00, 0x55, 0xff, 0xff},
+	{0x00, 0xff, 0x55, 0x55},
+	{0x00, 0xff, 0x55, 0xff},
+	{0x00, 0xff, 0xff, 0x55},
+	{0x00, 0xff, 0xff, 0xff}
+};
+
+#define FB_WIDTH		640
+#define FB_HEIGHT		480
+#define FB_DEPTH		16
+
+#define	VERSATILE_FONT_HEIGHT	16
+
+static struct video_adapter_softc va_softc;
+
+static struct resource_spec versatile_clcdc_mem_spec[] = {
+	{ SYS_RES_MEMORY, 0, RF_ACTIVE },
+	{ SYS_RES_MEMORY, 1, RF_ACTIVE },
+	{ -1, 0, 0 }
+};
+
+static int versatilefb_configure(int);
+static void versatilefb_update_margins(video_adapter_t *adp);
+
+static void
+versatile_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 = segs[0].ds_addr;
+}
+
+static int
+versatile_clcdc_probe(device_t dev)
+{
+
+	if (ofw_bus_is_compatible(dev, "arm,pl110")) {
+		device_set_desc(dev, "PL110 CLCD controller");
+		return (BUS_PROBE_DEFAULT);
+	}
+
+	return (ENXIO);
+}
+
+static int
+versatile_clcdc_attach(device_t dev)
+{
+	struct versatile_clcdc_softc *sc = device_get_softc(dev);
+	struct video_adapter_softc *va_sc = &va_softc;
+	int err;
+	uint32_t reg;
+	int clcdid;
+	int dma_size;
+
+	/* Request memory resources */
+	err = bus_alloc_resources(dev, versatile_clcdc_mem_spec,
+		sc->mem_res);
+	if (err) {
+		device_printf(dev, "Error: could not allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	reg = versatile_clcdc_sys_read_4(sc, SYS_CLCD);
+	clcdid = (reg >> SYS_CLCD_CLCDID_SHIFT) & SYS_CLCD_CLCDID_MASK;
+	switch (clcdid) {
+		case 31:
+			device_printf(dev, "QEMU VGA 640x480\n");
+			sc->width = 640;
+			sc->height = 480;
+			break;
+		default:
+			device_printf(dev, "Unsupported: %d\n", clcdid);
+			goto fail;
+	}
+
+	reg &= ~SYS_CLCD_LCD_MODE_MASK;
+	reg |= CLCD_MODE_RGB565;
+	sc->mode = CLCD_MODE_RGB565;
+	versatile_clcdc_sys_write_4(sc, SYS_CLCD, reg);
+	dma_size = sc->width*sc->height*2;
+
+	/*
+	 * Power on LCD
+	 */
+	reg |= SYS_CLCD_PWR3V5VSWITCH | SYS_CLCD_NLCDIOON;
+	versatile_clcdc_sys_write_4(sc, SYS_CLCD, reg);
+
+	/*
+	 * XXX: hardcoded timing for VGA. For other modes/panels
+	 * we need to keep table of timing register values
+	 */
+	/*
+	 * XXX: set SYS_OSC1 
+	 */
+	versatile_clcdc_write_4(sc, CLCDC_TIMING0, 0x3F1F3F9C);
+	versatile_clcdc_write_4(sc, CLCDC_TIMING1, 0x090B61DF);
+	versatile_clcdc_write_4(sc, CLCDC_TIMING2, 0x067F1800);
+	/* XXX: timing 3? */
+
+	/*
+	 * Now allocate framebuffer memory
+	 */
+	err = bus_dma_tag_create(
+	    bus_get_dma_tag(dev),
+	    4, 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_base,
+	    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_base,
+	    dma_size, versatile_fb_dmamap_cb, &sc->fb_phys, BUS_DMA_NOWAIT);
+
+	if (err) {
+		device_printf(dev, "cannot load DMA map\n");
+		goto fail;
+	}
+
+	/* Make sure it's blank */
+	memset(sc->fb_base, 0x00, dma_size);
+
+	versatile_clcdc_write_4(sc, CLCDC_UPBASE, sc->fb_phys);
+
+	err = (sc_attach_unit(device_get_unit(dev),
+	    device_get_flags(dev) | SC_AUTODETECT_KBD));
+
+	if (err) {
+		device_printf(dev, "failed to attach syscons\n");
+		goto fail;
+	}
+
+	/*
+	 * XXX: hardcoded for VGA
+	 */
+	reg = CONTROL_VCOMP_BP | CONTROL_TFT | CONTROL_BGR | CONTROL_EN;
+	reg |= CONTROL_BPP16;
+	versatile_clcdc_write_4(sc, CLCDC_CONTROL, reg);
+	DELAY(20);
+	reg |= CONTROL_PWR;
+	versatile_clcdc_write_4(sc, CLCDC_CONTROL, reg);
+
+	va_sc->fb_addr = (vm_offset_t)sc->fb_base;
+	va_sc->fb_size = dma_size;
+	va_sc->width = sc->width;
+	va_sc->height = sc->height;
+	va_sc->depth = 16;
+	va_sc->stride = sc->width * 2;
+	versatilefb_update_margins(&va_sc->va);
+
+	return (0);
+
+fail:
+	if (sc->fb_base)
+		bus_dmamem_free(sc->dma_tag, sc->fb_base, sc->dma_map);
+	if (sc->dma_map)
+		bus_dmamap_destroy(sc->dma_tag, sc->dma_map);
+	if (sc->dma_tag)
+		bus_dma_tag_destroy(sc->dma_tag);
+	return (err);
+}
+
+static device_method_t versatile_clcdc_methods[] = {
+	DEVMETHOD(device_probe,		versatile_clcdc_probe),
+	DEVMETHOD(device_attach,	versatile_clcdc_attach),
+
+	DEVMETHOD_END
+};
+
+static driver_t versatile_clcdc_driver = {
+	"clcdc",
+	versatile_clcdc_methods,
+	sizeof(struct versatile_clcdc_softc),
+};
+
+static devclass_t versatile_clcdc_devclass;
+
+DRIVER_MODULE(versatile_clcdc, simplebus, versatile_clcdc_driver, versatile_clcdc_devclass, 0, 0);
+
+/*
+ * Video driver routines and glue.
+ */
+static vi_probe_t		versatilefb_probe;
+static vi_init_t		versatilefb_init;
+static vi_get_info_t		versatilefb_get_info;
+static vi_query_mode_t		versatilefb_query_mode;
+static vi_set_mode_t		versatilefb_set_mode;
+static vi_save_font_t		versatilefb_save_font;
+static vi_load_font_t		versatilefb_load_font;
+static vi_show_font_t		versatilefb_show_font;
+static vi_save_palette_t	versatilefb_save_palette;
+static vi_load_palette_t	versatilefb_load_palette;
+static vi_set_border_t		versatilefb_set_border;
+static vi_save_state_t		versatilefb_save_state;
+static vi_load_state_t		versatilefb_load_state;
+static vi_set_win_org_t		versatilefb_set_win_org;
+static vi_read_hw_cursor_t	versatilefb_read_hw_cursor;
+static vi_set_hw_cursor_t	versatilefb_set_hw_cursor;
+static vi_set_hw_cursor_shape_t	versatilefb_set_hw_cursor_shape;
+static vi_blank_display_t	versatilefb_blank_display;
+static vi_mmap_t		versatilefb_mmap;
+static vi_ioctl_t		versatilefb_ioctl;
+static vi_clear_t		versatilefb_clear;
+static vi_fill_rect_t		versatilefb_fill_rect;
+static vi_bitblt_t		versatilefb_bitblt;
+static vi_diag_t		versatilefb_diag;
+static vi_save_cursor_palette_t	versatilefb_save_cursor_palette;
+static vi_load_cursor_palette_t	versatilefb_load_cursor_palette;
+static vi_copy_t		versatilefb_copy;
+static vi_putp_t		versatilefb_putp;
+static vi_putc_t		versatilefb_putc;
+static vi_puts_t		versatilefb_puts;
+static vi_putm_t		versatilefb_putm;
+
+static video_switch_t versatilefbvidsw = {
+	.probe			= versatilefb_probe,
+	.init			= versatilefb_init,
+	.get_info		= versatilefb_get_info,
+	.query_mode		= versatilefb_query_mode,
+	.set_mode		= versatilefb_set_mode,
+	.save_font		= versatilefb_save_font,
+	.load_font		= versatilefb_load_font,
+	.show_font		= versatilefb_show_font,
+	.save_palette		= versatilefb_save_palette,
+	.load_palette		= versatilefb_load_palette,
+	.set_border		= versatilefb_set_border,
+	.save_state		= versatilefb_save_state,
+	.load_state		= versatilefb_load_state,
+	.set_win_org		= versatilefb_set_win_org,
+	.read_hw_cursor		= versatilefb_read_hw_cursor,
+	.set_hw_cursor		= versatilefb_set_hw_cursor,
+	.set_hw_cursor_shape	= versatilefb_set_hw_cursor_shape,
+	.blank_display		= versatilefb_blank_display,
+	.mmap			= versatilefb_mmap,
+	.ioctl			= versatilefb_ioctl,
+	.clear			= versatilefb_clear,
+	.fill_rect		= versatilefb_fill_rect,
+	.bitblt			= versatilefb_bitblt,
+	.diag			= versatilefb_diag,
+	.save_cursor_palette	= versatilefb_save_cursor_palette,
+	.load_cursor_palette	= versatilefb_load_cursor_palette,
+	.copy			= versatilefb_copy,
+	.putp			= versatilefb_putp,
+	.putc			= versatilefb_putc,
+	.puts			= versatilefb_puts,
+	.putm			= versatilefb_putm,
+};
+
+VIDEO_DRIVER(versatilefb, versatilefbvidsw, versatilefb_configure);
+
+extern sc_rndr_sw_t txtrndrsw;
+RENDERER(versatilefb, 0, txtrndrsw, gfb_set);
+RENDERER_MODULE(versatilefb, gfb_set);
+
+static uint16_t versatilefb_static_window[ROW*COL];
+extern u_char dflt_font_16[];
+
+/*
+ * Update videoadapter settings after changing resolution
+ */
+static void
+versatilefb_update_margins(video_adapter_t *adp)
+{
+	struct video_adapter_softc *sc;
+	video_info_t *vi;
+
+	sc = (struct video_adapter_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;
+}
+
+static int
+versatilefb_configure(int flags)
+{
+	struct video_adapter_softc *va_sc;
+
+	va_sc = &va_softc;
+
+	if (va_sc->initialized)
+		return (0);
+
+	va_sc->width = FB_WIDTH;
+	va_sc->height = FB_HEIGHT;
+	va_sc->depth = FB_DEPTH;
+
+	versatilefb_init(0, &va_sc->va, 0);
+
+	va_sc->initialized = 1;
+
+	return (0);
+}
+
+static int
+versatilefb_probe(int unit, video_adapter_t **adp, void *arg, int flags)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_init(int unit, video_adapter_t *adp, int flags)
+{
+	struct video_adapter_softc *sc;
+	video_info_t *vi;
+
+	sc = (struct video_adapter_softc *)adp;
+	vi = &adp->va_info;
+
+	vid_init_struct(adp, "versatilefb", -1, unit);
+
+	sc->font = dflt_font_16;
+	vi->vi_cheight = VERSATILE_FONT_HEIGHT;
+	vi->vi_cwidth = 8;
+
+	vi->vi_width = sc->width/8;
+	vi->vi_height = sc->height/vi->vi_cheight;
+
+	/*
+	 * Clamp width/height to syscons maximums
+	 */
+	if (vi->vi_width > COL)
+		vi->vi_width = COL;
+	if (vi->vi_height > ROW)
+		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;
+
+
+	adp->va_window = (vm_offset_t) versatilefb_static_window;
+	adp->va_flags |= V_ADP_FONT /* | V_ADP_COLOR | V_ADP_MODECHANGE */;
+
+	vid_register(&sc->va);
+
+	return (0);
+}
+
+static int
+versatilefb_get_info(video_adapter_t *adp, int mode, video_info_t *info)
+{
+	bcopy(&adp->va_info, info, sizeof(*info));
+	return (0);
+}
+
+static int
+versatilefb_query_mode(video_adapter_t *adp, video_info_t *info)
+{
+	return (0);
+}
+
+static int
+versatilefb_set_mode(video_adapter_t *adp, int mode)
+{
+	return (0);
+}
+
+static int
+versatilefb_save_font(video_adapter_t *adp, int page, int size, int width,
+    u_char *data, int c, int count)
+{
+	return (0);
+}
+
+static int
+versatilefb_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;
+
+	sc->font = data;
+
+	return (0);
+}
+
+static int
+versatilefb_show_font(video_adapter_t *adp, int page)
+{
+	return (0);
+}
+
+static int
+versatilefb_save_palette(video_adapter_t *adp, u_char *palette)
+{
+	return (0);
+}
+
+static int
+versatilefb_load_palette(video_adapter_t *adp, u_char *palette)
+{
+	return (0);
+}
+
+static int
+versatilefb_set_border(video_adapter_t *adp, int border)
+{
+	return (versatilefb_blank_display(adp, border));
+}
+
+static int
+versatilefb_save_state(video_adapter_t *adp, void *p, size_t size)
+{
+	return (0);
+}
+
+static int
+versatilefb_load_state(video_adapter_t *adp, void *p)
+{
+	return (0);
+}
+
+static int
+versatilefb_set_win_org(video_adapter_t *adp, off_t offset)
+{
+	return (0);
+}
+
+static int
+versatilefb_read_hw_cursor(video_adapter_t *adp, int *col, int *row)
+{
+	*col = *row = 0;
+
+	return (0);
+}
+
+static int
+versatilefb_set_hw_cursor(video_adapter_t *adp, int col, int row)
+{
+	return (0);
+}
+
+static int
+versatilefb_set_hw_cursor_shape(video_adapter_t *adp, int base, int height,
+    int celsize, int blink)
+{
+	return (0);
+}
+
+static int
+versatilefb_blank_display(video_adapter_t *adp, int mode)
+{
+
+	struct video_adapter_softc *sc;
+
+	sc = (struct video_adapter_softc *)adp;
+	if (sc && sc->fb_addr)
+		memset((void*)sc->fb_addr, 0, sc->fb_size);
+
+	return (0);
+}
+
+static int
+versatilefb_mmap(video_adapter_t *adp, vm_ooffset_t offset, vm_paddr_t *paddr,
+    int prot, vm_memattr_t *memattr)
+{
+	struct video_adapter_softc *sc;
+
+	sc = (struct video_adapter_softc *)adp;
+
+	/*
+	 * This might be a legacy VGA mem request: if so, just point it at the
+	 * framebuffer, since it shouldn't be touched
+	 */
+	if (offset < sc->stride*sc->height) {
+		*paddr = sc->fb_addr + offset;
+		return (0);
+	}
+
+	return (EINVAL);
+}
+
+static int
+versatilefb_ioctl(video_adapter_t *adp, u_long cmd, caddr_t data)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_clear(video_adapter_t *adp)
+{
+
+	return (versatilefb_blank_display(adp, 0));
+}
+
+static int
+versatilefb_fill_rect(video_adapter_t *adp, int val, int x, int y, int cx, int cy)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_bitblt(video_adapter_t *adp, ...)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_diag(video_adapter_t *adp, int level)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_save_cursor_palette(video_adapter_t *adp, u_char *palette)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_load_cursor_palette(video_adapter_t *adp, u_char *palette)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_copy(video_adapter_t *adp, vm_offset_t src, vm_offset_t dst, int n)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_putp(video_adapter_t *adp, vm_offset_t off, uint32_t p, uint32_t a,
+    int size, int bpp, int bit_ltor, int byte_ltor)
+{
+
+	return (0);
+}
+
+static int
+versatilefb_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;
+	u_char *p;
+	uint8_t fg, bg, color;
+	uint16_t rgb;
+
+	sc = (struct video_adapter_softc *)adp;
+
+	if (sc->fb_addr == 0)
+		return (0);
+
+	row = (off / adp->va_info.vi_width) * adp->va_info.vi_cheight;
+	col = (off % adp->va_info.vi_width) * adp->va_info.vi_cwidth;
+	p = sc->font + c*VERSATILE_FONT_HEIGHT;
+	addr = (uint8_t *)sc->fb_addr
+	    + (row + sc->ymargin)*(sc->stride)
+	    + (sc->depth/8) * (col + sc->xmargin);
+
+	fg = a & 0xf ;
+	bg = (a >> 8) & 0xf;
+
+	for (i = 0; i < VERSATILE_FONT_HEIGHT; i++) {
+		for (j = 0, k = 7; j < 8; j++, k--) {
+			if ((p[i] & (1 << k)) == 0)
+				color = bg;
+			else
+				color = fg;
+
+			switch (sc->depth) {
+			case 16:
+				rgb = (versatilefb_palette[color].r >> 3) << 11;
+				rgb |= (versatilefb_palette[color].g >> 2) << 5;
+				rgb |= (versatilefb_palette[color].b >> 3);
+				addr[2*j] = rgb & 0xff;
+				addr[2*j + 1] = (rgb >> 8) & 0xff;
+			default:
+				/* Not supported yet */
+				break;
+			}
+		}
+
+		addr += (sc->stride);
+	}
+
+        return (0);
+}
+
+static int
+versatilefb_puts(video_adapter_t *adp, vm_offset_t off, u_int16_t *s, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++) 
+		versatilefb_putc(adp, off + i, s[i] & 0xff, (s[i] & 0xff00) >> 8);
+
+	return (0);
+}
+
+static int
+versatilefb_putm(video_adapter_t *adp, int x, int y, uint8_t *pixel_image,
+    uint32_t pixel_mask, int size, int width)
+{
+
+	return (0);
+}
+
+/*
+ * Define a stub keyboard driver in case one hasn't been
+ * compiled into the kernel
+ */
+#include <sys/kbio.h>
+#include <dev/kbd/kbdreg.h>
+
+static int dummy_kbd_configure(int flags);
+
+keyboard_switch_t bcmdummysw;
+
+static int
+dummy_kbd_configure(int flags)
+{
+
+	return (0);
+}
+KEYBOARD_DRIVER(bcmdummy, bcmdummysw, dummy_kbd_configure);
diff --git a/sys/arm/versatile/versatile_machdep.c b/sys/arm/versatile/versatile_machdep.c
new file mode 100644
index 0000000..cd29a5a
--- /dev/null
+++ b/sys/arm/versatile/versatile_machdep.c
@@ -0,0 +1,122 @@
+/*-
+ * Copyright (c) 2012 Oleksandr Tymoshenko.
+ * All rights reserved.
+ *
+ * This code is derived from software written for Brini by Mark Brinicombe
+ *
+ * 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.
+ * 3. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *      This product includes software developed by Brini.
+ * 4. The name of the company nor the name of the author may be used to
+ *    endorse or promote products derived from this software without specific
+ *    prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BRINI ``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 BRINI 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 "opt_ddb.h"
+#include "opt_platform.h"
+#include "opt_global.h"
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#define _ARM32_BUS_DMA_PRIVATE
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+
+#include <vm/vm.h>
+#include <vm/pmap.h>
+
+#include <machine/bus.h>
+#include <machine/frame.h> /* For trapframe_t, used in <machine/machdep.h> */
+#include <machine/machdep.h>
+#include <machine/pmap.h>
+
+#include <dev/fdt/fdt_common.h>
+
+/* Start of address space used for bootstrap map */
+#define DEVMAP_BOOTSTRAP_MAP_START	0xE0000000
+
+vm_offset_t
+initarm_lastaddr(void)
+{
+
+	return (DEVMAP_BOOTSTRAP_MAP_START - ARM_NOCACHE_KVA_SIZE);
+}
+
+void
+initarm_gpio_init(void)
+{
+}
+
+void
+initarm_late_init(void)
+{
+}
+
+#define FDT_DEVMAP_MAX	(2)		/* FIXME */
+static struct pmap_devmap fdt_devmap[FDT_DEVMAP_MAX] = {
+	{ 0, 0, 0, 0, 0, },
+	{ 0, 0, 0, 0, 0, }
+};
+
+
+/*
+ * Construct pmap_devmap[] with DT-derived config data.
+ */
+int
+platform_devmap_init(void)
+{
+	int i = 0;
+	fdt_devmap[i].pd_va = 0xf0100000;
+	fdt_devmap[i].pd_pa = 0x10100000;
+	fdt_devmap[i].pd_size = 0x01000000;       /* 1 MB */
+	fdt_devmap[i].pd_prot = VM_PROT_READ | VM_PROT_WRITE;
+	fdt_devmap[i].pd_cache = PTE_DEVICE;
+
+	pmap_devmap_bootstrap_table = &fdt_devmap[0];
+	return (0);
+}
+
+struct arm32_dma_range *
+bus_dma_get_range(void)
+{
+
+	return (NULL);
+}
+
+int
+bus_dma_get_range_nb(void)
+{
+
+	return (0);
+}
+
+void
+cpu_reset()
+{
+	printf("cpu_reset\n");
+	while (1);
+}
+
diff --git a/sys/arm/versatile/versatile_pci.c b/sys/arm/versatile/versatile_pci.c
new file mode 100644
index 0000000..1ef92ef
--- /dev/null
+++ b/sys/arm/versatile/versatile_pci.c
@@ -0,0 +1,510 @@
+/*
+ * Copyright (c) 2012 Oleksandr Tymoshenko <gonzo@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/watchdog.h>
+#include <machine/bus.h>
+#include <machine/cpu.h>
+#include <machine/frame.h>
+#include <machine/intr.h>
+
+#include <dev/pci/pcivar.h>
+#include <dev/pci/pcireg.h>
+
+#include <dev/pci/pcib_private.h>
+#include "pcib_if.h"
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <machine/bus.h>
+#include <machine/fdt.h>
+
+#include <arm/versatile/versatile_pci_bus_space.h>
+
+#define	MEM_SYS		0
+#define	MEM_CORE	1
+#define	MEM_BASE	2
+#define	MEM_CONF_BASE	3
+#define MEM_REGIONS	4
+
+#define	SYS_PCICTL		0x00
+
+#define	PCI_CORE_IMAP0		0x00
+#define	PCI_CORE_IMAP1		0x04
+#define	PCI_CORE_IMAP2		0x08
+#define	PCI_CORE_SELFID		0x0C
+#define	PCI_CORE_SMAP0		0x10
+#define	PCI_CORE_SMAP1		0x14
+#define	PCI_CORE_SMAP2		0x18
+
+#define	VERSATILE_PCI_DEV	0x030010ee
+#define	VERSATILE_PCI_CLASS	0x0b400000
+
+#define	PCI_IO_WINDOW		0x44000000
+#define	PCI_IO_SIZE		0x0c000000
+#define	PCI_NPREFETCH_WINDOW	0x50000000
+#define	PCI_NPREFETCH_SIZE	0x10000000
+#define	PCI_PREFETCH_WINDOW	0x60000000
+#define	PCI_PREFETCH_SIZE	0x10000000
+
+#define	VERSATILE_PCI_IRQ_START	27
+#define	VERSATILE_PCI_IRQ_END	30
+
+#ifdef DEBUG
+#define dprintf(fmt, args...) do { printf("%s(): ", __func__);   \
+    printf(fmt,##args); } while (0)
+#else
+#define dprintf(fmt, args...)
+#endif
+
+
+#define	versatile_pci_sys_read_4(reg)	\
+	bus_read_4(sc->mem_res[MEM_SYS], (reg))
+#define	versatile_pci_sys_write_4(reg, val)	\
+	bus_write_4(sc->mem_res[MEM_SYS], (reg), (val))
+
+#define	versatile_pci_core_read_4(reg)	\
+	bus_read_4(sc->mem_res[MEM_CORE], (reg))
+#define	versatile_pci_core_write_4(reg, val)	\
+	bus_write_4(sc->mem_res[MEM_CORE], (reg), (val))
+
+#define	versatile_pci_read_4(reg)	\
+	bus_read_4(sc->mem_res[MEM_BASE], (reg))
+#define	versatile_pci_write_4(reg, val)	\
+	bus_write_4(sc->mem_res[MEM_BASE], (reg), (val))
+
+#define	versatile_pci_conf_read_4(reg)	\
+	bus_read_4(sc->mem_res[MEM_CONF_BASE], (reg))
+#define	versatile_pci_conf_write_4(reg, val)	\
+	bus_write_4(sc->mem_res[MEM_CONF_BASE], (reg), (val))
+#define	versatile_pci_conf_write_2(reg, val)	\
+	bus_write_2(sc->mem_res[MEM_CONF_BASE], (reg), (val))
+#define	versatile_pci_conf_write_1(reg, val)	\
+	bus_write_1(sc->mem_res[MEM_CONF_BASE], (reg), (val))
+
+struct versatile_pci_softc {
+	struct resource*	mem_res[MEM_REGIONS];
+	struct resource*	irq_res;
+	void*			intr_hl;
+
+	int			pcib_slot;
+
+	/* Bus part */
+	int			busno;
+	struct rman		io_rman;
+	struct rman		irq_rman;
+	struct rman		mem_rman;
+
+	struct mtx		mtx;
+};
+
+static struct resource_spec versatile_pci_mem_spec[] = {
+	{ SYS_RES_MEMORY, 0, RF_ACTIVE },
+	{ SYS_RES_MEMORY, 1, RF_ACTIVE },
+	{ SYS_RES_MEMORY, 2, RF_ACTIVE },
+	{ SYS_RES_MEMORY, 3, RF_ACTIVE },
+	{ -1, 0, 0 }
+};
+
+static int
+versatile_pci_probe(device_t dev)
+{
+
+	if (ofw_bus_is_compatible(dev, "versatile,pci")) {
+		device_set_desc(dev, "Versatile PCI controller");
+		return (BUS_PROBE_DEFAULT);
+	}
+
+	return (ENXIO);
+}
+
+static int
+versatile_pci_attach(device_t dev)
+{
+	struct versatile_pci_softc *sc = device_get_softc(dev);
+	int err;
+	int slot;
+	uint32_t vendordev_id, class_id;
+	uint32_t val;
+
+	/* Request memory resources */
+	err = bus_alloc_resources(dev, versatile_pci_mem_spec,
+		sc->mem_res);
+	if (err) {
+		device_printf(dev, "Error: could not allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	/*
+	 * Setup memory windows
+	 */
+	versatile_pci_core_write_4(PCI_CORE_IMAP0, (PCI_IO_WINDOW >> 11));
+	versatile_pci_core_write_4(PCI_CORE_IMAP1, (PCI_NPREFETCH_WINDOW >> 11));
+	versatile_pci_core_write_4(PCI_CORE_IMAP2, (PCI_PREFETCH_WINDOW >> 11));
+
+	/*
+	 * XXX: this is SDRAM offset >> 28
+	 */
+	versatile_pci_core_write_4(PCI_CORE_SMAP0, 0);
+	versatile_pci_core_write_4(PCI_CORE_SMAP1, 0);
+	versatile_pci_core_write_4(PCI_CORE_SMAP2, 0);
+
+	versatile_pci_sys_write_4(SYS_PCICTL, 1);
+
+	for (slot = 0; slot <= PCI_SLOTMAX; slot++) {
+		vendordev_id = versatile_pci_read_4((slot << 11) + PCIR_DEVVENDOR);
+		class_id = versatile_pci_read_4((slot << 11) + PCIR_REVID);
+		if ((vendordev_id == VERSATILE_PCI_DEV) &&
+		    (class_id == VERSATILE_PCI_CLASS))
+			break;
+	}
+
+	if (slot == (PCI_SLOTMAX + 1)) {
+		bus_release_resources(dev, versatile_pci_mem_spec,
+		    sc->mem_res);
+		device_printf(dev, "Versatile PCI core not found\n");
+		return (ENXIO);
+	}
+
+	sc->pcib_slot = slot;
+	device_printf(dev, "PCI core at slot #%d\n", slot);
+
+	versatile_pci_core_write_4(PCI_CORE_SELFID, slot);
+	val = versatile_pci_conf_read_4((slot << 11) + PCIR_COMMAND);
+	val |= (PCIM_CMD_BUSMASTEREN | PCIM_CMD_MEMEN | PCIM_CMD_MWRICEN);
+	versatile_pci_conf_write_4((slot << 11) + PCIR_COMMAND, val);
+
+	/* Again SDRAM start >> 28  */
+	versatile_pci_write_4((slot << 11) + PCIR_BAR(0), 0);
+	versatile_pci_write_4((slot << 11) + PCIR_BAR(1), 0);
+	versatile_pci_write_4((slot << 11) + PCIR_BAR(2), 0);
+
+	/* Prepare resource managers */
+	sc->mem_rman.rm_type = RMAN_ARRAY;
+	sc->mem_rman.rm_descr = "versatile PCI memory window";
+	if (rman_init(&sc->mem_rman) != 0 || 
+	    rman_manage_region(&sc->mem_rman, PCI_NPREFETCH_WINDOW, 
+		PCI_NPREFETCH_WINDOW + PCI_NPREFETCH_SIZE - 1) != 0) {
+		panic("versatile_pci_attach: failed to set up memory rman");
+	}
+
+	bootverbose = 1;
+	sc->io_rman.rm_type = RMAN_ARRAY;
+	sc->io_rman.rm_descr = "versatile PCI IO window";
+	if (rman_init(&sc->io_rman) != 0 || 
+	    rman_manage_region(&sc->io_rman, PCI_IO_WINDOW, 
+		PCI_IO_WINDOW + PCI_IO_SIZE - 1) != 0) {
+		panic("versatile_pci_attach: failed to set up I/O rman");
+	}
+
+	sc->irq_rman.rm_type = RMAN_ARRAY;
+	sc->irq_rman.rm_descr = "versatile PCI IRQs";
+	if (rman_init(&sc->irq_rman) != 0 ||
+	    rman_manage_region(&sc->irq_rman, VERSATILE_PCI_IRQ_START, 
+	        VERSATILE_PCI_IRQ_END) != 0) {
+		panic("versatile_pci_attach: failed to set up IRQ rman");
+	}
+
+	mtx_init(&sc->mtx, device_get_nameunit(dev), "versatilepci",
+			MTX_SPIN);
+
+	val = versatile_pci_conf_read_4((12 << 11) + PCIR_COMMAND);
+
+	for (slot = 0; slot <= PCI_SLOTMAX; slot++) {
+		vendordev_id = versatile_pci_read_4((slot << 11) + PCIR_DEVVENDOR);
+		class_id = versatile_pci_read_4((slot << 11) + PCIR_REVID);
+
+		if (slot == sc->pcib_slot)
+			continue;
+
+		if ((vendordev_id == 0xffffffff) &&
+		    (class_id == 0xffffffff))
+			continue;
+
+		val = versatile_pci_conf_read_4((slot << 11) + PCIR_COMMAND);
+		val |= PCIM_CMD_MEMEN | PCIM_CMD_PORTEN;
+		versatile_pci_conf_write_4((slot << 11) + PCIR_COMMAND, val);
+	}
+
+	device_add_child(dev, "pci", 0);
+	return (bus_generic_attach(dev));
+}
+
+static int
+versatile_pci_read_ivar(device_t dev, device_t child, int which,
+    uintptr_t *result)
+{
+	struct versatile_pci_softc *sc = device_get_softc(dev);
+
+	switch (which) {
+	case PCIB_IVAR_DOMAIN:
+		*result = 0;
+		return (0);
+	case PCIB_IVAR_BUS:
+		*result = sc->busno;
+		return (0);
+	}
+
+	return (ENOENT);
+}
+
+static int
+versatile_pci_write_ivar(device_t dev, device_t child, int which,
+    uintptr_t result)
+{
+	struct versatile_pci_softc * sc = device_get_softc(dev);
+
+	switch (which) {
+	case PCIB_IVAR_BUS:
+		sc->busno = result;
+		return (0);
+	}
+
+	return (ENOENT);
+}
+
+static struct resource *
+versatile_pci_alloc_resource(device_t bus, device_t child, int type, int *rid,
+    u_long start, u_long end, u_long count, u_int flags)
+{
+
+	struct versatile_pci_softc *sc = device_get_softc(bus);
+	struct resource *rv;
+	struct rman *rm;
+
+	printf("Alloc resources %d, %08lx..%08lx, %ld\n", type, start, end, count);
+
+	switch (type) {
+	case SYS_RES_IOPORT:
+		rm = &sc->io_rman;
+		break;
+	case SYS_RES_IRQ:
+		rm = &sc->irq_rman;
+		break;
+	case SYS_RES_MEMORY:
+		rm = &sc->mem_rman;
+		break;
+	default:
+		return (NULL);
+	}
+
+	rv = rman_reserve_resource(rm, start, end, count, flags, child);
+
+	if (rv == NULL)
+		return (NULL);
+
+	rman_set_rid(rv, *rid);
+
+	if (flags & RF_ACTIVE) {
+		if (bus_activate_resource(child, type, *rid, rv)) {
+			rman_release_resource(rv);
+			return (NULL);
+		}
+	}
+	return (rv);
+}
+
+static int
+versatile_pci_activate_resource(device_t bus, device_t child, int type, int rid,
+    struct resource *r)
+{
+	vm_offset_t vaddr;
+	int res = (BUS_ACTIVATE_RESOURCE(device_get_parent(bus),
+	    child, type, rid, r));
+
+	if (!res) {
+		switch(type) {
+		case SYS_RES_MEMORY:
+		case SYS_RES_IOPORT:
+			vaddr = (vm_offset_t)pmap_mapdev(rman_get_start(r),
+					rman_get_size(r));
+			rman_set_bushandle(r, vaddr);
+			rman_set_bustag(r, versatile_bus_space_pcimem);
+			break;
+		}
+	}
+	return (res);
+}
+
+static int
+versatile_pci_setup_intr(device_t bus, device_t child, struct resource *ires,
+	    int flags, driver_filter_t *filt, driver_intr_t *handler,
+	    void *arg, void **cookiep)
+{
+
+	return BUS_SETUP_INTR(device_get_parent(bus), bus, ires, flags,
+	    filt, handler, arg, cookiep);
+}
+
+static int
+versatile_pci_teardown_intr(device_t dev, device_t child, struct resource *ires,
+    void *cookie)
+{
+
+	return BUS_TEARDOWN_INTR(device_get_parent(dev), dev, ires, cookie);
+}
+
+
+
+static int
+versatile_pci_maxslots(device_t dev)
+{
+
+	return (PCI_SLOTMAX);
+}
+
+static int
+versatile_pci_route_interrupt(device_t pcib, device_t device, int pin)
+{
+
+	return (27 + ((pci_get_slot(device) + pin - 1) & 3));
+}
+
+static uint32_t
+versatile_pci_read_config(device_t dev, u_int bus, u_int slot, u_int func,
+    u_int reg, int bytes)
+{
+	struct versatile_pci_softc *sc = device_get_softc(dev);
+	uint32_t data;
+	uint32_t shift, mask;
+	uint32_t addr;
+
+	if (sc->pcib_slot == slot) {
+		switch (bytes) {
+			case 4: 
+				return (0xffffffff);
+				break;
+			case 2:
+				return (0xffff);
+				break;
+			case 1:
+				return (0xff);
+				break;
+		}
+	}
+
+	addr = (bus << 16) | (slot << 11) | (func << 8) | (reg & ~3);
+
+	/* register access is 32-bit aligned */
+	shift = (reg & 3) * 8;
+
+	/* Create a mask based on the width, post-shift */
+	if (bytes == 2)
+		mask = 0xffff;
+	else if (bytes == 1)
+		mask = 0xff;
+	else
+		mask = 0xffffffff;
+
+	dprintf("%s: tag (%x, %x, %x) reg %d(%d)\n", __func__, bus, slot, 
+	    func, reg, bytes);
+
+	mtx_lock_spin(&sc->mtx);
+	data = versatile_pci_conf_read_4(addr);
+	mtx_unlock_spin(&sc->mtx);
+
+	/* get request bytes from 32-bit word */
+	data = (data >> shift) & mask;
+
+	dprintf("%s: read 0x%x\n", __func__, data);
+
+	return (data);
+}
+
+static void
+versatile_pci_write_config(device_t dev, u_int bus, u_int slot, u_int func,
+    u_int reg, uint32_t data, int bytes)
+{
+
+	struct versatile_pci_softc *sc = device_get_softc(dev);
+	uint32_t addr;
+
+	dprintf("%s: tag (%x, %x, %x) reg %d(%d)\n", __func__, bus, slot,
+	    func, reg, bytes);
+
+	if (sc->pcib_slot == slot)
+		return;
+
+	addr = (bus << 16) | (slot << 11) | (func << 8) | reg;
+	mtx_lock_spin(&sc->mtx);
+	switch (bytes) {
+		case 4: 
+			versatile_pci_conf_write_4(addr, data);
+			break;
+		case 2:
+			versatile_pci_conf_write_2(addr, data);
+			break;
+		case 1:
+			versatile_pci_conf_write_1(addr, data);
+			break;
+	}
+	mtx_unlock_spin(&sc->mtx);
+}
+
+static device_method_t versatile_pci_methods[] = {
+	DEVMETHOD(device_probe,		versatile_pci_probe),
+	DEVMETHOD(device_attach,	versatile_pci_attach),
+
+	/* Bus interface */
+	DEVMETHOD(bus_read_ivar,	versatile_pci_read_ivar),
+	DEVMETHOD(bus_write_ivar,	versatile_pci_write_ivar),
+	DEVMETHOD(bus_alloc_resource,	versatile_pci_alloc_resource),
+	DEVMETHOD(bus_release_resource,	bus_generic_release_resource),
+	DEVMETHOD(bus_activate_resource, versatile_pci_activate_resource),
+	DEVMETHOD(bus_deactivate_resource, bus_generic_deactivate_resource),
+	DEVMETHOD(bus_setup_intr,	versatile_pci_setup_intr),
+	DEVMETHOD(bus_teardown_intr,	versatile_pci_teardown_intr),
+
+	/* pcib interface */
+	DEVMETHOD(pcib_maxslots,	versatile_pci_maxslots),
+	DEVMETHOD(pcib_read_config,	versatile_pci_read_config),
+	DEVMETHOD(pcib_write_config,	versatile_pci_write_config),
+	DEVMETHOD(pcib_route_interrupt,	versatile_pci_route_interrupt),
+
+	DEVMETHOD_END
+};
+
+static driver_t versatile_pci_driver = {
+	"pcib",
+	versatile_pci_methods,
+	sizeof(struct versatile_pci_softc),
+};
+
+static devclass_t versatile_pci_devclass;
+
+DRIVER_MODULE(versatile_pci, simplebus, versatile_pci_driver, versatile_pci_devclass, 0, 0);
diff --git a/sys/arm/versatile/versatile_pci_bus_space.c b/sys/arm/versatile/versatile_pci_bus_space.c
new file mode 100644
index 0000000..2c2e0b5
--- /dev/null
+++ b/sys/arm/versatile/versatile_pci_bus_space.c
@@ -0,0 +1,153 @@
+/*-
+ * Copyright (c) 2009, Oleksandr Tymoshenko <gonzo@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, this list of conditions, and the following
+ *    disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/endian.h>
+
+#include <machine/bus.h>
+#include <arm/versatile/versatile_pci_bus_space.h>
+
+/* Prototypes for all the bus_space structure functions */
+bs_protos(generic);
+bs_protos(generic_armv4);
+
+/*
+ * Bus space that handles offsets in word for 1/2 bytes read/write access.
+ * Byte order of values is handled by device drivers itself. 
+ */
+static struct bus_space bus_space_pcimem = {
+	/* cookie */
+	(void *) 0,
+
+	/* mapping/unmapping */
+	generic_bs_map,
+	generic_bs_unmap,
+	generic_bs_subregion,
+
+	/* allocation/deallocation */
+	NULL,
+	NULL,
+
+	/* barrier */
+	generic_bs_barrier,
+
+	/* read (single) */
+	generic_bs_r_1,
+	generic_armv4_bs_r_2,
+	generic_bs_r_4,
+	NULL,
+
+	/* read multiple */
+	generic_bs_rm_1,
+	generic_armv4_bs_rm_2,
+	generic_bs_rm_4,
+	NULL,
+
+	/* read region */
+	generic_bs_rr_1,
+	generic_armv4_bs_rr_2,
+	generic_bs_rr_4,
+	NULL,
+
+	/* write (single) */
+	generic_bs_w_1,
+	generic_armv4_bs_w_2,
+	generic_bs_w_4,
+	NULL,
+
+	/* write multiple */
+	generic_bs_wm_1,
+	generic_armv4_bs_wm_2,
+	generic_bs_wm_4,
+	NULL,
+
+	/* write region */
+	generic_bs_wr_1,
+	generic_armv4_bs_wr_2,
+	generic_bs_wr_4,
+	NULL,
+
+	/* set multiple */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+
+	/* set region */
+	NULL,
+	NULL,
+	generic_bs_sr_4,
+	NULL,
+
+	/* copy */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+
+	/* read (single) stream */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+
+	/* read multiple stream */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+
+	/* read region stream */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+
+	/* write (single) stream */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+
+	/* write multiple stream */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+
+	/* write region stream */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+};
+
+bus_space_tag_t versatile_bus_space_pcimem = &bus_space_pcimem;
diff --git a/sys/arm/versatile/versatile_pci_bus_space.h b/sys/arm/versatile/versatile_pci_bus_space.h
new file mode 100644
index 0000000..d5b54ef
--- /dev/null
+++ b/sys/arm/versatile/versatile_pci_bus_space.h
@@ -0,0 +1,35 @@
+/*-
+ * Copyright (c) 2009, Oleksandr Tymoshenko <gonzo@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, 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 __VERSATILE_PCI_BUS_SPACEH__
+#define __VERSATILE_PCI_BUS_SPACEH__
+
+extern bus_space_tag_t versatile_bus_space_pcimem;
+
+#endif /* __VERSATILE_PCI_BUS_SPACEH__ */
diff --git a/sys/arm/versatile/versatile_sic.c b/sys/arm/versatile/versatile_sic.c
new file mode 100644
index 0000000..d3eee6e
--- /dev/null
+++ b/sys/arm/versatile/versatile_sic.c
@@ -0,0 +1,133 @@
+/*-
+ * Copyright (c) 2012 Oleksandr Tymoshenko <gonzo@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/ktr.h>
+#include <sys/module.h>
+#include <sys/rman.h>
+#include <machine/bus.h>
+#include <machine/intr.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#ifdef  DEBUG
+#define dprintf(fmt, args...) printf(fmt, ##args)
+#else
+#define dprintf(fmt, args...)
+#endif
+
+#define	SIC_STATUS	0x00
+#define	SIC_RAWSTAT	0x04
+#define	SIC_ENABLE	0x08
+#define	SIC_ENSET	0x08
+#define	SIC_ENCLR	0x0C
+#define	SIC_SOFTINTSET	0x10
+#define	SIC_SOFTINTCLR	0x14
+#define	SIC_PICENABLE	0x20
+#define	SIC_PICENSET	0x20
+#define	SIC_PICENCLR	0x24
+
+struct versatile_sic_softc {
+	device_t		sc_dev;
+	struct resource *	mem_res;
+};
+
+#define	sic_read_4(sc, reg)			\
+    bus_read_4(sc->mem_res, (reg))
+#define	sic_write_4(sc, reg, val)		\
+    bus_write_4(sc->mem_res, (reg), (val))
+
+static int
+versatile_sic_probe(device_t dev)
+{
+	if (!ofw_bus_is_compatible(dev, "arm,versatile-sic"))
+		return (ENXIO);
+	device_set_desc(dev, "ARM Versatile SIC");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+versatile_sic_attach(device_t dev)
+{
+	struct		versatile_sic_softc *sc = device_get_softc(dev);
+	uint32_t	pass_irqs;
+	int		rid;
+
+	sc->sc_dev = dev;
+
+	/* Request memory resources */
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "Error: could not allocate memory resources\n");
+		return (ENXIO);
+	}
+
+	/* Disable all interrupts on SIC */
+	sic_write_4(sc, SIC_ENCLR, 0xffffffff);
+
+	/* 
+	 * XXX: Enable IRQ3 for KMI
+	 * Should be replaced by proper interrupts cascading
+	 */
+	sic_write_4(sc, SIC_ENSET, (1 << 3));
+
+	/*
+	 * Let PCI and Ethernet interrupts pass through
+	 * IRQ25, IRQ27..IRQ31
+	 */
+	pass_irqs = (7 << 27) | (1 << 25);
+	sic_write_4(sc, SIC_PICENSET, pass_irqs);
+
+	return (0);
+}
+
+static device_method_t versatile_sic_methods[] = {
+	DEVMETHOD(device_probe,		versatile_sic_probe),
+	DEVMETHOD(device_attach,	versatile_sic_attach),
+	{ 0, 0 }
+};
+
+static driver_t versatile_sic_driver = {
+	"sic",
+	versatile_sic_methods,
+	sizeof(struct versatile_sic_softc),
+};
+
+static devclass_t versatile_sic_devclass;
+
+DRIVER_MODULE(sic, simplebus, versatile_sic_driver, versatile_sic_devclass, 0, 0);
diff --git a/sys/arm/versatile/versatile_timer.c b/sys/arm/versatile/versatile_timer.c
new file mode 100644
index 0000000..ab46a0a
--- /dev/null
+++ b/sys/arm/versatile/versatile_timer.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2012 Oleksandr Tymoshenko <gonzo@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/timeet.h>
+#include <sys/timetc.h>
+#include <sys/watchdog.h>
+#include <machine/bus.h>
+#include <machine/cpu.h>
+#include <machine/frame.h>
+#include <machine/intr.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <machine/bus.h>
+#include <machine/fdt.h>
+
+void
+cpu_initclocks(void)
+{
+	cpu_initclocks_bsp();
+}
+
+