path: root/patches/linux-3.6.7/0005-bcm2708-vchiq-driver.patch

From: popcornmix <popcornmix@gmail.com>
Date: Tue, 17 Jan 2012 19:22:19 +0000
Subject: [PATCH] bcm2708 vchiq driver

Signed-off-by: popcornmix <popcornmix@gmail.com>
---
 drivers/misc/Kconfig                               |    2 +
 drivers/misc/Makefile                              |    2 +
 drivers/misc/vc04_services/Kconfig                 |    7 +
 drivers/misc/vc04_services/Makefile                |   19 +
 .../misc/vc04_services/interface/vchi/vchi_mh.h    |   19 +
 .../misc/vc04_services/interface/vchiq_arm/vchiq.h |   27 +
 .../vc04_services/interface/vchiq_arm/vchiq_2835.h |   27 +
 .../interface/vchiq_arm/vchiq_2835_arm.c           |  487 ++++
 .../vc04_services/interface/vchiq_arm/vchiq_arm.c  | 1293 ++++++++++
 .../vc04_services/interface/vchiq_arm/vchiq_arm.h  |   38 +
 .../vc04_services/interface/vchiq_arm/vchiq_cfg.h  |   43 +
 .../interface/vchiq_arm/vchiq_connected.c          |  101 +
 .../interface/vchiq_arm/vchiq_connected.h          |   32 +
 .../vc04_services/interface/vchiq_arm/vchiq_core.c | 2604 ++++++++++++++++++++
 .../vc04_services/interface/vchiq_arm/vchiq_core.h |  480 ++++
 .../vc04_services/interface/vchiq_arm/vchiq_if.h   |  148 ++
 .../interface/vchiq_arm/vchiq_ioctl.h              |  105 +
 .../interface/vchiq_arm/vchiq_kern_lib.c           |  297 +++
 .../vc04_services/interface/vchiq_arm/vchiq_lib.c  | 1518 ++++++++++++
 .../interface/vchiq_arm/vchiq_memdrv.h             |   45 +
 .../interface/vchiq_arm/vchiq_pagelist.h           |   43 +
 .../vc04_services/interface/vchiq_arm/vchiq_shim.c |  970 ++++++++
 .../vc04_services/interface/vchiq_arm/vchiq_util.c |   97 +
 .../vc04_services/interface/vchiq_arm/vchiq_util.h |   47 +
 .../interface/vcos/generic/vcos_cmd.c              |  681 +++++
 .../interface/vcos/generic/vcos_common.h           |   76 +
 .../vcos/generic/vcos_generic_blockpool.h          |  260 ++
 .../vcos/generic/vcos_generic_event_flags.c        |  297 +++
 .../vcos/generic/vcos_generic_event_flags.h        |  104 +
 .../vcos/generic/vcos_generic_named_sem.h          |   81 +
 .../vcos/generic/vcos_generic_quickslow_mutex.h    |   75 +
 .../vcos/generic/vcos_generic_reentrant_mtx.h      |   75 +
 .../interface/vcos/generic/vcos_generic_tls.h      |  144 ++
 .../vcos/generic/vcos_joinable_thread_from_plain.h |  202 ++
 .../interface/vcos/generic/vcos_latch_from_sem.h   |   48 +
 .../interface/vcos/generic/vcos_logcat.c           |  549 +++++
 .../interface/vcos/generic/vcos_mem_from_malloc.c  |   73 +
 .../interface/vcos/generic/vcos_mem_from_malloc.h  |   54 +
 .../vcos/generic/vcos_mutexes_are_reentrant.h      |   68 +
 .../interface/vcos/generic/vcos_thread_reaper.h    |   35 +
 .../interface/vcos/linuxkernel/stdint.h            |   17 +
 .../interface/vcos/linuxkernel/vcos_linuxkernel.c  |  616 +++++
 .../vcos/linuxkernel/vcos_linuxkernel_cfg.c        |  332 +++
 .../vcos/linuxkernel/vcos_linuxkernel_misc.c       |  113 +
 .../interface/vcos/linuxkernel/vcos_mod_init.c     |   64 +
 .../interface/vcos/linuxkernel/vcos_platform.h     |  496 ++++
 .../vcos/linuxkernel/vcos_platform_types.h         |   47 +
 .../interface/vcos/linuxkernel/vcos_thread_map.c   |  129 +
 .../interface/vcos/linuxkernel/vcos_thread_map.h   |   39 +
 drivers/misc/vc04_services/interface/vcos/vcos.h   |  201 ++
 .../vc04_services/interface/vcos/vcos_assert.h     |  269 ++
 .../interface/vcos/vcos_atomic_flags.h             |   72 +
 .../vc04_services/interface/vcos/vcos_build_info.h |    5 +
 .../misc/vc04_services/interface/vcos/vcos_cfg.h   |  113 +
 .../misc/vc04_services/interface/vcos/vcos_cmd.h   |   98 +
 .../misc/vc04_services/interface/vcos/vcos_ctype.h |   29 +
 .../misc/vc04_services/interface/vcos/vcos_dlfcn.h |   69 +
 .../misc/vc04_services/interface/vcos/vcos_event.h |   97 +
 .../interface/vcos/vcos_event_flags.h              |   98 +
 .../misc/vc04_services/interface/vcos/vcos_init.h  |   43 +
 .../vc04_services/interface/vcos/vcos_logging.h    |  279 +++
 .../interface/vcos/vcos_lowlevel_thread.h          |  107 +
 .../misc/vc04_services/interface/vcos/vcos_mem.h   |   81 +
 .../vc04_services/interface/vcos/vcos_msgqueue.h   |  157 ++
 .../misc/vc04_services/interface/vcos/vcos_mutex.h |   92 +
 .../misc/vc04_services/interface/vcos/vcos_once.h  |   42 +
 .../vc04_services/interface/vcos/vcos_semaphore.h  |  115 +
 .../vc04_services/interface/vcos/vcos_stdbool.h    |   17 +
 .../vc04_services/interface/vcos/vcos_stdint.h     |  193 ++
 .../vc04_services/interface/vcos/vcos_string.h     |   73 +
 .../vc04_services/interface/vcos/vcos_thread.h     |  259 ++
 .../interface/vcos/vcos_thread_attr.h              |   73 +
 .../misc/vc04_services/interface/vcos/vcos_timer.h |   95 +
 .../misc/vc04_services/interface/vcos/vcos_types.h |  197 ++
 74 files changed, 16000 insertions(+)
 create mode 100644 drivers/misc/vc04_services/Kconfig
 create mode 100644 drivers/misc/vc04_services/Makefile
 create mode 100644 drivers/misc/vc04_services/interface/vchi/vchi_mh.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.c
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_cfg.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.c
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.c
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_if.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_lib.c
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_memdrv.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_pagelist.h
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_shim.c
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.c
 create mode 100644 drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_cmd.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_common.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_blockpool.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_named_sem.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_quickslow_mutex.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_reentrant_mtx.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_tls.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_joinable_thread_from_plain.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_latch_from_sem.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_logcat.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_mutexes_are_reentrant.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/generic/vcos_thread_reaper.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/stdint.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_cfg.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_misc.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_mod_init.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform_types.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_thread_map.c
 create mode 100644 drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_thread_map.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_assert.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_atomic_flags.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_build_info.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_cfg.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_cmd.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_ctype.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_dlfcn.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_event.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_event_flags.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_init.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_logging.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_lowlevel_thread.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_mem.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_msgqueue.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_mutex.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_once.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_semaphore.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_stdbool.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_stdint.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_string.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_thread.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_thread_attr.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_timer.h
 create mode 100644 drivers/misc/vc04_services/interface/vcos/vcos_types.h

diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 98a442d..f738bf3 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -517,4 +517,6 @@ source "drivers/misc/lis3lv02d/Kconfig"
 source "drivers/misc/carma/Kconfig"
 source "drivers/misc/altera-stapl/Kconfig"
 source "drivers/misc/mei/Kconfig"
+source "drivers/misc/vc04_services/Kconfig"
 endmenu
+
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index b88df7a..aeeb47e 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -50,3 +50,5 @@ obj-y				+= carma/
 obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o
 obj-$(CONFIG_ALTERA_STAPL)	+=altera-stapl/
 obj-$(CONFIG_INTEL_MEI)		+= mei/
+obj-y				+= vc04_services/
+
diff --git a/drivers/misc/vc04_services/Kconfig b/drivers/misc/vc04_services/Kconfig
new file mode 100644
index 0000000..d97a1e2
--- /dev/null
+++ b/drivers/misc/vc04_services/Kconfig
@@ -0,0 +1,7 @@
+config BCM2708_VCHIQ
+	tristate "Videocore VCHIQ"
+	depends on MACH_BCM2708
+        default y
+        help
+          Helper for communication for VideoCore.
+
diff --git a/drivers/misc/vc04_services/Makefile b/drivers/misc/vc04_services/Makefile
new file mode 100644
index 0000000..d26ece7
--- /dev/null
+++ b/drivers/misc/vc04_services/Makefile
@@ -0,0 +1,19 @@
+obj-$(CONFIG_BCM2708_VCHIQ)	+= vchiq.o
+
+vchiq-objs := \
+   interface/vchiq_arm/vchiq_core.o  \
+   interface/vchiq_arm/vchiq_arm.o \
+   interface/vchiq_arm/vchiq_kern_lib.o \
+   interface/vchiq_arm/vchiq_2835_arm.o \
+   interface/vcos/linuxkernel/vcos_linuxkernel.o \
+   interface/vcos/linuxkernel/vcos_thread_map.o \
+   interface/vcos/linuxkernel/vcos_linuxkernel_cfg.o \
+   interface/vcos/generic/vcos_generic_event_flags.o \
+   interface/vcos/generic/vcos_logcat.o \
+   interface/vcos/generic/vcos_mem_from_malloc.o \
+   interface/vcos/generic/vcos_cmd.o
+
+EXTRA_CFLAGS += -DVCOS_VERIFY_BKPTS=1 -Idrivers/misc/vc04_services -Idrivers/misc/vc04_services/interface/vcos/linuxkernel
+
+
+
diff --git a/drivers/misc/vc04_services/interface/vchi/vchi_mh.h b/drivers/misc/vc04_services/interface/vchi/vchi_mh.h
new file mode 100644
index 0000000..01732bf
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchi/vchi_mh.h
@@ -0,0 +1,19 @@
+/*=============================================================================
+Copyright (c) 2010 Broadcom Europe Limited. All rights reserved.
+
+Project  :  vchi
+Module   :  vchi
+
+FILE DESCRIPTION:
+Definitions for memory handle types.
+=============================================================================*/
+
+#ifndef VCHI_MH_H_
+#define VCHI_MH_H_
+
+#include <interface/vcos/vcos.h>
+
+typedef int32_t VCHI_MEM_HANDLE_T;
+#define VCHI_MEM_HANDLE_INVALID 0
+
+#endif
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq.h
new file mode 100644
index 0000000..49d3087
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_VCHIQ_H
+#define VCHIQ_VCHIQ_H
+
+#include "vchiq_if.h"
+#include "vchiq_util.h"
+#include "interface/vcos/vcos.h"
+
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835.h
new file mode 100644
index 0000000..959405e
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_2835_H
+#define VCHIQ_2835_H
+
+#include "vchiq_pagelist.h"
+
+#define VCHIQ_PLATFORM_FRAGMENTS_OFFSET_IDX 0
+#define VCHIQ_PLATFORM_FRAGMENTS_COUNT_IDX  1
+
+#endif /* VCHIQ_2835_H */
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
new file mode 100644
index 0000000..ca065ff
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
@@ -0,0 +1,487 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/pagemap.h>
+#include <linux/dma-mapping.h>
+#include <linux/version.h>
+#include <asm/pgtable.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+
+#include <mach/irqs.h>
+
+#include <mach/platform.h>
+#include <mach/vcio.h>
+
+#define TOTAL_SLOTS (VCHIQ_SLOT_ZERO_SLOTS + 2 * 32)
+
+#define VCHIQ_DOORBELL_IRQ IRQ_ARM_DOORBELL_0
+#define VCHIQ_ARM_ADDRESS(x) __virt_to_bus(x)
+
+#include "vchiq_arm.h"
+#include "vchiq_2835.h"
+
+#define MAX_FRAGMENTS (VCHIQ_NUM_CURRENT_BULKS * 2)
+
+#define VCOS_LOG_CATEGORY (&vchiq_arm_log_category)
+
+static char *g_slot_mem;
+static int g_slot_mem_size;
+dma_addr_t g_slot_phys;
+static FRAGMENTS_T *g_fragments_base;
+static FRAGMENTS_T *g_free_fragments;
+struct semaphore g_free_fragments_sema;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,36)
+static DEFINE_SEMAPHORE(g_free_fragments_mutex);
+#else
+static DECLARE_MUTEX(g_free_fragments_mutex);
+#endif
+
+static irqreturn_t
+vchiq_doorbell_irq(int irq, void *dev_id);
+
+static int
+create_pagelist(char __user *buf, size_t count, unsigned short type,
+	struct task_struct *task, PAGELIST_T ** ppagelist);
+
+static void
+free_pagelist(PAGELIST_T *pagelist, int actual);
+
+int __init
+vchiq_platform_vcos_init(void)
+{
+	return (vcos_init() == VCOS_SUCCESS) ? 0 : -EINVAL;
+}
+
+int __init
+vchiq_platform_init(VCHIQ_STATE_T *state)
+{
+	VCHIQ_SLOT_ZERO_T *vchiq_slot_zero;
+	int frag_mem_size;
+	int err;
+	int i;
+
+	/* Allocate space for the channels in coherent memory */
+	g_slot_mem_size = PAGE_ALIGN(TOTAL_SLOTS * VCHIQ_SLOT_SIZE);
+	frag_mem_size = PAGE_ALIGN(sizeof(FRAGMENTS_T) * MAX_FRAGMENTS);
+
+	g_slot_mem = dma_alloc_coherent(NULL, g_slot_mem_size + frag_mem_size,
+		&g_slot_phys, GFP_ATOMIC);
+
+	if (!g_slot_mem) {
+		vcos_log_error("Unable to allocate channel memory");
+		err = -ENOMEM;
+		goto failed_alloc;
+	}
+
+	vcos_assert(((int)g_slot_mem & (PAGE_SIZE - 1)) == 0);
+
+	vchiq_slot_zero = vchiq_init_slots(g_slot_mem, g_slot_mem_size);
+	if (!vchiq_slot_zero)
+	{
+	   err = -EINVAL;
+	   goto failed_init_slots;
+	}
+
+	vchiq_slot_zero->platform_data[VCHIQ_PLATFORM_FRAGMENTS_OFFSET_IDX] = (int)g_slot_phys + g_slot_mem_size;
+	vchiq_slot_zero->platform_data[VCHIQ_PLATFORM_FRAGMENTS_COUNT_IDX] = MAX_FRAGMENTS;
+
+	g_fragments_base = (FRAGMENTS_T *)(g_slot_mem + g_slot_mem_size);
+	g_slot_mem_size += frag_mem_size;
+
+	g_free_fragments = g_fragments_base;
+	for (i = 0; i < (MAX_FRAGMENTS - 1); i++) {
+		*(FRAGMENTS_T **) & g_fragments_base[i] =
+			&g_fragments_base[i + 1];
+	}
+	*(FRAGMENTS_T **) & g_fragments_base[i] = NULL;
+	sema_init(&g_free_fragments_sema, MAX_FRAGMENTS);
+
+	if (vchiq_init_state(state, vchiq_slot_zero, 0/*slave*/) !=
+		VCHIQ_SUCCESS)
+	{
+		err = -EINVAL;
+		goto failed_vchiq_init;
+	}
+
+	err = request_irq(VCHIQ_DOORBELL_IRQ, vchiq_doorbell_irq,
+		IRQF_SAMPLE_RANDOM | IRQF_IRQPOLL, "VCHIQ doorbell",
+		state);
+	if (err < 0)
+	{
+		printk( KERN_ERR "%s: failed to register irq=%d err=%d\n", __func__,
+			VCHIQ_DOORBELL_IRQ, err );
+		goto failed_request_irq;
+	}
+
+	/* Send the base address of the slots to VideoCore */
+
+	dsb(); /* Ensure all writes have completed */
+
+	bcm_mailbox_write(MBOX_CHAN_VCHIQ, (unsigned int)g_slot_phys);
+
+	vcos_log_info("vchiq_init - done (slots %x, phys %x)",
+		(unsigned int)vchiq_slot_zero, g_slot_phys);
+
+	return 0;
+
+failed_request_irq:
+failed_vchiq_init:
+failed_init_slots:
+	dma_free_coherent(NULL, g_slot_mem_size, g_slot_mem, g_slot_phys);
+
+failed_alloc:
+	return err;
+}
+
+void __exit
+vchiq_platform_exit(VCHIQ_STATE_T *state)
+{
+	free_irq(VCHIQ_DOORBELL_IRQ, state);
+	dma_free_coherent(NULL, g_slot_mem_size,
+		g_slot_mem, g_slot_phys);
+}
+
+void
+remote_event_signal(REMOTE_EVENT_T *event)
+{
+	event->fired = 1;
+
+	/* The test on the next line also ensures the write on the previous line
+		has completed */
+
+	if (event->armed) {
+		/* trigger vc interrupt */
+		dsb();         /* data barrier operation */
+
+		writel(0, __io_address(ARM_0_BELL2));
+	}
+}
+
+int
+vchiq_copy_from_user(void *dst, const void *src, int size)
+{
+	return copy_from_user(dst, src, size);
+}
+
+VCHIQ_STATUS_T
+vchiq_prepare_bulk_data(VCHIQ_BULK_T *bulk, VCHI_MEM_HANDLE_T memhandle,
+	void *offset, int size, int dir)
+{
+	PAGELIST_T *pagelist;
+	int ret;
+
+	vcos_assert(memhandle == VCHI_MEM_HANDLE_INVALID);
+
+	ret = create_pagelist((char __user *)offset, size,
+			(dir == VCHIQ_BULK_RECEIVE)
+			? PAGELIST_READ
+			: PAGELIST_WRITE,
+			current,
+			&pagelist);
+	if (ret != 0)
+		return VCHIQ_ERROR;
+
+	bulk->handle = memhandle;
+	bulk->data = VCHIQ_ARM_ADDRESS(pagelist);
+
+	/* Store the pagelist address in remote_data, which isn't used by the
+	   slave. */
+	bulk->remote_data = pagelist;
+
+	return VCHIQ_SUCCESS;
+}
+
+void
+vchiq_complete_bulk(VCHIQ_BULK_T *bulk)
+{
+	free_pagelist((PAGELIST_T *)bulk->remote_data, bulk->actual);
+}
+
+void
+vchiq_transfer_bulk(VCHIQ_BULK_T *bulk)
+{
+	/*
+	 * This should only be called on the master (VideoCore) side, but
+	 * provide an implementation to avoid the need for ifdefery.
+	 */
+	vcos_assert(!"This code should not be called by the ARM on BCM2835");
+}
+
+void
+vchiq_dump_platform_state(void *dump_context)
+{
+        char buf[80];
+        int len;
+        len = vcos_snprintf(buf, sizeof(buf),
+                "  Platform: 2835 (VC master)");
+        vchiq_dump(dump_context, buf, len + 1);
+}
+
+void
+vchiq_platform_paused(VCHIQ_STATE_T *state)
+{
+   vcos_unused(state);
+   vcos_assert_msg(0, "Suspend/resume not supported");
+}
+
+void
+vchiq_platform_resumed(VCHIQ_STATE_T *state)
+{
+   vcos_unused(state);
+   vcos_assert_msg(0, "Suspend/resume not supported");
+}
+
+VCHIQ_STATUS_T
+vchiq_use_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   if (!service)
+      return VCHIQ_ERROR;
+   return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_release_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   if (!service)
+      return VCHIQ_ERROR;
+   return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_check_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   if (!service)
+      return VCHIQ_ERROR;
+   return VCHIQ_SUCCESS;
+}
+
+/*
+ * Local functions
+ */
+
+static irqreturn_t
+vchiq_doorbell_irq(int irq, void *dev_id)
+{
+   VCHIQ_STATE_T *state = dev_id;
+	irqreturn_t ret = IRQ_NONE;
+	unsigned int status;
+
+	/* Read (and clear) the doorbell */
+	status = readl(__io_address(ARM_0_BELL0));
+
+	if (status & 0x4) {  /* Was the doorbell rung? */
+		remote_event_pollall(state);
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+/* There is a potential problem with partial cache lines (pages?)
+	at the ends of the block when reading. If the CPU accessed anything in
+	the same line (page?) then it may have pulled old data into the cache,
+	obscuring the new data underneath. We can solve this by transferring the
+	partial cache lines separately, and allowing the ARM to copy into the
+	cached area.
+
+	N.B. This implementation plays slightly fast and loose with the Linux
+	driver programming rules, e.g. its use of __virt_to_bus instead of
+	dma_map_single, but it isn't a multi-platform driver and it benefits
+	from increased speed as a result.
+ */
+
+static int
+create_pagelist(char __user *buf, size_t count, unsigned short type,
+	struct task_struct *task, PAGELIST_T ** ppagelist)
+{
+	PAGELIST_T *pagelist;
+	struct page **pages;
+	struct page *page;
+	unsigned long *addrs;
+	unsigned int num_pages, offset, i;
+	char *addr, *base_addr, *next_addr;
+	int run, addridx, actual_pages;
+
+	offset = (unsigned int)buf & (PAGE_SIZE - 1);
+	num_pages = (count + offset + PAGE_SIZE - 1) / PAGE_SIZE;
+
+	*ppagelist = NULL;
+
+	/* Allocate enough storage to hold the page pointers and the page list */
+	pagelist = (PAGELIST_T *) kmalloc(sizeof(PAGELIST_T) +
+		(num_pages * sizeof(unsigned long)) +
+		(num_pages * sizeof(pages[0])),
+		GFP_KERNEL);
+
+	vcos_log_trace("create_pagelist - %x", (unsigned int)pagelist);
+	if (!pagelist)
+		return -ENOMEM;
+
+	addrs = pagelist->addrs;
+	pages = (struct page **)(addrs + num_pages);
+
+	down_read(&task->mm->mmap_sem);
+	actual_pages = get_user_pages(task, task->mm,
+		(unsigned long)buf & ~(PAGE_SIZE - 1), num_pages,
+		(type == PAGELIST_READ) /*Write */ , 0 /*Force */ ,
+		pages, NULL /*vmas */ );
+	up_read(&task->mm->mmap_sem);
+
+        if (actual_pages != num_pages)
+	{
+		for (i = 0; i < actual_pages; i++) {
+			page_cache_release(pages[i]);
+		}
+		kfree(pagelist);
+		return -EINVAL;
+	}
+
+	pagelist->length = count;
+	pagelist->type = type;
+	pagelist->offset = offset;
+
+	/* Group the pages into runs of contiguous pages */
+
+	base_addr = VCHIQ_ARM_ADDRESS(page_address(pages[0]));
+	next_addr = base_addr + PAGE_SIZE;
+	addridx = 0;
+	run = 0;
+
+	for (i = 1; i < num_pages; i++) {
+		addr = VCHIQ_ARM_ADDRESS(page_address(pages[i]));
+		if ((addr == next_addr) && (run < (PAGE_SIZE - 1))) {
+			next_addr += PAGE_SIZE;
+			run++;
+		} else {
+			addrs[addridx] = (unsigned long)base_addr + run;
+			addridx++;
+			base_addr = addr;
+			next_addr = addr + PAGE_SIZE;
+			run = 0;
+		}
+	}
+
+	addrs[addridx] = (unsigned long)base_addr + run;
+	addridx++;
+
+	/* Partial cache lines (fragments) require special measures */
+	if ((type == PAGELIST_READ) &&
+		((pagelist->offset & (CACHE_LINE_SIZE - 1)) ||
+		((pagelist->offset + pagelist->length) & (CACHE_LINE_SIZE - 1)))) {
+		FRAGMENTS_T *fragments;
+
+		if (down_interruptible(&g_free_fragments_sema) != 0) {
+			kfree(pagelist);
+			return -EINTR;
+		}
+
+		vcos_assert(g_free_fragments != NULL);
+
+		down(&g_free_fragments_mutex);
+		fragments = (FRAGMENTS_T *) g_free_fragments;
+		vcos_assert(fragments != NULL);
+		g_free_fragments = *(FRAGMENTS_T **) g_free_fragments;
+		up(&g_free_fragments_mutex);
+		pagelist->type =
+			 PAGELIST_READ_WITH_FRAGMENTS + (fragments -
+							 g_fragments_base);
+	}
+
+	for (page = virt_to_page(pagelist);
+		page <= virt_to_page(addrs + num_pages - 1); page++) {
+		flush_dcache_page(page);
+	}
+
+	*ppagelist = pagelist;
+
+	return 0;
+}
+
+static void
+free_pagelist(PAGELIST_T *pagelist, int actual)
+{
+	struct page **pages;
+	unsigned int num_pages, i;
+
+	vcos_log_trace("free_pagelist - %x, %d", (unsigned int)pagelist, actual);
+
+	num_pages =
+		 (pagelist->length + pagelist->offset + PAGE_SIZE - 1) / PAGE_SIZE;
+
+	pages = (struct page **)(pagelist->addrs + num_pages);
+
+	/* Deal with any partial cache lines (fragments) */
+	if (pagelist->type >= PAGELIST_READ_WITH_FRAGMENTS) {
+		FRAGMENTS_T *fragments =
+			 g_fragments_base + (pagelist->type -
+					PAGELIST_READ_WITH_FRAGMENTS);
+		int head_bytes, tail_bytes;
+
+		if (actual >= 0)
+		{
+			if ((head_bytes = (CACHE_LINE_SIZE - pagelist->offset) & (CACHE_LINE_SIZE - 1)) != 0) {
+				if (head_bytes > actual)
+					head_bytes = actual;
+
+				memcpy((char *)page_address(pages[0]) +
+						 pagelist->offset, fragments->headbuf,
+						 head_bytes);
+			}
+			if ((head_bytes < actual) &&
+				(tail_bytes =
+				(pagelist->offset + actual) & (CACHE_LINE_SIZE -
+										1)) != 0) {
+				memcpy((char *)page_address(pages[num_pages - 1]) +
+						 ((pagelist->offset + actual) & (PAGE_SIZE -
+									1) & ~(CACHE_LINE_SIZE - 1)),
+						 fragments->tailbuf, tail_bytes);
+			}
+		}
+
+		down(&g_free_fragments_mutex);
+		*(FRAGMENTS_T **) fragments = g_free_fragments;
+		g_free_fragments = fragments;
+		up(&g_free_fragments_mutex);
+		up(&g_free_fragments_sema);
+	}
+
+	for (i = 0; i < num_pages; i++) {
+		if (pagelist->type != PAGELIST_WRITE)
+			set_page_dirty(pages[i]);
+		page_cache_release(pages[i]);
+	}
+
+	kfree(pagelist);
+}
+
+VCHIQ_STATUS_T
+vchiq_platform_suspend(VCHIQ_STATE_T *state)
+{
+   vcos_unused(state);
+   return VCHIQ_ERROR;
+}
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.c
new file mode 100644
index 0000000..2cb4364
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.c
@@ -0,0 +1,1293 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+
+#include "vchiq_core.h"
+#include "vchiq_ioctl.h"
+#include "vchiq_arm.h"
+
+#define DEVICE_NAME "vchiq"
+
+/* Override the default prefix, which would be vchiq_arm (from the filename) */
+#undef MODULE_PARAM_PREFIX
+#define MODULE_PARAM_PREFIX DEVICE_NAME "."
+
+#define VCHIQ_MINOR 0
+
+/* Some per-instance constants */
+#define MAX_COMPLETIONS 16
+#define MAX_SERVICES 64
+#define MAX_ELEMENTS 8
+#define MSG_QUEUE_SIZE 64
+
+#define VCOS_LOG_CATEGORY (&vchiq_arm_log_category)
+
+typedef struct client_service_struct {
+   VCHIQ_SERVICE_T *service;
+   void *userdata;
+   VCHIQ_INSTANCE_T instance;
+   int handle;
+   int is_vchi;
+   volatile int dequeue_pending;
+   volatile int message_available_pos;
+   volatile int msg_insert;
+   volatile int msg_remove;
+   VCOS_EVENT_T insert_event;
+   VCOS_EVENT_T remove_event;
+   VCHIQ_HEADER_T *msg_queue[MSG_QUEUE_SIZE];
+} USER_SERVICE_T;
+
+struct vchiq_instance_struct {
+   VCHIQ_STATE_T *state;
+   VCHIQ_COMPLETION_DATA_T completions[MAX_COMPLETIONS];
+   volatile int completion_insert;
+   volatile int completion_remove;
+   VCOS_EVENT_T insert_event;
+   VCOS_EVENT_T remove_event;
+
+   USER_SERVICE_T services[MAX_SERVICES];
+
+   int connected;
+   int closing;
+   int pid;
+   int mark;
+};
+
+typedef struct dump_context_struct
+{
+   char __user *buf;
+   size_t actual;
+   size_t space;
+   loff_t offset;
+} DUMP_CONTEXT_T;
+
+VCOS_LOG_CAT_T vchiq_arm_log_category;
+
+static struct cdev    vchiq_cdev;
+static dev_t          vchiq_devid;
+static VCHIQ_STATE_T g_state;
+static struct class  *vchiq_class;
+static struct device *vchiq_dev;
+
+static const char *ioctl_names[] =
+{
+   "CONNECT",
+   "SHUTDOWN",
+   "CREATE_SERVICE",
+   "REMOVE_SERVICE",
+   "QUEUE_MESSAGE",
+   "QUEUE_BULK_TRANSMIT",
+   "QUEUE_BULK_RECEIVE",
+   "AWAIT_COMPLETION",
+   "DEQUEUE_MESSAGE",
+   "GET_CLIENT_ID",
+   "GET_CONFIG",
+   "CLOSE_SERVICE",
+   "USE_SERVICE",
+   "RELEASE_SERIVCE"
+};
+
+VCOS_LOG_LEVEL_T vchiq_default_arm_log_level = VCOS_LOG_WARN;
+
+/****************************************************************************
+*
+*   find_service_by_handle
+*
+***************************************************************************/
+
+static inline USER_SERVICE_T *find_service_by_handle(
+	VCHIQ_INSTANCE_T instance, int handle )
+{
+   USER_SERVICE_T *user_service;
+
+   if (( handle >= 0 )
+      && ( handle < MAX_SERVICES ))
+   {
+      user_service = &instance->services[ handle ];
+
+      if ( user_service->service != NULL )
+      {
+         return user_service;
+      }
+   }
+
+   return NULL;
+}
+
+/****************************************************************************
+*
+*   find_avail_service_handle
+*
+***************************************************************************/
+
+static inline USER_SERVICE_T *find_avail_service_handle(
+   VCHIQ_INSTANCE_T instance)
+{
+   int handle;
+
+   for ( handle = 0; handle < MAX_SERVICES; handle++ )
+   {
+      if ( instance->services[handle].service == NULL )
+      {
+         instance->services[handle].instance = instance;
+         instance->services[handle].handle = handle;
+
+         return &instance->services[handle];
+      }
+   }
+   return NULL;
+}
+
+/****************************************************************************
+*
+*   add_completion
+*
+***************************************************************************/
+
+static VCHIQ_STATUS_T
+add_completion(VCHIQ_INSTANCE_T instance, VCHIQ_REASON_T reason,
+   VCHIQ_HEADER_T *header, USER_SERVICE_T *service, void *bulk_userdata)
+{
+   VCHIQ_COMPLETION_DATA_T *completion;
+   DEBUG_INITIALISE(g_state.local)
+
+   while (instance->completion_insert ==
+          (instance->completion_remove + MAX_COMPLETIONS)) {
+      /* Out of space - wait for the client */
+      DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+      vcos_log_trace("add_completion - completion queue full");
+      DEBUG_COUNT(COMPLETION_QUEUE_FULL_COUNT);
+      if (vcos_event_wait(&instance->remove_event) != VCOS_SUCCESS) {
+         vcos_log_info("service_callback interrupted");
+         return VCHIQ_RETRY;
+      } else if (instance->closing) {
+         vcos_log_info("service_callback closing");
+         return VCHIQ_ERROR;
+      }
+      DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+   }
+
+   completion =
+       &instance->
+       completions[instance->completion_insert & (MAX_COMPLETIONS - 1)];
+
+   completion->header = header;
+   completion->reason = reason;
+   completion->service_userdata = service;
+   completion->bulk_userdata = bulk_userdata;
+
+   /* A write barrier is needed here to ensure that the entire completion
+      record is written out before the insert point. */
+   vcos_wmb(&completion->bulk_userdata);
+
+   if (reason == VCHIQ_MESSAGE_AVAILABLE)
+      service->message_available_pos = instance->completion_insert;
+   instance->completion_insert++;
+
+   vcos_event_signal(&instance->insert_event);
+
+   return VCHIQ_SUCCESS;
+}
+
+/****************************************************************************
+*
+*   service_callback
+*
+***************************************************************************/
+
+static VCHIQ_STATUS_T
+service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header,
+   VCHIQ_SERVICE_HANDLE_T handle, void *bulk_userdata)
+{
+   /* How do we ensure the callback goes to the right client?
+      The service_user data points to a USER_SERVICE_T record containing the
+      original callback and the user state structure, which contains a circular
+      buffer for completion records.
+    */
+   USER_SERVICE_T *service =
+       (USER_SERVICE_T *) VCHIQ_GET_SERVICE_USERDATA(handle);
+   VCHIQ_INSTANCE_T instance = service->instance;
+   DEBUG_INITIALISE(g_state.local)
+
+   DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+   vcos_log_trace
+       ("service_callback - service %lx(%d), reason %d, header %lx, "
+        "instance %lx, bulk_userdata %lx",
+        (unsigned long)service, ((VCHIQ_SERVICE_T *) handle)->localport,
+        reason, (unsigned long)header,
+        (unsigned long)instance, (unsigned long)bulk_userdata);
+
+   if (!instance || instance->closing) {
+      return VCHIQ_SUCCESS;
+   }
+
+   if (header && service->is_vchi)
+   {
+      while (service->msg_insert == (service->msg_remove + MSG_QUEUE_SIZE))
+      {
+         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+         DEBUG_COUNT(MSG_QUEUE_FULL_COUNT);
+         vcos_log_trace("service_callback - msg queue full");
+         /* If there is no MESSAGE_AVAILABLE in the completion queue, add one */
+         if ((service->message_available_pos - instance->completion_remove) < 0)
+         {
+            VCHIQ_STATUS_T status;
+            vcos_log_warn("Inserting extra MESSAGE_AVAILABLE");
+            DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+            status = add_completion(instance, reason, NULL, service, bulk_userdata);
+            if (status != VCHIQ_SUCCESS)
+            {
+               DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+               return status;
+            }
+         }
+         
+         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+         if (vcos_event_wait(&service->remove_event) != VCOS_SUCCESS) {
+            vcos_log_info("service_callback interrupted");
+            DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+            return VCHIQ_RETRY;
+         } else if (instance->closing) {
+            vcos_log_info("service_callback closing");
+            DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+            return VCHIQ_ERROR;
+         }
+         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+      }
+
+      service->msg_queue[service->msg_insert & (MSG_QUEUE_SIZE - 1)] =
+         header;
+
+      /* A write memory barrier is needed to ensure that the store of header
+         is completed before the insertion point is updated */
+      vcos_wmb(&service->msg_queue[service->msg_insert & (MSG_QUEUE_SIZE - 1)]);
+
+      service->msg_insert++;
+      vcos_event_signal(&service->insert_event);
+
+      /* If there is a thread waiting in DEQUEUE_MESSAGE, or if
+         there is a MESSAGE_AVAILABLE in the completion queue then
+         bypass the completion queue. */
+      if (((service->message_available_pos - instance->completion_remove) >= 0) ||
+          service->dequeue_pending)
+      {
+         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+         service->dequeue_pending = 0;
+         return VCHIQ_SUCCESS;
+      }
+
+      header = NULL;
+   }
+   DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+
+   return add_completion(instance, reason, header, service, bulk_userdata);
+}
+
+/****************************************************************************
+*
+*   vchiq_ioctl
+*
+***************************************************************************/
+
+static long
+vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+   VCHIQ_INSTANCE_T instance = file->private_data;
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+   long ret = 0;
+   int i, rc;
+   DEBUG_INITIALISE(g_state.local)
+
+   vcos_log_trace("vchiq_ioctl - instance %x, cmd %s, arg %lx",
+      (unsigned int)instance,
+      ((_IOC_TYPE(cmd) == VCHIQ_IOC_MAGIC) && (_IOC_NR(cmd) <= VCHIQ_IOC_MAX)) ?
+      ioctl_names[_IOC_NR(cmd)] : "<invalid>", arg);
+
+   switch (cmd) {
+   case VCHIQ_IOC_SHUTDOWN:
+      if (!instance->connected)
+         break;
+
+      /* Remove all services */
+      for (i = 0; i < MAX_SERVICES; i++) {
+         USER_SERVICE_T *service = &instance->services[i];
+         if (service->service != NULL) {
+            status = vchiq_remove_service(&service->service->base);
+            if (status != VCHIQ_SUCCESS)
+               break;
+            service->service = NULL;
+         }
+      }
+
+      if (status == VCHIQ_SUCCESS) {
+         /* Wake the completion thread and ask it to exit */
+         instance->closing = 1;
+         vcos_event_signal(&instance->insert_event);
+      }
+
+      break;
+
+   case VCHIQ_IOC_CONNECT:
+      if (instance->connected) {
+         ret = -EINVAL;
+         break;
+      }
+      if ((rc=vcos_mutex_lock(&instance->state->mutex)) != VCOS_SUCCESS) {
+         vcos_log_error("vchiq: connect: could not lock mutex for state %d: %d",
+                        instance->state->id, rc);
+         ret = -EINTR;
+         break;
+      }
+      status = vchiq_connect_internal(instance->state, instance);
+      vcos_mutex_unlock(&instance->state->mutex);
+
+      if (status == VCHIQ_SUCCESS)
+         instance->connected = 1;
+      else
+         vcos_log_error("vchiq: could not connect: %d", status);
+      break;
+
+   case VCHIQ_IOC_CREATE_SERVICE:
+      {
+         VCHIQ_CREATE_SERVICE_T args;
+         VCHIQ_SERVICE_T *service = NULL;
+         USER_SERVICE_T *user_service = NULL;
+         void *userdata;
+         int srvstate;
+
+         if (copy_from_user
+             (&args, (const void __user *)arg,
+              sizeof(args)) != 0) {
+            ret = -EFAULT;
+            break;
+         }
+
+         for (i = 0; i < MAX_SERVICES; i++) {
+            if (instance->services[i].service == NULL) {
+               user_service = &instance->services[i];
+               break;
+            }
+         }
+
+         if (!user_service) {
+            ret = -EMFILE;
+            break;
+         }
+
+         if (args.is_open) {
+            if (instance->connected)
+               srvstate = VCHIQ_SRVSTATE_OPENING;
+            else {
+               ret = -ENOTCONN;
+               break;
+            }
+         } else {
+            srvstate =
+                instance->connected ?
+                VCHIQ_SRVSTATE_LISTENING :
+                VCHIQ_SRVSTATE_HIDDEN;
+         }
+
+         vcos_mutex_lock(&instance->state->mutex);
+
+         userdata = args.params.userdata;
+         args.params.callback = service_callback;
+         args.params.userdata = user_service;
+         service =
+             vchiq_add_service_internal(instance->state,
+                         &args.params, srvstate,
+                         instance);
+
+         vcos_mutex_unlock(&instance->state->mutex);
+
+         if (service != NULL) {
+            user_service->service = service;
+            user_service->userdata = userdata;
+            user_service->instance = instance;
+            user_service->handle = i;
+            user_service->is_vchi = args.is_vchi;
+            user_service->dequeue_pending = 0;
+            user_service->message_available_pos = instance->completion_remove - 1;
+            user_service->msg_insert = 0;
+            user_service->msg_remove = 0;
+            vcos_event_create(&user_service->insert_event, "insert_event");
+            vcos_event_create(&user_service->remove_event, "remove_event");
+
+            if (args.is_open) {
+               status =
+                   vchiq_open_service_internal
+                   (service, instance->pid);
+               if (status != VCHIQ_SUCCESS) {
+                  vchiq_remove_service
+                      (&service->base);
+                  ret =
+                      (status ==
+                       VCHIQ_RETRY) ? -EINTR :
+                      -EIO;
+                  user_service->service = NULL;
+                  user_service->instance = NULL;
+                  vcos_event_delete(&user_service->insert_event);
+                  vcos_event_delete(&user_service->remove_event);
+                  break;
+               }
+            }
+
+            if (copy_to_user((void __user *)
+                   &(((VCHIQ_CREATE_SERVICE_T __user
+                       *) arg)->handle),
+                   (const void *)&user_service->
+                   handle,
+                   sizeof(user_service->
+                     handle)) != 0)
+               ret = -EFAULT;
+         } else {
+            ret = -EEXIST;
+         }
+      }
+      break;
+
+   case VCHIQ_IOC_CLOSE_SERVICE:
+      {
+         USER_SERVICE_T *user_service;
+         int handle = (int)arg;
+
+         user_service = find_service_by_handle(instance, handle);
+         if (user_service != NULL)
+         {
+            int is_server = (user_service->service->public_fourcc != VCHIQ_FOURCC_INVALID);
+
+            status =
+                vchiq_close_service(&user_service->service->base);
+            if ((status == VCHIQ_SUCCESS) && !is_server)
+            {
+               vcos_event_delete(&user_service->insert_event);
+               vcos_event_delete(&user_service->remove_event);
+               user_service->service = NULL;
+            }
+         } else
+            ret = -EINVAL;
+      }
+      break;
+
+   case VCHIQ_IOC_REMOVE_SERVICE:
+      {
+         USER_SERVICE_T *user_service;
+         int handle = (int)arg;
+
+         user_service = find_service_by_handle(instance, handle);
+         if (user_service != NULL)
+         {
+            status =
+                vchiq_remove_service(&user_service->service->base);
+            if (status == VCHIQ_SUCCESS)
+            {
+               vcos_event_delete(&user_service->insert_event);
+               vcos_event_delete(&user_service->remove_event);
+               user_service->service = NULL;
+            }
+         } else
+            ret = -EINVAL;
+      }
+      break;
+
+   case VCHIQ_IOC_USE_SERVICE:
+   case VCHIQ_IOC_RELEASE_SERVICE:
+      {
+         USER_SERVICE_T *user_service;
+         int handle = (int)arg;
+
+         user_service = find_service_by_handle(instance, handle);
+         if (user_service != NULL)
+         {
+            status = (cmd == VCHIQ_IOC_USE_SERVICE) ? vchiq_use_service(&user_service->service->base) : vchiq_release_service(&user_service->service->base);
+            if (status != VCHIQ_SUCCESS)
+            {
+               ret = -EINVAL; // ???
+            }
+         }
+      }
+      break;
+
+   case VCHIQ_IOC_QUEUE_MESSAGE:
+      {
+         VCHIQ_QUEUE_MESSAGE_T args;
+         USER_SERVICE_T *user_service;
+
+         if (copy_from_user
+             (&args, (const void __user *)arg,
+              sizeof(args)) != 0) {
+            ret = -EFAULT;
+            break;
+         }
+         user_service = find_service_by_handle(instance, args.handle);
+         if ((user_service != NULL) && (args.count <= MAX_ELEMENTS))
+         {
+            /* Copy elements into kernel space */
+            VCHIQ_ELEMENT_T elements[MAX_ELEMENTS];
+            if (copy_from_user
+                (elements, args.elements,
+                 args.count * sizeof(VCHIQ_ELEMENT_T)) == 0)
+               status =
+                  vchiq_queue_message
+                  (&user_service->service->base,
+                   elements, args.count);
+            else
+               ret = -EFAULT;
+         } else {
+            ret = -EINVAL;
+         }
+      }
+      break;
+
+   case VCHIQ_IOC_QUEUE_BULK_TRANSMIT:
+   case VCHIQ_IOC_QUEUE_BULK_RECEIVE:
+      {
+         VCHIQ_QUEUE_BULK_TRANSFER_T args;
+         USER_SERVICE_T *user_service;
+         VCHIQ_BULK_DIR_T dir =
+           (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT) ?
+           VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE;
+
+         if (copy_from_user
+             (&args, (const void __user *)arg,
+              sizeof(args)) != 0) {
+            ret = -EFAULT;
+            break;
+         }
+         user_service = find_service_by_handle(instance, args.handle);
+         if (user_service != NULL)
+         {
+            status =
+               vchiq_bulk_transfer
+               ((VCHIQ_SERVICE_T *)user_service->service,
+                VCHI_MEM_HANDLE_INVALID,
+                args.data, args.size,
+                args.userdata, args.mode,
+                dir);
+         } else {
+            ret = -EINVAL;
+         }
+      }
+      break;
+
+   case VCHIQ_IOC_AWAIT_COMPLETION:
+      {
+         VCHIQ_AWAIT_COMPLETION_T args;
+
+         DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+         if (!instance->connected) {
+            ret = -ENOTCONN;
+            break;
+         }
+
+         if (copy_from_user
+             (&args, (const void __user *)arg,
+              sizeof(args)) != 0) {
+            ret = -EFAULT;
+            break;
+         }
+         DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+         while ((instance->completion_remove ==
+            instance->completion_insert)
+                && !instance->closing) {
+            DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+            if (vcos_event_wait(&instance->insert_event) !=
+                VCOS_SUCCESS) {
+               DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+               vcos_log_info
+                   ("AWAIT_COMPLETION interrupted");
+               ret = -EINTR;
+               break;
+            }
+         }
+         DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+
+         /* A read memory barrier is needed to stop prefetch of a stale
+            completion record */
+         vcos_rmb();
+
+         if (ret == 0) {
+            int msgbufcount = args.msgbufcount;
+            for (ret = 0; ret < args.count; ret++) {
+               VCHIQ_COMPLETION_DATA_T *completion;
+               USER_SERVICE_T *service;
+               VCHIQ_HEADER_T *header;
+               if (instance->completion_remove ==
+                   instance->completion_insert)
+                  break;
+               completion =
+                   &instance->
+                   completions
+                   [instance->completion_remove &
+                    (MAX_COMPLETIONS - 1)];
+
+               service = (USER_SERVICE_T *)completion->service_userdata;
+               completion->service_userdata = service->userdata;
+
+               header = completion->header;
+               if (header)
+               {
+                  void __user *msgbuf;
+                  int msglen;
+
+                  msglen = header->size + sizeof(VCHIQ_HEADER_T);
+                  /* This must be a VCHIQ-style service */
+                  if (args.msgbufsize < msglen)
+                  {
+                     vcos_log_error("header %x: msgbufsize %x < msglen %x",
+                        (unsigned int)header, args.msgbufsize, msglen);
+                     vcos_assert(0);
+                     if (ret == 0)
+                        ret = -EMSGSIZE;
+                     break;
+                  }
+                  if (msgbufcount <= 0)
+                  {
+                     /* Stall here for lack of a buffer for the message */
+                     break;
+                  }
+                  /* Get the pointer from user space */
+                  msgbufcount--;
+                  if (copy_from_user(&msgbuf,
+                     (const void __user *)&args.msgbufs[msgbufcount],
+                     sizeof(msgbuf)) != 0)
+                  {
+                     if (ret == 0)
+                        ret = -EFAULT;
+                     break;
+                  }
+
+                  /* Copy the message to user space */
+                  if (copy_to_user(msgbuf, header, msglen) != 0)
+                  {
+                     if (ret == 0)
+                        ret = -EFAULT;
+                     break;
+                  }
+
+                  /* Now it has been copied, the message can be released. */
+                  vchiq_release_message(&service->service->base, header);
+
+                  /* The completion must point to the msgbuf */
+                  completion->header = msgbuf;
+               }
+
+               if (copy_to_user
+                   ((void __user *)((size_t) args.buf +
+                          ret *
+                          sizeof
+                          (VCHIQ_COMPLETION_DATA_T)),
+                    completion,
+                    sizeof(VCHIQ_COMPLETION_DATA_T)) !=
+                   0) {
+                  if (ret == 0)
+                     ret = -EFAULT;
+                  break;
+               }
+               instance->completion_remove++;
+            }
+
+            if (msgbufcount != args.msgbufcount)
+            {
+               if (copy_to_user((void __user *)
+                  &((VCHIQ_AWAIT_COMPLETION_T *)arg)->msgbufcount,
+                  &msgbufcount, sizeof(msgbufcount)) != 0)
+               {
+                  ret = -EFAULT;
+                  break;
+               }
+            }
+         }
+
+         if (ret != 0)
+            vcos_event_signal(&instance->remove_event);
+         DEBUG_TRACE(AWAIT_COMPLETION_LINE);
+      }
+      break;
+
+   case VCHIQ_IOC_DEQUEUE_MESSAGE:
+      {
+         VCHIQ_DEQUEUE_MESSAGE_T args;
+         USER_SERVICE_T *user_service;
+         VCHIQ_HEADER_T *header;
+
+         DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+         if (copy_from_user
+             (&args, (const void __user *)arg,
+              sizeof(args)) != 0) {
+            ret = -EFAULT;
+            break;
+         }
+         user_service = &instance->services[args.handle];
+         if ((args.handle < 0) || (args.handle >= MAX_SERVICES) ||
+            (user_service->service == NULL) ||
+            (user_service->is_vchi == 0)) {
+            ret = -EINVAL;
+            break;
+         }
+         if (user_service->msg_remove == user_service->msg_insert)
+         {
+            if (!args.blocking)
+            {
+               DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+               ret = -EWOULDBLOCK;
+               break;
+            }
+            user_service->dequeue_pending = 1;
+            do {
+               DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+               if (vcos_event_wait(&user_service->insert_event) !=
+                   VCOS_SUCCESS) {
+                  vcos_log_info("DEQUEUE_MESSAGE interrupted");
+                  ret = -EINTR;
+                  break;
+               }
+            }
+            while (user_service->msg_remove == user_service->msg_insert);
+         }
+
+         /* A read memory barrier is needed to stop prefetch of a stale
+            header value */
+         vcos_rmb();
+
+         header = user_service->msg_queue[user_service->msg_remove &
+            (MSG_QUEUE_SIZE - 1)];
+         if (header == NULL)
+            ret = -ENOTCONN;
+         else if (header->size <= args.bufsize)
+         {
+            /* Copy to user space if msgbuf is not NULL */
+            if ((args.buf == NULL) ||
+               (copy_to_user((void __user *)args.buf, header->data,
+               header->size) == 0))
+            {
+               ret = header->size;
+               vchiq_release_message(&user_service->service->base,
+                  header);
+               user_service->msg_remove++;
+               vcos_event_signal(&user_service->remove_event);
+            }
+            else
+               ret = -EFAULT;
+         }
+         else
+         {
+            vcos_log_error("header %x: bufsize %x < size %x",
+               (unsigned int)header, args.bufsize, header->size);
+            vcos_assert(0);
+            ret = -EMSGSIZE;
+         }
+         DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+      }
+      break;
+
+   case VCHIQ_IOC_GET_CLIENT_ID:
+      {
+         USER_SERVICE_T *user_service;
+         int handle = (int)arg;
+
+         user_service = find_service_by_handle(instance, handle);
+         if (user_service != NULL)
+            ret = vchiq_get_client_id(&user_service->service->base);
+         else
+            ret = 0;
+      }
+      break;
+
+   case VCHIQ_IOC_GET_CONFIG:
+      {
+         VCHIQ_GET_CONFIG_T args;
+         VCHIQ_CONFIG_T config;
+
+         if (copy_from_user
+             (&args, (const void __user *)arg,
+              sizeof(args)) != 0) {
+            ret = -EFAULT;
+            break;
+         }
+         if (args.config_size > sizeof(config))
+         {
+            ret = -EINVAL;
+            break;
+         }
+         status = vchiq_get_config(instance, args.config_size, &config);
+         if (status == VCHIQ_SUCCESS)
+         {
+            if (copy_to_user((void __user *)args.pconfig,
+                   &config, args.config_size) != 0)
+            {
+               ret = -EFAULT;
+               break;
+            }
+         }
+      }
+      break;
+
+   case VCHIQ_IOC_SET_SERVICE_OPTION:
+      {
+         VCHIQ_SET_SERVICE_OPTION_T args;
+         USER_SERVICE_T *user_service;
+
+         if (copy_from_user(
+            &args, (const void __user *)arg,
+            sizeof(args)) != 0)
+         {
+            ret = -EFAULT;
+            break;
+         }
+
+         user_service = find_service_by_handle(instance, args.handle);
+         if (user_service != NULL)
+         {
+            status = vchiq_set_service_option(
+               &user_service->service->base,
+               args.option, args.value);
+         }
+         else
+         {
+            ret = -EINVAL;
+         }
+      }
+      break;
+
+   default:
+      ret = -ENOTTY;
+      break;
+   }
+
+   if (ret == 0) {
+      if (status == VCHIQ_ERROR)
+         ret = -EIO;
+      else if (status == VCHIQ_RETRY)
+         ret = -EINTR;
+   }
+
+   if ((ret < 0) && (ret != -EINTR) && (ret != -EWOULDBLOCK))
+      vcos_log_warn("  ioctl instance %lx, cmd %s -> status %d, %ld",
+         (unsigned long)instance,
+         (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ? ioctl_names[_IOC_NR(cmd)] :
+         "<invalid>", status, ret);
+   else
+      vcos_log_trace("  ioctl instance %lx, cmd %s -> status %d, %ld",
+         (unsigned long)instance,
+         (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ? ioctl_names[_IOC_NR(cmd)] :
+         "<invalid>", status, ret);
+
+   return ret;
+}
+
+/****************************************************************************
+*
+*   vchiq_open
+*
+***************************************************************************/
+
+static int
+vchiq_open(struct inode *inode, struct file *file)
+{
+   int dev = iminor(inode) & 0x0f;
+   vcos_log_info("vchiq_open");
+   switch (dev) {
+   case VCHIQ_MINOR:
+      {
+         VCHIQ_STATE_T *state = vchiq_get_state();
+         VCHIQ_INSTANCE_T instance;
+
+         if (!state)
+         {
+            vcos_log_error( "vchiq has no connection to VideoCore");
+            return -ENOTCONN;
+         }
+
+         instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+         if (!instance)
+            return -ENOMEM;
+
+         instance->state = state;
+         instance->pid = current->tgid;
+         vcos_event_create(&instance->insert_event, DEVICE_NAME);
+         vcos_event_create(&instance->remove_event, DEVICE_NAME);
+
+         file->private_data = instance;
+      }
+      break;
+
+   default:
+      vcos_log_error("Unknown minor device: %d", dev);
+      return -ENXIO;
+   }
+
+   return 0;
+}
+
+/****************************************************************************
+*
+*   vchiq_release
+*
+***************************************************************************/
+
+static int
+vchiq_release(struct inode *inode, struct file *file)
+{
+   int dev = iminor(inode) & 0x0f;
+   int ret = 0;
+   switch (dev) {
+   case VCHIQ_MINOR:
+      {
+         VCHIQ_INSTANCE_T instance = file->private_data;
+         int i;
+
+         vcos_log_info("vchiq_release: instance=%lx",
+                  (unsigned long)instance);
+
+         instance->closing = 1;
+
+         /* Wake the slot handler if the completion queue is full */
+         vcos_event_signal(&instance->remove_event);
+
+         /* Mark all services for termination... */
+
+         for (i = 0; i < MAX_SERVICES; i++) {
+            USER_SERVICE_T *user_service =
+                &instance->services[i];
+            if (user_service->service != NULL)
+            {
+               /* Wake the slot handler if the msg queue is full */
+               vcos_event_signal(&user_service->remove_event);
+
+               if ((user_service->service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) &&
+                  (user_service->service->srvstate != VCHIQ_SRVSTATE_LISTENING))
+               {
+                  vchiq_terminate_service_internal(user_service->service);
+               }
+            }
+         }
+
+         /* ...and wait for them to die */
+
+         for (i = 0; i < MAX_SERVICES; i++) {
+            USER_SERVICE_T *user_service =
+                &instance->services[i];
+            if (user_service->service != NULL)
+            {
+               /* Wait in this non-portable fashion because interruptible
+                  calls will not block in this context. */
+               while ((user_service->service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) &&
+                  (user_service->service->srvstate != VCHIQ_SRVSTATE_LISTENING))
+               {
+                  down(&user_service->service->remove_event);
+               }
+
+               vchiq_free_service_internal
+                      (user_service->service);
+            }
+         }
+
+         vcos_event_delete(&instance->insert_event);
+         vcos_event_delete(&instance->remove_event);
+
+         kfree(instance);
+         file->private_data = NULL;
+      }
+      break;
+
+   default:
+      vcos_log_error("Unknown minor device: %d", dev);
+      ret = -ENXIO;
+   }
+
+   return ret;
+}
+
+/****************************************************************************
+*
+*   vchiq_dump
+*
+***************************************************************************/
+
+void
+vchiq_dump(void *dump_context, const char *str, int len)
+{
+   DUMP_CONTEXT_T *context = (DUMP_CONTEXT_T *)dump_context;
+
+   if ((context->actual >= 0) && (context->actual < context->space))
+   {
+      int copy_bytes;
+      if (context->offset > 0)
+      {
+         int skip_bytes = vcos_min(len, context->offset);
+         str += skip_bytes;
+         len -= skip_bytes;
+         context->offset -= skip_bytes;
+         if (context->offset > 0)
+            return;
+      }
+      copy_bytes = vcos_min(len, context->space - context->actual);
+      if (copy_bytes == 0)
+         return;
+      if (copy_to_user(context->buf + context->actual, str, copy_bytes))
+         context->actual = -EFAULT;
+      context->actual += copy_bytes;
+      len -= copy_bytes;
+
+      /* If tne terminating NUL is included in the length, then it marks
+       * the end of a line and should be replaced with a carriage return.
+       */
+      if ((len == 0) && (str[copy_bytes - 1] == '\0'))
+      {
+         char cr = '\n';
+         if (copy_to_user(context->buf + context->actual - 1, &cr, 1))
+         {
+	    context->actual = -EFAULT;
+         }
+      }
+   }
+}
+
+/****************************************************************************
+*
+*   vchiq_dump_platform_instance_state
+*
+***************************************************************************/
+
+void
+vchiq_dump_platform_instances(void *dump_context)
+{
+   VCHIQ_STATE_T *state = vchiq_get_state();
+   char buf[80];
+   int len;
+   int i;
+
+   /* There is no list of instances, so instead scan all services,
+      marking those that have been dumped. */
+
+   for (i = 0; i < state->unused_service; i++)
+   {
+      VCHIQ_SERVICE_T *service = state->services[i];
+      VCHIQ_INSTANCE_T instance;
+
+      if (service
+         && ((instance = service->instance) != NULL)
+         && (service->base.callback == service_callback))
+         instance->mark = 0;
+   }
+
+   for (i = 0; i < state->unused_service; i++)
+   {
+      VCHIQ_SERVICE_T *service = state->services[i];
+      VCHIQ_INSTANCE_T instance;
+
+      if (service
+         && ((instance = service->instance) != NULL)
+         && (service->base.callback == service_callback))
+      {
+         if (!instance->mark)
+         {
+            len = vcos_snprintf(buf, sizeof(buf),
+               "Instance %x: pid %d,%s completions %d/%d",
+               (unsigned int)instance, instance->pid,
+               instance->connected ? " connected," : "",
+               instance->completion_insert - instance->completion_remove,
+               MAX_COMPLETIONS);
+
+            vchiq_dump(dump_context, buf, len + 1);
+
+            instance->mark = 1;
+         }
+      }
+   }
+}
+
+/****************************************************************************
+*
+*   vchiq_dump_platform_service_state
+*
+***************************************************************************/
+
+void
+vchiq_dump_platform_service_state(void *dump_context, VCHIQ_SERVICE_T *service)
+{
+   USER_SERVICE_T *user_service = (USER_SERVICE_T *)service->base.userdata;
+   char buf[80];
+   int len;
+
+   len = vcos_snprintf(buf, sizeof(buf), "  instance %x",
+      service->instance);
+
+   if ((service->base.callback == service_callback) && user_service->is_vchi)
+   {
+      len += vcos_snprintf(buf + len, sizeof(buf) - len,
+         ", %d/%d messages",
+         user_service->msg_insert - user_service->msg_remove,
+         MSG_QUEUE_SIZE);
+
+      if (user_service->dequeue_pending)
+         len += vcos_snprintf(buf + len, sizeof(buf) - len,
+            " (dequeue pending)");
+   }
+
+   vchiq_dump(dump_context, buf, len + 1);
+}
+
+/****************************************************************************
+*
+*   vchiq_read
+*
+***************************************************************************/
+
+static ssize_t
+vchiq_read(struct file * file, char __user * buf,
+   size_t count, loff_t *ppos)
+{
+   DUMP_CONTEXT_T context;
+   context.buf = buf;
+   context.actual = 0;
+   context.space = count;
+   context.offset = *ppos;
+
+   vchiq_dump_state(&context, &g_state);
+
+   if (context.actual >= 0)
+      *ppos += context.actual;
+
+   return context.actual;
+}
+
+VCHIQ_STATE_T *
+vchiq_get_state(void)
+{
+
+   if (g_state.remote == NULL)
+   {
+      printk( "%s: g_state.remote == NULL\n", __func__ );
+   }
+   else
+   {
+      if ( g_state.remote->initialised != 1)
+      {
+         printk( "%s: g_state.remote->initialised != 1 (%d)\n", __func__, g_state.remote->initialised );
+      }
+   }
+
+   return ((g_state.remote != NULL) &&
+      (g_state.remote->initialised == 1)) ? &g_state : NULL;
+}
+
+static const struct file_operations
+vchiq_fops = {
+   .owner = THIS_MODULE,
+   .unlocked_ioctl = vchiq_ioctl,
+   .open = vchiq_open,
+   .release = vchiq_release,
+   .read = vchiq_read
+};
+
+/****************************************************************************
+*
+*   vchiq_init - called when the module is loaded.
+*
+***************************************************************************/
+
+static int __init
+vchiq_init(void)
+{
+   int err;
+   void *ptr_err;
+
+   err = vchiq_platform_vcos_init();
+   if (err != 0)
+      goto failed_platform_vcos_init;
+
+   vcos_log_set_level(VCOS_LOG_CATEGORY, vchiq_default_arm_log_level);
+   vcos_log_register("vchiq_arm", VCOS_LOG_CATEGORY);
+
+   if ((err =
+        alloc_chrdev_region(&vchiq_devid, VCHIQ_MINOR, 1,
+             DEVICE_NAME)) != 0) {
+      vcos_log_error("Unable to allocate device number");
+      goto failed_alloc_chrdev;
+   }
+   cdev_init(&vchiq_cdev, &vchiq_fops);
+   vchiq_cdev.owner = THIS_MODULE;
+   if ((err = cdev_add(&vchiq_cdev, vchiq_devid, 1)) != 0) {
+      vcos_log_error("Unable to register device");
+      goto failed_cdev_add;
+   }
+
+   /* create sysfs entries */
+   vchiq_class = class_create(THIS_MODULE, DEVICE_NAME);
+   if (IS_ERR(ptr_err = vchiq_class))
+      goto failed_class_create;
+
+   vchiq_dev = device_create(vchiq_class, NULL,
+      vchiq_devid, NULL, "vchiq");
+   if (IS_ERR(ptr_err = vchiq_dev))
+      goto failed_device_create;
+
+   err = vchiq_platform_init(&g_state);
+   if (err != 0)
+      goto failed_platform_init;
+
+   vcos_log_error("vchiq: initialised - version %d (min %d), device %d.%d",
+      VCHIQ_VERSION, VCHIQ_VERSION_MIN,
+      MAJOR(vchiq_devid), MINOR(vchiq_devid));
+
+   return 0;
+
+failed_platform_init:
+   device_destroy(vchiq_class, vchiq_devid);
+failed_device_create:
+   class_destroy(vchiq_class);
+failed_class_create:
+   cdev_del(&vchiq_cdev);
+   err = PTR_ERR(ptr_err);
+failed_cdev_add:
+   unregister_chrdev_region(vchiq_devid, 1);
+failed_alloc_chrdev:
+failed_platform_vcos_init:
+   printk(KERN_WARNING "could not load vchiq\n");
+   return err;
+}
+/****************************************************************************
+*
+*   vchiq_exit - called when the module is unloaded.
+*
+***************************************************************************/
+
+static void __exit
+vchiq_exit(void)
+{
+   vchiq_platform_exit(&g_state);
+   device_destroy(vchiq_class, vchiq_devid);
+   class_destroy(vchiq_class);
+   cdev_del(&vchiq_cdev);
+   unregister_chrdev_region(vchiq_devid, 1);
+   vcos_log_unregister(VCOS_LOG_CATEGORY);
+}
+
+module_init(vchiq_init);
+module_exit(vchiq_exit);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Broadcom Corporation");
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.h
new file mode 100644
index 0000000..679e0a3
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_ARM_H
+#define VCHIQ_ARM_H
+
+#include "vchiq_core.h"
+
+extern VCOS_LOG_CAT_T vchiq_arm_log_category;
+
+extern int __init
+vchiq_platform_vcos_init(void);
+
+extern int __init
+vchiq_platform_init(VCHIQ_STATE_T *state);
+
+extern void __exit
+vchiq_platform_exit(VCHIQ_STATE_T *state);
+
+extern VCHIQ_STATE_T *
+vchiq_get_state(void);
+
+#endif /* VCHIQ_ARM_H */
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_cfg.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_cfg.h
new file mode 100644
index 0000000..cd51d99
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_cfg.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_CFG_H
+#define VCHIQ_CFG_H
+
+#define VCHIQ_MAGIC              VCHIQ_MAKE_FOURCC('V','C','H','I')
+/* The version of VCHIQ - change with any non-trivial change */
+#define VCHIQ_VERSION            2
+/* The minimum compatible version - update to match VCHIQ_VERSION with any incompatible change */
+#define VCHIQ_VERSION_MIN        2
+
+#define VCHIQ_MAX_SERVICES       4096
+#define VCHIQ_MAX_SLOTS          128
+#define VCHIQ_MAX_SLOTS_PER_SIDE 64
+
+#define VCHIQ_NUM_CURRENT_BULKS        32
+#define VCHIQ_NUM_SERVICE_BULKS        4
+
+#ifndef VCHIQ_ENABLE_DEBUG
+#define VCHIQ_ENABLE_DEBUG             1
+#endif
+
+#ifndef VCHIQ_ENABLE_STATS
+#define VCHIQ_ENABLE_STATS             1
+#endif
+
+#endif /* VCHIQ_CFG_H */
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.c
new file mode 100644
index 0000000..62e3006
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.c
@@ -0,0 +1,101 @@
+/*****************************************************************************
+* Copyright 2001 - 2010 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+#include "vcos.h"
+#include "vchiq_connected.h"
+#include <linux/module.h>
+
+#define  MAX_CALLBACKS  10
+
+static   int                        g_connected = 0;
+static   int                        g_num_deferred_callbacks;
+static   VCHIQ_CONNECTED_CALLBACK_T g_deferred_callback[ MAX_CALLBACKS ];
+static   VCOS_ONCE_T                g_once_init;
+static   VCOS_MUTEX_T               g_connected_mutex;
+
+extern VCOS_LOG_CAT_T vchiq_core_log_category;
+#define  VCOS_LOG_CATEGORY (&vchiq_core_log_category)
+
+/****************************************************************************
+*
+* Function to initialize our lock.
+*
+***************************************************************************/
+
+static void connected_init( void )
+{
+   vcos_mutex_create( &g_connected_mutex, "connected_mutex");
+}
+
+/****************************************************************************
+*
+* This function is used to defer initialization until the vchiq stack is
+* initialized. If the stack is already initialized, then the callback will
+* be made immediately, otherwise it will be deferred until
+* vchiq_call_connected_callbacks is called.
+*
+***************************************************************************/
+
+void vchiq_add_connected_callback( VCHIQ_CONNECTED_CALLBACK_T callback )
+{
+   vcos_once( &g_once_init, connected_init );
+
+   vcos_mutex_lock( &g_connected_mutex );
+
+   if ( g_connected )
+   {
+      // We're already connected. Call the callback immediately.
+
+      callback();
+   }
+   else
+   {
+      if ( g_num_deferred_callbacks >= MAX_CALLBACKS )
+      {
+         vcos_log_error( "There already %d callback registered - please increase MAX_CALLBACKS",
+                         g_num_deferred_callbacks );
+      }
+      else
+      {
+         g_deferred_callback[ g_num_deferred_callbacks ] = callback;
+         g_num_deferred_callbacks++;
+      }
+   }
+   vcos_mutex_unlock( &g_connected_mutex );
+}
+
+/****************************************************************************
+*
+* This function is called by the vchiq stack once it has been connected to
+* the videocore and clients can start to use the stack.
+*
+***************************************************************************/
+
+void vchiq_call_connected_callbacks( void )
+{
+   int   i;
+
+   vcos_once( &g_once_init, connected_init );
+
+   vcos_mutex_lock( &g_connected_mutex );
+   for ( i = 0; i <  g_num_deferred_callbacks; i++ )\
+   {
+      g_deferred_callback[i]();
+   }
+   g_num_deferred_callbacks = 0;
+   g_connected = 1;
+   vcos_mutex_unlock( &g_connected_mutex );
+}
+
+EXPORT_SYMBOL( vchiq_add_connected_callback );
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.h
new file mode 100644
index 0000000..88cfa58
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.h
@@ -0,0 +1,32 @@
+/*****************************************************************************
+* Copyright 2001 - 2010 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+#ifndef VCHIQ_CONNECTED_H
+#define VCHIQ_CONNECTED_H
+
+/* ---- Include Files ----------------------------------------------------- */
+
+/* ---- Constants and Types ---------------------------------------------- */
+
+typedef void (*VCHIQ_CONNECTED_CALLBACK_T)( void );
+
+/* ---- Variable Externs ------------------------------------------------- */
+
+/* ---- Function Prototypes ---------------------------------------------- */
+
+void vchiq_add_connected_callback( VCHIQ_CONNECTED_CALLBACK_T callback );
+void vchiq_call_connected_callbacks( void );
+
+#endif /* VCHIQ_CONNECTED_H */
+
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.c
new file mode 100644
index 0000000..5f2d111
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.c
@@ -0,0 +1,2604 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include "vchiq_core.h"
+
+#define VCHIQ_SLOT_HANDLER_STACK 8192
+
+#define SLOT_INFO_FROM_INDEX(state, index) (state->slot_info + (index))
+#define SLOT_DATA_FROM_INDEX(state, index) (state->slot_data + (index))
+#define SLOT_INDEX_FROM_DATA(state, data) (((unsigned int)((char *)data - (char *)state->slot_data)) / VCHIQ_SLOT_SIZE)
+#define SLOT_INDEX_FROM_INFO(state, info) ((unsigned int)(info - state->slot_info))
+#define SLOT_QUEUE_INDEX_FROM_POS(pos) ((int)((unsigned int)(pos) / VCHIQ_SLOT_SIZE))
+
+#define VCOS_LOG_CATEGORY (&vchiq_core_log_category)
+
+#define BULK_INDEX(x) (x & (VCHIQ_NUM_SERVICE_BULKS - 1))
+
+typedef struct bulk_waiter_struct
+{
+   VCOS_EVENT_T event;
+   int actual;
+} BULK_WAITER_T;
+
+typedef struct vchiq_open_payload_struct{
+   int fourcc;
+   int client_id;
+   short version;
+   short version_min;
+} VCHIQ_OPEN_PAYLOAD_T;
+
+vcos_static_assert(sizeof(VCHIQ_HEADER_T) == 8);   /* we require this for consistency between endpoints */
+vcos_static_assert(IS_POW2(sizeof(VCHIQ_HEADER_T)));
+vcos_static_assert(IS_POW2(VCHIQ_NUM_CURRENT_BULKS));
+vcos_static_assert(IS_POW2(VCHIQ_NUM_SERVICE_BULKS));
+
+VCOS_LOG_CAT_T vchiq_core_log_category;
+VCOS_LOG_CAT_T vchiq_core_msg_log_category;
+VCOS_LOG_LEVEL_T vchiq_default_core_log_level = VCOS_LOG_WARN;
+VCOS_LOG_LEVEL_T vchiq_default_core_msg_log_level = VCOS_LOG_WARN;
+
+static const char *const srvstate_names[] =
+{
+   "FREE",
+   "HIDDEN",
+   "LISTENING",
+   "OPENING",
+   "OPEN",
+   "CLOSESENT",
+   "CLOSING",
+   "CLOSEWAIT"
+};
+
+static const char *const reason_names[] =
+{
+   "SERVICE_OPENED",
+   "SERVICE_CLOSED",
+   "MESSAGE_AVAILABLE",
+   "BULK_TRANSMIT_DONE",
+   "BULK_RECEIVE_DONE",
+   "BULK_TRANSMIT_ABORTED",
+   "BULK_RECEIVE_ABORTED"
+};
+
+static const char *const conn_state_names[] =
+{
+   "DISCONNECTED",
+   "CONNECTED",
+   "PAUSING",
+   "PAUSE_SENT",
+   "PAUSED",
+   "RESUMING"
+};
+
+static const char *msg_type_str( unsigned int msg_type )
+{
+   switch (msg_type) {
+   case VCHIQ_MSG_PADDING:       return "PADDING";
+   case VCHIQ_MSG_CONNECT:       return "CONNECT";
+   case VCHIQ_MSG_OPEN:          return "OPEN";
+   case VCHIQ_MSG_OPENACK:       return "OPENACK";
+   case VCHIQ_MSG_CLOSE:         return "CLOSE";
+   case VCHIQ_MSG_DATA:          return "DATA";
+   case VCHIQ_MSG_BULK_RX:       return "BULK_RX";
+   case VCHIQ_MSG_BULK_TX:       return "BULK_TX";
+   case VCHIQ_MSG_BULK_RX_DONE:  return "BULK_RX_DONE";
+   case VCHIQ_MSG_BULK_TX_DONE:  return "BULK_TX_DONE";
+   case VCHIQ_MSG_PAUSE:         return "PAUSE";
+   case VCHIQ_MSG_RESUME:        return "RESUME";
+   }
+   return "???";
+}
+
+static inline void
+vchiq_set_service_state(VCHIQ_SERVICE_T *service, int newstate)
+{
+   vcos_log_info("%d: srv:%d %s->%s", service->state->id, service->localport,
+      srvstate_names[service->srvstate],
+      srvstate_names[newstate]);
+   service->srvstate = newstate;
+}
+
+static inline VCHIQ_STATUS_T
+make_service_callback(VCHIQ_SERVICE_T *service, VCHIQ_REASON_T reason,
+   VCHIQ_HEADER_T *header, void *bulk_userdata)
+{
+   vcos_log_trace("%d: callback:%d (%s, %x, %x)", service->state->id,
+      service->localport, reason_names[reason],
+      (unsigned int)header, (unsigned int)bulk_userdata);
+   return service->base.callback(reason, header, &service->base, bulk_userdata);
+}
+
+static inline void
+vchiq_set_conn_state(VCHIQ_STATE_T *state, VCHIQ_CONNSTATE_T newstate)
+{
+   vcos_log_info("%d: %s->%s", state->id,
+      conn_state_names[state->conn_state],
+      conn_state_names[newstate]);
+   state->conn_state = newstate;
+}
+
+static inline void
+remote_event_create(REMOTE_EVENT_T *event)
+{
+   event->armed = 0;
+   /* Don't clear the 'fired' flag because it may already have been set by the other side */
+   vcos_event_create(event->event, "vchiq");
+}
+
+static inline void
+remote_event_destroy(REMOTE_EVENT_T *event)
+{
+   vcos_event_delete(event->event);
+}
+
+static inline int
+remote_event_wait(REMOTE_EVENT_T *event)
+{
+   if (!event->fired)
+   {
+      event->armed = 1;
+      if (event->fired) /* Also ensures the write has completed */
+         event->armed = 0;
+      else if (vcos_event_wait(event->event) != VCOS_SUCCESS)
+         return 0;
+   }
+
+   event->fired = 0;
+   return 1;
+}
+
+static inline void
+remote_event_signal_local(REMOTE_EVENT_T *event)
+{
+   event->armed = 0;
+   vcos_event_signal(event->event);
+}
+
+static inline void
+remote_event_poll(REMOTE_EVENT_T *event)
+{
+   if (event->armed)
+      remote_event_signal_local(event);
+}
+
+void
+remote_event_pollall(VCHIQ_STATE_T *state)
+{
+   remote_event_poll(&state->local->trigger);
+   remote_event_poll(&state->local->recycle);
+}
+
+/* Round up message sizes so that any space at the end of a slot is always big
+   enough for a header. This relies on header size being a power of two, which
+   has been verified earlier by a static assertion. */
+
+static inline unsigned int
+calc_stride(unsigned int size)
+{
+   /* Allow room for the header */
+   size += sizeof(VCHIQ_HEADER_T);
+
+   /* Round up */
+   return (size + sizeof(VCHIQ_HEADER_T) - 1) & ~(sizeof(VCHIQ_HEADER_T) - 1);
+}
+
+static VCHIQ_SERVICE_T *
+get_listening_service(VCHIQ_STATE_T *state, int fourcc)
+{
+   int i;
+
+   vcos_assert(fourcc != VCHIQ_FOURCC_INVALID);
+
+   for (i = 0; i < state->unused_service; i++)
+   {
+      VCHIQ_SERVICE_T *service = state->services[i];
+      if (service &&
+         (service->public_fourcc == fourcc) &&
+         ((service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
+         ((service->srvstate == VCHIQ_SRVSTATE_OPEN) &&
+         (service->remoteport == VCHIQ_PORT_FREE))))
+         return service;
+   }
+
+   return NULL;
+}
+
+static VCHIQ_SERVICE_T *
+get_connected_service(VCHIQ_STATE_T *state, unsigned int port)
+{
+   int i;
+   for (i = 0; i < state->unused_service; i++) {
+      VCHIQ_SERVICE_T *service = state->services[i];
+      if (service && (service->srvstate == VCHIQ_SRVSTATE_OPEN)
+         && (service->remoteport == port)) {
+         return service;
+      }
+   }
+   return NULL;
+}
+
+static inline void
+request_poll(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service, int poll_type)
+{
+   if (service)
+   {
+      vcos_atomic_flags_or(&service->poll_flags, (1 << poll_type));
+      vcos_atomic_flags_or(&state->poll_services[service->localport>>5],
+         (1 <<(service->localport & 0x1f)));
+   }
+
+   state->poll_needed = 1;
+   vcos_wmb(&state->poll_needed);
+
+   /* ... and ensure the slot handler runs. */
+   remote_event_signal_local(&state->local->trigger);
+}
+
+/* Called from queue_message, by the slot handler and application threads,
+   with slot_mutex held */
+static VCHIQ_HEADER_T *
+reserve_space(VCHIQ_STATE_T *state, int space, int is_blocking)
+{
+   VCHIQ_SHARED_STATE_T *local = state->local;
+   int tx_pos = state->local_tx_pos;
+   int slot_space = VCHIQ_SLOT_SIZE - (tx_pos & VCHIQ_SLOT_MASK);
+
+   if (space > slot_space) {
+      VCHIQ_HEADER_T *header;
+      /* Fill the remaining space with padding */
+      vcos_assert(state->tx_data != NULL);
+      header = (VCHIQ_HEADER_T *) (state->tx_data + (tx_pos & VCHIQ_SLOT_MASK));
+      header->msgid = VCHIQ_MSGID_PADDING;
+      header->size = slot_space - sizeof(VCHIQ_HEADER_T);
+
+      tx_pos += slot_space;
+   }
+
+   /* If necessary, get the next slot. */
+   if ((tx_pos & VCHIQ_SLOT_MASK) == 0)
+   {
+      int slot_index;
+
+      /* If there is no free slot... */
+      if (tx_pos == (state->slot_queue_available * VCHIQ_SLOT_SIZE))
+      {
+         /* ...wait for one. */
+         VCHIQ_STATS_INC(state, slot_stalls);
+
+         /* But first, flush through the last slot. */
+         local->tx_pos = tx_pos;
+         remote_event_signal(&state->remote->trigger);
+
+         do {
+            if (!is_blocking ||
+               (vcos_event_wait(&state->slot_available_event) != VCOS_SUCCESS))
+            {
+               return NULL; /* No space available now */
+            }
+         }
+         while (tx_pos == (state->slot_queue_available * VCHIQ_SLOT_SIZE));
+      }
+
+      slot_index = local->slot_queue[SLOT_QUEUE_INDEX_FROM_POS(tx_pos) & VCHIQ_SLOT_QUEUE_MASK];
+      state->tx_data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
+   }
+
+   state->local_tx_pos = tx_pos + space;
+
+   return (VCHIQ_HEADER_T *)(state->tx_data + (tx_pos & VCHIQ_SLOT_MASK));
+}
+
+/* Called with slot_mutex held */
+static void
+process_free_queue(VCHIQ_STATE_T *state)
+{
+   VCHIQ_SHARED_STATE_T *local = state->local;
+   BITSET_T service_found[BITSET_SIZE(VCHIQ_MAX_SERVICES)];
+   int slot_queue_available;
+
+   /* Use a read memory barrier to ensure that any state that may have
+      been modified by another thread is not masked by stale prefetched
+      values. */
+   vcos_rmb();
+
+   /* Find slots which have been freed by the other side, and return them to
+      the available queue. */
+   slot_queue_available = state->slot_queue_available;
+
+   while (slot_queue_available != local->slot_queue_recycle)
+   {
+      int pos;
+      int slot_index = local->slot_queue[slot_queue_available++ & VCHIQ_SLOT_QUEUE_MASK];
+      char *data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
+
+      vcos_log_trace("%d: pfq %d=%x %x %x", state->id, slot_index,
+         (unsigned int)data, local->slot_queue_recycle,
+         slot_queue_available);
+
+      /* Initialise the bitmask for services which have used this slot */
+      BITSET_ZERO(service_found);
+
+      pos = 0;
+
+      while (pos < VCHIQ_SLOT_SIZE)
+      {
+         VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)(data + pos);
+         int msgid = header->msgid;
+         if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA)
+         {
+            int port = VCHIQ_MSG_SRCPORT(msgid);
+            if (!BITSET_IS_SET(service_found, port))
+            {
+               VCHIQ_SERVICE_QUOTA_T *service_quota =
+                  &state->service_quotas[port];
+
+               /* Set the found bit for this service */
+               BITSET_SET(service_found, port);
+
+               if (service_quota->slot_use_count > 0)
+               {
+                  service_quota->slot_use_count--;
+                  /* Signal the service in case it has dropped below its quota */
+                  vcos_event_signal(&service_quota->quota_event);
+                  vcos_log_trace("%d: pfq:%d %x@%x - slot_use->%d",
+                     state->id, port,
+                     header->size, (unsigned int)header,
+                     service_quota->slot_use_count);
+               }
+               else
+               {
+                  vcos_log_error("service %d slot_use_count=%d (header %x,"
+                                 " msgid %x, header->msgid %x, header->size %x)",
+                     port, service_quota->slot_use_count,
+                     (unsigned int)header, msgid, header->msgid,
+                     header->size);
+                  vcos_assert(0);
+               }
+            }
+         }
+
+         pos += calc_stride(header->size);
+         if (pos > VCHIQ_SLOT_SIZE)
+         {
+            vcos_log_error("pos %x: header %x, msgid %x, header->msgid %x, header->size %x",
+               pos, (unsigned int)header, msgid, header->msgid, header->size);
+            vcos_assert(0);
+         }
+      }
+   }
+
+   if (slot_queue_available != state->slot_queue_available)
+   {
+      state->slot_queue_available = slot_queue_available;
+      vcos_wmb(&state->slot_queue_available);
+      vcos_event_signal(&state->slot_available_event);
+   }
+}
+
+/* Called by the slot handler and application threads */
+static VCHIQ_STATUS_T
+queue_message(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
+   int msgid, const VCHIQ_ELEMENT_T *elements,
+   int count, int size, int is_blocking)
+{
+   VCHIQ_SHARED_STATE_T *local;
+   VCHIQ_SERVICE_QUOTA_T *service_quota = NULL;
+   VCHIQ_HEADER_T *header;
+
+   unsigned int stride;
+
+   local = state->local;
+
+   stride = calc_stride(size);
+
+   vcos_assert(stride <= VCHIQ_SLOT_SIZE);
+
+   /* On platforms where vcos_mutex_lock cannot fail, the return will never
+      be taken and the compiler may optimise out that code. Let Coverity
+      know this is intentional.
+   */
+   /* coverity[constant_expression_result] */
+   if ((VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_RESUME) &&
+      (vcos_mutex_lock(&state->slot_mutex) != VCOS_SUCCESS))
+      return VCHIQ_RETRY;
+
+   if (service)
+   {
+      int tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1);
+
+      if (service->srvstate != VCHIQ_SRVSTATE_OPEN)
+      {
+         /* The service has been closed, probably while waiting for the mutex */
+         vcos_mutex_unlock(&state->slot_mutex);
+         return VCHIQ_ERROR;
+      }
+
+      service_quota = &state->service_quotas[service->localport];
+
+      /* ...ensure it doesn't use more than its quota of slots */
+      while ((tx_end_index != service_quota->previous_tx_index) &&
+         (service_quota->slot_use_count == service_quota->slot_quota))
+      {
+         vcos_log_trace("%d: qm:%d %s,%x - quota stall",
+            state->id, service->localport,
+            msg_type_str(VCHIQ_MSG_TYPE(msgid)), size);
+         VCHIQ_SERVICE_STATS_INC(service, quota_stalls);
+         vcos_mutex_unlock(&state->slot_mutex);
+         if (vcos_event_wait(&service_quota->quota_event) != VCOS_SUCCESS)
+            return VCHIQ_RETRY;
+         if (vcos_mutex_lock(&state->slot_mutex) != VCOS_SUCCESS)
+            return VCHIQ_RETRY;
+         vcos_assert(service_quota->slot_use_count <= service_quota->slot_quota);
+         tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1);
+      }
+   }
+
+   header = reserve_space(state, stride, is_blocking);
+
+   if (!header) {
+      if (service)
+         VCHIQ_SERVICE_STATS_INC(service, slot_stalls);
+      vcos_mutex_unlock(&state->slot_mutex);
+      return VCHIQ_RETRY;
+   }
+
+   if (service) {
+      int i, pos;
+      int tx_end_index;
+
+      vcos_log_info("%d: qm %s@%x,%x (%d->%d)", state->id,
+         msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+         (unsigned int)header, size,
+         VCHIQ_MSG_SRCPORT(msgid),
+         VCHIQ_MSG_DSTPORT(msgid));
+
+      for (i = 0, pos = 0; i < (unsigned int)count;
+         pos += elements[i++].size)
+         if (elements[i].size) {
+            if (vchiq_copy_from_user
+               (header->data + pos, elements[i].data,
+               (size_t) elements[i].size) !=
+               VCHIQ_SUCCESS) {
+               vcos_mutex_unlock(&state->slot_mutex);
+               VCHIQ_SERVICE_STATS_INC(service, error_count);
+               return VCHIQ_ERROR;
+            }
+            if (i == 0) {
+               vcos_log_dump_mem( &vchiq_core_msg_log_category,
+                              "Sent", 0, header->data + pos,
+                              vcos_min( 64, elements[0].size ));
+            }
+         }
+
+      /* If this transmission can't fit in the last slot used by this service... */
+      tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos - 1);
+      if (tx_end_index != service_quota->previous_tx_index)
+      {
+         service_quota->slot_use_count++;
+         vcos_log_trace("%d: qm:%d %s,%x - slot_use->%d",
+            state->id, service->localport,
+            msg_type_str(VCHIQ_MSG_TYPE(msgid)), size,
+            service_quota->slot_use_count);
+      }
+
+      service_quota->previous_tx_index = tx_end_index;
+      VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count);
+      VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
+   } else {
+      vcos_log_info("%d: qm %s@%x,%x (%d->%d)", state->id,
+         msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+         (unsigned int)header, size,
+         VCHIQ_MSG_SRCPORT(msgid),
+         VCHIQ_MSG_DSTPORT(msgid));
+      if (size != 0)
+      { 
+         vcos_assert((count == 1) && (size == elements[0].size));
+         memcpy(header->data, elements[0].data, elements[0].size);
+      }
+      VCHIQ_STATS_INC(state, ctrl_tx_count);
+   }
+
+   header->msgid = msgid;
+   header->size = size;
+
+   if (vcos_is_log_enabled( &vchiq_core_msg_log_category, VCOS_LOG_INFO))
+   {
+      int svc_fourcc;
+
+      svc_fourcc = service
+         ? service->base.fourcc
+         : VCHIQ_MAKE_FOURCC('?','?','?','?');
+
+      vcos_log_impl( &vchiq_core_msg_log_category,
+         VCOS_LOG_INFO,
+         "Sent Msg %s(%u) to %c%c%c%c s:%u d:%d len:%d",
+         msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+         VCHIQ_MSG_TYPE(msgid),
+         VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
+         VCHIQ_MSG_SRCPORT(msgid),
+         VCHIQ_MSG_DSTPORT(msgid),
+         size );
+   }
+
+   /* Make the new tx_pos visible to the peer. */
+   local->tx_pos = state->local_tx_pos;
+   vcos_wmb(&local->tx_pos);
+
+   if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_PAUSE)
+      vcos_mutex_unlock(&state->slot_mutex);
+
+   remote_event_signal(&state->remote->trigger);
+
+   return VCHIQ_SUCCESS;
+}
+
+static inline void
+claim_slot(VCHIQ_SLOT_INFO_T *slot)
+{
+   slot->use_count++;
+}
+
+static void
+release_slot(VCHIQ_STATE_T *state, VCHIQ_SLOT_INFO_T *slot_info)
+{
+   int release_count;
+   vcos_mutex_lock(&state->recycle_mutex);
+
+   release_count = slot_info->release_count;
+   slot_info->release_count = ++release_count;
+
+   if (release_count == slot_info->use_count)
+   {
+      int slot_queue_recycle;
+      /* Add to the freed queue */
+
+      /* A read barrier is necessary here to prevent speculative fetches of
+         remote->slot_queue_recycle from overtaking the mutex. */
+      vcos_rmb();
+
+      slot_queue_recycle = state->remote->slot_queue_recycle;
+      state->remote->slot_queue[slot_queue_recycle & VCHIQ_SLOT_QUEUE_MASK] =
+         SLOT_INDEX_FROM_INFO(state, slot_info);
+      state->remote->slot_queue_recycle = slot_queue_recycle + 1;
+      vcos_log_info("%d: release_slot %d - recycle->%x",
+         state->id, SLOT_INDEX_FROM_INFO(state, slot_info),
+         state->remote->slot_queue_recycle);
+
+      /* A write barrier is necessary, but remote_event_signal contains one. */
+      remote_event_signal(&state->remote->recycle);
+   }
+
+   vcos_mutex_unlock(&state->recycle_mutex);
+}
+
+/* Called by the slot handler - don't hold the bulk mutex */
+static VCHIQ_STATUS_T
+notify_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
+{
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+   vcos_log_trace("%d: nb:%d %cx - p=%x rn=%x r=%x",
+      service->state->id, service->localport,
+      (queue == &service->bulk_tx) ? 't' : 'r',
+      queue->process, queue->remote_notify, queue->remove);
+
+   if (service->state->is_master)
+   {
+      while (queue->remote_notify != queue->process)
+      {
+         VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->remote_notify)];
+         int msgtype = (bulk->dir == VCHIQ_BULK_TRANSMIT) ?
+            VCHIQ_MSG_BULK_RX_DONE : VCHIQ_MSG_BULK_TX_DONE;
+         int msgid = VCHIQ_MAKE_MSG(msgtype, service->localport, service->remoteport);
+         VCHIQ_ELEMENT_T element = { &bulk->actual, 4 };
+         /* Only reply to non-dummy bulk requests */
+         if (bulk->remote_data)
+         {
+            status = queue_message(service->state, NULL, msgid, &element, 1, 4, 0);
+            if (status != VCHIQ_SUCCESS)
+               break;
+         }
+         queue->remote_notify++;
+      }
+   }
+   else
+   {
+      queue->remote_notify = queue->process;
+   }
+
+   if (status == VCHIQ_SUCCESS)
+   {
+      while (queue->remove != queue->remote_notify)
+      {
+         VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->remove)];
+
+         /* Only generate callbacks for non-dummy bulk requests */
+         if (bulk->data)
+         {
+            if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED)
+            {
+               if (bulk->dir == VCHIQ_BULK_TRANSMIT)
+               {
+                  VCHIQ_SERVICE_STATS_INC(service, bulk_tx_count);
+                  VCHIQ_SERVICE_STATS_ADD(service, bulk_tx_bytes, bulk->actual);
+               }
+               else
+               {
+                  VCHIQ_SERVICE_STATS_INC(service, bulk_rx_count);
+                  VCHIQ_SERVICE_STATS_ADD(service, bulk_rx_bytes, bulk->actual);
+               }
+            }
+            else
+            {
+               VCHIQ_SERVICE_STATS_INC(service, bulk_aborted_count);
+            }
+            if (bulk->mode == VCHIQ_BULK_MODE_BLOCKING)
+            {
+               BULK_WAITER_T *waiter = (BULK_WAITER_T *)bulk->userdata;
+               if (waiter)
+               {
+                  waiter->actual = bulk->actual;
+                  vcos_event_signal(&waiter->event);
+               }
+            }
+            else if (bulk->mode == VCHIQ_BULK_MODE_CALLBACK)
+            {
+               VCHIQ_REASON_T reason = (bulk->dir == VCHIQ_BULK_TRANSMIT) ?
+                  ((bulk->actual == VCHIQ_BULK_ACTUAL_ABORTED) ?
+                     VCHIQ_BULK_TRANSMIT_ABORTED : VCHIQ_BULK_TRANSMIT_DONE) :
+                  ((bulk->actual == VCHIQ_BULK_ACTUAL_ABORTED) ?
+                     VCHIQ_BULK_RECEIVE_ABORTED : VCHIQ_BULK_RECEIVE_DONE);
+               status = make_service_callback(service, reason,
+                  NULL, bulk->userdata);
+               if (status == VCHIQ_RETRY)
+                  break;
+            }
+         }
+
+         queue->remove++;
+         vcos_event_signal(&service->bulk_remove_event);
+      }
+   }
+
+   if (status != VCHIQ_SUCCESS)
+      request_poll(service->state, service, (queue == &service->bulk_tx) ?
+         VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY);
+
+   return status;
+}
+
+/* Called by the slot handler thread */
+static void
+poll_services(VCHIQ_STATE_T *state)
+{
+   int group, i;
+
+   for (group = 0; group < BITSET_SIZE(state->unused_service); group++)
+   {
+      uint32_t flags;
+      flags = vcos_atomic_flags_get_and_clear(&state->poll_services[group]);
+      for (i = 0; flags; i++)
+      {
+         if (flags & (1 << i))
+         {
+            VCHIQ_SERVICE_T *service = state->services[(group<<5) + i];
+            uint32_t service_flags =
+               vcos_atomic_flags_get_and_clear(&service->poll_flags);
+            if (service_flags & (1 << VCHIQ_POLL_TERMINATE))
+            {
+               vcos_log_info("%d: ps - terminate %d<->%d", state->id, service->localport, service->remoteport);
+               if (vchiq_close_service_internal(service, 0/*!close_recvd*/) != VCHIQ_SUCCESS)
+                  request_poll(state, service, VCHIQ_POLL_TERMINATE);
+            }
+            if (service_flags & (1 << VCHIQ_POLL_TXNOTIFY))
+               notify_bulks(service, &service->bulk_tx);
+            if (service_flags & (1 << VCHIQ_POLL_RXNOTIFY))
+               notify_bulks(service, &service->bulk_rx);
+            flags &= ~(1 << i);
+         }
+      }
+   }
+}
+
+/* Called by the slot handler or application threads, holding the bulk mutex. */
+static int
+resolve_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
+{
+   VCHIQ_STATE_T *state = service->state;
+   int resolved = 0;
+
+   while ((queue->process != queue->local_insert) &&
+      (queue->process != queue->remote_insert))
+   {
+      VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->process)];
+
+      vcos_log_trace("%d: rb:%d %cx - li=%x ri=%x p=%x",
+         state->id, service->localport,
+         (queue == &service->bulk_tx) ? 't' : 'r',
+         queue->local_insert, queue->remote_insert,
+         queue->process);
+
+      vcos_assert((int)(queue->local_insert - queue->process) > 0);
+      vcos_assert((int)(queue->remote_insert - queue->process) > 0);
+      vchiq_transfer_bulk(bulk);
+
+      if (vcos_is_log_enabled( &vchiq_core_msg_log_category, VCOS_LOG_INFO))
+      {
+         const char *header = (queue == &service->bulk_tx) ?
+            "Send Bulk to" : "Recv Bulk from";
+         if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED)
+            vcos_log_impl( &vchiq_core_msg_log_category,
+               VCOS_LOG_INFO,
+               "%s %c%c%c%c d:%d len:%d %x<->%x",
+               header,
+               VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+               service->remoteport,
+               bulk->size,
+               (unsigned int)bulk->data,
+               (unsigned int)bulk->remote_data );
+         else
+            vcos_log_impl( &vchiq_core_msg_log_category,
+               VCOS_LOG_INFO,
+               "%s %c%c%c%c d:%d ABORTED - tx len:%d, rx len:%d %x<->%x",
+               header,
+               VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+               service->remoteport,
+               bulk->size,
+               bulk->remote_size,
+               (unsigned int)bulk->data,
+               (unsigned int)bulk->remote_data );
+      }
+
+      vchiq_complete_bulk(bulk);
+      queue->process++;
+      resolved++;
+   }
+   return resolved;
+}
+
+/* Called with the bulk_mutex held */
+static void
+abort_outstanding_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
+{
+   int is_tx = (queue == &service->bulk_tx);
+   vcos_log_trace("%d: aob:%d %cx - li=%x ri=%x p=%x",
+      service->state->id, service->localport, is_tx ? 't' : 'r',
+      queue->local_insert, queue->remote_insert, queue->process);
+
+   vcos_assert((int)(queue->local_insert - queue->process) >= 0);
+   vcos_assert((int)(queue->remote_insert - queue->process) >= 0);
+
+   while ((queue->process != queue->local_insert) ||
+      (queue->process != queue->remote_insert))
+   {
+      VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->process)];
+
+      if (queue->process == queue->remote_insert)
+      {
+         /* fabricate a matching dummy bulk */
+         bulk->remote_data = NULL;
+         bulk->remote_size = 0;
+         queue->remote_insert++;
+      }
+
+      if (queue->process != queue->local_insert)
+      {
+         vchiq_complete_bulk(bulk);
+
+         if (vcos_is_log_enabled( &vchiq_core_msg_log_category, VCOS_LOG_INFO))
+         {
+            vcos_log_impl( &vchiq_core_msg_log_category,
+               VCOS_LOG_INFO,
+               "%s %c%c%c%c d:%d ABORTED - tx len:%d, rx len:%d",
+               is_tx ? "Send Bulk to" : "Recv Bulk from",
+               VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+               service->remoteport,
+               bulk->size,
+               bulk->remote_size );
+         }
+      }
+      else
+      {
+         /* fabricate a matching dummy bulk */
+         bulk->data = NULL;
+         bulk->size = 0;
+         bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED;
+         bulk->dir = is_tx ? VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE;
+         queue->local_insert++;
+      }
+
+      queue->process++;
+   }
+}
+
+static void
+pause_bulks(VCHIQ_STATE_T *state)
+{
+   int i;
+
+   /* Block bulk transfers from all services */
+   for (i = 0; i < state->unused_service; i++)
+   {
+      VCHIQ_SERVICE_T *service = state->services[i];
+      if (!service || (service->srvstate != VCHIQ_SRVSTATE_OPEN))
+         continue;
+
+      vcos_log_trace("locking bulk_mutex for service %d", i);
+      vcos_mutex_lock(&service->bulk_mutex);
+   }
+}
+
+static void
+resume_bulks(VCHIQ_STATE_T *state)
+{
+   int i;
+
+   /* Poll all services in case any bulk transfers have been
+      deferred */
+   for (i = 0; i < state->unused_service; i++)
+   {
+      VCHIQ_SERVICE_T *service = state->services[i];
+      if (!service || (service->srvstate != VCHIQ_SRVSTATE_OPEN))
+         continue;
+
+      if (resolve_bulks(service, &service->bulk_tx))
+         request_poll(state, service, VCHIQ_POLL_TXNOTIFY);
+      if (resolve_bulks(service, &service->bulk_rx))
+         request_poll(state, service, VCHIQ_POLL_RXNOTIFY);
+      vcos_log_trace("unlocking bulk_mutex for service %d", i);
+      vcos_mutex_unlock(&service->bulk_mutex);
+   }
+}
+
+/* Called by the slot handler thread */
+static void
+parse_rx_slots(VCHIQ_STATE_T *state)
+{
+   VCHIQ_SHARED_STATE_T *remote = state->remote;
+   int tx_pos;
+   DEBUG_INITIALISE(state->local)
+
+   tx_pos = remote->tx_pos;
+
+   while (state->rx_pos != tx_pos) {
+      VCHIQ_SERVICE_T *service = NULL;
+      VCHIQ_HEADER_T *header;
+      int msgid, size;
+      int type;
+      unsigned int localport, remoteport;
+
+      DEBUG_TRACE(PARSE_LINE);
+      if (!state->rx_data)
+      {
+         int rx_index;
+         vcos_assert((state->rx_pos & VCHIQ_SLOT_MASK) == 0);
+         rx_index = remote->slot_queue[SLOT_QUEUE_INDEX_FROM_POS(state->rx_pos) & VCHIQ_SLOT_QUEUE_MASK];
+         state->rx_data = (char *)SLOT_DATA_FROM_INDEX(state, rx_index);
+         state->rx_info = SLOT_INFO_FROM_INDEX(state, rx_index);
+
+         /* Initialise use_count to one, and increment release_count at the end
+            of the slot to avoid releasing the slot prematurely. */
+         state->rx_info->use_count = 1;
+         state->rx_info->release_count = 0;
+      }
+
+      header = (VCHIQ_HEADER_T *)(state->rx_data + (state->rx_pos & VCHIQ_SLOT_MASK));
+      DEBUG_VALUE(PARSE_HEADER, (int)header);
+      msgid = header->msgid;
+      DEBUG_VALUE(PARSE_MSGID, msgid);
+      size = header->size;
+      type = VCHIQ_MSG_TYPE(msgid);
+      localport = VCHIQ_MSG_DSTPORT(msgid);
+      remoteport = VCHIQ_MSG_SRCPORT(msgid);
+
+      if (type != VCHIQ_MSG_DATA)
+      {
+         VCHIQ_STATS_INC(state, ctrl_rx_count);
+      }
+
+      switch (type)
+      {
+      case VCHIQ_MSG_OPENACK:
+      case VCHIQ_MSG_CLOSE:
+      case VCHIQ_MSG_DATA:
+      case VCHIQ_MSG_BULK_RX:
+      case VCHIQ_MSG_BULK_TX:
+      case VCHIQ_MSG_BULK_RX_DONE:
+      case VCHIQ_MSG_BULK_TX_DONE:
+         if (localport <= VCHIQ_PORT_MAX)
+         {
+            service = state->services[localport];
+            if (service && (service->srvstate == VCHIQ_SRVSTATE_FREE))
+               service = NULL;
+         }
+         if (!service)
+         {
+            vcos_log_error(
+               "%d: prs %s@%x (%d->%d) - invalid/closed service %d",
+               state->id, msg_type_str(type), (unsigned int)header,
+               remoteport, localport, localport);
+            goto skip_message;
+         }
+      default:
+         break;
+      }
+
+      if ( vcos_is_log_enabled( &vchiq_core_msg_log_category, VCOS_LOG_INFO))
+      {
+         int svc_fourcc;
+
+         svc_fourcc = service
+            ? service->base.fourcc
+            : VCHIQ_MAKE_FOURCC('?','?','?','?');
+         vcos_log_impl( &vchiq_core_msg_log_category,
+            VCOS_LOG_INFO,
+            "Rcvd Msg %s(%u) from %c%c%c%c s:%d d:%d len:%d",
+            msg_type_str(type), type,
+            VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
+            remoteport, localport, size );
+         if (size > 0) {
+            vcos_log_dump_mem( &vchiq_core_msg_log_category,
+                           "Rcvd", 0, header->data,
+                           vcos_min( 64, size ));
+         }
+      }
+
+      if (((unsigned int)header & VCHIQ_SLOT_MASK) + calc_stride(size) > VCHIQ_SLOT_SIZE)
+      {
+         vcos_log_error("header %x (msgid %x) - size %x too big for slot",
+            (unsigned int)header, (unsigned int)msgid, (unsigned int)size);
+         vcos_assert(0);
+      }
+
+      switch (type) {
+      case VCHIQ_MSG_OPEN:
+         vcos_assert(VCHIQ_MSG_DSTPORT(msgid) == 0);
+         if (vcos_verify(size == sizeof(VCHIQ_OPEN_PAYLOAD_T))) {
+            const VCHIQ_OPEN_PAYLOAD_T *payload = (VCHIQ_OPEN_PAYLOAD_T *)header->data;
+            unsigned int fourcc;
+
+            fourcc = payload->fourcc;
+            vcos_log_info("%d: prs OPEN@%x (%d->'%c%c%c%c')",
+               state->id, (unsigned int)header,
+               localport,
+               VCHIQ_FOURCC_AS_4CHARS(fourcc));
+
+            service = get_listening_service(state, fourcc);
+
+            if (service)
+            {
+               /* A matching service exists */
+               short version = payload->version;
+               short version_min = payload->version_min;
+               if ((service->version < version_min) ||
+                  (version < service->version_min))
+               {
+                  /* Version mismatch */
+                  vcos_log_error("%d: service %d (%c%c%c%c) version mismatch -"
+                     " local (%d, min %d) vs. remote (%d, min %d)",
+                     state->id, service->localport,
+                     VCHIQ_FOURCC_AS_4CHARS(fourcc),
+                     service->version, service->version_min,
+                     version, version_min);
+                  goto fail_open;
+               }
+               if (service->srvstate == VCHIQ_SRVSTATE_LISTENING)
+               {
+                  /* Acknowledge the OPEN */
+                  if (queue_message(state, NULL,
+                     VCHIQ_MAKE_MSG(VCHIQ_MSG_OPENACK, service->localport, remoteport),
+                     NULL, 0, 0, 0) == VCHIQ_RETRY)
+                     return;  /* Bail out if not ready */
+
+                  /* The service is now open */
+                  vchiq_set_service_state(service, VCHIQ_SRVSTATE_OPEN);
+               }
+
+               service->remoteport = remoteport;
+               service->client_id = ((int *)header->data)[1];
+               if (make_service_callback(service, VCHIQ_SERVICE_OPENED,
+                  NULL, NULL) == VCHIQ_RETRY)
+               {
+                  /* Bail out if not ready */
+                  service->remoteport = VCHIQ_PORT_FREE;
+                  return;
+               }
+
+               /* Break out, and skip the failure handling */
+               break;
+            }
+         }
+      fail_open:
+         /* No available service, or an invalid request - send a CLOSE */
+         if (queue_message(state, NULL,
+            VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, 0, VCHIQ_MSG_SRCPORT(msgid)),
+            NULL, 0, 0, 0) == VCHIQ_RETRY)
+            return;  /* Bail out if not ready */
+         break;
+      case VCHIQ_MSG_OPENACK:
+         {
+            vcos_log_info("%d: prs OPENACK@%x (%d->%d)",
+               state->id, (unsigned int)header,
+               remoteport, localport);
+            if (service->srvstate == VCHIQ_SRVSTATE_OPENING) {
+               service->remoteport = remoteport;
+               vchiq_set_service_state(service,
+                        VCHIQ_SRVSTATE_OPEN);
+               vcos_event_signal(&service->remove_event);
+            }
+         }
+         break;
+      case VCHIQ_MSG_CLOSE:
+         {
+            vcos_assert(size == 0); /* There should be no data */
+
+            vcos_log_info("%d: prs CLOSE@%x (%d->%d)",
+               state->id, (unsigned int)header,
+               remoteport, localport);
+
+            if ((service->remoteport != remoteport) &&
+               VCHIQ_PORT_IS_VALID(service->remoteport)) {
+               /* This could be from a client which hadn't yet received
+                  the OPENACK - look for the connected service */
+               service = get_connected_service(state, remoteport);
+               if (!service)
+                  break;
+            }
+
+            if (vchiq_close_service_internal(service,
+               1/*close_recvd*/) == VCHIQ_RETRY)
+               return;  /* Bail out if not ready */
+
+            if (vcos_is_log_enabled( &vchiq_core_msg_log_category, VCOS_LOG_INFO))
+            {
+               vcos_log_impl( &vchiq_core_msg_log_category,
+                           VCOS_LOG_INFO,
+                           "Close Service %c%c%c%c s:%u d:%d",
+                           VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+                           service->localport,
+                           service->remoteport );
+            }
+         }
+         break;
+      case VCHIQ_MSG_DATA:
+         {
+            vcos_log_trace("%d: prs DATA@%x,%x (%d->%d)",
+               state->id, (unsigned int)header, size,
+               remoteport, localport);
+
+            if ((service->remoteport == remoteport)
+               && (service->srvstate ==
+               VCHIQ_SRVSTATE_OPEN)) {
+               header->msgid = msgid | VCHIQ_MSGID_CLAIMED;
+               claim_slot(state->rx_info);
+               DEBUG_TRACE(PARSE_LINE);
+               if (make_service_callback(service,
+                  VCHIQ_MESSAGE_AVAILABLE, header,
+                  NULL) == VCHIQ_RETRY)
+               {
+                  DEBUG_TRACE(PARSE_LINE);
+                  return;  /* Bail out if not ready */
+               }
+               VCHIQ_SERVICE_STATS_INC(service, ctrl_rx_count);
+               VCHIQ_SERVICE_STATS_ADD(service, ctrl_rx_bytes, size);
+            }
+            else
+            {
+               VCHIQ_STATS_INC(state, error_count);
+            }
+         }
+         break;
+      case VCHIQ_MSG_CONNECT:
+         vcos_log_info("%d: prs CONNECT@%x",
+            state->id, (unsigned int)header);
+         vcos_event_signal(&state->connect);
+         break;
+      case VCHIQ_MSG_BULK_RX:
+      case VCHIQ_MSG_BULK_TX:
+         {
+            VCHIQ_BULK_QUEUE_T *queue;
+            vcos_assert(state->is_master);
+            queue = (type == VCHIQ_MSG_BULK_RX) ?
+               &service->bulk_tx : &service->bulk_rx;
+            if ((service->remoteport == remoteport)
+               && (service->srvstate ==
+               VCHIQ_SRVSTATE_OPEN))
+            {
+               VCHIQ_BULK_T *bulk;
+               int resolved;
+
+               vcos_assert(queue->remote_insert < queue->remove +
+                  VCHIQ_NUM_SERVICE_BULKS);
+               bulk = &queue->bulks[BULK_INDEX(queue->remote_insert)];
+               bulk->remote_data = (void *)((int *)header->data)[0];
+               bulk->remote_size = ((int *)header->data)[1];
+
+               vcos_log_info("%d: prs %s@%x (%d->%d) %x@%x",
+                  state->id, msg_type_str(type),
+                  (unsigned int)header,
+                  remoteport, localport,
+                  bulk->remote_size,
+                  (unsigned int)bulk->remote_data);
+
+               queue->remote_insert++;
+
+               if (state->conn_state != VCHIQ_CONNSTATE_CONNECTED)
+                  break;
+
+               DEBUG_TRACE(PARSE_LINE);
+               if (vcos_mutex_lock(&service->bulk_mutex) != VCOS_SUCCESS)
+               {
+                  DEBUG_TRACE(PARSE_LINE);
+                  return;
+               }
+               DEBUG_TRACE(PARSE_LINE);
+               resolved = resolve_bulks(service, queue);
+               vcos_mutex_unlock(&service->bulk_mutex);
+               if (resolved)
+                  notify_bulks(service, queue);
+            }
+         }
+         break;
+      case VCHIQ_MSG_BULK_RX_DONE:
+      case VCHIQ_MSG_BULK_TX_DONE:
+         {
+            vcos_assert(!state->is_master);
+            if ((service->remoteport == remoteport)
+               && (service->srvstate !=
+               VCHIQ_SRVSTATE_FREE)) {
+               VCHIQ_BULK_QUEUE_T *queue;
+               VCHIQ_BULK_T *bulk;
+
+               queue = (type == VCHIQ_MSG_BULK_RX_DONE) ?
+                  &service->bulk_rx : &service->bulk_tx;
+
+               bulk = &queue->bulks[BULK_INDEX(queue->process)];
+               bulk->actual = *(int *)header->data;
+
+               vcos_log_info("%d: prs %s@%x (%d->%d) %x@%x",
+                  state->id, msg_type_str(type),
+                  (unsigned int)header,
+                  remoteport, localport,
+                  bulk->actual, (unsigned int)bulk->data);
+
+               vcos_log_trace("%d: prs:%d %cx li=%x ri=%x p=%x",
+                  state->id, localport,
+                  (type == VCHIQ_MSG_BULK_RX_DONE) ? 'r' : 't',
+                  queue->local_insert,
+                  queue->remote_insert, queue->process);
+
+               DEBUG_TRACE(PARSE_LINE);
+               if (vcos_mutex_lock(&service->bulk_mutex) != VCOS_SUCCESS)
+               {
+                  DEBUG_TRACE(PARSE_LINE);
+                  return;
+               }
+               DEBUG_TRACE(PARSE_LINE);
+               vcos_assert(queue->process != queue->local_insert);
+               vchiq_complete_bulk(bulk);
+               queue->process++;
+               vcos_mutex_unlock(&service->bulk_mutex);
+               DEBUG_TRACE(PARSE_LINE);
+               notify_bulks(service, queue);
+               DEBUG_TRACE(PARSE_LINE);
+            }
+         }
+         break;
+      case VCHIQ_MSG_PADDING:
+         vcos_log_trace("%d: prs PADDING@%x,%x",
+            state->id, (unsigned int)header, size);
+         break;
+      case VCHIQ_MSG_PAUSE:
+         /* If initiated, signal the application thread */
+         vcos_log_trace("%d: prs PAUSE@%x,%x",
+            state->id, (unsigned int)header, size);
+         if (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT)
+         {
+            /* Send a PAUSE in response */
+            if (queue_message(state, NULL,
+               VCHIQ_MAKE_MSG(VCHIQ_MSG_PAUSE, 0, 0),
+               NULL, 0, 0, 0) == VCHIQ_RETRY)
+               return;  /* Bail out if not ready */
+            if (state->is_master)
+               pause_bulks(state);
+         }
+         /* At this point slot_mutex is held */
+         vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSED);
+         vchiq_platform_paused(state);
+         break;
+      case VCHIQ_MSG_RESUME:
+         vcos_log_trace("%d: prs RESUME@%x,%x",
+            state->id, (unsigned int)header, size);
+         /* Release the slot mutex */
+         vcos_mutex_unlock(&state->slot_mutex);
+         if (state->is_master)
+            resume_bulks(state);
+         vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
+         vchiq_platform_resumed(state);
+         break;
+      default:
+         vcos_log_error("%d: prs invalid msgid %x@%x,%x",
+            state->id, msgid, (unsigned int)header, size);
+         vcos_assert(0);
+         break;
+      }
+
+   skip_message:
+      state->rx_pos += calc_stride(size);
+
+      DEBUG_TRACE(PARSE_LINE);
+      /* Perform some housekeeping when the end of the slot is reached. */
+      if ((state->rx_pos & VCHIQ_SLOT_MASK) == 0)
+      {
+         /* Remove the extra reference count. */
+         release_slot(state, state->rx_info);
+         state->rx_data = NULL;
+      }
+   }
+}
+
+/* Called by the slot handler thread */
+static void *
+slot_handler_func(void *v)
+{
+   VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
+   VCHIQ_SHARED_STATE_T *local = state->local;
+   DEBUG_INITIALISE(local)
+
+   while (1) {
+      DEBUG_COUNT(SLOT_HANDLER_COUNT);
+      DEBUG_TRACE(SLOT_HANDLER_LINE);
+      remote_event_wait(&local->trigger);
+
+      vcos_rmb();
+
+      DEBUG_TRACE(SLOT_HANDLER_LINE);
+      if (state->poll_needed)
+      {
+         state->poll_needed = 0;
+
+         /* Handle service polling and other rare conditions here out
+            of the mainline code */
+         switch (state->conn_state)
+         {
+         case VCHIQ_CONNSTATE_CONNECTED:
+            /* Poll the services as requested */
+            poll_services(state);
+            break;
+
+         case VCHIQ_CONNSTATE_PAUSING:
+            if (queue_message(state, NULL,
+               VCHIQ_MAKE_MSG(VCHIQ_MSG_PAUSE, 0, 0), NULL, 0, 0, 0)
+               != VCHIQ_RETRY)
+            {
+               if (state->is_master)
+                  pause_bulks(state);
+               vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSE_SENT);
+            }
+            else
+            {
+               state->poll_needed = 1; /* Retry later */
+            }
+            break;
+
+         case VCHIQ_CONNSTATE_RESUMING:
+            if (queue_message(state, NULL,
+               VCHIQ_MAKE_MSG(VCHIQ_MSG_RESUME, 0, 0), NULL, 0, 0, 0)
+               != VCHIQ_RETRY)
+            {
+               if (state->is_master)
+                  resume_bulks(state);
+               vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
+               vchiq_platform_resumed(state);
+            }
+            else
+            {
+               /* This should really be impossible, since the PAUSE should
+                  have flushed through outstanding messages. */
+               vcos_log_error("Failed to send RESUME message");
+               vcos_demand(0);
+            }
+            break;
+         default:
+            break;
+         }
+      }
+
+      DEBUG_TRACE(SLOT_HANDLER_LINE);
+      parse_rx_slots(state);
+   }
+   return NULL;
+}
+
+extern VCHIQ_STATUS_T
+vchiq_platform_suspend(VCHIQ_STATE_T *state);
+
+/* Called by the recycle thread */
+static void *
+recycle_func(void *v)
+{
+   VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
+   VCHIQ_SHARED_STATE_T *local = state->local;
+
+   while (1) {
+      remote_event_wait(&local->recycle);
+
+      vcos_mutex_lock(&state->slot_mutex);
+
+      process_free_queue(state);
+
+      vcos_mutex_unlock(&state->slot_mutex);
+   }
+   return NULL;
+}
+
+/* Called by the lp thread */
+static void *
+lp_func(void *v)
+{
+   VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
+
+   while (1) {
+      vcos_event_wait(&state->lp_evt);
+      vcos_mutex_lock(&state->use_count_mutex);
+      if (state->videocore_use_count == 0)
+      {
+         vchiq_platform_suspend(state);
+      }
+      vcos_mutex_unlock(&state->use_count_mutex);
+   }
+   return NULL;
+}
+
+static void
+init_bulk_queue(VCHIQ_BULK_QUEUE_T *queue)
+{
+   queue->local_insert = 0;
+   queue->remote_insert = 0;
+   queue->process = 0;
+   queue->remote_notify = 0;
+   queue->remove = 0;
+}
+
+VCHIQ_SLOT_ZERO_T *
+vchiq_init_slots(void *mem_base, int mem_size)
+{
+   int mem_align = (VCHIQ_SLOT_SIZE - (int)mem_base) & VCHIQ_SLOT_MASK;
+   VCHIQ_SLOT_ZERO_T *slot_zero = (VCHIQ_SLOT_ZERO_T *)((char *)mem_base + mem_align);
+   int num_slots = (mem_size - mem_align)/VCHIQ_SLOT_SIZE;
+   int first_data_slot = VCHIQ_SLOT_ZERO_SLOTS;
+
+   /* Ensure there is enough memory to run an absolutely minimum system */
+   num_slots -= first_data_slot;
+
+   if (num_slots < 4)
+   {
+      vcos_log_error("vchiq_init_slots - insufficient memory %x bytes", mem_size);
+      return NULL;
+   }
+
+   memset(slot_zero, 0, sizeof(VCHIQ_SLOT_ZERO_T));
+
+   slot_zero->magic = VCHIQ_MAGIC;
+   slot_zero->version = VCHIQ_VERSION;
+   slot_zero->version_min = VCHIQ_VERSION_MIN;
+   slot_zero->slot_zero_size = sizeof(VCHIQ_SLOT_ZERO_T);
+   slot_zero->slot_size = VCHIQ_SLOT_SIZE;
+   slot_zero->max_slots = VCHIQ_MAX_SLOTS;
+   slot_zero->max_slots_per_side = VCHIQ_MAX_SLOTS_PER_SIDE;
+
+   slot_zero->master.slot_first = first_data_slot;
+   slot_zero->slave.slot_first = first_data_slot + (num_slots/2);
+   slot_zero->master.slot_last = slot_zero->slave.slot_first - 1;
+   slot_zero->slave.slot_last = first_data_slot + num_slots - 1;
+
+   return slot_zero;
+}
+
+VCHIQ_STATUS_T
+vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero, int is_master)
+{
+   VCHIQ_SHARED_STATE_T *local;
+   VCHIQ_SHARED_STATE_T *remote;
+   VCOS_THREAD_ATTR_T attrs;
+   char threadname[10];
+   static int id = 0;
+   int i;
+
+   vcos_log_set_level(&vchiq_core_log_category, vchiq_default_core_log_level);
+   vcos_log_set_level(&vchiq_core_msg_log_category, vchiq_default_core_msg_log_level);
+   vcos_log_register("vchiq_core", &vchiq_core_log_category);
+   vcos_log_register("vchiq_core_msg", &vchiq_core_msg_log_category);
+
+   vcos_log_warn( "%s: slot_zero = 0x%08lx, is_master = %d\n", __func__, (unsigned long)slot_zero, is_master );
+
+   /* Check the input configuration */
+
+   if (slot_zero->magic != VCHIQ_MAGIC)
+   {
+      vcos_log_error("slot_zero=%x: magic=%x (expected %x)",
+         (unsigned int)slot_zero, slot_zero->magic, VCHIQ_MAGIC);
+      return VCHIQ_ERROR;
+   }
+
+   if (slot_zero->version < VCHIQ_VERSION_MIN)
+   {
+      vcos_log_error("slot_zero=%x: peer_version=%x (minimum %x)",
+         (unsigned int)slot_zero, slot_zero->version, VCHIQ_VERSION_MIN);
+      return VCHIQ_ERROR;
+   }
+
+   if (VCHIQ_VERSION < slot_zero->version_min)
+   {
+      vcos_log_error("slot_zero=%x: version=%x (peer minimum %x)",
+         (unsigned int)slot_zero, VCHIQ_VERSION, slot_zero->version_min);
+      return VCHIQ_ERROR;
+   }
+
+   if (slot_zero->slot_zero_size != sizeof(VCHIQ_SLOT_ZERO_T))
+   {
+      vcos_log_error("slot_zero=%x: slot_zero_size=%x (expected %x)",
+         (unsigned int)slot_zero, slot_zero->slot_zero_size, sizeof(VCHIQ_SLOT_ZERO_T));
+      return VCHIQ_ERROR;
+   }
+
+   if (slot_zero->slot_size != VCHIQ_SLOT_SIZE)
+   {
+      vcos_log_error("slot_zero=%x: slot_size=%d (expected %d",
+         (unsigned int)slot_zero, slot_zero->slot_size, VCHIQ_SLOT_SIZE);
+      return VCHIQ_ERROR;
+   }
+
+   if (slot_zero->max_slots != VCHIQ_MAX_SLOTS)
+   {
+      vcos_log_error("slot_zero=%x: max_slots=%d (expected %d)",
+         (unsigned int)slot_zero, slot_zero->max_slots, VCHIQ_MAX_SLOTS);
+      return VCHIQ_ERROR;
+   }
+
+   if (slot_zero->max_slots_per_side != VCHIQ_MAX_SLOTS_PER_SIDE)
+   {
+      vcos_log_error("slot_zero=%x: max_slots_per_side=%d (expected %d)",
+         (unsigned int)slot_zero, slot_zero->max_slots_per_side,
+         VCHIQ_MAX_SLOTS_PER_SIDE);
+      return VCHIQ_ERROR;
+   }
+
+   if (is_master)
+   {
+      local = &slot_zero->master;
+      remote = &slot_zero->slave;
+   }
+   else
+   {
+      local = &slot_zero->slave;
+      remote = &slot_zero->master;
+   }
+
+   if (local->initialised)
+   {
+      if (remote->initialised)
+         vcos_log_error("vchiq: FATAL: local state has already been initialised");
+      else
+         vcos_log_error("vchiq: FATAL: master/slave mismatch - two %ss", is_master ? "master" : "slave");
+      return VCHIQ_ERROR;
+   }
+
+   memset(state, 0, sizeof(VCHIQ_STATE_T));
+   state->id = id++;
+   state->is_master = is_master;
+
+   /*
+      initialize shared state pointers
+    */
+
+   state->local = local;
+   state->remote = remote;
+   state->slot_data = (VCHIQ_SLOT_T *)slot_zero;
+
+   /*
+      initialize events and mutexes
+    */
+
+   vcos_event_create(&state->connect, "v.connect");
+   vcos_mutex_create(&state->mutex, "v.mutex");
+   vcos_event_create(&state->trigger_event, "v.trigger_event");
+   vcos_event_create(&state->recycle_event, "v.recycle_event");
+
+   vcos_mutex_create(&state->slot_mutex, "v.slot_mutex");
+   vcos_mutex_create(&state->recycle_mutex, "v.recycle_mutex");
+   vcos_mutex_create(&state->use_count_mutex, "v.use_count_mutex");
+   vcos_mutex_create(&state->suspend_resume_mutex, "v.susp_res_mutex");
+
+   vcos_event_create(&state->slot_available_event, "v.slot_available_event");
+   vcos_event_create(&state->slot_remove_event, "v.slot_remove_event");
+
+   state->slot_queue_available = 0;
+
+   for (i = 0; i < VCHIQ_MAX_SERVICES; i++)
+   {
+      VCHIQ_SERVICE_QUOTA_T *service_quota = &state->service_quotas[i];
+      vcos_event_create(&service_quota->quota_event, "v.quota_event");
+   }
+
+   for (i = local->slot_first; i <= local->slot_last; i++)
+   {
+      local->slot_queue[state->slot_queue_available++] = i;
+   }
+
+   state->default_slot_quota = state->slot_queue_available/2;
+
+   local->trigger.event = &state->trigger_event;
+   remote_event_create(&local->trigger);
+   local->tx_pos = 0;
+
+   local->recycle.event = &state->recycle_event;
+   remote_event_create(&local->recycle);
+   local->slot_queue_recycle = state->slot_queue_available;
+
+   vcos_event_create(&state->lp_evt, "LP_EVT");
+
+   local->debug[DEBUG_ENTRIES] = DEBUG_MAX;
+
+   /*
+      bring up slot handler thread
+    */
+
+   vcos_thread_attr_init(&attrs);
+   vcos_thread_attr_setstacksize(&attrs, VCHIQ_SLOT_HANDLER_STACK);
+   vcos_thread_attr_setpriority(&attrs, VCOS_THREAD_PRI_REALTIME);
+   vcos_snprintf(threadname, sizeof(threadname), "VCHIQ-%d", state->id);
+   if (vcos_thread_create(&state->slot_handler_thread, threadname,
+            &attrs, slot_handler_func, state) != VCOS_SUCCESS)
+      return VCHIQ_ERROR;
+
+   vcos_thread_attr_init(&attrs);
+   vcos_thread_attr_setstacksize(&attrs, VCHIQ_SLOT_HANDLER_STACK);
+   vcos_thread_attr_setpriority(&attrs, VCOS_THREAD_PRI_REALTIME);
+   vcos_snprintf(threadname, sizeof(threadname), "VCHIQr-%d", state->id);
+   if (vcos_thread_create(&state->recycle_thread, threadname,
+            &attrs, recycle_func, state) != VCOS_SUCCESS)
+      return VCHIQ_ERROR;
+
+   vcos_thread_attr_init(&attrs);
+   vcos_thread_attr_setstacksize(&attrs, VCHIQ_SLOT_HANDLER_STACK);
+   vcos_thread_attr_setpriority(&attrs, VCOS_THREAD_PRI_LOWEST);
+   vcos_snprintf(threadname, sizeof(threadname), "VCHIQl-%d", state->id);
+   if (vcos_thread_create(&state->lp_thread, threadname,
+            &attrs, lp_func, state) != VCOS_SUCCESS)
+      return VCHIQ_ERROR;
+
+   /* Indicate readiness to the other side */
+   local->initialised = 1;
+
+   return VCHIQ_SUCCESS;
+}
+
+/* Called from application thread when a client or server service is created. */
+VCHIQ_SERVICE_T *
+vchiq_add_service_internal(VCHIQ_STATE_T *state,
+   const VCHIQ_SERVICE_PARAMS_T *params, int srvstate,
+   VCHIQ_INSTANCE_T instance)
+{
+   VCHIQ_SERVICE_T **pservice = NULL;
+   VCHIQ_SERVICE_T *service = NULL;
+   int i;
+
+   /* Prepare to use a previously unused service */
+   if (state->unused_service < VCHIQ_MAX_SERVICES)
+   {
+      pservice = &state->services[state->unused_service];
+   }
+
+   if (srvstate == VCHIQ_SRVSTATE_OPENING) {
+      for (i = 0; i < state->unused_service; i++) {
+         VCHIQ_SERVICE_T *srv = state->services[i];
+         if (!srv)
+         {
+            pservice = &state->services[i];
+            break;
+         }
+         if (srv->srvstate == VCHIQ_SRVSTATE_FREE) {
+            service = srv;
+            break;
+         }
+      }
+   } else {
+      for (i = (state->unused_service - 1); i >= 0; i--) {
+         VCHIQ_SERVICE_T *srv = state->services[i];
+         if (!srv)
+            pservice = &state->services[i];
+         else if (srv->srvstate == VCHIQ_SRVSTATE_FREE) {
+            service = srv;
+         } else if ((srv->public_fourcc == params->fourcc) &&
+            ((srv->instance != instance)
+            || (srv->base.callback != params->callback))) {
+            /* There is another server using this fourcc which doesn't match */
+            pservice = NULL;
+            service = NULL;
+         }
+      }
+   }
+
+   if (pservice && !service)
+   {
+      service = vcos_malloc(sizeof(VCHIQ_SERVICE_T), "VCHIQ service");
+      if (service)
+      {
+         service->srvstate = VCHIQ_SRVSTATE_FREE;
+         service->localport = (pservice - state->services);
+         vcos_event_create(&service->remove_event, "v.remove_event");
+         vcos_event_create(&service->bulk_remove_event, "v.bulk_remove_event");
+         vcos_mutex_create(&service->bulk_mutex, "v.bulk_mutex");
+         *pservice = service;
+      }
+      else
+      {
+         vcos_log_error("vchiq: Out of memory");
+      }
+   }
+
+   if (service) {
+      VCHIQ_SERVICE_QUOTA_T *service_quota =
+         &state->service_quotas[service->localport];
+      if (vcos_is_log_enabled( &vchiq_core_msg_log_category, VCOS_LOG_INFO)) {
+         vcos_log_impl( &vchiq_core_msg_log_category,
+                     VCOS_LOG_INFO,
+                     "%s Service %c%c%c%c SrcPort:%d",
+                     ( srvstate == VCHIQ_SRVSTATE_OPENING )
+                     ? "Open" : "Add",
+                     VCHIQ_FOURCC_AS_4CHARS(params->fourcc),
+                     service->localport );
+      }
+      service->state = state;
+      service->base.fourcc   = params->fourcc;
+      service->base.callback = params->callback;
+      service->base.userdata = params->userdata;
+      service->version       = params->version;
+      service->version_min   = params->version_min;
+      vchiq_set_service_state(service, srvstate);
+      service->public_fourcc =
+         (srvstate ==
+         VCHIQ_SRVSTATE_OPENING) ? VCHIQ_FOURCC_INVALID : params->fourcc;
+      service->instance = instance;
+      service->remoteport = VCHIQ_PORT_FREE;
+      service->client_id = 0;
+      service->auto_close = 1;
+      service->service_use_count = 0;
+      init_bulk_queue(&service->bulk_tx);
+      init_bulk_queue(&service->bulk_rx);
+      service_quota->slot_quota = state->default_slot_quota;
+      if (service_quota->slot_use_count == 0)
+         service_quota->previous_tx_index =
+            SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos) - 1;
+      memset(&service->stats, 0, sizeof(service->stats));
+      vcos_atomic_flags_create(&service->poll_flags);
+
+      /* Ensure the events are unsignalled */
+      while (vcos_event_try(&service->remove_event) == VCOS_SUCCESS)
+         continue;
+      while (vcos_event_try(&service_quota->quota_event) == VCOS_SUCCESS)
+         continue;
+
+      if (pservice == &state->services[state->unused_service])
+         state->unused_service++;
+   }
+
+   return service;
+}
+
+VCHIQ_STATUS_T
+vchiq_open_service_internal(VCHIQ_SERVICE_T *service, int client_id)
+{
+   VCHIQ_OPEN_PAYLOAD_T payload = {
+      service->base.fourcc,
+      client_id,
+      service->version,
+      service->version_min
+   };
+   VCHIQ_ELEMENT_T body = { &payload, sizeof(payload) };
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+   service->client_id = client_id;
+   vchiq_use_service(&service->base);
+   status = queue_message(service->state, NULL,
+                     VCHIQ_MAKE_MSG(VCHIQ_MSG_OPEN, service->localport, 0),
+                     &body, 1, sizeof(payload), 1);
+   if (status == VCHIQ_SUCCESS) {
+      if (vcos_event_wait(&service->remove_event) != VCOS_SUCCESS) {
+         status = VCHIQ_RETRY;
+         vchiq_release_service(&service->base);
+      } else if (service->srvstate != VCHIQ_SRVSTATE_OPEN) {
+         vcos_log_info("%d: osi - srvstate = %d", service->state->id, service->srvstate);
+         vcos_assert(service->srvstate == VCHIQ_SRVSTATE_CLOSEWAIT);
+         status = VCHIQ_ERROR;
+         VCHIQ_SERVICE_STATS_INC(service, error_count);
+         vchiq_release_service(&service->base);
+      }
+   }
+   return status;
+}
+
+/* Called by the slot handler */
+VCHIQ_STATUS_T
+vchiq_close_service_internal(VCHIQ_SERVICE_T *service, int close_recvd)
+{
+   VCHIQ_STATE_T *state = service->state;
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+   vcos_log_trace("%d: csi:%d (%s)",
+      service->state->id, service->localport,
+      srvstate_names[service->srvstate]);
+
+   switch (service->srvstate)
+   {
+   case VCHIQ_SRVSTATE_OPENING:
+      if (close_recvd)
+      {
+         /* The open was rejected - tell the user */
+         vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSEWAIT);
+         vcos_event_signal(&service->remove_event);
+      }
+      else
+      {
+         /* Shutdown mid-open - let the other side know */
+         status = queue_message(state, NULL,
+            VCHIQ_MAKE_MSG
+            (VCHIQ_MSG_CLOSE,
+            service->localport,
+            VCHIQ_MSG_DSTPORT(service->remoteport)),
+            NULL, 0, 0, 0);
+
+         if (status == VCHIQ_SUCCESS)
+            vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
+      }
+      break;
+
+   case VCHIQ_SRVSTATE_OPEN:
+      if (state->is_master)
+      {
+         /* Abort any outstanding bulk transfers */
+         vcos_mutex_lock(&service->bulk_mutex);
+         abort_outstanding_bulks(service, &service->bulk_tx);
+         abort_outstanding_bulks(service, &service->bulk_rx);
+         status = notify_bulks(service, &service->bulk_tx);
+         if (status == VCHIQ_SUCCESS)
+            status = notify_bulks(service, &service->bulk_rx);
+         vcos_mutex_unlock(&service->bulk_mutex);
+      }
+
+      if (status == VCHIQ_SUCCESS)
+         status = queue_message(state, NULL,
+            VCHIQ_MAKE_MSG
+            (VCHIQ_MSG_CLOSE,
+            service->localport,
+            VCHIQ_MSG_DSTPORT(service->remoteport)),
+            NULL, 0, 0, 0);
+
+      if (status == VCHIQ_SUCCESS)
+      {
+         if (close_recvd)
+            vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSING);
+         else
+            vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
+      }
+      break;
+
+   case VCHIQ_SRVSTATE_CLOSESENT:
+      vcos_assert(close_recvd);
+
+      if (!state->is_master)
+      {
+         /* Abort any outstanding bulk transfers */
+         vcos_mutex_lock(&service->bulk_mutex);
+         abort_outstanding_bulks(service, &service->bulk_tx);
+         abort_outstanding_bulks(service, &service->bulk_rx);
+         status = notify_bulks(service, &service->bulk_tx);
+         if (status == VCHIQ_SUCCESS)
+            status = notify_bulks(service, &service->bulk_rx);
+         vcos_mutex_unlock(&service->bulk_mutex);
+      }
+
+      if (status == VCHIQ_SUCCESS)
+         vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSING);
+      break;
+
+   case VCHIQ_SRVSTATE_CLOSING:
+      /* We may come here after a retry */
+      vcos_assert(!close_recvd);
+      break;
+
+   default:
+      vcos_log_error("vchiq_close_service_internal(%d) called in state %s",
+         close_recvd, srvstate_names[service->srvstate]);
+      vcos_assert(0);
+      break;
+   }
+
+   if (service->srvstate == VCHIQ_SRVSTATE_CLOSING)
+   {
+      /* Complete the close process */
+      vchiq_release_service(&service->base);
+
+      service->client_id = 0;
+
+      /* Now tell the client that the services is closed */
+      if (service->instance)
+      {
+         int oldstate = service->srvstate;
+
+         /* Change the service state now for the benefit of the callback */
+         vchiq_set_service_state(service,
+            ((service->public_fourcc == VCHIQ_FOURCC_INVALID) ||
+            !service->auto_close) ?
+            VCHIQ_SRVSTATE_CLOSEWAIT :
+            VCHIQ_SRVSTATE_LISTENING);
+
+         status = make_service_callback(service, VCHIQ_SERVICE_CLOSED, NULL, NULL);
+
+         if (status == VCHIQ_RETRY)
+         {
+            /* Restore the old state, to be retried later */
+            vchiq_set_service_state(service, oldstate);
+         }
+         else
+         {
+            if (status == VCHIQ_ERROR) {
+               /* Signal an error (fatal, since the other end will probably have closed) */
+               vchiq_set_service_state(service, VCHIQ_SRVSTATE_OPEN);
+            }
+         }
+      }
+
+      if (status != VCHIQ_RETRY)
+      {
+         if (service->srvstate == VCHIQ_SRVSTATE_CLOSING)
+            vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSEWAIT);
+         vcos_event_signal(&service->remove_event);
+      }
+   }
+
+   return status;
+}
+
+/* Called from the application process upon process death */
+void
+vchiq_terminate_service_internal(VCHIQ_SERVICE_T *service)
+{
+   VCHIQ_STATE_T *state = service->state;
+
+   vcos_log_info("%d: tsi - (%d<->%d)", state->id, service->localport, service->remoteport);
+
+   /* Disconnect from the instance, to prevent any callbacks */
+   service->instance = NULL;
+
+   /* Mark the service for termination by the slot handler */
+   request_poll(state, service, VCHIQ_POLL_TERMINATE);
+}
+
+/* Called from the application process upon process death, and from
+   vchiq_remove_service */
+void
+vchiq_free_service_internal(VCHIQ_SERVICE_T *service)
+{
+   VCHIQ_STATE_T *state = service->state;
+   int slot_last = state->remote->slot_last;
+   int i;
+
+   vcos_log_info("%d: fsi - (%d)", state->id, service->localport);
+
+   vcos_mutex_lock(&state->mutex);
+
+   /* Release any claimed messages */
+   for (i = state->remote->slot_first; i <= slot_last; i++)
+   {
+      VCHIQ_SLOT_INFO_T *slot_info = SLOT_INFO_FROM_INDEX(state, i);
+      if (slot_info->release_count != slot_info->use_count)
+      {
+         char *data = (char *)SLOT_DATA_FROM_INDEX(state, i);
+         int pos, end;
+
+         end = VCHIQ_SLOT_SIZE;
+         if (data == state->rx_data)
+         {
+            /* This buffer is still being read from - stop at the current read position */
+            end = state->rx_pos & VCHIQ_SLOT_MASK;
+         }
+
+         pos = 0;
+
+         while (pos < end)
+         {
+            VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)(data + pos);
+            int msgid = header->msgid;
+            int port = VCHIQ_MSG_DSTPORT(msgid);
+            if (port == service->localport)
+            {
+               if (msgid & VCHIQ_MSGID_CLAIMED)
+               {
+                  header->msgid = msgid & ~VCHIQ_MSGID_CLAIMED;
+                  vcos_log_info("  fsi - hdr %x", (unsigned int)header);
+                  release_slot(state, slot_info);
+               }
+            }
+            pos += calc_stride(header->size);
+         }
+      }
+   }
+
+   vcos_assert(state->services[service->localport] == service);
+   vchiq_set_service_state(service, VCHIQ_SRVSTATE_FREE);
+   state->services[service->localport] = NULL;
+   vcos_free(service);
+   vcos_mutex_unlock(&state->mutex);
+}
+
+VCHIQ_STATUS_T
+vchiq_connect_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance)
+{
+   int i;
+
+   /* Find all services registered to this client and enable them. */
+   for (i = 0; i < state->unused_service; i++)
+   {
+      VCHIQ_SERVICE_T *service = state->services[i];
+      if (service && (service->instance == instance)) {
+         if (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)
+            vchiq_set_service_state(service,
+               VCHIQ_SRVSTATE_LISTENING);
+      }
+   }
+
+   if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) {
+      if (queue_message(state, NULL,
+         VCHIQ_MAKE_MSG(VCHIQ_MSG_CONNECT, 0, 0), NULL, 0,
+         0, 1) == VCHIQ_RETRY)
+         return VCHIQ_RETRY;
+      vcos_event_wait(&state->connect);
+
+      vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
+   }
+
+   return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_shutdown_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance)
+{
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+   int i;
+
+   /* Find all services registered to this client and close them. */
+   for (i = 0; i < state->unused_service; i++)
+   {
+      VCHIQ_SERVICE_T *service = state->services[i];
+      if (service && (service->instance == instance) &&
+         ((service->srvstate == VCHIQ_SRVSTATE_OPEN) ||
+         (service->srvstate == VCHIQ_SRVSTATE_LISTENING)))
+      {
+         status = vchiq_remove_service(&service->base);
+         if (status != VCHIQ_SUCCESS)
+            break;
+      }
+   }
+
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_pause_internal(VCHIQ_STATE_T *state)
+{
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+   switch (state->conn_state)
+   {
+   case VCHIQ_CONNSTATE_CONNECTED:
+      /* Request a pause */
+      vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSING);
+      request_poll(state, NULL, 0);
+      break;
+   case VCHIQ_CONNSTATE_PAUSED:
+      break;
+   default:
+      status = VCHIQ_ERROR;
+      VCHIQ_STATS_INC(state, error_count);
+      break;
+   }
+
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_resume_internal(VCHIQ_STATE_T *state)
+{
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+   if (state->conn_state == VCHIQ_CONNSTATE_PAUSED)
+   {
+      vchiq_set_conn_state(state, VCHIQ_CONNSTATE_RESUMING);
+      request_poll(state, NULL, 0);
+   }
+   else
+   {
+      status = VCHIQ_ERROR;
+      VCHIQ_STATS_INC(state, error_count);
+   }
+
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_close_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   /* Unregister the service */
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *) handle;
+   VCHIQ_STATUS_T status = VCHIQ_ERROR;
+
+   if (service == NULL)
+      return VCHIQ_ERROR;
+
+   vcos_log_info("%d: close_service:%d", service->state->id, service->localport);
+
+   if (service->public_fourcc != VCHIQ_FOURCC_INVALID)
+   {
+      if (service->srvstate == VCHIQ_SRVSTATE_CLOSEWAIT)
+      {
+         /* This is a non-auto-close server */
+         vchiq_set_service_state(service, VCHIQ_SRVSTATE_LISTENING);
+         status = VCHIQ_SUCCESS;
+      }
+   }
+   else
+   {
+      /* For clients, make it an alias of vchiq_remove_service */
+      status = vchiq_remove_service(handle);
+   }
+
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_remove_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   /* Unregister the service */
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *) handle;
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+   if (service == NULL)
+      return VCHIQ_ERROR;
+
+   vcos_log_info("%d: remove_service:%d", service->state->id, service->localport);
+
+   switch (service->srvstate)
+   {
+   case VCHIQ_SRVSTATE_OPENING:
+   case VCHIQ_SRVSTATE_OPEN:
+      /* Mark the service for termination by the slot handler */
+      request_poll(service->state, service, VCHIQ_POLL_TERMINATE);
+
+      /* Drop through... */
+   case VCHIQ_SRVSTATE_CLOSESENT:
+   case VCHIQ_SRVSTATE_CLOSING:
+      while ((service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) &&
+         (service->srvstate != VCHIQ_SRVSTATE_LISTENING))
+      {
+         if (vcos_event_wait(&service->remove_event) != VCOS_SUCCESS) {
+            status = VCHIQ_RETRY;
+            break;
+         }
+      }
+      break;
+
+   default:
+      break;
+   }
+
+   if (status == VCHIQ_SUCCESS) {
+      if (service->srvstate == VCHIQ_SRVSTATE_OPEN)
+         status = VCHIQ_ERROR;
+      else
+      {
+         service->instance = NULL;
+         vchiq_free_service_internal(service);
+      }
+   }
+
+   return status;
+}
+
+
+VCHIQ_STATUS_T
+vchiq_bulk_transfer(VCHIQ_SERVICE_T *service,
+   VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata,
+   VCHIQ_BULK_MODE_T mode, VCHIQ_BULK_DIR_T dir)
+{
+   VCHIQ_BULK_QUEUE_T *queue = (dir == VCHIQ_BULK_TRANSMIT) ?
+      &service->bulk_tx : &service->bulk_rx;
+   VCHIQ_BULK_T *bulk;
+   VCHIQ_STATE_T *state;
+   BULK_WAITER_T bulk_waiter;
+   const char dir_char = (dir == VCHIQ_BULK_TRANSMIT) ? 't' : 'r';
+   const int dir_msgtype = (dir == VCHIQ_BULK_TRANSMIT) ? VCHIQ_MSG_BULK_TX : VCHIQ_MSG_BULK_RX;
+   VCHIQ_STATUS_T status = VCHIQ_ERROR;
+
+   if ((service == NULL) ||
+       ((memhandle == VCHI_MEM_HANDLE_INVALID) && (offset == NULL)))
+      return VCHIQ_ERROR;
+
+   state = service->state;
+
+   if (service->srvstate != VCHIQ_SRVSTATE_OPEN)
+      return VCHIQ_ERROR;  /* Must be connected */
+
+   if (vcos_mutex_lock(&service->bulk_mutex) != VCOS_SUCCESS)
+      return VCHIQ_RETRY;
+
+   if (queue->local_insert == queue->remove + VCHIQ_NUM_SERVICE_BULKS)
+   {
+      VCHIQ_SERVICE_STATS_INC(service, bulk_stalls);
+      do {
+         vcos_mutex_unlock(&service->bulk_mutex);
+         if (vcos_event_wait(&service->bulk_remove_event) != VCOS_SUCCESS)
+            return VCHIQ_RETRY;
+         if (vcos_mutex_lock(&service->bulk_mutex) != VCOS_SUCCESS)
+            return VCHIQ_RETRY;
+      } while (queue->local_insert == queue->remove + VCHIQ_NUM_SERVICE_BULKS);
+   }
+
+   bulk = &queue->bulks[BULK_INDEX(queue->local_insert)];
+
+   if (mode == VCHIQ_BULK_MODE_BLOCKING)
+   {
+      vcos_event_create(&bulk_waiter.event, "bulk_waiter");
+      bulk_waiter.actual = 0;
+      userdata = &bulk_waiter;
+   }
+
+   bulk->mode = mode;
+   bulk->dir = dir;
+   bulk->userdata = userdata;
+   bulk->size = size;
+   bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED;
+
+   if (vchiq_prepare_bulk_data(bulk, memhandle, offset, size, dir) != VCHIQ_SUCCESS)
+   {
+      goto error_exit;
+   }
+
+   vcos_log_info("%d: bt (%d->%d) %cx %x@%x %x", state->id,
+      service->localport, service->remoteport, dir_char,
+      size, (unsigned int)bulk->data, (unsigned int)userdata);
+
+   if (state->is_master)
+   {
+      queue->local_insert++;
+      if (resolve_bulks(service, queue))
+         request_poll(state, service, (dir == VCHIQ_BULK_TRANSMIT) ?
+            VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY);
+   }
+   else
+   {
+      int payload[2] = { (int)bulk->data, bulk->size };
+      VCHIQ_ELEMENT_T element = { payload, sizeof(payload) };
+
+      if (queue_message(state, NULL,
+         VCHIQ_MAKE_MSG(dir_msgtype,
+            service->localport, service->remoteport),
+         &element, 1, sizeof(payload), 1) != VCHIQ_SUCCESS)
+      {
+         vchiq_complete_bulk(bulk);
+         goto error_exit;
+      }
+      queue->local_insert++;
+      queue->remote_insert++;
+   }
+
+   vcos_mutex_unlock(&service->bulk_mutex);
+ 
+   vcos_log_trace("%d: bt:%d %cx li=%x ri=%x p=%x", state->id,
+      service->localport, dir_char,
+      queue->local_insert, queue->remote_insert, queue->process);
+
+   status = VCHIQ_SUCCESS;
+
+   if (mode == VCHIQ_BULK_MODE_BLOCKING)
+   {
+      if (vcos_event_wait(&bulk_waiter.event) != VCOS_SUCCESS)
+      {
+         vcos_log_info("bulk wait interrupted");
+         /* Stop notify_bulks signalling a non-existent waiter */
+         bulk->userdata = NULL;
+         status = VCHIQ_ERROR;
+      }
+      else if (bulk_waiter.actual == VCHIQ_BULK_ACTUAL_ABORTED)
+         status = VCHIQ_ERROR;
+
+      vcos_event_delete(&bulk_waiter.event);
+   }
+
+   return status;
+
+error_exit:
+   if (mode == VCHIQ_BULK_MODE_BLOCKING)
+      vcos_event_delete(&bulk_waiter.event);
+   vcos_mutex_unlock(&service->bulk_mutex);
+
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_bulk_transmit(VCHIQ_SERVICE_HANDLE_T handle,
+   const void *data, int size, void *userdata)
+{
+   return vchiq_bulk_transfer((VCHIQ_SERVICE_T *)handle,
+      VCHI_MEM_HANDLE_INVALID, (void *)data, size, userdata,
+      VCHIQ_BULK_MODE_CALLBACK, VCHIQ_BULK_TRANSMIT);
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_bulk_receive(VCHIQ_SERVICE_HANDLE_T handle, void *data, int size,
+   void *userdata)
+{
+   return vchiq_bulk_transfer((VCHIQ_SERVICE_T *)handle,
+      VCHI_MEM_HANDLE_INVALID, data, size, userdata,
+      VCHIQ_BULK_MODE_CALLBACK, VCHIQ_BULK_RECEIVE);
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_bulk_transmit_handle(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHI_MEM_HANDLE_T memhandle, const void *offset, int size, void *userdata)
+{
+   return vchiq_bulk_transfer((VCHIQ_SERVICE_T *)handle,
+      memhandle, (void *)offset, size, userdata,
+      VCHIQ_BULK_MODE_CALLBACK, VCHIQ_BULK_TRANSMIT);
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_bulk_receive_handle(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata)
+{
+   return vchiq_bulk_transfer((VCHIQ_SERVICE_T *)handle,
+      memhandle, offset, size, userdata,
+      VCHIQ_BULK_MODE_CALLBACK, VCHIQ_BULK_RECEIVE);
+}
+
+VCHIQ_STATUS_T
+vchiq_bulk_transmit(VCHIQ_SERVICE_HANDLE_T handle, const void *data, int size,
+   void *userdata, VCHIQ_BULK_MODE_T mode)
+{
+   return vchiq_bulk_transfer((VCHIQ_SERVICE_T *)handle,
+      VCHI_MEM_HANDLE_INVALID, (void *)data, size, userdata,
+      mode, VCHIQ_BULK_TRANSMIT);
+}
+
+VCHIQ_STATUS_T
+vchiq_bulk_receive(VCHIQ_SERVICE_HANDLE_T handle, void *data, int size,
+   void *userdata, VCHIQ_BULK_MODE_T mode)
+{
+   return vchiq_bulk_transfer((VCHIQ_SERVICE_T *)handle,
+      VCHI_MEM_HANDLE_INVALID, data, size, userdata,
+      mode, VCHIQ_BULK_RECEIVE);
+}
+
+VCHIQ_STATUS_T
+vchiq_bulk_transmit_handle(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHI_MEM_HANDLE_T memhandle, const void *offset, int size, void *userdata,
+   VCHIQ_BULK_MODE_T mode)
+{
+   return vchiq_bulk_transfer((VCHIQ_SERVICE_T *)handle,
+      memhandle, (void *)offset, size, userdata,
+      mode, VCHIQ_BULK_TRANSMIT);
+}
+
+VCHIQ_STATUS_T
+vchiq_bulk_receive_handle(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata,
+   VCHIQ_BULK_MODE_T mode)
+{
+   return vchiq_bulk_transfer((VCHIQ_SERVICE_T *)handle,
+      memhandle, offset, size, userdata,
+      mode, VCHIQ_BULK_RECEIVE);
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
+   const VCHIQ_ELEMENT_T *elements, int count)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *) handle;
+
+   unsigned int size = 0;
+   unsigned int i;
+
+   if ((service == NULL) ||
+      (service->srvstate != VCHIQ_SRVSTATE_OPEN))
+      return VCHIQ_ERROR;
+
+   for (i = 0; i < (unsigned int)count; i++)
+   {
+      if (elements[i].size)
+      {
+         if (elements[i].data == NULL)
+         {
+            VCHIQ_SERVICE_STATS_INC(service, error_count);
+            return VCHIQ_ERROR;
+         }
+         size += elements[i].size;
+      }
+   }
+
+   if (size > VCHIQ_MAX_MSG_SIZE)
+   {
+      VCHIQ_SERVICE_STATS_INC(service, error_count);
+      return VCHIQ_ERROR;
+   }
+
+   return queue_message(service->state, service,
+            VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA, service->localport,
+               service->remoteport), elements, count, size, 1);
+}
+
+void
+vchiq_release_message(VCHIQ_SERVICE_HANDLE_T handle, VCHIQ_HEADER_T *header)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   VCHIQ_STATE_T *state;
+   int slot_index;
+   int msgid;
+
+   if (service == NULL)
+      return;
+
+   state = service->state;
+
+   slot_index = SLOT_INDEX_FROM_DATA(state, (void *)header);
+
+   if ((slot_index >= state->remote->slot_first) &&
+      (slot_index <= state->remote->slot_last) &&
+      ((msgid = header->msgid) & VCHIQ_MSGID_CLAIMED))
+   {
+      VCHIQ_SLOT_INFO_T *slot_info = SLOT_INFO_FROM_INDEX(state, slot_index);
+
+      /* Rewrite the message header to prevent a double release */
+      header->msgid = msgid & ~VCHIQ_MSGID_CLAIMED;
+
+      release_slot(state, slot_info);
+   }
+}
+
+int
+vchiq_get_client_id(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   return service ? service->client_id : 0;
+}
+
+VCHIQ_STATUS_T
+vchiq_get_config(VCHIQ_INSTANCE_T instance,
+   int config_size, VCHIQ_CONFIG_T *pconfig)
+{
+   VCHIQ_CONFIG_T config;
+
+   vcos_unused(instance);
+
+   config.max_msg_size           = VCHIQ_MAX_MSG_SIZE;
+   config.bulk_threshold         = VCHIQ_MAX_MSG_SIZE;
+   config.max_outstanding_bulks  = VCHIQ_NUM_SERVICE_BULKS;
+   config.max_services           = VCHIQ_MAX_SERVICES;
+   config.version                = VCHIQ_VERSION;
+   config.version_min            = VCHIQ_VERSION_MIN;
+
+   if (config_size > sizeof(VCHIQ_CONFIG_T))
+      return VCHIQ_ERROR;
+
+   memcpy(pconfig, &config, vcos_min(config_size, sizeof(VCHIQ_CONFIG_T)));
+
+   return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_set_service_option(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHIQ_SERVICE_OPTION_T option, int value)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   VCHIQ_STATUS_T status = VCHIQ_ERROR;
+
+   if (service)
+   {
+      switch (option)
+      {
+      case VCHIQ_SERVICE_OPTION_AUTOCLOSE:
+         service->auto_close = value;
+         status = VCHIQ_SUCCESS;
+         break;
+
+      default:
+         break;
+      }
+   }
+
+   return status;
+}
+
+void
+vchiq_dump_shared_state(void *dump_context, VCHIQ_STATE_T *state,
+   VCHIQ_SHARED_STATE_T *shared, const char *label)
+{
+   static const char *const debug_names[] =
+   {
+      "<entries>",
+      "SLOT_HANDLER_COUNT",
+      "SLOT_HANDLER_LINE",
+      "PARSE_LINE",
+      "PARSE_HEADER",
+      "PARSE_MSGID",
+      "AWAIT_COMPLETION_LINE",
+      "DEQUEUE_MESSAGE_LINE",
+      "SERVICE_CALLBACK_LINE",
+      "MSG_QUEUE_FULL_COUNT",
+      "COMPLETION_QUEUE_FULL_COUNT"
+   };
+   int i;
+
+   char buf[80];
+   int len;
+   len = vcos_snprintf(buf, sizeof(buf),
+      "  %s: slots %d-%d tx_pos=%x recycle=%x",
+      label, shared->slot_first, shared->slot_last,
+      shared->tx_pos, shared->slot_queue_recycle);
+   vchiq_dump(dump_context, buf, len + 1);
+
+   len = vcos_snprintf(buf, sizeof(buf),
+      "    Slots claimed:"); 
+   vchiq_dump(dump_context, buf, len + 1);
+
+   for (i = shared->slot_first; i <= shared->slot_last; i++)
+   {
+      VCHIQ_SLOT_INFO_T slot_info = *SLOT_INFO_FROM_INDEX(state, i);
+      if (slot_info.use_count != slot_info.release_count)
+      {
+         len = vcos_snprintf(buf, sizeof(buf),
+            "      %d: %d/%d", i, slot_info.use_count, slot_info.release_count);
+         vchiq_dump(dump_context, buf, len + 1);
+      }
+   }
+
+   for (i = 1; i < shared->debug[DEBUG_ENTRIES]; i++)
+   {
+      len = vcos_snprintf(buf, sizeof(buf), "    DEBUG: %s = %d(%x)",
+         debug_names[i], shared->debug[i], shared->debug[i]);
+      vchiq_dump(dump_context, buf, len + 1);
+   }
+}
+
+void
+vchiq_dump_state(void *dump_context, VCHIQ_STATE_T *state)
+{
+   char buf[80];
+   int len;
+   int i;
+
+   len = vcos_snprintf(buf, sizeof(buf), "State %d: %s", state->id,
+      conn_state_names[state->conn_state]);
+   vchiq_dump(dump_context, buf, len + 1);
+
+   len = vcos_snprintf(buf, sizeof(buf),
+      "  tx_pos=%x(@%x), rx_pos=%x(@%x)",
+      state->id, state->local->tx_pos,
+      (uint32_t)state->tx_data + (state->local_tx_pos & VCHIQ_SLOT_MASK),
+      state->rx_pos,
+      (uint32_t)state->rx_data + (state->rx_pos & VCHIQ_SLOT_MASK));
+   vchiq_dump(dump_context, buf, len + 1);
+
+   len = vcos_snprintf(buf, sizeof(buf),
+      "  Version: %d (min %d)",
+      VCHIQ_VERSION, VCHIQ_VERSION_MIN);
+   vchiq_dump(dump_context, buf, len + 1);
+
+   if (VCHIQ_ENABLE_STATS)
+   {
+      len = vcos_snprintf(buf, sizeof(buf),
+         "  Stats: ctrl_tx_count=%d, ctrl_rx_count=%d, error_count=%d",
+         state->stats.ctrl_tx_count, state->stats.ctrl_rx_count,
+         state->stats.slot_stalls);
+      vchiq_dump(dump_context, buf, len + 1);
+   }
+
+   len = vcos_snprintf(buf, sizeof(buf),
+      "  Slots: %d available, %d recyclable, %d stalls",
+      state->slot_queue_available - SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos),
+      state->local->slot_queue_recycle - state->slot_queue_available,
+      state->stats.slot_stalls);
+   vchiq_dump(dump_context, buf, len + 1);
+
+   vchiq_dump_platform_state(dump_context);
+
+   vchiq_dump_shared_state(dump_context, state, state->local, "Local");
+   vchiq_dump_shared_state(dump_context, state, state->remote, "Remote");
+
+   vchiq_dump_platform_instances(dump_context);
+
+   for (i = 0; i < state->unused_service; i++) {
+      VCHIQ_SERVICE_T *service = state->services[i];
+
+      if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE))
+         vchiq_dump_service_state(dump_context, service);
+   }
+}
+
+void
+vchiq_dump_service_state(void *dump_context, VCHIQ_SERVICE_T *service)
+{
+   char buf[80];
+   int len;
+
+   len = vcos_snprintf(buf, sizeof(buf), "Service %d: %s",
+      service->localport, srvstate_names[service->srvstate]);
+
+   if (service->srvstate != VCHIQ_SRVSTATE_FREE)
+   {
+      char remoteport[30];
+      VCHIQ_SERVICE_QUOTA_T *service_quota =
+         &service->state->service_quotas[service->localport];
+      int fourcc = service->base.fourcc;
+      if (service->remoteport != VCHIQ_PORT_FREE)
+      {
+         int len2 = vcos_snprintf(remoteport, sizeof(remoteport), "%d",
+            service->remoteport);
+         if (service->public_fourcc != VCHIQ_FOURCC_INVALID)
+            vcos_snprintf(remoteport + len2, sizeof(remoteport) - len2,
+               " (client %x)", service->client_id);
+      }
+      else
+         vcos_strcpy(remoteport, "n/a");
+
+      len += vcos_snprintf(buf + len, sizeof(buf) - len,
+         " '%c%c%c%c' remote %s (slot use %d/%d)",
+         VCHIQ_FOURCC_AS_4CHARS(fourcc),
+         remoteport,
+         service_quota->slot_use_count,
+         service_quota->slot_quota);
+
+      if (VCHIQ_ENABLE_STATS)
+      {
+         vchiq_dump(dump_context, buf, len + 1);
+
+         len = vcos_snprintf(buf, sizeof(buf),
+            "  Ctrl: tx_count=%d, tx_bytes=%" PRIu64 ", rx_count=%d, rx_bytes=%" PRIu64,
+            service->stats.ctrl_tx_count, service->stats.ctrl_tx_bytes,
+            service->stats.ctrl_rx_count, service->stats.ctrl_rx_bytes);
+         vchiq_dump(dump_context, buf, len + 1);
+
+         len = vcos_snprintf(buf, sizeof(buf),
+            "  Bulk: tx_count=%d, tx_bytes=%" PRIu64 ", rx_count=%d, rx_bytes=%" PRIu64,
+            service->stats.bulk_tx_count, service->stats.bulk_tx_bytes,
+            service->stats.bulk_rx_count, service->stats.bulk_rx_bytes);
+         vchiq_dump(dump_context, buf, len + 1);
+
+         len = vcos_snprintf(buf, sizeof(buf),
+            "  %d quota stalls, %d slot stalls, %d bulk stalls, %d aborted, %d errors",
+            service->stats.quota_stalls, service->stats.slot_stalls,
+            service->stats.bulk_stalls, service->stats.bulk_aborted_count,
+            service->stats.error_count);
+       }
+   }
+
+   vchiq_dump(dump_context, buf, len + 1);
+
+   vchiq_dump_platform_service_state(dump_context, service);
+}
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.h
new file mode 100644
index 0000000..641f6a6
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.h
@@ -0,0 +1,480 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_CORE_H
+#define VCHIQ_CORE_H
+
+#include "vchiq_cfg.h"
+
+#include "vchiq.h"
+
+#define IS_POW2(x) (x && ((x & (x - 1)) == 0))
+
+/* Ensure that the slot size and maximum number of slots are powers of 2 */
+vcos_static_assert(IS_POW2(VCHIQ_SLOT_SIZE));
+vcos_static_assert(IS_POW2(VCHIQ_MAX_SLOTS));
+vcos_static_assert(IS_POW2(VCHIQ_MAX_SLOTS_PER_SIDE));
+
+#define VCHIQ_SLOT_MASK        (VCHIQ_SLOT_SIZE - 1)
+#define VCHIQ_SLOT_QUEUE_MASK  (VCHIQ_MAX_SLOTS_PER_SIDE - 1)
+#define VCHIQ_SLOT_ZERO_SLOTS  ((sizeof(VCHIQ_SLOT_ZERO_T) + \
+   VCHIQ_SLOT_SIZE - 1) / VCHIQ_SLOT_SIZE)
+
+#define VCHIQ_MSG_PADDING              0  // -
+#define VCHIQ_MSG_CONNECT              1  // -
+#define VCHIQ_MSG_OPEN                 2  // + (srcport, -), fourcc, client_id
+#define VCHIQ_MSG_OPENACK              3  // + (srcport, dstport)
+#define VCHIQ_MSG_CLOSE                4  // + (srcport, dstport)
+#define VCHIQ_MSG_DATA                 5  // + (srcport, dstport)
+#define VCHIQ_MSG_BULK_RX              6  // + (srcport, dstport), data, size
+#define VCHIQ_MSG_BULK_TX              7  // + (srcport, dstport), data, size
+#define VCHIQ_MSG_BULK_RX_DONE         8  // + (srcport, dstport), actual
+#define VCHIQ_MSG_BULK_TX_DONE         9  // + (srcport, dstport), actual
+#define VCHIQ_MSG_PAUSE               10  // -
+#define VCHIQ_MSG_RESUME              11  // -
+
+#define VCHIQ_PORT_MAX                 (VCHIQ_MAX_SERVICES - 1)
+#define VCHIQ_PORT_FREE                0x1000
+#define VCHIQ_PORT_IS_VALID(port)      (port < VCHIQ_PORT_FREE)
+#define VCHIQ_MAKE_MSG(type,srcport,dstport)      ((type<<24) | (srcport<<12) | (dstport<<0))
+#define VCHIQ_MSG_TYPE(msgid)          ((unsigned int)msgid >> 24)
+#define VCHIQ_MSG_SRCPORT(msgid)       (unsigned short)(((unsigned int)msgid >> 12) & 0xfff)
+#define VCHIQ_MSG_DSTPORT(msgid)       ((unsigned short)msgid & 0xfff)
+
+#define VCHIQ_FOURCC_AS_4CHARS(fourcc)	\
+   ((fourcc) >> 24) & 0xff, \
+   ((fourcc) >> 16) & 0xff, \
+   ((fourcc) >>  8) & 0xff, \
+   ((fourcc)      ) & 0xff
+
+/* Ensure the fields are wide enough */
+vcos_static_assert(VCHIQ_MSG_SRCPORT(VCHIQ_MAKE_MSG(0,0,VCHIQ_PORT_MAX)) == 0);
+vcos_static_assert(VCHIQ_MSG_TYPE(VCHIQ_MAKE_MSG(0,VCHIQ_PORT_MAX,0)) == 0);
+vcos_static_assert((unsigned int)VCHIQ_PORT_MAX < (unsigned int)VCHIQ_PORT_FREE);
+
+#define VCHIQ_MSGID_PADDING            VCHIQ_MAKE_MSG(VCHIQ_MSG_PADDING,0,0)
+#define VCHIQ_MSGID_CLAIMED            0x40000000
+
+#define VCHIQ_FOURCC_INVALID           0x00000000
+#define VCHIQ_FOURCC_IS_LEGAL(fourcc)  (fourcc != VCHIQ_FOURCC_INVALID)
+
+#define VCHIQ_BULK_ACTUAL_ABORTED -1
+
+typedef uint32_t BITSET_T;
+
+vcos_static_assert((sizeof(BITSET_T) * 8) == 32);
+
+#define BITSET_SIZE(b)        ((b + 31) >> 5)
+#define BITSET_WORD(b)        (b >> 5)
+#define BITSET_BIT(b)         (1 << (b & 31))
+#define BITSET_ZERO(bs)       memset(bs, 0, sizeof(bs))
+#define BITSET_IS_SET(bs, b)  (bs[BITSET_WORD(b)] & BITSET_BIT(b))
+#define BITSET_SET(bs, b)     (bs[BITSET_WORD(b)] |= BITSET_BIT(b))
+#define BITSET_CLR(bs, b)     (bs[BITSET_WORD(b)] &= ~BITSET_BIT(b))
+
+#if VCHIQ_ENABLE_STATS
+#define VCHIQ_STATS_INC(state, stat) (state->stats. stat ++)
+#define VCHIQ_SERVICE_STATS_INC(service, stat) (service->stats. stat ++)
+#define VCHIQ_SERVICE_STATS_ADD(service, stat, addend) (service->stats. stat += addend)
+#else
+#define VCHIQ_STATS_INC(state, stat) ((void)0)
+#define VCHIQ_SERVICE_STATS_INC(service, stat) ((void)0)
+#define VCHIQ_SERVICE_STATS_ADD(service, stat, addend) ((void)0)
+#endif
+
+enum
+{
+   DEBUG_ENTRIES,
+#if VCHIQ_ENABLE_DEBUG
+   DEBUG_SLOT_HANDLER_COUNT,
+   DEBUG_SLOT_HANDLER_LINE,
+   DEBUG_PARSE_LINE,
+   DEBUG_PARSE_HEADER,
+   DEBUG_PARSE_MSGID,
+   DEBUG_AWAIT_COMPLETION_LINE,
+   DEBUG_DEQUEUE_MESSAGE_LINE,
+   DEBUG_SERVICE_CALLBACK_LINE,
+   DEBUG_MSG_QUEUE_FULL_COUNT,
+   DEBUG_COMPLETION_QUEUE_FULL_COUNT,
+#endif
+   DEBUG_MAX
+};
+
+#if VCHIQ_ENABLE_DEBUG
+
+#define DEBUG_INITIALISE(local) volatile int *debug_ptr = (local)->debug;
+#define DEBUG_TRACE(d) debug_ptr[DEBUG_ ## d] = __LINE__
+#define DEBUG_VALUE(d,v) debug_ptr[DEBUG_ ## d] = (v)
+#define DEBUG_COUNT(d) debug_ptr[DEBUG_ ## d]++
+
+#else /* VCHIQ_ENABLE_DEBUG */
+
+#define DEBUG_INITIALISE(local)
+#define DEBUG_TRACE(d)
+#define DEBUG_VALUE(d,v)
+#define DEBUG_COUNT(d)
+
+#endif /* VCHIQ_ENABLE_DEBUG */
+
+typedef enum
+{
+   VCHIQ_CONNSTATE_DISCONNECTED,
+   VCHIQ_CONNSTATE_CONNECTED,
+   VCHIQ_CONNSTATE_PAUSING,
+   VCHIQ_CONNSTATE_PAUSE_SENT,
+   VCHIQ_CONNSTATE_PAUSED,
+   VCHIQ_CONNSTATE_RESUMING
+} VCHIQ_CONNSTATE_T;
+
+enum
+{
+   VCHIQ_SRVSTATE_FREE,
+   VCHIQ_SRVSTATE_HIDDEN,
+   VCHIQ_SRVSTATE_LISTENING,
+   VCHIQ_SRVSTATE_OPENING,
+   VCHIQ_SRVSTATE_OPEN,
+   VCHIQ_SRVSTATE_CLOSESENT,
+   VCHIQ_SRVSTATE_CLOSING,
+   VCHIQ_SRVSTATE_CLOSEWAIT
+};
+
+enum
+{
+   VCHIQ_POLL_TERMINATE,
+   VCHIQ_POLL_TXNOTIFY,
+   VCHIQ_POLL_RXNOTIFY,
+   VCHIQ_POLL_COUNT
+};
+
+typedef enum
+{
+   VCHIQ_BULK_TRANSMIT,
+   VCHIQ_BULK_RECEIVE
+} VCHIQ_BULK_DIR_T;
+
+typedef struct vchiq_bulk_struct {
+   short mode;
+   short dir;
+   void *userdata;
+   VCHI_MEM_HANDLE_T handle;
+   void *data;
+   int size;
+   void *remote_data;
+   int remote_size;
+   int actual;
+} VCHIQ_BULK_T;
+
+typedef struct vchiq_bulk_queue_struct {
+   int local_insert;  /* Where to insert the next local bulk */
+   int remote_insert; /* Where to insert the next remote bulk (master) */
+   int process;       /* Bulk to transfer next */
+   int remote_notify; /* Bulk to notify the remote client of next (master) */
+   int remove;        /* Bulk to notify the local client of, and remove, next */
+   VCHIQ_BULK_T bulks[VCHIQ_NUM_SERVICE_BULKS];
+} VCHIQ_BULK_QUEUE_T;
+
+typedef struct remote_event_struct {
+   volatile int armed;
+   volatile int fired;
+   VCOS_EVENT_T * event;
+} REMOTE_EVENT_T;
+
+typedef struct vchiq_state_struct VCHIQ_STATE_T;
+
+typedef struct vchiq_slot_struct {
+   char data[VCHIQ_SLOT_SIZE];
+} VCHIQ_SLOT_T;
+
+typedef struct vchiq_slot_info_struct {
+   /* Use two counters rather than one to avoid the need for a mutex. */
+   volatile short use_count;
+   volatile short release_count;
+} VCHIQ_SLOT_INFO_T;
+
+typedef struct vchiq_service_struct {
+   VCHIQ_SERVICE_BASE_T base;
+   volatile int srvstate;
+   unsigned int localport;
+   unsigned int remoteport;
+   int public_fourcc;
+   int client_id;
+   int auto_close;
+   VCOS_ATOMIC_FLAGS_T poll_flags;
+   short version;
+   short version_min;
+
+   VCHIQ_STATE_T *state;
+   VCHIQ_INSTANCE_T instance;
+
+   int service_use_count;
+
+   VCHIQ_BULK_QUEUE_T bulk_tx;
+   VCHIQ_BULK_QUEUE_T bulk_rx;
+
+   VCOS_EVENT_T remove_event;
+   VCOS_EVENT_T bulk_remove_event;
+   VCOS_MUTEX_T bulk_mutex;
+
+   struct service_stats_struct
+   {
+      int quota_stalls;
+      int slot_stalls;
+      int bulk_stalls;
+      int error_count;
+      int ctrl_tx_count;
+      int ctrl_rx_count;
+      int bulk_tx_count;
+      int bulk_rx_count;
+      int bulk_aborted_count;
+      uint64_t ctrl_tx_bytes;
+      uint64_t ctrl_rx_bytes;
+      uint64_t bulk_tx_bytes;
+      uint64_t bulk_rx_bytes;
+   } stats;
+} VCHIQ_SERVICE_T;
+
+/* The quota information is outside VCHIQ_SERVICE_T so that it can be
+   statically allocated, since for accounting reasons a service's slot
+   usage is carried over between users of the same port number.
+ */
+typedef struct vchiq_service_quota_struct {
+   int slot_quota;
+   int slot_use_count;
+   VCOS_EVENT_T quota_event;
+   int previous_tx_index;
+} VCHIQ_SERVICE_QUOTA_T;
+
+typedef struct vchiq_shared_state_struct {
+
+   /* A non-zero value here indicates that the content is valid. */
+   int initialised;
+
+   /* The first and last (inclusive) slots allocated to the owner. */
+   int slot_first;
+   int slot_last;
+
+   /* Signalling this event indicates that owner's slot handler thread should
+      run. */
+   REMOTE_EVENT_T trigger;
+
+   /* Indicates the byte position within the stream where the next message
+      will be written. The least significant bits are an index into the slot.
+      The next bits are the index of the slot in slot_queue. */
+   volatile int tx_pos;
+
+   /* This event should be signalled when a slot is recycled. */
+   REMOTE_EVENT_T recycle;
+
+   /* The slot_queue index where the next recycled slot will be written. */
+   volatile int slot_queue_recycle;
+
+   /* A circular buffer of slot indexes. */
+   int slot_queue[VCHIQ_MAX_SLOTS_PER_SIDE];
+
+   /* Debugging state */
+   volatile int debug[DEBUG_MAX];
+} VCHIQ_SHARED_STATE_T;
+
+typedef struct vchiq_slot_zero_struct {
+   int magic;
+   short version;
+   short version_min;
+   int slot_zero_size;
+   int slot_size;
+   int max_slots;
+   int max_slots_per_side;
+   int platform_data[2];
+   VCHIQ_SHARED_STATE_T master;
+   VCHIQ_SHARED_STATE_T slave;
+   VCHIQ_SLOT_INFO_T slots[VCHIQ_MAX_SLOTS];
+} VCHIQ_SLOT_ZERO_T;
+
+struct vchiq_state_struct {
+   int id;
+   int initialised;
+   VCHIQ_CONNSTATE_T conn_state;
+   int is_master;
+
+   VCHIQ_SHARED_STATE_T *local;
+   VCHIQ_SHARED_STATE_T *remote;
+   VCHIQ_SLOT_T *slot_data;
+
+   int default_slot_quota;
+
+   VCOS_EVENT_T connect;      // event indicating connect message received
+   VCOS_MUTEX_T mutex;        // mutex protecting services
+   VCHIQ_INSTANCE_T *instance;
+
+   VCOS_THREAD_T slot_handler_thread;  // processes incoming messages
+   VCOS_THREAD_T recycle_thread;       // processes recycled slots
+   VCOS_THREAD_T lp_thread;            // processes low priority messages (eg suspend)
+
+   /* Local implementation of the trigger remote event */
+   VCOS_EVENT_T trigger_event;
+
+   /* Local implementation of the recycle remote event */
+   VCOS_EVENT_T recycle_event;
+
+   VCOS_EVENT_T lp_evt;
+
+   char *tx_data;
+   char *rx_data;
+   VCHIQ_SLOT_INFO_T *rx_info;
+
+   VCOS_MUTEX_T slot_mutex;
+
+   VCOS_MUTEX_T recycle_mutex;
+
+   VCOS_MUTEX_T suspend_resume_mutex;
+   VCOS_MUTEX_T use_count_mutex;
+
+   /* Global use count for videocore.
+    * This is equal to the sum of the use counts for all services.  When this hits
+    * zero the videocore suspend procedure will be initiated. */
+   int videocore_use_count;
+
+   /* Flag to indicate whether videocore is currently suspended */
+   int videocore_suspended;
+
+   /* Indicates the byte position within the stream from where the next message
+      will be read. The least significant bits are an index into the slot.
+      The next bits are the index of the slot in remote->slot_queue. */
+   int rx_pos;
+
+   /* A cached copy of local->tx_pos. Only write to local->tx_pos, and read
+      from remote->tx_pos. */
+   int local_tx_pos;
+
+   /* The slot_queue index of the slot to become available next. */
+   int slot_queue_available;
+
+   /* A flag to indicate if any poll has been requested */
+   int poll_needed;
+
+   /* An array of bit sets indicating which services must be polled. */
+   VCOS_ATOMIC_FLAGS_T poll_services[BITSET_SIZE(VCHIQ_MAX_SERVICES)];
+
+   /* The number of the first unused service */
+   int unused_service;
+
+   /* Signalled when a free slot becomes available. */
+   VCOS_EVENT_T slot_available_event;
+
+   VCOS_EVENT_T slot_remove_event;
+
+   struct state_stats_struct
+   {
+      int slot_stalls;
+      int ctrl_tx_count;
+      int ctrl_rx_count;
+      int error_count;
+   } stats;
+
+   VCHIQ_SERVICE_T *services[VCHIQ_MAX_SERVICES];
+   VCHIQ_SERVICE_QUOTA_T service_quotas[VCHIQ_MAX_SERVICES];
+   VCHIQ_SLOT_INFO_T slot_info[VCHIQ_MAX_SLOTS];
+};
+
+extern VCHIQ_SLOT_ZERO_T *
+vchiq_init_slots(void *mem_base, int mem_size);
+
+extern VCHIQ_STATUS_T
+vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero, int is_master);
+
+extern VCHIQ_STATUS_T
+vchiq_connect_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance);
+
+extern VCHIQ_SERVICE_T *
+vchiq_add_service_internal(VCHIQ_STATE_T *state,
+   const VCHIQ_SERVICE_PARAMS_T *params, int srvstate,
+   VCHIQ_INSTANCE_T instance);
+
+extern VCHIQ_STATUS_T
+vchiq_open_service_internal(VCHIQ_SERVICE_T *service, int client_id);
+
+extern VCHIQ_STATUS_T
+vchiq_close_service_internal(VCHIQ_SERVICE_T *service, int close_recvd);
+
+extern void
+vchiq_terminate_service_internal(VCHIQ_SERVICE_T *service);
+
+extern void
+vchiq_free_service_internal(VCHIQ_SERVICE_T *service);
+
+extern VCHIQ_STATUS_T
+vchiq_shutdown_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance);
+
+extern VCHIQ_STATUS_T
+vchiq_pause_internal(VCHIQ_STATE_T *state);
+
+extern VCHIQ_STATUS_T
+vchiq_resume_internal(VCHIQ_STATE_T *state);
+
+extern void
+remote_event_pollall(VCHIQ_STATE_T *state);
+
+extern VCHIQ_STATUS_T
+vchiq_bulk_transfer(VCHIQ_SERVICE_T *service,
+   VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata,
+   VCHIQ_BULK_MODE_T mode, VCHIQ_BULK_DIR_T dir);
+
+extern void
+vchiq_dump_state(void *dump_context, VCHIQ_STATE_T *state);
+
+extern void
+vchiq_dump_service_state(void *dump_context, VCHIQ_SERVICE_T *service);
+
+/* The following functions are called from vchiq_core, and external
+   implementations must be provided. */
+
+extern VCHIQ_STATUS_T
+vchiq_prepare_bulk_data(VCHIQ_BULK_T *bulk,
+   VCHI_MEM_HANDLE_T memhandle, void *offset, int size, int dir);
+
+extern void
+vchiq_transfer_bulk(VCHIQ_BULK_T *bulk);
+
+extern void
+vchiq_complete_bulk(VCHIQ_BULK_T *bulk);
+
+extern VCHIQ_STATUS_T
+vchiq_copy_from_user(void *dst, const void *src, int size);
+
+extern void
+remote_event_signal(REMOTE_EVENT_T *event);
+
+extern void
+vchiq_platform_paused(VCHIQ_STATE_T *state);
+
+extern void
+vchiq_platform_resumed(VCHIQ_STATE_T *state);
+
+extern void
+vchiq_dump(void *dump_context, const char *str, int len);
+
+extern void
+vchiq_dump_platform_state(void *dump_context);
+
+extern void
+vchiq_dump_platform_instances(void *dump_context);
+
+extern void
+vchiq_dump_platform_service_state(void *dump_context,
+   VCHIQ_SERVICE_T *service);
+
+#endif
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_if.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_if.h
new file mode 100644
index 0000000..af3fa7c
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_if.h
@@ -0,0 +1,148 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_IF_H
+#define VCHIQ_IF_H
+
+#include "interface/vchi/vchi_mh.h"
+
+#define VCHIQ_SLOT_SIZE          4096
+#define VCHIQ_MAX_MSG_SIZE       (VCHIQ_SLOT_SIZE - sizeof(VCHIQ_HEADER_T))
+#define VCHIQ_CHANNEL_SIZE       VCHIQ_MAX_MSG_SIZE /* For backwards compatibility */
+
+#define VCHIQ_MAKE_FOURCC(x0, x1, x2, x3)     (((x0) << 24) | ((x1) << 16) | ((x2) << 8) | (x3))
+#define VCHIQ_GET_SERVICE_USERDATA(service)   (service->userdata)
+#define VCHIQ_GET_SERVICE_FOURCC(service)     (service->fourcc)
+
+typedef enum {
+   VCHIQ_SERVICE_OPENED,         // service, -, -
+   VCHIQ_SERVICE_CLOSED,         // service, -, -
+   VCHIQ_MESSAGE_AVAILABLE,      // service, header, -
+   VCHIQ_BULK_TRANSMIT_DONE,     // service, -, bulk_userdata
+   VCHIQ_BULK_RECEIVE_DONE,      // service, -, bulk_userdata
+   VCHIQ_BULK_TRANSMIT_ABORTED,  // service, -, bulk_userdata
+   VCHIQ_BULK_RECEIVE_ABORTED    // service, -, bulk_userdata
+} VCHIQ_REASON_T;
+
+typedef enum
+{
+   VCHIQ_ERROR   = -1,
+   VCHIQ_SUCCESS = 0,
+   VCHIQ_RETRY   = 1
+} VCHIQ_STATUS_T;
+
+typedef enum
+{
+   VCHIQ_BULK_MODE_CALLBACK,
+   VCHIQ_BULK_MODE_BLOCKING,
+   VCHIQ_BULK_MODE_NOCALLBACK
+} VCHIQ_BULK_MODE_T;
+
+typedef enum
+{
+   VCHIQ_SERVICE_OPTION_AUTOCLOSE
+} VCHIQ_SERVICE_OPTION_T;
+
+#ifdef __HIGHC__
+/* Allow zero-sized arrays without warnings */
+#pragma warning (push)
+#pragma warning (disable : 4200)
+#endif
+
+typedef struct vchiq_header_struct {
+   /* The message identifier - opaque to applications. */
+   int msgid;
+
+   /* Size of message data. */
+   unsigned int size;      
+
+   char data[0];           /* message */
+} VCHIQ_HEADER_T;
+
+#ifdef __HIGHC__
+#pragma warning (pop)
+#endif
+
+typedef struct {
+   const void *data;
+   int size;
+} VCHIQ_ELEMENT_T;
+
+typedef const struct vchiq_service_base_struct *VCHIQ_SERVICE_HANDLE_T;
+
+typedef VCHIQ_STATUS_T (*VCHIQ_CALLBACK_T)(VCHIQ_REASON_T, VCHIQ_HEADER_T *, VCHIQ_SERVICE_HANDLE_T, void *);
+
+typedef struct vchiq_service_base_struct {
+   int fourcc;
+   VCHIQ_CALLBACK_T callback;
+   void *userdata;
+} VCHIQ_SERVICE_BASE_T;
+
+typedef struct vchiq_service_params_struct {
+  int fourcc;
+  VCHIQ_CALLBACK_T callback;
+  void *userdata;
+  short version;       /* Increment for non-trivial changes */
+  short version_min;   /* Update for incompatible changes */
+} VCHIQ_SERVICE_PARAMS_T;
+
+typedef struct vchiq_config_struct {
+   int max_msg_size;
+   int bulk_threshold; /* The message size aboce which it is better to use
+                          a bulk transfer (<= max_msg_size) */
+   int max_outstanding_bulks;
+   int max_services;
+   short version;      /* The version of VCHIQ */
+   short version_min;  /* The minimum compatible version of VCHIQ */
+} VCHIQ_CONFIG_T;
+
+typedef struct vchiq_instance_struct *VCHIQ_INSTANCE_T;
+
+extern VCHIQ_STATUS_T vchiq_initialise(VCHIQ_INSTANCE_T *pinstance);
+extern VCHIQ_STATUS_T vchiq_shutdown(VCHIQ_INSTANCE_T instance);
+extern VCHIQ_STATUS_T vchiq_connect(VCHIQ_INSTANCE_T instance);
+extern VCHIQ_STATUS_T vchiq_add_service(VCHIQ_INSTANCE_T instance, int fourcc, VCHIQ_CALLBACK_T callback, void *userdata, VCHIQ_SERVICE_HANDLE_T *pservice);
+extern VCHIQ_STATUS_T vchiq_open_service(VCHIQ_INSTANCE_T instance, int fourcc, VCHIQ_CALLBACK_T callback, void *userdata, VCHIQ_SERVICE_HANDLE_T *pservice);
+extern VCHIQ_STATUS_T vchiq_add_service_params(VCHIQ_INSTANCE_T instance,
+   const VCHIQ_SERVICE_PARAMS_T *params,
+   VCHIQ_SERVICE_HANDLE_T *pservice);
+extern VCHIQ_STATUS_T vchiq_open_service_params(VCHIQ_INSTANCE_T instance,
+   const VCHIQ_SERVICE_PARAMS_T *params,
+   VCHIQ_SERVICE_HANDLE_T *pservice);
+extern VCHIQ_STATUS_T vchiq_close_service(VCHIQ_SERVICE_HANDLE_T service);
+extern VCHIQ_STATUS_T vchiq_remove_service(VCHIQ_SERVICE_HANDLE_T service);
+extern VCHIQ_STATUS_T vchiq_use_service(VCHIQ_SERVICE_HANDLE_T service);
+extern VCHIQ_STATUS_T vchiq_release_service(VCHIQ_SERVICE_HANDLE_T service);
+
+extern VCHIQ_STATUS_T vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T service, const VCHIQ_ELEMENT_T *elements, int count);
+extern void           vchiq_release_message(VCHIQ_SERVICE_HANDLE_T service, VCHIQ_HEADER_T *header);
+extern VCHIQ_STATUS_T vchiq_queue_bulk_transmit(VCHIQ_SERVICE_HANDLE_T service, const void *data, int size, void *userdata);
+extern VCHIQ_STATUS_T vchiq_queue_bulk_receive(VCHIQ_SERVICE_HANDLE_T service, void *data, int size, void *userdata);
+extern VCHIQ_STATUS_T vchiq_queue_bulk_transmit_handle(VCHIQ_SERVICE_HANDLE_T service, VCHI_MEM_HANDLE_T handle, const void *offset, int size, void *userdata);
+extern VCHIQ_STATUS_T vchiq_queue_bulk_receive_handle(VCHIQ_SERVICE_HANDLE_T service, VCHI_MEM_HANDLE_T handle, void *offset, int size, void *userdata);
+extern VCHIQ_STATUS_T vchiq_bulk_transmit(VCHIQ_SERVICE_HANDLE_T service, const void *data, int size, void *userdata, VCHIQ_BULK_MODE_T mode);
+extern VCHIQ_STATUS_T vchiq_bulk_receive(VCHIQ_SERVICE_HANDLE_T service, void *data, int size, void *userdata, VCHIQ_BULK_MODE_T mode);
+extern VCHIQ_STATUS_T vchiq_bulk_transmit_handle(VCHIQ_SERVICE_HANDLE_T service, VCHI_MEM_HANDLE_T handle, const void *offset, int size, void *userdata, VCHIQ_BULK_MODE_T mode);
+extern VCHIQ_STATUS_T vchiq_bulk_receive_handle(VCHIQ_SERVICE_HANDLE_T service, VCHI_MEM_HANDLE_T handle, void *offset, int size, void *userdata, VCHIQ_BULK_MODE_T mode);
+extern int            vchiq_get_client_id(VCHIQ_SERVICE_HANDLE_T service);
+extern VCHIQ_STATUS_T vchiq_get_config(VCHIQ_INSTANCE_T instance, int config_size, VCHIQ_CONFIG_T *pconfig);
+extern VCHIQ_STATUS_T vchiq_set_service_option(VCHIQ_SERVICE_HANDLE_T service, VCHIQ_SERVICE_OPTION_T option, int value);
+
+extern VCHIQ_STATUS_T vchiq_dump_phys_mem( VCHIQ_SERVICE_HANDLE_T service, void *ptr, size_t num_bytes );
+
+#endif /* VCHIQ_IF_H */
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_ioctl.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
new file mode 100644
index 0000000..83824e5
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_IOCTLS_H
+#define VCHIQ_IOCTLS_H
+
+#include <linux/ioctl.h>
+#include "vchiq_if.h"
+
+#define VCHIQ_IOC_MAGIC 0xc4
+#define VCHIQ_INVALID_HANDLE -1
+
+typedef struct {
+   VCHIQ_SERVICE_PARAMS_T params;
+   int is_open;
+   int is_vchi;
+   int handle;       /* OUT */
+} VCHIQ_CREATE_SERVICE_T;
+
+typedef struct {
+   int handle;
+   int count;
+   const VCHIQ_ELEMENT_T *elements;
+} VCHIQ_QUEUE_MESSAGE_T;
+
+typedef struct {
+   int handle;
+   void *data;
+   int size;
+   void *userdata;
+   VCHIQ_BULK_MODE_T mode;
+} VCHIQ_QUEUE_BULK_TRANSFER_T;
+
+typedef struct {
+   VCHIQ_REASON_T reason;
+   VCHIQ_HEADER_T *header;
+   void *service_userdata;
+   void *bulk_userdata;
+} VCHIQ_COMPLETION_DATA_T;
+
+typedef struct {
+   int count;
+   VCHIQ_COMPLETION_DATA_T *buf;
+   int msgbufsize;
+   int msgbufcount; /* IN/OUT */
+   void **msgbufs;
+} VCHIQ_AWAIT_COMPLETION_T;
+
+typedef struct {
+   int handle;
+   int blocking;
+   int bufsize;
+   void *buf;
+} VCHIQ_DEQUEUE_MESSAGE_T;
+
+typedef struct {
+   int config_size;
+   VCHIQ_CONFIG_T *pconfig;
+} VCHIQ_GET_CONFIG_T;
+
+typedef struct {
+   int handle;
+   VCHIQ_SERVICE_OPTION_T option;
+   int value;
+} VCHIQ_SET_SERVICE_OPTION_T;
+
+typedef struct {
+   void     *virt_addr;
+   size_t    num_bytes;
+} VCHIQ_DUMP_MEM_T;
+
+#define VCHIQ_IOC_CONNECT              _IO(VCHIQ_IOC_MAGIC,   0)
+#define VCHIQ_IOC_SHUTDOWN             _IO(VCHIQ_IOC_MAGIC,   1)
+#define VCHIQ_IOC_CREATE_SERVICE       _IOWR(VCHIQ_IOC_MAGIC, 2, VCHIQ_CREATE_SERVICE_T)
+#define VCHIQ_IOC_REMOVE_SERVICE       _IO(VCHIQ_IOC_MAGIC,   3)
+#define VCHIQ_IOC_QUEUE_MESSAGE        _IOW(VCHIQ_IOC_MAGIC,  4, VCHIQ_QUEUE_MESSAGE_T)
+#define VCHIQ_IOC_QUEUE_BULK_TRANSMIT  _IOW(VCHIQ_IOC_MAGIC,  5, VCHIQ_QUEUE_BULK_TRANSFER_T)
+#define VCHIQ_IOC_QUEUE_BULK_RECEIVE   _IOW(VCHIQ_IOC_MAGIC,  6, VCHIQ_QUEUE_BULK_TRANSFER_T)
+#define VCHIQ_IOC_AWAIT_COMPLETION     _IOW(VCHIQ_IOC_MAGIC,  7, VCHIQ_AWAIT_COMPLETION_T)
+#define VCHIQ_IOC_DEQUEUE_MESSAGE      _IOW(VCHIQ_IOC_MAGIC,  8, VCHIQ_DEQUEUE_MESSAGE_T)
+#define VCHIQ_IOC_GET_CLIENT_ID        _IO(VCHIQ_IOC_MAGIC,   9)
+#define VCHIQ_IOC_GET_CONFIG           _IOW(VCHIQ_IOC_MAGIC, 10, VCHIQ_GET_CONFIG_T)
+#define VCHIQ_IOC_CLOSE_SERVICE        _IO(VCHIQ_IOC_MAGIC,  11)
+#define VCHIQ_IOC_USE_SERVICE          _IO(VCHIQ_IOC_MAGIC,  12)
+#define VCHIQ_IOC_RELEASE_SERVICE      _IO(VCHIQ_IOC_MAGIC,  13)
+#define VCHIQ_IOC_SET_SERVICE_OPTION   _IOW(VCHIQ_IOC_MAGIC, 14, VCHIQ_SET_SERVICE_OPTION_T)
+#define VCHIQ_IOC_DUMP_PHYS_MEM        _IOW(VCHIQ_IOC_MAGIC, 15, VCHIQ_DUMP_MEM_T)
+#define VCHIQ_IOC_MAX                  15
+
+#endif
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c
new file mode 100644
index 0000000..e1768e4
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c
@@ -0,0 +1,297 @@
+/*****************************************************************************
+* Copyright 2001 - 2011 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+/* ---- Include Files ---------------------------------------------------- */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include "vchiq_core.h"
+#include "vchiq_arm.h"
+#include "interface/vcos/vcos_logging.h"
+
+/* ---- Public Variables ------------------------------------------------- */
+
+extern VCOS_LOG_CAT_T vchiq_core_log_category;
+#define  VCOS_LOG_CATEGORY (&vchiq_core_log_category)
+
+/* ---- Private Constants and Types -------------------------------------- */
+
+struct vchiq_instance_struct {
+   VCHIQ_STATE_T *state;
+
+   int connected;
+};
+
+/****************************************************************************
+*
+*   vchiq_initialise
+*
+***************************************************************************/
+
+VCHIQ_STATUS_T vchiq_initialise( VCHIQ_INSTANCE_T *instanceOut )
+{
+   VCHIQ_STATUS_T status = VCHIQ_ERROR;
+   VCHIQ_STATE_T *state;
+   VCHIQ_INSTANCE_T instance = NULL;
+
+   vcos_log_trace( "%s called", __func__ );
+
+   state = vchiq_get_state();
+   if (!state)
+   {
+      printk( KERN_ERR "%s: videocore not initialized\n", __func__ );
+      goto failed;
+   }
+
+   instance = kzalloc( sizeof(*instance), GFP_KERNEL );
+   if( !instance )
+   {
+      printk( KERN_ERR "%s: error allocating vchiq instance\n", __func__ );
+      goto failed;
+   }
+
+   instance->connected = 0;
+   instance->state = state;
+
+   *instanceOut = instance;
+   
+   status = VCHIQ_SUCCESS;
+
+failed:
+   vcos_log_trace( "%s(%p): returning %d", __func__, instance, status );
+
+   return status;
+}
+
+/****************************************************************************
+*
+*   vchiq_shutdown
+*
+***************************************************************************/
+
+VCHIQ_STATUS_T vchiq_shutdown( VCHIQ_INSTANCE_T instance )
+{
+   VCHIQ_STATUS_T status;
+   VCHIQ_STATE_T *state = instance->state;
+
+   vcos_log_trace( "%s(%p) called", __func__, instance );
+
+   vcos_mutex_lock(&state->mutex);
+
+   /* Remove all services */
+   status = vchiq_shutdown_internal(state, instance);
+
+   vcos_mutex_unlock(&state->mutex);
+
+   if (status == VCHIQ_SUCCESS)
+      kfree(instance);
+
+   vcos_log_trace( "%s(%p): returning %d", __func__, instance, status );
+
+   return status;
+}
+
+/****************************************************************************
+*
+*   vchiq_is_connected
+*
+***************************************************************************/
+
+int vchiq_is_connected(VCHIQ_INSTANCE_T instance)
+{
+   return instance->connected;
+}
+
+/****************************************************************************
+*
+*   vchiq_connect
+*
+***************************************************************************/
+
+VCHIQ_STATUS_T vchiq_connect(VCHIQ_INSTANCE_T instance)
+{
+   VCHIQ_STATUS_T status;
+   VCHIQ_STATE_T *state = instance->state;
+
+   vcos_log_trace( "%s(%p) called", __func__, instance );
+
+   if (vcos_mutex_lock(&state->mutex) != VCOS_SUCCESS) {
+      vcos_log_trace( "%s: call to vcos_mutex_lock failed", __func__ );
+      status = VCHIQ_RETRY;
+      goto failed;
+   }
+   status = vchiq_connect_internal(state, instance);
+
+   if (status == VCHIQ_SUCCESS)
+      instance->connected = 1;
+
+   vcos_mutex_unlock(&state->mutex);
+
+failed:
+   vcos_log_trace( "%s(%p): returning %d", __func__, instance, status );
+
+   return status;
+}
+
+/****************************************************************************
+*
+*   vchiq_add_service
+*
+***************************************************************************/
+
+VCHIQ_STATUS_T vchiq_add_service(
+   VCHIQ_INSTANCE_T        instance,
+   int                     fourcc,
+   VCHIQ_CALLBACK_T        callback,
+   void                   *userdata,
+   VCHIQ_SERVICE_HANDLE_T *pservice)
+{
+   VCHIQ_SERVICE_PARAMS_T params;
+
+   params.fourcc        = fourcc;
+   params.callback      = callback;
+   params.userdata      = userdata;
+   params.version       = 0;
+   params.version_min   = 0;
+
+   return vchiq_add_service_params(instance, &params, pservice);
+}
+
+/****************************************************************************
+*
+*   vchiq_open_service
+*
+***************************************************************************/
+
+VCHIQ_STATUS_T vchiq_open_service(
+   VCHIQ_INSTANCE_T        instance,
+   int                     fourcc,
+   VCHIQ_CALLBACK_T        callback,
+   void                   *userdata,
+   VCHIQ_SERVICE_HANDLE_T *pservice)
+{
+   VCHIQ_SERVICE_PARAMS_T params;
+
+   params.fourcc        = fourcc;
+   params.callback      = callback;
+   params.userdata      = userdata;
+   params.version       = 0;
+   params.version_min   = 0;
+
+   return vchiq_open_service_params(instance, &params, pservice);
+}
+
+/****************************************************************************
+*
+*   vchiq_add_service_params
+*
+***************************************************************************/
+
+VCHIQ_STATUS_T vchiq_add_service_params(
+   VCHIQ_INSTANCE_T              instance,
+   const VCHIQ_SERVICE_PARAMS_T *params,
+   VCHIQ_SERVICE_HANDLE_T       *pservice)
+{
+   VCHIQ_STATUS_T status;
+   VCHIQ_STATE_T *state = instance->state;
+   VCHIQ_SERVICE_T *service;
+   int srvstate;
+
+   vcos_log_trace( "%s(%p) called", __func__, instance );
+
+   *pservice = NULL;
+
+   srvstate = vchiq_is_connected( instance )
+      ? VCHIQ_SRVSTATE_LISTENING
+      : VCHIQ_SRVSTATE_HIDDEN;
+
+   vcos_mutex_lock(&state->mutex);
+
+   service = vchiq_add_service_internal(
+      state,
+      params,
+      srvstate,
+      instance);
+
+   vcos_mutex_unlock(&state->mutex);
+
+   if ( service  )
+   {
+      *pservice = &service->base;
+      status = VCHIQ_SUCCESS;
+   }
+   else
+   {
+      status = VCHIQ_ERROR;
+   }
+
+   vcos_log_trace( "%s(%p): returning %d", __func__, instance, status );
+
+   return status;
+}
+
+/****************************************************************************
+*
+*   vchiq_open_service_params
+*
+***************************************************************************/
+
+VCHIQ_STATUS_T vchiq_open_service_params(
+   VCHIQ_INSTANCE_T              instance,
+   const VCHIQ_SERVICE_PARAMS_T *params,
+   VCHIQ_SERVICE_HANDLE_T       *pservice)
+{
+   VCHIQ_STATUS_T   status = VCHIQ_ERROR;
+   VCHIQ_STATE_T   *state = instance->state;
+   VCHIQ_SERVICE_T *service;
+
+   vcos_log_trace( "%s(%p) called", __func__, instance );
+
+   *pservice = NULL;
+
+   if (!vchiq_is_connected(instance))
+      goto failed;
+
+   vcos_mutex_lock(&state->mutex);
+
+   service = vchiq_add_service_internal(state,
+      params,
+      VCHIQ_SRVSTATE_OPENING,
+      instance);
+
+   vcos_mutex_unlock(&state->mutex);
+
+   if ( service  )
+   {
+      status = vchiq_open_service_internal(service, current->pid);
+      if ( status == VCHIQ_SUCCESS )
+         *pservice = &service->base;
+      else
+         vchiq_remove_service(&service->base);
+   }
+
+failed:
+   vcos_log_trace( "%s(%p): returning %d", __func__, instance, status );
+
+   return status;
+}
+
+EXPORT_SYMBOL(vchiq_initialise);
+EXPORT_SYMBOL(vchiq_shutdown);
+EXPORT_SYMBOL(vchiq_connect);
+EXPORT_SYMBOL(vchiq_add_service);
+EXPORT_SYMBOL(vchiq_open_service);
+EXPORT_SYMBOL(vchiq_add_service_params);
+EXPORT_SYMBOL(vchiq_open_service_params);
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_lib.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_lib.c
new file mode 100644
index 0000000..dd1723d
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_lib.c
@@ -0,0 +1,1518 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <stdio.h>
+
+#include "vchiq.h"
+#include "vchiq_cfg.h"
+#include "vchiq_ioctl.h"
+#include "interface/vchi/vchi.h"
+#include "interface/vchi/common/endian.h"
+#include "interface/vcos/vcos.h"
+
+#define VCHIQ_MAX_INSTANCE_SERVICES 32
+#define MSGBUF_SIZE (VCHIQ_MAX_MSG_SIZE + sizeof(VCHIQ_HEADER_T))
+
+#define RETRY(r,x) do { r = x; } while ((r == -1) && (errno == EINTR))
+
+#define VCOS_LOG_CATEGORY (&vchiq_lib_log_category)
+
+typedef struct vchiq_service_struct
+{
+   VCHIQ_SERVICE_BASE_T base;
+   int handle;
+   int fd;
+   VCHI_CALLBACK_T vchi_callback;
+   void *peek_buf;
+   int peek_size;
+   int client_id;
+} VCHIQ_SERVICE_T;
+
+typedef struct vchiq_service_struct VCHI_SERVICE_T;
+
+struct vchiq_instance_struct
+{
+   int fd;
+   int initialised;
+   int connected;
+   VCOS_THREAD_T completion_thread;
+   VCOS_MUTEX_T mutex;
+   int used_services;
+   VCHIQ_SERVICE_T services[VCHIQ_MAX_INSTANCE_SERVICES];
+} vchiq_instance;
+
+typedef struct vchiq_instance_struct VCHI_STATE_T;
+
+/* Local data */
+static VCOS_LOG_LEVEL_T vchiq_default_lib_log_level = VCOS_LOG_WARN;
+static VCOS_LOG_CAT_T vchiq_lib_log_category;
+static VCOS_MUTEX_T vchiq_lib_mutex;
+static void *free_msgbufs;
+
+
+/* Local utility functions */
+static VCHIQ_INSTANCE_T
+vchiq_lib_init(void);
+
+static void *completion_thread(void *);
+
+static VCHIQ_STATUS_T
+create_service(VCHIQ_INSTANCE_T instance,
+   const VCHIQ_SERVICE_PARAMS_T *params,
+   VCHI_CALLBACK_T vchi_callback,
+   int is_open,
+   VCHIQ_SERVICE_HANDLE_T *pservice);
+
+static int
+fill_peek_buf(VCHI_SERVICE_T *service,
+   VCHI_FLAGS_T flags);
+
+static void *
+alloc_msgbuf(void);
+
+static void
+free_msgbuf(void *buf);
+
+static __inline int
+is_valid_instance(VCHIQ_INSTANCE_T instance)
+{
+   return (instance == &vchiq_instance) && (instance->initialised > 0);
+}
+
+/*
+ * VCHIQ API
+ */
+
+VCHIQ_STATUS_T
+vchiq_initialise(VCHIQ_INSTANCE_T *pinstance)
+{
+   VCHIQ_INSTANCE_T instance;
+
+   instance = vchiq_lib_init();
+
+   vcos_log_trace( "%s: returning instance handle %p", __func__, instance );
+
+   *pinstance = instance;
+
+   return (instance != NULL) ? VCHIQ_SUCCESS : VCHIQ_ERROR;
+}
+
+VCHIQ_STATUS_T
+vchiq_shutdown(VCHIQ_INSTANCE_T instance)
+{
+   vcos_log_trace( "%s called", __func__ );
+
+   if (!is_valid_instance(instance))
+      return VCHIQ_ERROR;
+
+   vcos_mutex_lock(&instance->mutex);
+
+   if (instance->initialised == 1)
+   {
+      int i;
+
+      instance->initialised = -1; /* Enter limbo */
+
+      /* Remove all services */
+
+      for (i = 0; i < instance->used_services; i++)
+      {
+         if (instance->services[i].handle != VCHIQ_INVALID_HANDLE)
+         {
+            vchiq_remove_service(&instance->services[i].base);
+            instance->services[i].handle = VCHIQ_INVALID_HANDLE;
+         }
+      }
+
+      if (instance->connected)
+      {
+         int ret;
+         RETRY(ret, ioctl(instance->fd, VCHIQ_IOC_SHUTDOWN, 0));
+         vcos_assert(ret == 0);
+         vcos_thread_join(&instance->completion_thread, NULL);
+         instance->connected = 0;
+      }
+
+      close(instance->fd);
+      instance->fd = -1;
+   }
+   else if (instance->initialised > 1)
+   {
+      instance->initialised--;
+   }
+
+   vcos_mutex_unlock(&instance->mutex);
+
+   vcos_global_lock();
+
+   if (instance->initialised == -1)
+   {
+      vcos_mutex_delete(&instance->mutex);
+      instance->initialised = 0;
+   }
+
+   vcos_global_unlock();
+
+   vcos_log_trace( "%s returning", __func__ );
+
+   return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_connect(VCHIQ_INSTANCE_T instance)
+{
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+   vcos_log_trace( "%s called", __func__ );
+
+   if (!is_valid_instance(instance))
+      return VCHIQ_ERROR;
+
+   vcos_mutex_lock(&instance->mutex);
+
+   if (!instance->connected)
+   {
+      int ret = ioctl(instance->fd, VCHIQ_IOC_CONNECT, 0);
+      if (ret == 0)
+      {
+         VCOS_THREAD_ATTR_T attrs;
+         instance->connected = 1;
+         vcos_thread_attr_init(&attrs);
+         vcos_thread_create(&instance->completion_thread, "VCHIQ completion",
+                            &attrs, completion_thread, instance);
+      }
+      else
+      {
+         status = VCHIQ_ERROR;
+      }
+   }
+
+   vcos_mutex_unlock(&instance->mutex);
+
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_add_service(VCHIQ_INSTANCE_T instance,
+   int fourcc,
+   VCHIQ_CALLBACK_T callback,
+   void *userdata,
+   VCHIQ_SERVICE_HANDLE_T *pservice)
+{
+   VCHIQ_SERVICE_PARAMS_T params;
+
+   params.fourcc        = fourcc;
+   params.callback      = callback;
+   params.userdata      = userdata;
+   params.version       = 0;
+   params.version_min   = 0;
+
+   return vchiq_add_service_params(instance, &params, pservice);
+}
+
+VCHIQ_STATUS_T
+vchiq_open_service(VCHIQ_INSTANCE_T instance,
+   int fourcc,
+   VCHIQ_CALLBACK_T callback,
+   void *userdata,
+   VCHIQ_SERVICE_HANDLE_T *pservice)
+{
+   VCHIQ_SERVICE_PARAMS_T params;
+
+   params.fourcc        = fourcc;
+   params.callback      = callback;
+   params.userdata      = userdata;
+   params.version       = 0;
+   params.version_min   = 0;
+
+   return vchiq_open_service_params(instance, &params, pservice);
+}
+
+VCHIQ_STATUS_T
+vchiq_add_service_params(VCHIQ_INSTANCE_T instance,
+   const VCHIQ_SERVICE_PARAMS_T *params,
+   VCHIQ_SERVICE_HANDLE_T *pservice)
+{
+   VCHIQ_STATUS_T status;
+
+   vcos_log_trace( "%s called fourcc = 0x%08x (%c%c%c%c)",
+                   __func__,
+                   params->fourcc,
+                   (params->fourcc >> 24) & 0xff,
+                   (params->fourcc >> 16) & 0xff,
+                   (params->fourcc >>  8) & 0xff,
+                   (params->fourcc      ) & 0xff );
+
+   if (!params->callback)
+      return VCHIQ_ERROR;
+
+   if (!is_valid_instance(instance))
+      return VCHIQ_ERROR;
+
+   status = create_service(instance,
+      params,
+      NULL/*vchi_callback*/,
+      0/*!open*/,
+      pservice);
+
+   vcos_log_trace( "%s returning service handle = 0x%08x", __func__, (uint32_t)*pservice );
+
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_open_service_params(VCHIQ_INSTANCE_T instance,
+   const VCHIQ_SERVICE_PARAMS_T *params,
+   VCHIQ_SERVICE_HANDLE_T *pservice)
+{
+   VCHIQ_STATUS_T status;
+
+   vcos_log_trace( "%s called fourcc = 0x%08x (%c%c%c%c)",
+                   __func__,
+                   params->fourcc,
+                   (params->fourcc >> 24) & 0xff,
+                   (params->fourcc >> 16) & 0xff,
+                   (params->fourcc >>  8) & 0xff,
+                   (params->fourcc      ) & 0xff );
+
+   if (!params->callback)
+      return VCHIQ_ERROR;
+
+   if (!is_valid_instance(instance))
+      return VCHIQ_ERROR;
+
+   status = create_service(instance,
+      params,
+      NULL/*vchi_callback*/,
+      1/*open*/,
+      pservice);
+
+   vcos_log_trace( "%s returning service handle = 0x%08x", __func__, (uint32_t)*pservice );
+
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_close_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   int ret;
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   RETRY(ret,ioctl(service->fd, VCHIQ_IOC_CLOSE_SERVICE, service->handle));
+
+   if (ret != 0)
+      return VCHIQ_ERROR;
+
+   service->handle = VCHIQ_INVALID_HANDLE;
+   return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_remove_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   int ret;
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   RETRY(ret,ioctl(service->fd, VCHIQ_IOC_REMOVE_SERVICE, service->handle));
+
+   if (ret != 0)
+      return VCHIQ_ERROR;
+
+   service->handle = VCHIQ_INVALID_HANDLE;
+   return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
+   const VCHIQ_ELEMENT_T *elements,
+   int count)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   VCHIQ_QUEUE_MESSAGE_T args;
+   int ret;
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   args.handle = service->handle;
+   args.elements = elements;
+   args.count = count;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_MESSAGE, &args));
+
+   return (ret >= 0) ? VCHIQ_SUCCESS : VCHIQ_ERROR;
+}
+
+void
+vchiq_release_message(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHIQ_HEADER_T *header)
+{
+   vcos_log_trace( "%s handle=%08x, header=%x", __func__, (uint32_t)handle, (uint32_t)header );
+
+   free_msgbuf(header);
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_bulk_transmit(VCHIQ_SERVICE_HANDLE_T handle,
+   const void *data,
+   int size,
+   void *userdata)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   VCHIQ_QUEUE_BULK_TRANSFER_T args;
+   int ret;
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   args.handle = service->handle;
+   args.data = (void *)data;
+   args.size = size;
+   args.userdata = userdata;
+   args.mode = VCHIQ_BULK_MODE_CALLBACK;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_BULK_TRANSMIT, &args));
+
+   return (ret >= 0) ? VCHIQ_SUCCESS : VCHIQ_ERROR;
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_bulk_receive(VCHIQ_SERVICE_HANDLE_T handle,
+   void *data,
+   int size,
+   void *userdata)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   VCHIQ_QUEUE_BULK_TRANSFER_T args;
+   int ret;
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   args.handle = service->handle;
+   args.data = data;
+   args.size = size;
+   args.userdata = userdata;
+   args.mode = VCHIQ_BULK_MODE_CALLBACK;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_BULK_RECEIVE, &args));
+
+   return (ret >= 0) ? VCHIQ_SUCCESS : VCHIQ_ERROR;
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_bulk_transmit_handle(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHI_MEM_HANDLE_T memhandle,
+   const void *offset,
+   int size,
+   void *userdata)
+{
+   vcos_assert(memhandle == VCHI_MEM_HANDLE_INVALID);
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   return vchiq_queue_bulk_transmit(handle, offset, size, userdata);
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_bulk_receive_handle(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHI_MEM_HANDLE_T memhandle,
+   void *offset,
+   int size,
+   void *userdata)
+{
+   vcos_assert(memhandle == VCHI_MEM_HANDLE_INVALID);
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   return vchiq_queue_bulk_receive(handle, offset, size, userdata);
+}
+
+VCHIQ_STATUS_T
+vchiq_bulk_transmit(VCHIQ_SERVICE_HANDLE_T handle,
+   const void *data,
+   int size,
+   void *userdata,
+   VCHIQ_BULK_MODE_T mode)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   VCHIQ_QUEUE_BULK_TRANSFER_T args;
+   int ret;
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   args.handle = service->handle;
+   args.data = (void *)data;
+   args.size = size;
+   args.userdata = userdata;
+   args.mode = mode;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_BULK_TRANSMIT, &args));
+
+   return (ret >= 0) ? VCHIQ_SUCCESS : VCHIQ_ERROR;
+}
+
+VCHIQ_STATUS_T
+vchiq_bulk_receive(VCHIQ_SERVICE_HANDLE_T handle,
+   void *data,
+   int size,
+   void *userdata,
+   VCHIQ_BULK_MODE_T mode)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   VCHIQ_QUEUE_BULK_TRANSFER_T args;
+   int ret;
+
+   vcos_log_trace( "%s called service handle = 0x%08x", __func__, (uint32_t)handle );
+
+   args.handle = service->handle;
+   args.data = data;
+   args.size = size;
+   args.userdata = userdata;
+   args.mode = mode;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_BULK_RECEIVE, &args));
+
+   return (ret >= 0) ? VCHIQ_SUCCESS : VCHIQ_ERROR;
+}
+
+VCHIQ_STATUS_T
+vchiq_bulk_transmit_handle(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHI_MEM_HANDLE_T memhandle,
+   const void *offset,
+   int size,
+   void *userdata,
+   VCHIQ_BULK_MODE_T mode)
+{
+   vcos_assert(memhandle == VCHI_MEM_HANDLE_INVALID);
+
+   return vchiq_bulk_transmit(handle, offset, size, userdata, mode);
+}
+
+VCHIQ_STATUS_T
+vchiq_bulk_receive_handle(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHI_MEM_HANDLE_T memhandle,
+   void *offset,
+   int size,
+   void *userdata,
+   VCHIQ_BULK_MODE_T mode)
+{
+   vcos_assert(memhandle == VCHI_MEM_HANDLE_INVALID);
+
+   return vchiq_bulk_receive(handle, offset, size, userdata, mode);
+}
+
+int
+vchiq_get_client_id(VCHIQ_SERVICE_HANDLE_T handle)
+{
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+
+   return ioctl(service->fd, VCHIQ_IOC_GET_CLIENT_ID, service->handle);
+}
+
+VCHIQ_STATUS_T
+vchiq_get_config(VCHIQ_INSTANCE_T instance,
+   int config_size,
+   VCHIQ_CONFIG_T *pconfig)
+{
+   VCHIQ_GET_CONFIG_T args;
+   int ret;
+
+   if (!is_valid_instance(instance))
+      return VCHIQ_ERROR;
+
+   args.config_size = config_size;
+   args.pconfig = pconfig;
+
+   RETRY(ret, ioctl(instance->fd, VCHIQ_IOC_GET_CONFIG, &args));
+
+   return (ret >= 0) ? VCHIQ_SUCCESS : VCHIQ_ERROR;
+}
+
+int32_t
+vchiq_use_service( const VCHIQ_SERVICE_HANDLE_T handle )
+{
+    VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+    int ret;
+    RETRY(ret,ioctl(service->fd, VCHIQ_IOC_USE_SERVICE, service->handle));
+    return ret;
+}
+
+int32_t
+vchiq_release_service( const VCHIQ_SERVICE_HANDLE_T handle )
+{
+    VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+    int ret;
+    RETRY(ret,ioctl(service->fd, VCHIQ_IOC_RELEASE_SERVICE, service->handle));
+    return ret;
+}
+
+VCHIQ_STATUS_T
+vchiq_set_service_option(VCHIQ_SERVICE_HANDLE_T handle,
+   VCHIQ_SERVICE_OPTION_T option, int value)
+{
+   VCHIQ_SET_SERVICE_OPTION_T args;
+   VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle;
+   int ret;
+
+   args.handle = service->handle;
+   args.option = option;
+   args.value  = value;
+
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_SET_SERVICE_OPTION, &args));
+
+   return (ret >= 0) ? VCHIQ_SUCCESS : VCHIQ_ERROR;
+}
+
+/*
+ * VCHI API
+ */
+
+/* ----------------------------------------------------------------------
+ * return pointer to the mphi message driver function table
+ * -------------------------------------------------------------------- */
+const VCHI_MESSAGE_DRIVER_T *
+vchi_mphi_message_driver_func_table( void )
+{
+   return NULL;
+}
+
+/* ----------------------------------------------------------------------
+ * return a pointer to the 'single' connection driver fops
+ * -------------------------------------------------------------------- */
+const VCHI_CONNECTION_API_T *
+single_get_func_table( void )
+{
+   return NULL;
+}
+
+VCHI_CONNECTION_T *
+vchi_create_connection( const VCHI_CONNECTION_API_T * function_table,
+   const VCHI_MESSAGE_DRIVER_T * low_level )
+{
+   vcos_unused(function_table);
+   vcos_unused(low_level);
+
+   return NULL;
+}
+
+/***********************************************************
+ * Name: vchi_msg_peek
+ *
+ * Arguments:  const VCHI_SERVICE_HANDLE_T handle,
+ *             void **data,
+ *             uint32_t *msg_size,
+ *             VCHI_FLAGS_T flags
+ *
+ * Description: Routine to return a pointer to the current message (to allow in place processing)
+ *              The message can be removed using vchi_msg_remove when you're finished
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_msg_peek( VCHI_SERVICE_HANDLE_T handle,
+   void **data,
+   uint32_t *msg_size,
+   VCHI_FLAGS_T flags )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   int ret;
+
+   ret = fill_peek_buf(service, flags);
+
+   if (ret == 0)
+   {
+      *data = service->peek_buf;
+      *msg_size = service->peek_size;
+   }
+
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_msg_remove
+ *
+ * Arguments:  const VCHI_SERVICE_HANDLE_T handle,
+ *
+ * Description: Routine to remove a message (after it has been read with vchi_msg_peek)
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_msg_remove( VCHI_SERVICE_HANDLE_T handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+
+   /* Why would you call vchi_msg_remove without calling vchi_msg_peek first? */
+   vcos_assert(service->peek_size >= 0);
+
+   /* Invalidate the content but reuse the buffer */
+   service->peek_size = -1;
+
+   return 0;
+}
+
+/***********************************************************
+ * Name: vchi_msg_queue
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             const void *data,
+ *             uint32_t data_size,
+ *             VCHI_FLAGS_T flags,
+ *             void *msg_handle,
+ *
+ * Description: Thin wrapper to queue a message onto a connection
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_msg_queue( VCHI_SERVICE_HANDLE_T handle,
+   const void * data,
+   uint32_t data_size,
+   VCHI_FLAGS_T flags,
+   void * msg_handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   VCHIQ_QUEUE_MESSAGE_T args;
+   VCHIQ_ELEMENT_T element = {data, data_size};
+   int ret;
+
+   vcos_unused(msg_handle);
+   vcos_assert(flags == VCHI_FLAGS_BLOCK_UNTIL_QUEUED);
+
+   args.handle = service->handle;
+   args.elements = &element;
+   args.count = 1;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_MESSAGE, &args));
+
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_bulk_queue_receive
+ *
+ * Arguments:  VCHI_BULK_HANDLE_T handle,
+ *             void *data_dst,
+ *             const uint32_t data_size,
+ *             VCHI_FLAGS_T flags
+ *             void *bulk_handle
+ *
+ * Description: Routine to setup a rcv buffer
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_bulk_queue_receive( VCHI_SERVICE_HANDLE_T handle,
+   void * data_dst,
+   uint32_t data_size,
+   VCHI_FLAGS_T flags,
+   void * bulk_handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   VCHIQ_QUEUE_BULK_TRANSFER_T args;
+   int ret;
+
+   switch ((int)flags) {
+   case VCHI_FLAGS_CALLBACK_WHEN_OP_COMPLETE | VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+      args.mode = VCHIQ_BULK_MODE_CALLBACK;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE:
+      args.mode = VCHIQ_BULK_MODE_BLOCKING;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+   case VCHI_FLAGS_NONE:
+      args.mode = VCHIQ_BULK_MODE_NOCALLBACK;
+      break;
+   default:
+      vcos_assert(0);
+      break;
+   }
+
+   args.handle = service->handle;
+   args.data = data_dst;
+   args.size = data_size;
+   args.userdata = bulk_handle;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_BULK_RECEIVE, &args));
+
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_bulk_queue_transmit
+ *
+ * Arguments:  VCHI_BULK_HANDLE_T handle,
+ *             const void *data_src,
+ *             uint32_t data_size,
+ *             VCHI_FLAGS_T flags,
+ *             void *bulk_handle
+ *
+ * Description: Routine to transmit some data
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_bulk_queue_transmit( VCHI_SERVICE_HANDLE_T handle,
+   const void * data_src,
+   uint32_t data_size,
+   VCHI_FLAGS_T flags,
+   void * bulk_handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   VCHIQ_QUEUE_BULK_TRANSFER_T args;
+   int ret;
+
+   switch ((int)flags) {
+   case VCHI_FLAGS_CALLBACK_WHEN_OP_COMPLETE | VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+      args.mode = VCHIQ_BULK_MODE_CALLBACK;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_DATA_READ:
+   case VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE:
+      args.mode = VCHIQ_BULK_MODE_BLOCKING;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+   case VCHI_FLAGS_NONE:
+      args.mode = VCHIQ_BULK_MODE_NOCALLBACK;
+      break;
+   default:
+      vcos_assert(0);
+      break;
+   }
+
+   args.handle = service->handle;
+   args.data = (void *)data_src;
+   args.size = data_size;
+   args.userdata = bulk_handle;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_BULK_TRANSMIT, &args));
+
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_msg_dequeue
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             void *data,
+ *             uint32_t max_data_size_to_read,
+ *             uint32_t *actual_msg_size
+ *             VCHI_FLAGS_T flags
+ *
+ * Description: Routine to dequeue a message into the supplied buffer
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_msg_dequeue( VCHI_SERVICE_HANDLE_T handle,
+   void *data,
+   uint32_t max_data_size_to_read,
+   uint32_t *actual_msg_size,
+   VCHI_FLAGS_T flags )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   VCHIQ_DEQUEUE_MESSAGE_T args;
+   int ret;
+
+   vcos_assert(flags == VCHI_FLAGS_NONE || flags == VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE);
+
+   if (service->peek_size >= 0)
+   {
+      fprintf(stderr, "vchi_msg_dequeue -> using peek buffer\n");
+      if ((uint32_t)service->peek_size <= max_data_size_to_read)
+      {
+         memcpy(data, service->peek_buf, service->peek_size);
+         *actual_msg_size = service->peek_size;
+         /* Invalidate the peek data, but retain the buffer */
+         service->peek_size = -1;
+         ret = 0;
+      }
+      else
+      {
+         ret = -1;
+      }
+   }
+   else
+   {
+      args.handle = service->handle;
+      args.blocking = (flags == VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE);
+      args.bufsize = max_data_size_to_read;
+      args.buf = data;
+      RETRY(ret, ioctl(service->fd, VCHIQ_IOC_DEQUEUE_MESSAGE, &args));
+      if (ret >= 0)
+      {
+         *actual_msg_size = ret;
+         ret = 0;
+      }
+   }
+
+   if ((ret < 0) && (errno != EWOULDBLOCK))
+      fprintf(stderr, "vchi_msg_dequeue -> %d(%d)\n", ret, errno);
+
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_msg_queuev
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             const void *data,
+ *             uint32_t data_size,
+ *             VCHI_FLAGS_T flags,
+ *             void *msg_handle
+ *
+ * Description: Thin wrapper to queue a message onto a connection
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+
+vcos_static_assert(sizeof(VCHI_MSG_VECTOR_T) == sizeof(VCHIQ_ELEMENT_T));
+vcos_static_assert(offsetof(VCHI_MSG_VECTOR_T, vec_base) == offsetof(VCHIQ_ELEMENT_T, data));
+vcos_static_assert(offsetof(VCHI_MSG_VECTOR_T, vec_len) == offsetof(VCHIQ_ELEMENT_T, size));
+
+int32_t
+vchi_msg_queuev( VCHI_SERVICE_HANDLE_T handle,
+   VCHI_MSG_VECTOR_T * vector,
+   uint32_t count,
+   VCHI_FLAGS_T flags,
+   void *msg_handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   VCHIQ_QUEUE_MESSAGE_T args;
+   int ret;
+
+   vcos_unused(msg_handle);
+
+   vcos_assert(flags == VCHI_FLAGS_BLOCK_UNTIL_QUEUED);
+
+   args.handle = service->handle;
+   args.elements = (const VCHIQ_ELEMENT_T *)vector;
+   args.count = count;
+   RETRY(ret, ioctl(service->fd, VCHIQ_IOC_QUEUE_MESSAGE, &args));
+
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_held_msg_release
+ *
+ * Arguments:  VCHI_HELD_MSG_T *message
+ *
+ * Description: Routine to release a held message (after it has been read with vchi_msg_hold)
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_held_msg_release( VCHI_HELD_MSG_T *message )
+{
+   int ret = -1;
+
+   if (message && message->message && !message->service)
+   {
+      free_msgbuf(message->message);
+      ret = 0;
+   }
+
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_msg_hold
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             void **data,
+ *             uint32_t *msg_size,
+ *             VCHI_FLAGS_T flags,
+ *             VCHI_HELD_MSG_T *message_handle
+ *
+ * Description: Routine to return a pointer to the current message (to allow in place processing)
+ *              The message is dequeued - don't forget to release the message using
+ *              vchi_held_msg_release when you're finished
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_msg_hold( VCHI_SERVICE_HANDLE_T handle,
+   void **data,
+   uint32_t *msg_size,
+   VCHI_FLAGS_T flags,
+   VCHI_HELD_MSG_T *message_handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   int ret;
+
+   ret = fill_peek_buf(service, flags);
+
+   if (ret == 0)
+   {
+      *data = service->peek_buf;
+      *msg_size = service->peek_size;
+
+      message_handle->message = service->peek_buf;
+      message_handle->service = NULL;
+
+      service->peek_size = -1;
+      service->peek_buf = NULL;
+   }
+
+   return 0;
+}
+
+/***********************************************************
+ * Name: vchi_initialise
+ *
+ * Arguments: VCHI_INSTANCE_T *instance_handle
+ *            VCHI_CONNECTION_T **connections
+ *            const uint32_t num_connections
+ *
+ * Description: Initialises the hardware but does not transmit anything
+ *              When run as a Host App this will be called twice hence the need
+ *              to malloc the state information
+ *
+ * Returns: 0 if successful, failure otherwise
+ *
+ ***********************************************************/
+int32_t
+vchi_initialise( VCHI_INSTANCE_T *instance_handle )
+{
+   VCHIQ_INSTANCE_T instance;
+
+   instance = vchiq_lib_init();
+
+   vcos_log_trace( "%s: returning instance handle %p", __func__, instance );
+
+   *instance_handle = (VCHI_INSTANCE_T)instance;
+
+   return (instance != NULL) ? 0 : -1;
+}
+
+/***********************************************************
+ * Name: vchi_connect
+ *
+ * Arguments: VCHI_CONNECTION_T **connections
+ *            const uint32_t num_connections
+ *            VCHI_INSTANCE_T instance_handle )
+ *
+ * Description: Starts the command service on each connection,
+ *              causing INIT messages to be pinged back and forth
+ *
+ * Returns: 0 if successful, failure otherwise
+ *
+ ***********************************************************/
+int32_t
+vchi_connect( VCHI_CONNECTION_T **connections,
+   const uint32_t num_connections,
+   VCHI_INSTANCE_T instance_handle )
+{
+   VCHIQ_STATUS_T status;
+
+   vcos_unused(connections);
+   vcos_unused(num_connections);
+
+   status = vchiq_connect((VCHIQ_INSTANCE_T)instance_handle);
+
+   return (status == VCHIQ_SUCCESS) ? 0 : -1;
+}
+
+
+/***********************************************************
+ * Name: vchi_disconnect
+ *
+ * Arguments: VCHI_INSTANCE_T instance_handle
+ *
+ * Description: Stops the command service on each connection,
+ *              causing DE-INIT messages to be pinged back and forth
+ *
+ * Returns: 0 if successful, failure otherwise
+ *
+ ***********************************************************/
+int32_t
+vchi_disconnect( VCHI_INSTANCE_T instance_handle )
+{
+   VCHIQ_STATUS_T status;
+   
+   status = vchiq_shutdown((VCHIQ_INSTANCE_T)instance_handle);
+
+   return (status == VCHIQ_SUCCESS) ? 0 : -1;
+}
+
+
+/***********************************************************
+ * Name: vchi_service_open
+ * Name: vchi_service_create
+ *
+ * Arguments: VCHI_INSTANCE_T *instance_handle
+ *            SERVICE_CREATION_T *setup,
+ *            VCHI_SERVICE_HANDLE_T *handle
+ *
+ * Description: Routine to open a service
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t
+vchi_service_open( VCHI_INSTANCE_T instance_handle,
+   SERVICE_CREATION_T *setup,
+   VCHI_SERVICE_HANDLE_T *handle )
+{
+   VCHIQ_SERVICE_PARAMS_T params;
+   VCHIQ_STATUS_T status;
+
+   memset(&params, 0, sizeof(params));
+   params.fourcc = setup->service_id;
+   params.userdata = setup->callback_param;
+
+   status = create_service((VCHIQ_INSTANCE_T)instance_handle,
+      &params,
+      setup->callback,
+      1/*open*/,
+      (VCHIQ_SERVICE_HANDLE_T *)handle);
+
+   return (status == VCHIQ_SUCCESS) ? 0 : -1;
+}
+
+int32_t
+vchi_service_create( VCHI_INSTANCE_T instance_handle,
+   SERVICE_CREATION_T *setup, VCHI_SERVICE_HANDLE_T *handle )
+{
+   VCHIQ_SERVICE_PARAMS_T params;
+   VCHIQ_STATUS_T status;
+
+   memset(&params, 0, sizeof(params));
+   params.fourcc = setup->service_id;
+   params.userdata = setup->callback_param;
+
+   status = create_service((VCHIQ_INSTANCE_T)instance_handle,
+      &params,
+      setup->callback,
+      0/*!open*/,
+      (VCHIQ_SERVICE_HANDLE_T *)handle);
+
+   return (status == VCHIQ_SUCCESS) ? 0 : -1;
+}
+
+int32_t
+vchi_service_close( const VCHI_SERVICE_HANDLE_T handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   int ret;
+   RETRY(ret,ioctl(service->fd, VCHIQ_IOC_REMOVE_SERVICE, service->handle));
+
+   if (ret == 0)
+      service->handle = VCHIQ_INVALID_HANDLE;
+
+   return ret;
+}
+
+int32_t
+vchi_service_destroy( const VCHI_SERVICE_HANDLE_T handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   int ret;
+   RETRY(ret,ioctl(service->fd, VCHIQ_IOC_REMOVE_SERVICE, service->handle));
+
+   if (ret == 0)
+      service->handle = VCHIQ_INVALID_HANDLE;
+
+   return ret;
+}
+
+/* ----------------------------------------------------------------------
+ * read a uint32_t from buffer.
+ * network format is defined to be little endian
+ * -------------------------------------------------------------------- */
+uint32_t
+vchi_readbuf_uint32( const void *_ptr )
+{
+   const unsigned char *ptr = _ptr;
+   return ptr[0] | (ptr[1] << 8) | (ptr[2] << 16) | (ptr[3] << 24);
+}
+
+/* ----------------------------------------------------------------------
+ * write a uint32_t to buffer.
+ * network format is defined to be little endian
+ * -------------------------------------------------------------------- */
+void
+vchi_writebuf_uint32( void *_ptr, uint32_t value )
+{
+   unsigned char *ptr = _ptr;
+   ptr[0] = (unsigned char)((value >> 0)  & 0xFF);
+   ptr[1] = (unsigned char)((value >> 8)  & 0xFF);
+   ptr[2] = (unsigned char)((value >> 16) & 0xFF);
+   ptr[3] = (unsigned char)((value >> 24) & 0xFF);
+}
+
+/* ----------------------------------------------------------------------
+ * read a uint16_t from buffer.
+ * network format is defined to be little endian
+ * -------------------------------------------------------------------- */
+uint16_t
+vchi_readbuf_uint16( const void *_ptr )
+{
+   const unsigned char *ptr = _ptr;
+   return ptr[0] | (ptr[1] << 8);
+}
+
+/* ----------------------------------------------------------------------
+ * write a uint16_t into the buffer.
+ * network format is defined to be little endian
+ * -------------------------------------------------------------------- */
+void
+vchi_writebuf_uint16( void *_ptr, uint16_t value )
+{
+   unsigned char *ptr = _ptr;
+   ptr[0] = (value >> 0)  & 0xFF;
+   ptr[1] = (value >> 8)  & 0xFF;
+}
+
+/***********************************************************
+ * Name: vchi_service_use
+ *
+ * Arguments: const VCHI_SERVICE_HANDLE_T handle
+ *
+ * Description: Routine to increment refcount on a service
+ *
+ * Returns: void
+ *
+ ***********************************************************/
+int32_t
+vchi_service_use( const VCHI_SERVICE_HANDLE_T handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   int ret;
+   RETRY(ret,ioctl(service->fd, VCHIQ_IOC_USE_SERVICE, service->handle));
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_service_release
+ *
+ * Arguments: const VCHI_SERVICE_HANDLE_T handle
+ *
+ * Description: Routine to decrement refcount on a service
+ *
+ * Returns: void
+ *
+ ***********************************************************/
+int32_t vchi_service_release( const VCHI_SERVICE_HANDLE_T handle )
+{
+   VCHI_SERVICE_T *service = (VCHI_SERVICE_T *)handle;
+   int ret;
+   RETRY(ret,ioctl(service->fd, VCHIQ_IOC_RELEASE_SERVICE, service->handle));
+   return ret;
+}
+
+/*
+ * Support functions
+ */
+
+static VCHIQ_INSTANCE_T
+vchiq_lib_init(void)
+{
+   static int mutex_initialised = 0;
+   static VCOS_MUTEX_T vchiq_lib_mutex;
+   VCHIQ_INSTANCE_T instance = &vchiq_instance;
+
+   vcos_global_lock();
+   if (!mutex_initialised)
+   {
+      vcos_mutex_create(&vchiq_lib_mutex, "vchiq-init");
+
+      vcos_log_set_level( &vchiq_lib_log_category, vchiq_default_lib_log_level );
+      vcos_log_register( "vchiq_lib", &vchiq_lib_log_category );
+
+      mutex_initialised = 1;
+   }
+   vcos_global_unlock();
+
+   vcos_mutex_lock(&vchiq_lib_mutex);
+
+   if (instance->initialised == 0)
+   {
+      instance->fd = open("/dev/vchiq", O_RDWR);
+      if (instance->fd >= 0)
+      {
+         VCHIQ_GET_CONFIG_T args;
+         VCHIQ_CONFIG_T config;
+         int ret;
+         args.config_size = sizeof(config);
+         args.pconfig = &config;
+         RETRY(ret, ioctl(instance->fd, VCHIQ_IOC_GET_CONFIG, &args));
+         if ((ret == 0) && (config.version >= VCHIQ_VERSION_MIN) && (config.version_min <= VCHIQ_VERSION))
+         {
+            instance->used_services = 0;
+            vcos_mutex_create(&instance->mutex, "VCHIQ instance");
+            instance->initialised = 1;
+         }
+         else
+         {
+            if (ret == 0)
+            {
+               vcos_log_error("Incompatible VCHIQ library - driver version %d (min %d), library version %d (min %d)",
+                  config.version, config.version_min, VCHIQ_VERSION, VCHIQ_VERSION_MIN);
+            }
+            else
+            {
+               vcos_log_error("Very incompatible VCHIQ library - cannot retrieve driver version");
+            }
+            close(instance->fd);
+            instance = NULL;
+         }
+      }
+      else
+      {
+         instance = NULL;
+      }
+   }
+   else if (instance->initialised > 0)
+   {
+      instance->initialised++;
+   }
+
+   vcos_mutex_unlock(&vchiq_lib_mutex);
+
+   return instance;
+}
+
+static void *
+completion_thread(void *arg)
+{
+   VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)arg;
+   VCHIQ_AWAIT_COMPLETION_T args;
+   VCHIQ_COMPLETION_DATA_T completions[8];
+   void *msgbufs[8];
+
+   static const VCHI_CALLBACK_REASON_T vchiq_reason_to_vchi[] =
+   {
+      VCHI_CALLBACK_SERVICE_OPENED,        // VCHIQ_SERVICE_OPENED
+      VCHI_CALLBACK_SERVICE_CLOSED,        // VCHIQ_SERVICE_CLOSED
+      VCHI_CALLBACK_MSG_AVAILABLE,         // VCHIQ_MESSAGE_AVAILABLE
+      VCHI_CALLBACK_BULK_SENT,             // VCHIQ_BULK_TRANSMIT_DONE
+      VCHI_CALLBACK_BULK_RECEIVED,         // VCHIQ_BULK_RECEIVE_DONE
+      VCHI_CALLBACK_BULK_TRANSMIT_ABORTED, // VCHIQ_BULK_TRANSMIT_ABORTED
+      VCHI_CALLBACK_BULK_RECEIVE_ABORTED,  // VCHIQ_BULK_RECEIVE_ABORTED
+   };
+
+   args.count = vcos_countof(completions);
+   args.buf = completions;
+   args.msgbufsize = MSGBUF_SIZE;
+   args.msgbufcount = 0;
+   args.msgbufs = msgbufs;
+
+   while (1)
+   {
+      int ret, i;
+
+      while ((unsigned int)args.msgbufcount < vcos_countof(msgbufs))
+      {
+         void *msgbuf = alloc_msgbuf();
+         if (msgbuf)
+         {
+            msgbufs[args.msgbufcount++] = msgbuf;
+         }
+         else
+         {
+            fprintf(stderr, "vchiq_lib: failed to allocate a message buffer\n");
+            vcos_demand(args.msgbufcount != 0);
+         }
+      }
+
+      RETRY(ret, ioctl(instance->fd, VCHIQ_IOC_AWAIT_COMPLETION, &args));
+
+      if (ret <= 0)
+         break;
+
+      for (i = 0; i < ret; i++)
+      {
+         VCHIQ_COMPLETION_DATA_T *completion = &completions[i];
+         VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)completion->service_userdata;
+         if (service->base.callback)
+         {
+            vcos_log_trace( "callback(%x, %x, %x, %x)",
+               completion->reason, (uint32_t)completion->header,
+               (uint32_t)&service->base, (uint32_t)completion->bulk_userdata );
+            service->base.callback(completion->reason, completion->header,
+               &service->base, completion->bulk_userdata);
+         }
+         else if (service->vchi_callback)
+         {
+            VCHI_CALLBACK_REASON_T vchi_reason =
+               vchiq_reason_to_vchi[completion->reason];
+            service->vchi_callback(service->base.userdata, vchi_reason, completion->bulk_userdata);
+         }
+      }
+   }
+   return NULL;
+}
+
+static VCHIQ_STATUS_T
+create_service(VCHIQ_INSTANCE_T instance,
+   const VCHIQ_SERVICE_PARAMS_T *params,
+   VCHI_CALLBACK_T vchi_callback,
+   int is_open,
+   VCHIQ_SERVICE_HANDLE_T *pservice)
+{
+   VCHIQ_SERVICE_T *service = NULL;
+   VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+   int i;
+
+   if (!is_valid_instance(instance))
+      return VCHIQ_ERROR;
+
+   vcos_mutex_lock(&instance->mutex);
+
+   /* Find a free service */
+   if (is_open)
+   {
+      /* Find a free service */
+      for (i = 0; i < instance->used_services; i++)
+      {
+         if (instance->services[i].handle == VCHIQ_INVALID_HANDLE)
+         {
+            service = &instance->services[i];
+            break;
+         }
+      }
+   }
+   else
+   {
+      for (i = (instance->used_services - 1); i >= 0; i--)
+      {
+         VCHIQ_SERVICE_T *srv = &instance->services[i];
+         if (srv->handle == VCHIQ_INVALID_HANDLE)
+         {
+            service = srv;
+         }
+         else if (
+            (srv->base.fourcc == params->fourcc) &&
+            ((srv->base.callback != params->callback) ||
+            (srv->vchi_callback != vchi_callback)))
+         {
+            /* There is another server using this fourcc which doesn't match */
+            service = NULL;
+            status = VCHIQ_ERROR;
+            break;
+         }
+      }
+   }
+
+   if (!service && (status == VCHIQ_SUCCESS) &&
+      (instance->used_services < VCHIQ_MAX_INSTANCE_SERVICES))
+      service = &instance->services[instance->used_services++];
+
+   if (service)
+   {
+      VCHIQ_CREATE_SERVICE_T args;
+      int ret;
+      service->base.fourcc = params->fourcc;
+      service->base.callback = params->callback;
+      service->vchi_callback = vchi_callback;
+      service->base.userdata = params->userdata;
+      service->fd = instance->fd;
+      service->peek_size = -1;
+      service->peek_buf = NULL;
+
+      args.params = *params;
+      args.params.userdata = service;
+      args.is_open = is_open;
+      args.is_vchi = (params->callback == NULL);
+      args.handle = -1; /* OUT parameter */
+      RETRY(ret, ioctl(instance->fd, VCHIQ_IOC_CREATE_SERVICE, &args));
+      if (ret == 0)
+         service->handle = args.handle;
+      else
+         status = VCHIQ_ERROR;
+   }
+
+   *pservice = (status == VCHIQ_SUCCESS) ? &service->base : NULL;
+
+   vcos_mutex_unlock(&instance->mutex);
+
+   return status;
+}
+
+static int
+fill_peek_buf(VCHI_SERVICE_T *service,
+   VCHI_FLAGS_T flags)
+{
+   VCHIQ_DEQUEUE_MESSAGE_T args;
+   int ret = 0;
+
+   vcos_assert(flags == VCHI_FLAGS_NONE || flags == VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE);
+
+   if (service->peek_size < 0)
+   {
+      if (!service->peek_buf)
+         service->peek_buf = alloc_msgbuf();
+
+      if (service->peek_buf)
+      {
+         args.handle = service->handle;
+         args.blocking = (flags == VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE);
+         args.bufsize = MSGBUF_SIZE;
+         args.buf = service->peek_buf;
+
+         RETRY(ret, ioctl(service->fd, VCHIQ_IOC_DEQUEUE_MESSAGE, &args));
+
+         if (ret >= 0)
+         {
+            service->peek_size = ret;
+            ret = 0;
+         }
+         else
+         {
+            ret = -1;
+         }
+      }
+      else
+      {
+         ret = -1;
+      }
+   }
+
+   return ret;
+}
+
+
+static void *
+alloc_msgbuf(void)
+{
+   void *msgbuf;
+   vcos_mutex_lock(&vchiq_lib_mutex);
+   msgbuf = free_msgbufs;
+   if (msgbuf)
+      free_msgbufs = *(void **)msgbuf;
+   vcos_mutex_unlock(&vchiq_lib_mutex);
+   if (!msgbuf)
+      msgbuf = malloc(MSGBUF_SIZE);
+   return msgbuf;
+}
+
+static void
+free_msgbuf(void *buf)
+{
+   vcos_mutex_lock(&vchiq_lib_mutex);
+   *(void **)buf = free_msgbufs;
+   free_msgbufs = buf;
+   vcos_mutex_unlock(&vchiq_lib_mutex);
+}
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_memdrv.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_memdrv.h
new file mode 100644
index 0000000..a465b91
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_memdrv.h
@@ -0,0 +1,45 @@
+/*****************************************************************************
+* Copyright 2001 - 2010 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+#ifndef VCHIQ_MEMDRV_H
+#define VCHIQ_MEMDRV_H
+
+/* ---- Include Files ----------------------------------------------------- */
+
+#include <linux/kernel.h>
+#include "vchiq_if.h"
+
+/* ---- Constants and Types ---------------------------------------------- */
+
+typedef struct
+{
+    void                   *armSharedMemVirt;
+    dma_addr_t              armSharedMemPhys;
+    size_t                  armSharedMemSize;
+
+    void                   *vcSharedMemVirt;
+    dma_addr_t              vcSharedMemPhys;
+    size_t                  vcSharedMemSize;
+
+} VCHIQ_SHARED_MEM_INFO_T;
+
+/* ---- Variable Externs ------------------------------------------------- */
+
+/* ---- Function Prototypes ---------------------------------------------- */
+
+void vchiq_get_shared_mem_info( VCHIQ_SHARED_MEM_INFO_T *info );
+
+VCHIQ_STATUS_T vchiq_memdrv_initialise(void);
+
+#endif
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_pagelist.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_pagelist.h
new file mode 100644
index 0000000..e8d4e30
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_pagelist.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_PAGELIST_H
+#define VCHIQ_PAGELIST_H
+
+#ifndef PAGE_SIZE
+#define PAGE_SIZE 4096
+#endif
+#define CACHE_LINE_SIZE 32
+#define PAGELIST_WRITE 0
+#define PAGELIST_READ 1
+#define PAGELIST_READ_WITH_FRAGMENTS 2
+
+typedef struct pagelist_struct {
+	unsigned long length;
+	unsigned short type;
+	unsigned short offset;
+	unsigned long addrs[1];	/* N.B. 12 LSBs hold the number of following
+				   pages at consecutive addresses. */
+} PAGELIST_T;
+
+typedef struct fragments_struct {
+	char headbuf[CACHE_LINE_SIZE];
+	char tailbuf[CACHE_LINE_SIZE];
+} FRAGMENTS_T;
+
+#endif /* VCHIQ_PAGELIST_H */
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_shim.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_shim.c
new file mode 100644
index 0000000..29aa23f
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_shim.c
@@ -0,0 +1,970 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include "interface/vchi/vchi.h"
+#include "vchiq.h"
+#include "vchiq_core.h"
+
+#include "vchiq_util.h"
+
+#include <stddef.h>
+
+#if defined(__KERNEL__)
+#include <linux/module.h>
+#endif
+
+#define vchiq_status_to_vchi(status) ((int32_t)status)
+
+typedef struct {
+   VCHIQ_SERVICE_HANDLE_T handle;
+
+   VCHIU_QUEUE_T queue;
+
+   VCHI_CALLBACK_T callback;
+   void *callback_param;
+} SHIM_SERVICE_T;
+
+/* ----------------------------------------------------------------------
+ * return pointer to the mphi message driver function table
+ * -------------------------------------------------------------------- */
+#ifdef WIN32
+const VCHI_MESSAGE_DRIVER_T *
+mphi_get_func_table( void )
+{
+   return NULL;
+}
+#endif
+
+/* ----------------------------------------------------------------------
+ * return pointer to the mphi message driver function table
+ * -------------------------------------------------------------------- */
+const VCHI_MESSAGE_DRIVER_T *
+vchi_mphi_message_driver_func_table( void )
+{
+   return NULL;
+}
+
+/* ----------------------------------------------------------------------
+ * return a pointer to the 'single' connection driver fops
+ * -------------------------------------------------------------------- */
+const VCHI_CONNECTION_API_T *
+single_get_func_table( void )
+{
+   return NULL;
+}
+
+VCHI_CONNECTION_T * vchi_create_connection( const VCHI_CONNECTION_API_T * function_table,
+                                            const VCHI_MESSAGE_DRIVER_T * low_level)
+{
+   vcos_unused(function_table);
+   vcos_unused(low_level);
+   return NULL;
+}
+
+/***********************************************************
+ * Name: vchi_msg_peek
+ *
+ * Arguments:  const VCHI_SERVICE_HANDLE_T handle,
+ *             void **data,
+ *             uint32_t *msg_size,
+ *             VCHI_FLAGS_T flags
+ *
+ * Description: Routine to return a pointer to the current message (to allow in place processing)
+ *              The message can be removed using vchi_msg_remove when you're finished
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_msg_peek( VCHI_SERVICE_HANDLE_T handle,
+                       void **data,
+                       uint32_t *msg_size,
+                       VCHI_FLAGS_T flags )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_HEADER_T *header;
+
+   vcos_assert(flags == VCHI_FLAGS_NONE || flags == VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE);
+
+   if (flags == VCHI_FLAGS_NONE)
+      if (vchiu_queue_is_empty(&service->queue))
+         return -1;
+
+   header = vchiu_queue_peek(&service->queue);
+
+   *data = header->data;
+   *msg_size = header->size;
+
+   return 0;
+}
+
+/***********************************************************
+ * Name: vchi_msg_remove
+ *
+ * Arguments:  const VCHI_SERVICE_HANDLE_T handle,
+ *
+ * Description: Routine to remove a message (after it has been read with vchi_msg_peek)
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_msg_remove( VCHI_SERVICE_HANDLE_T handle )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_HEADER_T *header;
+
+   header = vchiu_queue_pop(&service->queue);
+
+   vchiq_release_message(service->handle, header);
+
+   return 0;
+}
+
+/***********************************************************
+ * Name: vchi_msg_queue
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             const void *data,
+ *             uint32_t data_size,
+ *             VCHI_FLAGS_T flags,
+ *             void *msg_handle,
+ *
+ * Description: Thin wrapper to queue a message onto a connection
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_msg_queue( VCHI_SERVICE_HANDLE_T handle,
+                        const void * data,
+                        uint32_t data_size,
+                        VCHI_FLAGS_T flags,
+                        void * msg_handle )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_ELEMENT_T element = {data, data_size};
+   VCHIQ_STATUS_T status;
+
+   vcos_unused(msg_handle);
+
+   vcos_assert(flags == VCHI_FLAGS_BLOCK_UNTIL_QUEUED);
+
+   status = vchiq_queue_message(service->handle, &element, 1);
+
+   // On some platforms, like linux kernel, vchiq_queue_message() may return
+   // VCHIQ_RETRY, so we need to implment a retry mechanism since this
+   // function is supposed to block until queued
+   while ( status == VCHIQ_RETRY )
+   {
+      vcos_sleep( 1 );
+      status = vchiq_queue_message(service->handle, &element, 1);
+   }
+
+   return vchiq_status_to_vchi(status);
+}
+
+/***********************************************************
+ * Name: vchi_bulk_queue_receive
+ *
+ * Arguments:  VCHI_BULK_HANDLE_T handle,
+ *             void *data_dst,
+ *             const uint32_t data_size,
+ *             VCHI_FLAGS_T flags
+ *             void *bulk_handle
+ *
+ * Description: Routine to setup a rcv buffer
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_bulk_queue_receive( VCHI_SERVICE_HANDLE_T handle,
+                                 void * data_dst,
+                                 uint32_t data_size,
+                                 VCHI_FLAGS_T flags,
+                                 void * bulk_handle )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_BULK_MODE_T mode;
+   VCHIQ_STATUS_T status;
+
+   switch ((int)flags) {
+   case VCHI_FLAGS_CALLBACK_WHEN_OP_COMPLETE | VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+      vcos_assert(service->callback);
+      mode = VCHIQ_BULK_MODE_CALLBACK;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE:
+      mode = VCHIQ_BULK_MODE_BLOCKING;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+   case VCHI_FLAGS_NONE:
+      mode = VCHIQ_BULK_MODE_NOCALLBACK;
+      break;
+   default:
+      vcos_assert(0);
+      return vchiq_status_to_vchi(VCHIQ_ERROR);
+   }
+
+   status = vchiq_bulk_receive(service->handle, data_dst, data_size,
+      bulk_handle, mode);
+
+   // On some platforms, like linux kernel, vchiq_bulk_receive() may return
+   // VCHIQ_RETRY, so we need to implment a retry mechanism since this
+   // function is supposed to block until queued
+   while ( status == VCHIQ_RETRY )
+   {
+      vcos_sleep( 1 );
+      status = vchiq_bulk_receive(service->handle, data_dst, data_size,
+         bulk_handle, mode);
+   }
+
+   return vchiq_status_to_vchi(status);
+}
+
+/***********************************************************
+ * Name: vchi_bulk_queue_receive_reloc
+ *
+ * Arguments:  VCHI_BULK_HANDLE_T handle,
+ *             VCHI_MEM_HANDLE_T h
+ *             uint32_t offset
+ *             const uint32_t data_size,
+ *             VCHI_FLAGS_T flags
+ *             void *bulk_handle
+ *
+ * Description: Routine to setup a relocatable rcv buffer
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_bulk_queue_receive_reloc( const VCHI_SERVICE_HANDLE_T handle,
+                                       VCHI_MEM_HANDLE_T h,
+                                       uint32_t offset,
+                                       uint32_t data_size,
+                                       const VCHI_FLAGS_T flags,
+                                       void * const bulk_handle )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_BULK_MODE_T mode;
+   VCHIQ_STATUS_T status;
+
+   switch ((int)flags) {
+   case VCHI_FLAGS_CALLBACK_WHEN_OP_COMPLETE | VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+      vcos_assert(service->callback);
+      mode = VCHIQ_BULK_MODE_CALLBACK;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE:
+      mode = VCHIQ_BULK_MODE_BLOCKING;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+   case VCHI_FLAGS_NONE:
+      mode = VCHIQ_BULK_MODE_NOCALLBACK;
+      break;
+   default:
+      vcos_assert(0);
+      return vchiq_status_to_vchi(VCHIQ_ERROR);
+   }
+
+   status = vchiq_bulk_receive_handle(service->handle, h, (void*)offset,
+      data_size, bulk_handle, mode);
+
+   // On some platforms, like linux kernel, vchiq_bulk_receive_handle() may
+   // return VCHIQ_RETRY, so we need to implment a retry mechanism since
+   // this function is supposed to block until queued
+   while ( status == VCHIQ_RETRY )
+   {
+      vcos_sleep( 1 );
+      status = vchiq_bulk_receive_handle(service->handle, h, (void*)offset,
+         data_size, bulk_handle, mode);
+   }
+
+   return vchiq_status_to_vchi(status);
+}
+
+/***********************************************************
+ * Name: vchi_bulk_queue_transmit
+ *
+ * Arguments:  VCHI_BULK_HANDLE_T handle,
+ *             const void *data_src,
+ *             uint32_t data_size,
+ *             VCHI_FLAGS_T flags,
+ *             void *bulk_handle
+ *
+ * Description: Routine to transmit some data
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_bulk_queue_transmit( VCHI_SERVICE_HANDLE_T handle,
+                                  const void * data_src,
+                                  uint32_t data_size,
+                                  VCHI_FLAGS_T flags,
+                                  void * bulk_handle )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_BULK_MODE_T mode;
+   VCHIQ_STATUS_T status;
+
+   switch ((int)flags) {
+   case VCHI_FLAGS_CALLBACK_WHEN_OP_COMPLETE | VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+      vcos_assert(service->callback);
+      mode = VCHIQ_BULK_MODE_CALLBACK;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_DATA_READ:
+   case VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE:
+      mode = VCHIQ_BULK_MODE_BLOCKING;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+   case VCHI_FLAGS_NONE:
+      mode = VCHIQ_BULK_MODE_NOCALLBACK;
+      break;
+   default:
+      vcos_assert(0);
+      return vchiq_status_to_vchi(VCHIQ_ERROR);
+   }
+
+   status = vchiq_bulk_transmit(service->handle, data_src, data_size,
+      bulk_handle, mode);
+
+   // On some platforms, like linux kernel, vchiq_bulk_transmit() may return
+   // VCHIQ_RETRY, so we need to implment a retry mechanism since this
+   // function is supposed to block until queued
+   while ( status == VCHIQ_RETRY )
+   {
+      vcos_sleep( 1 );
+      status = vchiq_bulk_transmit(service->handle, data_src, data_size,
+         bulk_handle, mode);
+   }
+
+   return vchiq_status_to_vchi(status);
+}
+
+/***********************************************************
+ * Name: vchi_bulk_queue_transmit_reloc
+ *
+ * Arguments:  VCHI_BULK_HANDLE_T handle,
+ *             VCHI_MEM_HANDLE_T h_src,
+ *             uint32_t offset,
+ *             uint32_t data_size,
+ *             VCHI_FLAGS_T flags,
+ *             void *bulk_handle
+ *
+ * Description: Routine to transmit some data from a relocatable buffer
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+
+int32_t vchi_bulk_queue_transmit_reloc( VCHI_SERVICE_HANDLE_T handle,
+                                        VCHI_MEM_HANDLE_T h_src,
+                                        uint32_t offset,
+                                        uint32_t data_size,
+                                        VCHI_FLAGS_T flags,
+                                        void * const bulk_handle )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_BULK_MODE_T mode;
+   VCHIQ_STATUS_T status;
+
+   switch ((int)flags) {
+   case VCHI_FLAGS_CALLBACK_WHEN_OP_COMPLETE | VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+      vcos_assert(service->callback);
+      mode = VCHIQ_BULK_MODE_CALLBACK;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_DATA_READ:
+   case VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE:
+      mode = VCHIQ_BULK_MODE_BLOCKING;
+      break;
+   case VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
+   case VCHI_FLAGS_NONE:
+      mode = VCHIQ_BULK_MODE_NOCALLBACK;
+      break;
+   default:
+      vcos_assert(0);
+      return vchiq_status_to_vchi(VCHIQ_ERROR);
+   }
+
+   status = vchiq_bulk_transmit_handle(service->handle, h_src, (void*)offset,
+      data_size, bulk_handle, mode);
+
+   // On some platforms, like linux kernel, vchiq_bulk_transmit_handle() may
+   // return VCHIQ_RETRY, so we need to implment a retry mechanism since this
+   // function is supposed to block until queued
+   while ( status == VCHIQ_RETRY )
+   {
+      vcos_sleep( 1 );
+      status = vchiq_bulk_transmit_handle(service->handle, h_src, (void*)offset,
+         data_size, bulk_handle, mode);
+   }
+
+   return vchiq_status_to_vchi(status);
+}
+
+/***********************************************************
+ * Name: vchi_msg_dequeue
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             void *data,
+ *             uint32_t max_data_size_to_read,
+ *             uint32_t *actual_msg_size
+ *             VCHI_FLAGS_T flags
+ *
+ * Description: Routine to dequeue a message into the supplied buffer
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_msg_dequeue( VCHI_SERVICE_HANDLE_T handle,
+                          void *data,
+                          uint32_t max_data_size_to_read,
+                          uint32_t *actual_msg_size,
+                          VCHI_FLAGS_T flags )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_HEADER_T *header;
+
+   vcos_assert(flags == VCHI_FLAGS_NONE || flags == VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE);
+
+   if (flags == VCHI_FLAGS_NONE)
+      if (vchiu_queue_is_empty(&service->queue))
+         return -1;
+
+   header = vchiu_queue_pop(&service->queue);
+
+   memcpy(data, header->data, header->size < max_data_size_to_read ? header->size : max_data_size_to_read);
+
+   *actual_msg_size = header->size;
+
+   vchiq_release_message(service->handle, header);
+
+   return 0;
+}
+
+/***********************************************************
+ * Name: vchi_msg_queuev
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             const void *data,
+ *             uint32_t data_size,
+ *             VCHI_FLAGS_T flags,
+ *             void *msg_handle
+ *
+ * Description: Thin wrapper to queue a message onto a connection
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+
+vcos_static_assert(sizeof(VCHI_MSG_VECTOR_T) == sizeof(VCHIQ_ELEMENT_T));
+vcos_static_assert(offsetof(VCHI_MSG_VECTOR_T, vec_base) == offsetof(VCHIQ_ELEMENT_T, data));
+vcos_static_assert(offsetof(VCHI_MSG_VECTOR_T, vec_len) == offsetof(VCHIQ_ELEMENT_T, size));
+
+int32_t vchi_msg_queuev( VCHI_SERVICE_HANDLE_T handle,
+                         VCHI_MSG_VECTOR_T * vector,
+                         uint32_t count,
+                         VCHI_FLAGS_T flags,
+                         void *msg_handle )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+
+   vcos_unused(msg_handle);
+
+   vcos_assert(flags == VCHI_FLAGS_BLOCK_UNTIL_QUEUED);
+
+   return vchiq_status_to_vchi(vchiq_queue_message(service->handle, (const VCHIQ_ELEMENT_T *)vector, count));
+}
+
+#ifdef USE_MEMMGR
+
+/***********************************************************
+ * Name: vchi_msg_queuev_ex
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             VCHI_MSG_VECTOR_EX_T *vector
+ *             uint32_t count
+ *             VCHI_FLAGS_T flags,
+ *             void *msg_handle
+ *
+ * Description: Thin wrapper to queue an array of messages onto a connection
+ * Supports resolving MEM_HANDLE's at last possible moment to avoid deadlocks.
+ *
+ * Currently just a shim, so deadlocks are still possible!
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_msg_queuev_ex( const VCHI_SERVICE_HANDLE_T handle,
+                            VCHI_MSG_VECTOR_EX_T * const vector,
+                            const uint32_t count,
+                            const VCHI_FLAGS_T flags,
+                            void * const msg_handle )
+{
+   int32_t success = -1;
+   // For now, we don't actually support sending anything other than
+   // a pointer, so handles have to be patched up; this is likely
+   // to cause deadlocks. This code is not designed to be either
+   // pretty, efficient, or deadlock-free.
+
+   #define max_vecs 16
+   VCHI_MSG_VECTOR_T copy[max_vecs];
+   const uint8_t *orig[max_vecs];
+
+   int i;
+   vcos_unused(msg_handle);
+
+   if (count > sizeof(copy)/sizeof(copy[0]))
+   {
+      vcos_assert(0);
+      return -1;
+   }
+
+   for (i=0; i<count; i++)
+   {
+      VCHI_MSG_VECTOR_EX_T *v = vector+i;
+
+      switch (vector[i].type)
+      {
+      case VCHI_VEC_POINTER:
+         copy[i].vec_base = v->u.ptr.vec_base;
+         copy[i].vec_len =  v->u.ptr.vec_len;
+         break;
+      case VCHI_VEC_HANDLE:
+         vcos_assert(v->u.handle.offset+v->u.handle.vec_len <= mem_get_size(v->u.handle.handle));
+         copy[i].vec_base = (uint8_t*)mem_lock(v->u.handle.handle) + v->u.handle.offset;
+         orig[i] = copy[i].vec_base;
+         copy[i].vec_len = v->u.handle.vec_len;
+         break;
+      case VCHI_VEC_LIST:
+         vcos_assert(0); // FIXME: implement this
+         break;
+      default:
+         vcos_assert(0);
+      }
+   }
+   success = vchi_msg_queuev( handle,
+                              copy,
+                              count,
+                              flags &~ VCHI_FLAGS_INTERNAL,
+                              msg_handle );
+   if (vcos_verify(success == 0))
+   {
+      // now we need to patch up the vectors if any have been only partially consumed, and
+      // unlock memory handles.
+   
+      for (i=0; i<count; i++)
+      {
+         VCHI_MSG_VECTOR_EX_T *v = vector+i;
+
+         switch (vector[i].type)
+         {
+         case VCHI_VEC_POINTER:
+            if (flags & VCHI_FLAGS_ALLOW_PARTIAL)
+            {
+               v->u.ptr.vec_base = copy[i].vec_base;
+               v->u.ptr.vec_len  = copy[i].vec_len;
+            }
+            break;
+         case VCHI_VEC_HANDLE:
+            mem_unlock(v->u.handle.handle);
+            if (flags & VCHI_FLAGS_ALLOW_PARTIAL)
+            {
+               const uint8_t *old = orig[i];
+               uint32_t change = (const uint8_t*)copy[i].vec_base-old;
+               v->u.handle.offset += change;
+               v->u.handle.vec_len -= change;
+            }
+            break;
+         default:
+            vcos_assert(0);
+         }
+      }
+   }
+
+   return vchiq_status_to_vchi(success);
+}
+
+#endif
+
+/***********************************************************
+ * Name: vchi_held_msg_release
+ *
+ * Arguments:  VCHI_HELD_MSG_T *message
+ *
+ * Description: Routine to release a held message (after it has been read with vchi_msg_hold)
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_held_msg_release( VCHI_HELD_MSG_T *message )
+{
+   vchiq_release_message((VCHIQ_SERVICE_HANDLE_T)message->service, (VCHIQ_HEADER_T *)message->message);
+
+   return 0;
+}
+
+/***********************************************************
+ * Name: vchi_msg_hold
+ *
+ * Arguments:  VCHI_SERVICE_HANDLE_T handle,
+ *             void **data,
+ *             uint32_t *msg_size,
+ *             VCHI_FLAGS_T flags,
+ *             VCHI_HELD_MSG_T *message_handle
+ *
+ * Description: Routine to return a pointer to the current message (to allow in place processing)
+ *              The message is dequeued - don't forget to release the message using
+ *              vchi_held_msg_release when you're finished
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+int32_t vchi_msg_hold( VCHI_SERVICE_HANDLE_T handle,
+                       void **data,
+                       uint32_t *msg_size,
+                       VCHI_FLAGS_T flags,
+                       VCHI_HELD_MSG_T *message_handle )
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   VCHIQ_HEADER_T *header;
+
+   vcos_assert(flags == VCHI_FLAGS_NONE || flags == VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE);
+
+   if (flags == VCHI_FLAGS_NONE)
+      if (vchiu_queue_is_empty(&service->queue))
+         return -1;
+
+   header = vchiu_queue_pop(&service->queue);
+
+   *data = header->data;
+   *msg_size = header->size;
+
+   message_handle->service = (struct opaque_vchi_service_t *)service->handle;
+   message_handle->message = header;
+
+   return 0;
+}
+
+/***********************************************************
+ * Name: vchi_initialise
+ *
+ * Arguments: VCHI_INSTANCE_T *instance_handle
+ *            VCHI_CONNECTION_T **connections
+ *            const uint32_t num_connections
+ *
+ * Description: Initialises the hardware but does not transmit anything
+ *              When run as a Host App this will be called twice hence the need
+ *              to malloc the state information
+ *
+ * Returns: 0 if successful, failure otherwise
+ *
+ ***********************************************************/
+
+int32_t vchi_initialise( VCHI_INSTANCE_T *instance_handle )
+{
+   VCHIQ_INSTANCE_T instance;
+   VCHIQ_STATUS_T status;
+
+   status = vchiq_initialise(&instance);
+
+   *instance_handle = (VCHI_INSTANCE_T)instance;
+
+   return vchiq_status_to_vchi(status);
+}
+
+/***********************************************************
+ * Name: vchi_connect
+ *
+ * Arguments: VCHI_CONNECTION_T **connections
+ *            const uint32_t num_connections
+ *            VCHI_INSTANCE_T instance_handle )
+ *
+ * Description: Starts the command service on each connection,
+ *              causing INIT messages to be pinged back and forth
+ *
+ * Returns: 0 if successful, failure otherwise
+ *
+ ***********************************************************/
+int32_t vchi_connect( VCHI_CONNECTION_T **connections,
+                      const uint32_t num_connections,
+                      VCHI_INSTANCE_T instance_handle )
+{
+   VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
+
+   vcos_unused(connections);
+   vcos_unused(num_connections);
+
+   return vchiq_connect(instance);
+}
+
+
+/***********************************************************
+ * Name: vchi_disconnect
+ *
+ * Arguments: VCHI_INSTANCE_T instance_handle
+ *
+ * Description: Stops the command service on each connection,
+ *              causing DE-INIT messages to be pinged back and forth
+ *
+ * Returns: 0 if successful, failure otherwise
+ *
+ ***********************************************************/
+int32_t vchi_disconnect( VCHI_INSTANCE_T instance_handle )
+{
+   VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
+   return vchiq_status_to_vchi(vchiq_shutdown(instance));
+}
+
+
+/***********************************************************
+ * Name: vchi_service_open
+ * Name: vchi_service_create
+ *
+ * Arguments: VCHI_INSTANCE_T *instance_handle
+ *            SERVICE_CREATION_T *setup,
+ *            VCHI_SERVICE_HANDLE_T *handle
+ *
+ * Description: Routine to open a service
+ *
+ * Returns: int32_t - success == 0
+ *
+ ***********************************************************/
+
+static VCHIQ_STATUS_T shim_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header, VCHIQ_SERVICE_HANDLE_T handle, void *bulk_user)
+{
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)VCHIQ_GET_SERVICE_USERDATA(handle);
+
+   switch (reason) {
+   case VCHIQ_MESSAGE_AVAILABLE:
+      vchiu_queue_push(&service->queue, header);
+
+      if (service->callback)
+         service->callback(service->callback_param, VCHI_CALLBACK_MSG_AVAILABLE, NULL);
+      break;
+   case VCHIQ_BULK_TRANSMIT_DONE:
+      if (service->callback)
+         service->callback(service->callback_param, VCHI_CALLBACK_BULK_SENT, bulk_user);
+      break;
+   case VCHIQ_BULK_RECEIVE_DONE:
+      if (service->callback)
+         service->callback(service->callback_param, VCHI_CALLBACK_BULK_RECEIVED, bulk_user);
+      break;
+   case VCHIQ_SERVICE_CLOSED:
+      if (service->callback)
+         service->callback(service->callback_param, VCHI_CALLBACK_SERVICE_CLOSED, NULL);
+      break;
+   case VCHIQ_SERVICE_OPENED:
+      /* No equivalent VCHI reason */
+      break;
+   case VCHIQ_BULK_TRANSMIT_ABORTED:
+      if (service->callback)
+         service->callback(service->callback_param, VCHI_CALLBACK_BULK_TRANSMIT_ABORTED, bulk_user);
+      break;
+   case VCHIQ_BULK_RECEIVE_ABORTED:
+      if (service->callback)
+         service->callback(service->callback_param, VCHI_CALLBACK_BULK_RECEIVE_ABORTED, bulk_user);
+      break;
+   default:
+      vcos_assert(0);
+      break;
+   }
+
+   return VCHIQ_SUCCESS;
+}
+
+static SHIM_SERVICE_T *service_alloc(VCHIQ_INSTANCE_T instance,
+                                     SERVICE_CREATION_T *setup)
+{
+   SHIM_SERVICE_T *service = vcos_calloc(1, sizeof(SHIM_SERVICE_T), "vchiq_shim");
+
+   vcos_unused(instance);
+
+   if (service)
+   {
+      if (vchiu_queue_init(&service->queue, 64))
+      {
+         service->callback = setup->callback;
+         service->callback_param = setup->callback_param;
+      }
+      else
+      {
+         vcos_free(service);
+         service = NULL;
+      }
+   }
+
+   return service;
+}
+
+static void service_free(SHIM_SERVICE_T *service)
+{
+   if (service)
+   {
+      vchiu_queue_delete(&service->queue);
+      vcos_free((void*)service);
+   }
+}
+
+int32_t vchi_service_open( VCHI_INSTANCE_T instance_handle,
+                           SERVICE_CREATION_T *setup,
+                           VCHI_SERVICE_HANDLE_T *handle)
+{
+   VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
+   SHIM_SERVICE_T *service = service_alloc(instance, setup);
+   if (service)
+   {
+      VCHIQ_STATUS_T status = vchiq_open_service(instance, setup->service_id, shim_callback, service, &service->handle);
+      if (status != VCHIQ_SUCCESS)
+      {
+         service_free(service);
+         service = NULL;
+      }
+   }
+
+   *handle = (VCHI_SERVICE_HANDLE_T)service;
+
+   return (service != NULL) ? 0 : -1;
+}
+
+int32_t vchi_service_create( VCHI_INSTANCE_T instance_handle,
+                             SERVICE_CREATION_T *setup,
+                             VCHI_SERVICE_HANDLE_T *handle )
+{
+   VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
+   SHIM_SERVICE_T *service = service_alloc(instance, setup);
+   if (service)
+   {
+      VCHIQ_STATUS_T status = vchiq_add_service(instance, setup->service_id, shim_callback, service, &service->handle);
+      if (status != VCHIQ_SUCCESS)
+      {
+         service_free(service);
+         service = NULL;
+      }
+   }
+
+   *handle = (VCHI_SERVICE_HANDLE_T)service;
+
+   return (service != NULL) ? 0 : -1;
+}
+
+int32_t vchi_service_close( const VCHI_SERVICE_HANDLE_T handle )
+{
+   vcos_unused(handle);
+
+   // YTI??
+   return 0;
+}
+
+/* ----------------------------------------------------------------------
+ * read a uint32_t from buffer.
+ * network format is defined to be little endian
+ * -------------------------------------------------------------------- */
+uint32_t
+vchi_readbuf_uint32( const void *_ptr )
+{
+   const unsigned char *ptr = _ptr;
+   return ptr[0] | (ptr[1] << 8) | (ptr[2] << 16) | (ptr[3] << 24);
+}
+
+/* ----------------------------------------------------------------------
+ * write a uint32_t to buffer.
+ * network format is defined to be little endian
+ * -------------------------------------------------------------------- */
+void
+vchi_writebuf_uint32( void *_ptr, uint32_t value )
+{
+   unsigned char *ptr = _ptr;
+   ptr[0] = (unsigned char)((value >> 0)  & 0xFF);
+   ptr[1] = (unsigned char)((value >> 8)  & 0xFF);
+   ptr[2] = (unsigned char)((value >> 16) & 0xFF);
+   ptr[3] = (unsigned char)((value >> 24) & 0xFF);
+}
+
+/* ----------------------------------------------------------------------
+ * read a uint16_t from buffer.
+ * network format is defined to be little endian
+ * -------------------------------------------------------------------- */
+uint16_t
+vchi_readbuf_uint16( const void *_ptr )
+{
+   const unsigned char *ptr = _ptr;
+   return ptr[0] | (ptr[1] << 8);
+}
+
+/* ----------------------------------------------------------------------
+ * write a uint16_t into the buffer.
+ * network format is defined to be little endian
+ * -------------------------------------------------------------------- */
+void
+vchi_writebuf_uint16( void *_ptr, uint16_t value )
+{
+   unsigned char *ptr = _ptr;
+   ptr[0] = (value >> 0)  & 0xFF;
+   ptr[1] = (value >> 8)  & 0xFF;
+}
+
+/***********************************************************
+ * Name: vchi_service_use
+ *
+ * Arguments: const VCHI_SERVICE_HANDLE_T handle
+ *
+ * Description: Routine to increment refcount on a service
+ *
+ * Returns: void
+ *
+ ***********************************************************/
+int32_t vchi_service_use( const VCHI_SERVICE_HANDLE_T handle )
+{
+   int32_t ret = -1;
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   if(service)
+   {
+      ret = vchiq_status_to_vchi(vchiq_use_service(service->handle));
+   }
+   return ret;
+}
+
+/***********************************************************
+ * Name: vchi_service_release
+ *
+ * Arguments: const VCHI_SERVICE_HANDLE_T handle
+ *
+ * Description: Routine to decrement refcount on a service
+ *
+ * Returns: void
+ *
+ ***********************************************************/
+int32_t vchi_service_release( const VCHI_SERVICE_HANDLE_T handle )
+{
+   int32_t ret = -1;
+   SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+   if(service)
+   {
+      ret = vchiq_status_to_vchi(vchiq_release_service(service->handle));
+   }
+   return ret;
+}
+
+#if defined(__KERNEL__)
+EXPORT_SYMBOL(vchi_initialise);
+EXPORT_SYMBOL(vchi_connect);
+EXPORT_SYMBOL(vchi_bulk_queue_transmit);
+EXPORT_SYMBOL(vchi_msg_dequeue);
+EXPORT_SYMBOL(vchi_msg_queue);
+EXPORT_SYMBOL(vchi_msg_queuev);
+EXPORT_SYMBOL(vchi_service_close);
+EXPORT_SYMBOL(vchi_service_open);
+EXPORT_SYMBOL(vchi_service_create);
+EXPORT_SYMBOL(vchi_service_use);
+EXPORT_SYMBOL(vchi_service_release);
+#endif
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.c
new file mode 100644
index 0000000..20b67c3
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include "vchiq_util.h"
+
+#if !defined(__KERNEL__)
+#include <stdlib.h>
+#endif
+
+static __inline int is_pow2(int i)
+{
+  return i && !(i & (i - 1));
+}
+
+int vchiu_queue_init(VCHIU_QUEUE_T *queue, int size)
+{
+   vcos_assert(is_pow2(size));
+
+   queue->size = size;
+   queue->read = 0;
+   queue->write = 0;
+
+   vcos_event_create(&queue->pop, "vchiu");
+   vcos_event_create(&queue->push, "vchiu");
+
+   queue->storage = vcos_malloc(size * sizeof(VCHIQ_HEADER_T *), VCOS_FUNCTION);
+   if (queue->storage == NULL)
+   {
+      vchiu_queue_delete(queue);
+      return 0;
+   }
+   return 1;
+}
+
+void vchiu_queue_delete(VCHIU_QUEUE_T *queue)
+{
+   vcos_event_delete(&queue->pop);
+   vcos_event_delete(&queue->push);
+   if (queue->storage != NULL)
+      vcos_free(queue->storage);
+}
+
+int vchiu_queue_is_empty(VCHIU_QUEUE_T *queue)
+{
+   return queue->read == queue->write;
+}
+
+void vchiu_queue_push(VCHIU_QUEUE_T *queue, VCHIQ_HEADER_T *header)
+{
+   while (queue->write == queue->read + queue->size)
+      vcos_event_wait(&queue->pop);
+
+   queue->storage[queue->write & (queue->size - 1)] = header;
+
+   queue->write++;
+
+   vcos_event_signal(&queue->push);
+}
+
+VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue)
+{
+   while (queue->write == queue->read)
+      vcos_event_wait(&queue->push);
+
+   return queue->storage[queue->read & (queue->size - 1)];
+}
+
+VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue)
+{
+   VCHIQ_HEADER_T *header;
+
+   while (queue->write == queue->read)
+      vcos_event_wait(&queue->push);
+
+   header = queue->storage[queue->read & (queue->size - 1)];
+
+   queue->read++;
+
+   vcos_event_signal(&queue->pop);
+
+   return header;
+}
diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.h
new file mode 100644
index 0000000..6e0c84b
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VCHIQ_UTIL_H
+#define VCHIQ_UTIL_H
+
+#include "vchiq_if.h"
+#include "interface/vcos/vcos.h"
+
+typedef struct {
+   int size;
+   int read;
+   int write;
+
+   VCOS_EVENT_T pop;
+   VCOS_EVENT_T push;
+
+   VCHIQ_HEADER_T **storage;
+} VCHIU_QUEUE_T;
+
+extern int  vchiu_queue_init(VCHIU_QUEUE_T *queue, int size);
+extern void vchiu_queue_delete(VCHIU_QUEUE_T *queue);
+
+extern int vchiu_queue_is_empty(VCHIU_QUEUE_T *queue);
+
+extern void vchiu_queue_push(VCHIU_QUEUE_T *queue, VCHIQ_HEADER_T *header);
+
+extern VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue);
+extern VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue);
+
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_cmd.c b/drivers/misc/vc04_services/interface/vcos/generic/vcos_cmd.c
new file mode 100644
index 0000000..447d577
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_cmd.c
@@ -0,0 +1,681 @@
+/*****************************************************************************
+* Copyright 2009 - 2011 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+/***************************************************************************** 
+* 
+*    This file provides a generic command line interface which allows
+*    vcos internals to be manipulated and/or displayed.
+*  
+*****************************************************************************/
+
+/* ---- Include Files ---------------------------------------------------- */
+
+#include "interface/vcos/vcos.h"
+
+#ifdef HAVE_VCOS_VERSION
+#include "interface/vcos/vcos_build_info.h"
+#endif
+
+        #ifdef _VIDEOCORE
+#include vcfw/logging/logging.h
+#endif
+
+/* ---- Public Variables ------------------------------------------------- */
+
+/* ---- Private Constants and Types -------------------------------------- */
+
+#define  VCOS_LOG_CATEGORY (&vcos_cmd_log_category)
+VCOS_LOG_CAT_T vcos_cmd_log_category;
+
+/* ---- Private Variables ------------------------------------------------ */
+
+static struct VCOS_CMD_GLOBALS_T
+{
+    VCOS_MUTEX_T    lock;
+    VCOS_ONCE_T     initialized;
+
+    unsigned        num_cmd_entries;
+    unsigned        num_cmd_alloc;
+    VCOS_CMD_T     *cmd_entry;
+
+    VCOS_LOG_CAT_T *log_category;
+} cmd_globals;
+
+/* ---- Private Function Prototypes -------------------------------------- */
+
+static VCOS_STATUS_T help_cmd( VCOS_CMD_PARAM_T *param );
+
+/* ---- Functions  ------------------------------------------------------- */
+
+/***************************************************************************** 
+*
+*   Walks through the commands looking for a particular command
+*
+*****************************************************************************/
+
+static VCOS_CMD_T *find_cmd( VCOS_CMD_T *cmd_entry, const char *name )
+{
+    VCOS_CMD_T   *scan_entry = cmd_entry;
+
+    while ( scan_entry->name != NULL )
+    {
+        if ( vcos_strcmp( scan_entry->name, name ) == 0 )
+        {
+            return scan_entry;
+        }
+        scan_entry++;
+    }
+
+    return NULL;
+}
+
+/***************************************************************************** 
+*
+*   Saves away 
+*   each line individually.
+*
+*****************************************************************************/
+
+void vcos_cmd_always_log_output( VCOS_LOG_CAT_T *log_category )
+{
+    cmd_globals.log_category = log_category;
+}
+
+/***************************************************************************** 
+*
+*   Walks through a buffer containing newline separated lines, and logs
+*   each line individually.
+*
+*****************************************************************************/
+
+static void cmd_log_results( VCOS_CMD_PARAM_T *param )
+{
+    char    *start;
+    char    *end;
+
+    start = end = param->result_buf;
+
+    while ( *start != '\0' )
+    {
+        while (( *end != '\0' ) && ( *end != '\n' ))
+            end++;
+
+        if ( *end == '\n' )
+        {
+            *end++ = '\0';
+        }
+
+        if ( cmd_globals.log_category != NULL )
+        {
+            if ( vcos_is_log_enabled( cmd_globals.log_category, VCOS_LOG_INFO ))
+            {
+                vcos_log_impl( cmd_globals.log_category, VCOS_LOG_INFO, "%s", start );
+            }
+        }
+        else
+        {
+            vcos_log_info( "%s", start );
+        }
+
+        start = end;
+    }
+
+    /* Since we logged the buffer, reset the pointer back to the beginning. */
+
+    param->result_ptr = param->result_buf;
+    param->result_buf[0] = '\0';
+}
+
+/***************************************************************************** 
+*
+*   Since we may have limited output space, we create a generic routine
+*   which tries to use the result space, but will switch over to using
+*   logging if the output is too large.
+*
+*****************************************************************************/
+
+void vcos_cmd_vprintf( VCOS_CMD_PARAM_T *param, const char *fmt, va_list args )
+{
+    int     bytes_written;
+    int     bytes_remaining;
+
+    bytes_remaining = (int)(param->result_size - ( param->result_ptr - param->result_buf ));
+
+    bytes_written = vcos_vsnprintf( param->result_ptr, bytes_remaining, fmt, args );
+
+    if ( cmd_globals.log_category != NULL )
+    {
+        /* We're going to log each line as we encounter it. If the buffer
+         * doesn't end in a newline, then we'll wait for one first.
+         */
+
+        if ( (( bytes_written + 1 ) >= bytes_remaining ) 
+        ||   ( param->result_ptr[ bytes_written - 1 ] == '\n' ))
+        {
+            cmd_log_results( param );
+        }
+        else
+        {
+            param->result_ptr += bytes_written;
+        }
+    }
+    else
+    {
+        if (( bytes_written + 1 ) >= bytes_remaining )
+        {
+            /* Output doesn't fit - switch over to logging */
+
+            param->use_log = 1;
+
+            *param->result_ptr = '\0';  /* Zap the partial line that didn't fit above. */
+
+            cmd_log_results( param );   /* resets result_ptr */
+
+            bytes_written = vcos_vsnprintf( param->result_ptr, bytes_remaining, fmt, args );
+        }
+        param->result_ptr += bytes_written;
+    }
+}
+
+/***************************************************************************** 
+*
+*   Prints the output.
+*
+*****************************************************************************/
+
+void vcos_cmd_printf( VCOS_CMD_PARAM_T *param, const char *fmt, ... )
+{
+    va_list args;
+
+    va_start( args, fmt );
+    vcos_cmd_vprintf( param, fmt, args );
+    va_end( args );
+}
+
+/***************************************************************************** 
+*
+*   Prints the arguments which were on the command line prior to ours.
+*
+*****************************************************************************/
+
+static void print_argument_prefix( VCOS_CMD_PARAM_T *param )
+{
+    int arg_idx;
+
+    for ( arg_idx = 0; &param->argv_orig[arg_idx] != param->argv; arg_idx++ )
+    {
+        vcos_cmd_printf( param, "%s ", param->argv_orig[arg_idx] );
+    }
+}
+
+/***************************************************************************** 
+*
+*   Prints an error message, prefixed by the command chain required to get
+*   to where we're at.
+*
+*****************************************************************************/
+
+void vcos_cmd_error( VCOS_CMD_PARAM_T *param, const char *fmt, ... )
+{
+    va_list args;
+
+    print_argument_prefix( param );
+
+    va_start( args, fmt );
+    vcos_cmd_vprintf( param, fmt, args );
+    va_end( args );
+    vcos_cmd_printf( param, "\n" );
+}
+
+/****************************************************************************
+*
+*  usage - prints command usage for an array of commands.
+*
+***************************************************************************/
+
+static void usage( VCOS_CMD_PARAM_T *param, VCOS_CMD_T *cmd_entry )
+{
+    int         cmd_idx;
+    int         nameWidth = 0;
+    int         argsWidth = 0;
+    VCOS_CMD_T *scan_entry;
+
+    vcos_cmd_printf( param, "Usage: " );
+    print_argument_prefix( param );
+    vcos_cmd_printf( param, "command [args ...]\n" );
+    vcos_cmd_printf( param, "\n" );
+    vcos_cmd_printf( param, "Where command is one of the following:\n" );
+
+    for ( cmd_idx = 0; cmd_entry[cmd_idx].name != NULL; cmd_idx++ )
+    {
+        int aw;
+        int nw;
+
+        scan_entry = &cmd_entry[cmd_idx];
+
+        nw = vcos_strlen( scan_entry->name );
+        aw = vcos_strlen( scan_entry->args );
+
+        if ( nw > nameWidth )
+        {
+            nameWidth = nw;
+        }
+        if ( aw > argsWidth )
+        {
+            argsWidth = aw;
+        }
+    }
+
+    for ( cmd_idx = 0; cmd_entry[cmd_idx].name != NULL; cmd_idx++ )
+    {
+        scan_entry = &cmd_entry[cmd_idx];
+
+        vcos_cmd_printf( param, "  %-*s %-*s - %s\n", 
+                    nameWidth, scan_entry->name,
+                    argsWidth, scan_entry->args,
+                    scan_entry->descr );
+    }
+}
+
+/****************************************************************************
+*
+*  Prints the usage for the current command.
+*
+***************************************************************************/
+
+void vcos_cmd_usage( VCOS_CMD_PARAM_T *param )
+{
+    VCOS_CMD_T *cmd_entry;
+
+    cmd_entry = param->cmd_entry;
+
+    if ( cmd_entry->sub_cmd_entry != NULL )
+    {
+        /* This command is command with sub-commands */
+
+        usage( param, param->cmd_entry->sub_cmd_entry );
+    }
+    else
+    {
+        vcos_cmd_printf( param, "Usage: " );
+        print_argument_prefix( param );
+        vcos_cmd_printf( param, "%s - %s\n",
+                         param->cmd_entry->args,
+                         param->cmd_entry->descr );
+    }
+}
+
+/***************************************************************************** 
+*
+*   Command to print out the help
+* 
+*   This help command is only called from the main menu.
+* 
+*****************************************************************************/
+
+static VCOS_STATUS_T help_cmd( VCOS_CMD_PARAM_T *param )
+{
+    VCOS_CMD_T  *found_entry;
+
+#if 0
+    {
+        int arg_idx;
+
+        vcos_log_trace( "%s: argc = %d", __func__, param->argc );
+        for ( arg_idx = 0; arg_idx < param->argc; arg_idx++ )
+        {
+            vcos_log_trace( "%s:  argv[%d] = '%s'", __func__, arg_idx, param->argv[arg_idx] );
+        }
+    }
+#endif
+
+    /* If there is an argument after the word help, then we want to print
+     * help for that command.
+     */
+
+    if ( param->argc == 1 )
+    {
+        if ( param->cmd_parent_entry == cmd_globals.cmd_entry )
+        {
+            /* Bare help - print the command usage for the root */
+
+            usage( param, cmd_globals.cmd_entry );
+            return VCOS_SUCCESS;
+        }
+
+        /* For all other cases help requires an argument */
+            
+        vcos_cmd_error( param, "%s requires an argument", param->argv[0] );
+        return VCOS_EINVAL;
+    }
+
+    /* We were given an argument. */
+
+    if (( found_entry = find_cmd( param->cmd_parent_entry, param->argv[1] )) != NULL )
+    {
+        /* Make it look like the command that was specified is the one that's
+         * currently running
+         */
+
+        param->cmd_entry = found_entry;
+        param->argv[0] = param->argv[1];
+        param->argv++;
+        param->argc--;
+
+        vcos_cmd_usage( param );
+        return VCOS_SUCCESS;
+    }
+
+    vcos_cmd_error( param, "- unrecognized command: '%s'", param->argv[1] );
+    return VCOS_ENOENT;
+}
+
+/***************************************************************************** 
+*
+*   Command to print out the version/build information.
+*
+*****************************************************************************/
+
+#ifdef HAVE_VCOS_VERSION
+
+static VCOS_STATUS_T version_cmd( VCOS_CMD_PARAM_T *param )
+{
+    static const char* copyright = "Copyright (c) 2011 Broadcom";
+
+    vcos_cmd_printf( param, "%s %s\n%s\nversion %s\n",
+                     vcos_get_build_date(),
+                     vcos_get_build_time(),
+                     copyright,
+                     vcos_get_build_version() );
+
+    return VCOS_SUCCESS;
+}
+
+#endif
+
+/*****************************************************************************
+*
+*   Internal commands
+*
+*****************************************************************************/
+
+static VCOS_CMD_T cmd_help    = { "help",    "[command]", help_cmd,    NULL, "Prints command help information" };
+
+#ifdef HAVE_VCOS_VERSION
+static VCOS_CMD_T cmd_version = { "version", "",          version_cmd, NULL, "Prints build/version information" };
+#endif
+
+/***************************************************************************** 
+*
+*   Walks the command table and executes the commands
+*
+*****************************************************************************/
+
+static VCOS_STATUS_T execute_cmd( VCOS_CMD_PARAM_T *param, VCOS_CMD_T *cmd_entry )
+{
+    const char     *cmdStr;
+    VCOS_CMD_T     *found_entry;
+
+#if 0
+    {
+        int arg_idx;
+
+        vcos_cmd_printf( param, "%s: argc = %d", __func__, param->argc );
+        for ( arg_idx = 0; arg_idx < param->argc; arg_idx++ )
+        {
+            vcos_cmd_printf( param, " argv[%d] = '%s'", arg_idx, param->argv[arg_idx] );
+        }
+        vcos_cmd_printf( param, "\n" );
+    }
+#endif
+
+    if ( param->argc <= 1 )
+    {
+        /* No command specified */
+
+        vcos_cmd_error( param, "%s - no command specified", param->argv[0] );
+        return VCOS_EINVAL;
+    }
+
+    /* argv[0] is the command/program that caused us to get invoked, so we strip
+     * it off.
+     */
+
+    param->argc--;
+    param->argv++;
+    param->cmd_parent_entry = cmd_entry;
+
+    /* Not the help command, scan for the command and execute it. */
+
+    cmdStr = param->argv[0];
+
+    if (( found_entry = find_cmd( cmd_entry, cmdStr )) != NULL )
+    {
+        if ( found_entry->sub_cmd_entry != NULL )
+        {
+            return execute_cmd( param, found_entry->sub_cmd_entry );
+        }
+
+        param->cmd_entry = found_entry;
+        return found_entry->cmd_fn( param );
+    }
+
+    /* Unrecognized command - check to see if it was the help command */
+
+    if ( vcos_strcmp( cmdStr, cmd_help.name ) == 0 )
+    {
+        return help_cmd( param );
+    }
+
+    vcos_cmd_error( param, "- unrecognized command: '%s'", cmdStr );
+    return VCOS_ENOENT;
+}
+
+/***************************************************************************** 
+*
+*   Initializes the command line parser.
+*
+*****************************************************************************/
+
+static void vcos_cmd_init( void )
+{
+    vcos_mutex_create( &cmd_globals.lock, "vcos_cmd" );
+
+    cmd_globals.num_cmd_entries = 0;
+    cmd_globals.num_cmd_alloc = 0;
+    cmd_globals.cmd_entry = NULL;
+}
+
+/***************************************************************************** 
+*
+*   Command line processor.
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_cmd_execute( int argc, char **argv, size_t result_size, char *result_buf )
+{
+    VCOS_STATUS_T       rc = VCOS_EINVAL;
+    VCOS_CMD_PARAM_T    param;
+
+    vcos_once( &cmd_globals.initialized, vcos_cmd_init );
+
+    param.argc = argc;
+    param.argv = param.argv_orig = argv;
+
+    param.use_log = 0;
+    param.result_size = result_size;
+    param.result_ptr = result_buf;
+    param.result_buf = result_buf;
+
+	result_buf[0] = '\0';
+
+    vcos_mutex_lock( &cmd_globals.lock );
+
+    rc = execute_cmd( &param, cmd_globals.cmd_entry );
+
+    if ( param.use_log )
+    {
+        cmd_log_results( &param );
+        vcos_snprintf( result_buf, result_size, "results logged" );
+    }
+    else
+    if ( cmd_globals.log_category != NULL )
+    {
+        if ( result_buf[0] != '\0' )
+        {
+            /* There is a partial line still buffered. */
+
+            vcos_cmd_printf( &param, "\n" );
+        }
+    }
+
+    vcos_mutex_unlock( &cmd_globals.lock );
+
+    return rc;
+}
+
+/***************************************************************************** 
+*
+*   Registers a command entry with the command line processor
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_cmd_register( VCOS_CMD_T *cmd_entry )
+{
+    VCOS_STATUS_T   rc;
+    VCOS_UNSIGNED   new_num_cmd_alloc;
+    VCOS_CMD_T     *new_cmd_entry;
+    VCOS_CMD_T     *old_cmd_entry;
+    VCOS_CMD_T     *scan_entry;
+
+    vcos_once( &cmd_globals.initialized, vcos_cmd_init );
+
+    vcos_assert( cmd_entry != NULL );
+    vcos_assert( cmd_entry->name != NULL );
+
+    vcos_log_trace( "%s: cmd '%s'", __FUNCTION__, cmd_entry->name );
+
+    vcos_assert( cmd_entry->args != NULL );
+    vcos_assert(( cmd_entry->cmd_fn != NULL ) || ( cmd_entry->sub_cmd_entry != NULL ));
+    vcos_assert( cmd_entry->descr != NULL );
+
+    /* We expect vcos_cmd_init to be called before vcos_logging_init, so we
+     * need to defer registering our logging category until someplace
+     * like right here.
+     */
+
+    if ( vcos_cmd_log_category.name == NULL )
+    {
+        /*
+         * If you're using the command interface, you pretty much always want
+         * log messages from this file to show up. So we change the default
+         * from ERROR to be the more reasonable INFO level.
+         */
+
+        vcos_log_set_level(&vcos_cmd_log_category, VCOS_LOG_INFO);
+        vcos_log_register("vcos_cmd", &vcos_cmd_log_category);
+
+        /* We register a help command so that it shows up in the usage. */
+
+        vcos_cmd_register( &cmd_help );
+#ifdef HAVE_VCOS_VERSION
+        vcos_cmd_register( &cmd_version );
+#endif
+    }
+
+    vcos_mutex_lock( &cmd_globals.lock );
+
+    if ( cmd_globals.num_cmd_entries >= cmd_globals.num_cmd_alloc )
+    {
+        if ( cmd_globals.num_cmd_alloc == 0 )
+        {
+            /* We haven't allocated a table yet */
+        }
+
+        /* The number 8 is rather arbitrary. */
+
+        new_num_cmd_alloc = cmd_globals.num_cmd_alloc + 8;
+
+        /* The + 1 is to ensure that we always have a NULL entry at the end. */
+
+        new_cmd_entry = (VCOS_CMD_T *)vcos_calloc( new_num_cmd_alloc + 1, sizeof( *cmd_entry ), "vcos_cmd_entries" );
+        if ( new_cmd_entry == NULL )
+        {
+            rc = VCOS_ENOMEM;
+            goto out;
+        }
+        memcpy( new_cmd_entry, cmd_globals.cmd_entry, cmd_globals.num_cmd_entries * sizeof( *cmd_entry ));
+        cmd_globals.num_cmd_alloc = new_num_cmd_alloc;
+        old_cmd_entry = cmd_globals.cmd_entry;
+        cmd_globals.cmd_entry = new_cmd_entry;
+        vcos_free( old_cmd_entry );
+    }
+
+    if ( cmd_globals.num_cmd_entries == 0 )
+    {
+        /* This is the first command being registered */
+
+        cmd_globals.cmd_entry[0] = *cmd_entry;
+    }
+    else
+    {
+        /* Keep the list in alphabetical order. We start at the end and work backwards
+         * shuffling entries up one until we find an insertion point.
+         */
+
+        for ( scan_entry = &cmd_globals.cmd_entry[cmd_globals.num_cmd_entries - 1];
+              scan_entry >= cmd_globals.cmd_entry; scan_entry-- )
+        {
+            if ( vcos_strcmp( cmd_entry->name, scan_entry->name ) > 0 )
+            {
+                /* We found an insertion point. */
+
+                break;
+            }
+
+            scan_entry[1] = scan_entry[0];
+        }
+        scan_entry[1] = *cmd_entry;
+    }
+    cmd_globals.num_cmd_entries++;
+
+    rc = VCOS_SUCCESS;
+
+out:
+
+    vcos_mutex_unlock( &cmd_globals.lock );
+    return rc;
+}
+
+/***************************************************************************** 
+*
+*   Registers multiple commands.
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_cmd_register_multiple( VCOS_CMD_T *cmd_entry )
+{
+    VCOS_STATUS_T   status;
+
+    while ( cmd_entry->name != NULL )
+    {
+        if (( status = vcos_cmd_register( cmd_entry )) != VCOS_SUCCESS )
+        {
+            return status;
+        }
+        cmd_entry++;
+    }
+    return VCOS_SUCCESS;
+}
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_common.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_common.h
new file mode 100644
index 0000000..2493122
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_common.h
@@ -0,0 +1,76 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - common postamble code
+=============================================================================*/
+
+/** \file
+  *
+  * Postamble code included by the platform-specific header files
+  */
+
+#define VCOS_THREAD_PRI_DEFAULT VCOS_THREAD_PRI_NORMAL
+
+#if !defined(VCOS_THREAD_PRI_INCREASE)
+#error Which way to thread priorities go?
+#endif
+
+#if VCOS_THREAD_PRI_INCREASE < 0
+/* smaller numbers are higher priority */
+#define VCOS_THREAD_PRI_LESS(x) ((x)<VCOS_THREAD_PRI_MAX?(x)+1:VCOS_THREAD_PRI_MAX)
+#define VCOS_THREAD_PRI_MORE(x) ((x)>VCOS_THREAD_PRI_MIN?(x)-1:VCOS_THREAD_PRI_MIN)
+#else
+/* bigger numbers are lower priority */
+#define VCOS_THREAD_PRI_MORE(x) ((x)<VCOS_THREAD_PRI_MAX?(x)+1:VCOS_THREAD_PRI_MAX)
+#define VCOS_THREAD_PRI_LESS(x) ((x)>VCOS_THREAD_PRI_MIN?(x)-1:VCOS_THREAD_PRI_MIN)
+#endif
+
+/* Convenience for Brits: */
+#define VCOS_APPLICATION_INITIALISE VCOS_APPLICATION_INITIALIZE
+
+/*
+ * Check for constant definitions
+ */
+#ifndef VCOS_TICKS_PER_SECOND
+#error VCOS_TICKS_PER_SECOND not defined
+#endif
+
+#if !defined(VCOS_THREAD_PRI_MIN) || !defined(VCOS_THREAD_PRI_MAX)
+#error Priority range not defined
+#endif
+
+#if !defined(VCOS_THREAD_PRI_HIGHEST) || !defined(VCOS_THREAD_PRI_LOWEST) || !defined(VCOS_THREAD_PRI_NORMAL)
+#error Priority ordering not defined
+#endif
+
+#if !defined(VCOS_CAN_SET_STACK_ADDR)
+#error Can stack addresses be set on this platform? Please set this macro to either 0 or 1.
+#endif
+
+#if (_VCOS_AFFINITY_CPU0|_VCOS_AFFINITY_CPU1) & (~_VCOS_AFFINITY_MASK) 
+#error _VCOS_AFFINITY_CPUxxx values are not consistent with _VCOS_AFFINITY_MASK
+#endif
+
+/** Append to the end of a singly-linked queue, O(1). Works with
+  * any structure where list has members 'head' and 'tail' and
+  * item has a 'next' pointer.
+  */
+#define VCOS_QUEUE_APPEND_TAIL(list, item) {\
+   (item)->next = NULL;\
+   if (!(list)->head) {\
+      (list)->head = (list)->tail = (item); \
+   } else {\
+      (list)->tail->next = (item); \
+      (list)->tail = (item); \
+   } \
+}
+
+#ifndef VCOS_HAVE_TIMER
+VCOSPRE_ void VCOSPOST_ vcos_timer_init(void);
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_blockpool.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_blockpool.h
new file mode 100644
index 0000000..a048470
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_blockpool.h
@@ -0,0 +1,260 @@
+/*=============================================================================
+Copyright (c) 2011 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - event flags implemented via a semaphore
+=============================================================================*/
+
+#ifndef VCOS_GENERIC_BLOCKPOOL_H
+#define VCOS_GENERIC_BLOCKPOOL_H
+
+/**
+  * \file
+  *
+  * This provides a generic, thread safe implementation of a VCOS block pool
+  * fixed size memory allocator.
+  */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+
+/** Bits 0 to (VCOS_BLOCKPOOL_SUBPOOL_BITS - 1) are used to store the
+ * subpool id. */
+#define VCOS_BLOCKPOOL_SUBPOOL_BITS 3
+#define VCOS_BLOCKPOOL_MAX_SUBPOOLS (1 << VCOS_BLOCKPOOL_SUBPOOL_BITS)
+
+/* Make zero an invalid handle at the cost of decreasing the maximum
+ * number of blocks (2^28) by 1. Alternatively, a spare bit could be
+ * used to indicated valid blocks but there are likely to be better
+ * uses for spare bits. e.g. allowing more subpools
+ */
+#define INDEX_OFFSET 1
+
+#define VCOS_BLOCKPOOL_HANDLE_GET_INDEX(h) \
+   (((h) >> VCOS_BLOCKPOOL_SUBPOOL_BITS) - INDEX_OFFSET)
+
+#define VCOS_BLOCKPOOL_HANDLE_GET_SUBPOOL(h) \
+   ((h) & ((1 << VCOS_BLOCKPOOL_SUBPOOL_BITS) - 1))
+
+#define VCOS_BLOCKPOOL_HANDLE_CREATE(i,s) \
+   ((((i) + INDEX_OFFSET) << VCOS_BLOCKPOOL_SUBPOOL_BITS) | (s))
+
+#define VCOS_BLOCKPOOL_INVALID_HANDLE 0
+
+typedef struct VCOS_BLOCKPOOL_HEADER_TAG
+{
+   /* Blocks either refer to to the pool if they are allocated
+    * or the free list if they are available.
+    */
+   union {
+   struct VCOS_BLOCKPOOL_HEADER_TAG *next;
+   struct VCOS_BLOCKPOOL_SUBPOOL_TAG* subpool;
+   } owner;
+} VCOS_BLOCKPOOL_HEADER_T;
+
+typedef struct VCOS_BLOCKPOOL_SUBPOOL_TAG
+{
+   /** VCOS_BLOCKPOOL_SUBPOOL_MAGIC */
+   uint32_t magic;
+   VCOS_BLOCKPOOL_HEADER_T* free_list;
+   /* The start of the pool memory */
+   void *mem;
+   /* Address of the first block header */
+   void *start;
+   /** The number of blocks in this sub-pool */
+   VCOS_UNSIGNED num_blocks;
+   /** Current number of available blocks in this sub-pool */
+   VCOS_UNSIGNED available_blocks;
+   /** Pointers to the pool that owns this sub-pool */
+   struct VCOS_BLOCKPOOL_TAG* owner;
+   /** Define properties such as memory ownership */
+   uint32_t flags;
+} VCOS_BLOCKPOOL_SUBPOOL_T;
+
+typedef struct VCOS_BLOCKPOOL_TAG
+{
+   /** VCOS_BLOCKPOOL_MAGIC */
+   uint32_t magic;
+   /** Thread safety for Alloc, Free, Delete, Stats */
+   VCOS_MUTEX_T mutex;
+   /** The size of the block data */
+   size_t block_data_size;
+   /** Block size inc overheads */
+   size_t block_size;
+   /** Name for debugging */
+   const char *name;
+   /* The number of subpools that may be used */
+   VCOS_UNSIGNED num_subpools;
+   /** Number of blocks in each dynamically allocated subpool */
+   VCOS_UNSIGNED num_extension_blocks;
+   /** Array of subpools. Subpool zero is is not deleted until the pool is
+    * destroed. If the index of the pool is < num_subpools and
+    * subpool[index.mem] is null then the subpool entry is valid but
+    * "not currently allocated" */
+   VCOS_BLOCKPOOL_SUBPOOL_T subpools[VCOS_BLOCKPOOL_MAX_SUBPOOLS];
+} VCOS_BLOCKPOOL_T;
+
+#define VCOS_BLOCKPOOL_ROUND_UP(x,s)   (((x) + ((s) - 1)) & ~((s) - 1))
+/**
+ * Calculates the size in bytes required for a block pool containing
+ * num_blocks of size block_size plus any overheads.
+ *
+ * The block pool header (VCOS_BLOCKPOOL_T) is allocated separately
+ *
+ * Overheads:
+ * block_size + header must be a multiple of sizeof(void*)
+ * The start of the first block may need to be up to wordsize - 1 bytes
+ * into the given buffer because statically allocated buffers within structures
+ * are not guaranteed to be word aligned.
+ */
+#define VCOS_BLOCKPOOL_SIZE(num_blocks, block_size) \
+   ((VCOS_BLOCKPOOL_ROUND_UP((block_size) + sizeof(VCOS_BLOCKPOOL_HEADER_T), \
+                             sizeof(void*)) * (num_blocks)) + sizeof(void*))
+
+/**
+ * Sanity check to verify whether a handle is potentially a blockpool handle
+ * when the pool pointer is not available.
+ *
+ * If the pool pointer is availabe use vcos_blockpool_elem_to_handle instead.
+ *
+ * @param handle       the handle to verify
+ * @param max_blocks   the expected maximum number of block in the pool
+ *                     that the handle belongs to.
+ */
+#define VCOS_BLOCKPOOL_IS_VALID_HANDLE_FORMAT(handle, max_blocks) \
+    ((handle) != VCOS_BLOCKPOOL_INVALID_HANDLE \
+     && VCOS_BLOCKPOOL_HANDLE_GET_INDEX((handle)) < (max_blocks))
+
+VCOSPRE_
+   VCOS_STATUS_T VCOSPOST_ vcos_generic_blockpool_init(VCOS_BLOCKPOOL_T *pool,
+      VCOS_UNSIGNED num_blocks, VCOS_UNSIGNED block_size,
+      void *start, VCOS_UNSIGNED pool_size, const char *name);
+
+VCOSPRE_
+   VCOS_STATUS_T VCOSPOST_ vcos_generic_blockpool_create_on_heap(
+         VCOS_BLOCKPOOL_T *pool, VCOS_UNSIGNED num_blocks,
+         VCOS_UNSIGNED block_size, const char *name);
+
+VCOSPRE_
+   VCOS_STATUS_T VCOSPOST_ vcos_generic_blockpool_extend(VCOS_BLOCKPOOL_T *pool,
+         VCOS_UNSIGNED num_extensions, VCOS_UNSIGNED num_blocks);
+
+VCOSPRE_ void VCOSPOST_ *vcos_generic_blockpool_alloc(VCOS_BLOCKPOOL_T *pool);
+
+VCOSPRE_ void VCOSPOST_ *vcos_generic_blockpool_calloc(VCOS_BLOCKPOOL_T *pool);
+
+VCOSPRE_ void VCOSPOST_ vcos_generic_blockpool_free(void *block);
+
+VCOSPRE_
+   VCOS_UNSIGNED VCOSPOST_ vcos_generic_blockpool_available_count(
+         VCOS_BLOCKPOOL_T *pool);
+
+VCOSPRE_
+   VCOS_UNSIGNED VCOSPOST_ vcos_generic_blockpool_used_count(
+         VCOS_BLOCKPOOL_T *pool);
+
+VCOSPRE_ void VCOSPOST_ vcos_generic_blockpool_delete(VCOS_BLOCKPOOL_T *pool);
+
+VCOSPRE_ uint32_t VCOSPOST_ vcos_generic_blockpool_elem_to_handle(void *block);
+
+VCOSPRE_ void VCOSPOST_
+   *vcos_generic_blockpool_elem_from_handle(
+         VCOS_BLOCKPOOL_T *pool, uint32_t handle);
+
+VCOSPRE_ uint32_t VCOSPOST_
+   vcos_generic_blockpool_is_valid_elem(
+         VCOS_BLOCKPOOL_T *pool, const void *block);
+#if defined(VCOS_INLINE_BODIES)
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_blockpool_init(VCOS_BLOCKPOOL_T *pool,
+      VCOS_UNSIGNED num_blocks, VCOS_UNSIGNED block_size,
+      void *start, VCOS_UNSIGNED pool_size, const char *name)
+{
+   return vcos_generic_blockpool_init(pool, num_blocks, block_size,
+         start, pool_size, name);
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_blockpool_create_on_heap(VCOS_BLOCKPOOL_T *pool,
+      VCOS_UNSIGNED num_blocks, VCOS_UNSIGNED block_size, const char *name)
+{
+   return vcos_generic_blockpool_create_on_heap(
+         pool, num_blocks, block_size, name);
+}
+
+VCOS_INLINE_IMPL
+   VCOS_STATUS_T VCOSPOST_ vcos_blockpool_extend(VCOS_BLOCKPOOL_T *pool,
+         VCOS_UNSIGNED num_extensions, VCOS_UNSIGNED num_blocks)
+{
+    return vcos_generic_blockpool_extend(pool, num_extensions, num_blocks);
+}
+
+VCOS_INLINE_IMPL
+void *vcos_blockpool_alloc(VCOS_BLOCKPOOL_T *pool)
+{
+   return vcos_generic_blockpool_alloc(pool);
+}
+
+VCOS_INLINE_IMPL
+void *vcos_blockpool_calloc(VCOS_BLOCKPOOL_T *pool)
+{
+   return vcos_generic_blockpool_calloc(pool);
+}
+
+VCOS_INLINE_IMPL
+void vcos_blockpool_free(void *block)
+{
+   vcos_generic_blockpool_free(block);
+}
+
+VCOS_INLINE_IMPL
+VCOS_UNSIGNED vcos_blockpool_available_count(VCOS_BLOCKPOOL_T *pool)
+{
+   return vcos_generic_blockpool_available_count(pool);
+}
+
+VCOS_INLINE_IMPL
+VCOS_UNSIGNED vcos_blockpool_used_count(VCOS_BLOCKPOOL_T *pool)
+{
+   return vcos_generic_blockpool_used_count(pool);
+}
+
+VCOS_INLINE_IMPL
+void vcos_blockpool_delete(VCOS_BLOCKPOOL_T *pool)
+{
+   vcos_generic_blockpool_delete(pool);
+}
+
+VCOS_INLINE_IMPL
+uint32_t vcos_blockpool_elem_to_handle(void *block)
+{
+   return vcos_generic_blockpool_elem_to_handle(block);
+}
+
+VCOS_INLINE_IMPL
+void *vcos_blockpool_elem_from_handle(VCOS_BLOCKPOOL_T *pool, uint32_t handle)
+{
+   return vcos_generic_blockpool_elem_from_handle(pool, handle);
+}
+
+VCOS_INLINE_IMPL
+uint32_t vcos_blockpool_is_valid_elem(VCOS_BLOCKPOOL_T *pool, const void *block)
+{
+   return vcos_generic_blockpool_is_valid_elem(pool, block);
+}
+#endif /* VCOS_INLINE_BODIES */
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* VCOS_GENERIC_BLOCKPOOL_H */
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.c b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.c
new file mode 100644
index 0000000..8cd150f
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.c
@@ -0,0 +1,297 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - event flags implemented via mutexes
+=============================================================================*/
+
+#include "interface/vcos/vcos.h"
+#include "interface/vcos/generic/vcos_generic_event_flags.h"
+
+#include <stddef.h>
+
+/** A structure created by a thread that waits on the event flags
+  * for a particular combination of flags to arrive.
+  */
+typedef struct VCOS_EVENT_WAITER_T
+{
+   VCOS_UNSIGNED requested_events;  /**< The events wanted */
+   VCOS_UNSIGNED actual_events;     /**< Actual events found */
+   VCOS_UNSIGNED op;                /**< The event operation to be used */
+   VCOS_STATUS_T return_status;     /**< The return status the waiter should pass back */
+   VCOS_EVENT_FLAGS_T *flags;       /**< Pointer to the original 'flags' structure */
+   VCOS_THREAD_T *thread;           /**< Thread waiting */
+   struct VCOS_EVENT_WAITER_T *next;
+} VCOS_EVENT_WAITER_T;
+
+#ifndef NDEBUG
+static int waiter_list_valid(VCOS_EVENT_FLAGS_T *flags);
+#endif
+static void event_flags_timer_expired(void *cxt);
+
+VCOS_STATUS_T vcos_generic_event_flags_create(VCOS_EVENT_FLAGS_T *flags, const char *name)
+{
+   VCOS_STATUS_T rc;
+   if ((rc=vcos_mutex_create(&flags->lock, name)) != VCOS_SUCCESS)
+   {
+      return rc;
+   }
+
+   flags->events = 0;
+   flags->waiters.head = flags->waiters.tail = 0;
+   return rc;
+}
+
+void vcos_generic_event_flags_set(VCOS_EVENT_FLAGS_T *flags,
+                                  VCOS_UNSIGNED bitmask,
+                                  VCOS_OPTION op)
+{
+   vcos_assert(flags);
+   vcos_mutex_lock(&flags->lock);
+   if (op == VCOS_OR)
+   {
+      flags->events |= bitmask;
+   }
+   else if (op == VCOS_AND)
+   {
+      flags->events &= bitmask;
+   }
+   else
+   {
+      vcos_assert(0);
+   }
+
+   /* Now wake up any threads that have now become signalled. */
+   if (flags->waiters.head != NULL)
+   {
+      VCOS_UNSIGNED consumed_events = 0;
+      VCOS_EVENT_WAITER_T **pcurrent_waiter = &flags->waiters.head;
+      VCOS_EVENT_WAITER_T *prev_waiter = NULL;
+
+      /* Walk the chain of tasks suspend on this event flag group to determine
+       * if any of their requests can be satisfied.
+       */
+      while ((*pcurrent_waiter) != NULL)
+      {
+         VCOS_EVENT_WAITER_T *curr_waiter = *pcurrent_waiter;
+
+         /* Determine if this request has been satisfied */
+
+         /* First, find the event flags in common. */
+         VCOS_UNSIGNED waiter_satisfied = flags->events & curr_waiter->requested_events;
+
+         /* Second, determine if all the event flags must match */
+         if (curr_waiter->op & VCOS_AND)
+         {
+            /* All requested events must be present */
+            waiter_satisfied = (waiter_satisfied == curr_waiter->requested_events);
+         }
+
+         /* Wake this one up? */
+         if (waiter_satisfied)
+         {
+
+            if (curr_waiter->op & VCOS_CONSUME)
+            {
+               consumed_events |= curr_waiter->requested_events;
+            }
+
+            /* remove this block from the list, taking care at the end */
+            *pcurrent_waiter = curr_waiter->next;
+            if (curr_waiter->next == NULL)
+               flags->waiters.tail = prev_waiter;
+
+            vcos_assert(waiter_list_valid(flags));
+
+            curr_waiter->return_status = VCOS_SUCCESS;
+            curr_waiter->actual_events = flags->events;
+
+            _vcos_thread_sem_post(curr_waiter->thread);
+         }
+         else
+         {
+            /* move to next element in the list */
+            prev_waiter = *pcurrent_waiter;
+            pcurrent_waiter = &(curr_waiter->next);
+         }
+      }
+
+      flags->events &= ~consumed_events;
+
+   }
+
+   vcos_mutex_unlock(&flags->lock);
+}
+
+void vcos_generic_event_flags_delete(VCOS_EVENT_FLAGS_T *flags)
+{
+   vcos_mutex_delete(&flags->lock);
+}
+
+extern VCOS_STATUS_T vcos_generic_event_flags_get(VCOS_EVENT_FLAGS_T *flags,
+                                                  VCOS_UNSIGNED bitmask,
+                                                  VCOS_OPTION op,
+                                                  VCOS_UNSIGNED suspend,
+                                                  VCOS_UNSIGNED *retrieved_bits)
+{
+   VCOS_EVENT_WAITER_T waitreq;
+   VCOS_STATUS_T rc = VCOS_EAGAIN;
+   int satisfied = 0;
+
+   vcos_assert(flags);
+
+   /* default retrieved bits to 0 */
+   *retrieved_bits = 0;
+
+   vcos_mutex_lock(&flags->lock);
+   switch (op & VCOS_EVENT_FLAG_OP_MASK)
+   {
+   case VCOS_AND:
+      if ((flags->events & bitmask) == bitmask)
+      {
+         *retrieved_bits = flags->events;
+         rc = VCOS_SUCCESS;
+         satisfied = 1;
+         if (op & VCOS_CONSUME)
+            flags->events &= ~bitmask;
+      }
+      break;
+
+   case VCOS_OR:
+      if (flags->events & bitmask)
+      {
+         *retrieved_bits = flags->events;
+         rc = VCOS_SUCCESS;
+         satisfied = 1;
+         if (op & VCOS_CONSUME)
+            flags->events &= ~bitmask;
+      }
+      break;
+
+   default:
+      vcos_assert(0);
+      rc = VCOS_EINVAL;
+      break;
+   }
+
+   if (!satisfied && suspend)
+   {
+      /* Have to go to sleep.
+       *
+       * Append to tail so we get FIFO ordering.
+       */
+      waitreq.requested_events = bitmask;
+      waitreq.op = op;
+      waitreq.return_status = VCOS_EAGAIN;
+      waitreq.flags = flags;
+      waitreq.actual_events = 0;
+      waitreq.thread = vcos_thread_current();
+      waitreq.next = 0;
+      vcos_assert(waitreq.thread != (VCOS_THREAD_T*)-1);
+      VCOS_QUEUE_APPEND_TAIL(&flags->waiters, &waitreq);
+
+      if (suspend != (VCOS_UNSIGNED)-1)
+         _vcos_task_timer_set(event_flags_timer_expired, &waitreq, suspend);
+
+      vcos_mutex_unlock(&flags->lock);
+      /* go to sleep and wait to be signalled or timeout */
+
+      _vcos_thread_sem_wait();
+
+      *retrieved_bits = waitreq.actual_events;
+      rc = waitreq.return_status;
+
+      /* cancel the timer - do not do this while holding the mutex as it
+       * might be waiting for the timeout function to complete, which will
+       * try to take the mutex.
+       */
+      if (suspend != (VCOS_UNSIGNED)-1)
+         _vcos_task_timer_cancel();
+   }
+   else
+   {
+      vcos_mutex_unlock(&flags->lock);
+   }
+
+   return rc;
+}
+
+
+/** Called when a get call times out. Remove this thread's
+  * entry from the waiting queue, then resume the thread.
+  */
+static void event_flags_timer_expired(void *cxt)
+{
+   VCOS_EVENT_WAITER_T *waitreq = (VCOS_EVENT_WAITER_T *)cxt;
+   VCOS_EVENT_FLAGS_T *flags = waitreq->flags;
+   VCOS_EVENT_WAITER_T **plist;
+   VCOS_EVENT_WAITER_T *prev = NULL;
+   VCOS_THREAD_T *thread = 0;
+
+   vcos_assert(flags);
+
+   vcos_mutex_lock(&flags->lock);
+
+   /* walk the list of waiting threads on this event group, and remove
+    * the one that has expired.
+    *
+    * FIXME: could use doubly-linked list if lots of threads are found
+    * to be waiting on a single event flag instance.
+    */
+   plist = &flags->waiters.head;
+   while (*plist != NULL)
+   {
+      if (*plist == waitreq)
+      {
+         int at_end;
+         /* found it */
+         thread = (*plist)->thread;
+         at_end = ((*plist)->next == NULL);
+
+         /* link past */
+         *plist = (*plist)->next;
+         if (at_end)
+            flags->waiters.tail = prev;
+
+         break;
+      }
+      prev = *plist;
+      plist = &(*plist)->next;
+   }
+   vcos_assert(waiter_list_valid(flags));
+
+   vcos_mutex_unlock(&flags->lock);
+
+   if (thread)
+   {
+      _vcos_thread_sem_post(thread);
+   }
+}
+
+#ifndef NDEBUG
+
+static int waiter_list_valid(VCOS_EVENT_FLAGS_T *flags)
+{
+   int valid;
+   /* Either both head and tail are NULL, or neither are NULL */
+   if (flags->waiters.head == NULL)
+   {
+      valid = (flags->waiters.tail == NULL);
+   }
+   else
+   {
+      valid = (flags->waiters.tail != NULL);
+   }
+
+   /* If head and tail point at the same non-NULL element, then there
+    * is only one element in the list.
+    */
+   if (flags->waiters.head && (flags->waiters.head == flags->waiters.tail))
+   {
+      valid = (flags->waiters.head->next == NULL);
+   }
+   return valid;
+}
+
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.h
new file mode 100644
index 0000000..5205db7
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.h
@@ -0,0 +1,104 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - event flags implemented via a semaphore
+=============================================================================*/
+
+#ifndef VCOS_GENERIC_EVENT_FLAGS_H
+#define VCOS_GENERIC_EVENT_FLAGS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+
+/**
+  * \file
+  *
+  * This provides event flags (as per Nucleus Event Groups) based on a
+  * mutex, a semaphore (per waiting thread) and a timer (per waiting
+  * thread).
+  * 
+  * The data structure is a 32 bit unsigned int (the current set of
+  * flags) and a linked list of clients waiting to be 'satisfied'.
+  *
+  * The mutex merely locks access to the data structure. If a client
+  * calls vcos_event_flags_get() and the requested bits are not already
+  * present, it then sleeps on its per-thread semaphore after adding
+  * this semaphore to the queue waiting. It also sets up a timer.
+  *
+  * The per-thread semaphore and timer are actually stored in the
+  * thread context (joinable thread). In future it may become necessary
+  * to support non-VCOS threads by using thread local storage to
+  * create these objects and associate them with the thread.
+  */
+
+struct VCOS_EVENT_WAITER_T;
+
+typedef struct VCOS_EVENT_FLAGS_T
+{
+   VCOS_UNSIGNED events;      /**< Events currently set */
+   VCOS_MUTEX_T lock;         /**< Serialize access */
+   struct
+   {
+      struct VCOS_EVENT_WAITER_T *head;   /**< List of threads waiting */
+      struct VCOS_EVENT_WAITER_T *tail;   /**< List of threads waiting */
+   } waiters;
+} VCOS_EVENT_FLAGS_T;
+
+#define VCOS_OR      1
+#define VCOS_AND     2
+#define VCOS_CONSUME 4
+#define VCOS_OR_CONSUME (VCOS_OR | VCOS_CONSUME)
+#define VCOS_AND_CONSUME (VCOS_AND | VCOS_CONSUME)
+#define VCOS_EVENT_FLAG_OP_MASK (VCOS_OR|VCOS_AND)
+
+VCOSPRE_  VCOS_STATUS_T VCOSPOST_ vcos_generic_event_flags_create(VCOS_EVENT_FLAGS_T *flags, const char *name);
+VCOSPRE_  void VCOSPOST_ vcos_generic_event_flags_set(VCOS_EVENT_FLAGS_T *flags,
+                                                      VCOS_UNSIGNED events,
+                                                      VCOS_OPTION op);
+VCOSPRE_  void VCOSPOST_ vcos_generic_event_flags_delete(VCOS_EVENT_FLAGS_T *);
+VCOSPRE_  VCOS_STATUS_T VCOSPOST_ vcos_generic_event_flags_get(VCOS_EVENT_FLAGS_T *flags,
+                                                               VCOS_UNSIGNED requested_events,
+                                                               VCOS_OPTION op,
+                                                               VCOS_UNSIGNED suspend,
+                                                               VCOS_UNSIGNED *retrieved_events);
+
+#ifdef VCOS_INLINE_BODIES
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_event_flags_create(VCOS_EVENT_FLAGS_T *flags, const char *name) {
+   return vcos_generic_event_flags_create(flags, name);
+}
+
+VCOS_INLINE_IMPL
+void vcos_event_flags_set(VCOS_EVENT_FLAGS_T *flags,
+                          VCOS_UNSIGNED events,
+                          VCOS_OPTION op) {
+   vcos_generic_event_flags_set(flags, events, op);
+}
+
+VCOS_INLINE_IMPL
+void vcos_event_flags_delete(VCOS_EVENT_FLAGS_T *f) {
+   vcos_generic_event_flags_delete(f);
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_event_flags_get(VCOS_EVENT_FLAGS_T *flags,
+                                   VCOS_UNSIGNED requested_events,
+                                   VCOS_OPTION op,
+                                   VCOS_UNSIGNED suspend,
+                                   VCOS_UNSIGNED *retrieved_events) {
+   return vcos_generic_event_flags_get(flags, requested_events, op, suspend, retrieved_events);
+}
+
+#endif /* VCOS_INLINE_BODIES */
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_named_sem.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_named_sem.h
new file mode 100644
index 0000000..15b332d
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_named_sem.h
@@ -0,0 +1,81 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - named semaphores
+=============================================================================*/
+
+#ifndef VCOS_GENERIC_NAMED_SEM_H
+#define VCOS_GENERIC_NAMED_SEM_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+
+/**
+ * \file
+ *
+ * Generic support for named semaphores, using regular ones. This is only
+ * suitable for emulating them on an embedded MMUless system, since there is
+ * no support for opening semaphores across process boundaries.
+ *
+ */
+
+#define VCOS_NAMED_SEMAPHORE_NAMELEN   64
+
+/* In theory we could use the name facility provided within Nucleus. However, this
+ * is hard to do as semaphores are constantly being created and destroyed; we
+ * would need to stop everything while allocating the memory for the semaphore
+ * list and then walking it. So keep our own list.
+ */
+typedef struct VCOS_NAMED_SEMAPHORE_T
+{
+   struct VCOS_NAMED_SEMAPHORE_IMPL_T *actual; /**< There are 'n' named semaphores per 1 actual semaphore  */
+   VCOS_SEMAPHORE_T *sem;                      /**< Pointer to actual underlying semaphore */
+} VCOS_NAMED_SEMAPHORE_T;
+
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_
+vcos_generic_named_semaphore_create(VCOS_NAMED_SEMAPHORE_T *sem, const char *name, VCOS_UNSIGNED count);
+
+VCOSPRE_ void VCOSPOST_ vcos_named_semaphore_delete(VCOS_NAMED_SEMAPHORE_T *sem);
+
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ _vcos_named_semaphore_init(void);
+VCOSPRE_ void VCOSPOST_ _vcos_named_semaphore_deinit(void);
+
+#if defined(VCOS_INLINE_BODIES)
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_named_semaphore_create(VCOS_NAMED_SEMAPHORE_T *sem, const char *name, VCOS_UNSIGNED count) {
+   return vcos_generic_named_semaphore_create(sem, name, count);
+}
+
+VCOS_INLINE_IMPL
+void vcos_named_semaphore_wait(VCOS_NAMED_SEMAPHORE_T *sem) {
+   vcos_semaphore_wait(sem->sem);
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_named_semaphore_trywait(VCOS_NAMED_SEMAPHORE_T *sem) {
+   return vcos_semaphore_trywait(sem->sem);
+}
+
+VCOS_INLINE_IMPL
+void vcos_named_semaphore_post(VCOS_NAMED_SEMAPHORE_T *sem) {
+   vcos_semaphore_post(sem->sem);
+}
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_quickslow_mutex.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_quickslow_mutex.h
new file mode 100644
index 0000000..e4ae649
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_quickslow_mutex.h
@@ -0,0 +1,75 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - reentrant mutexes created from regular ones.
+=============================================================================*/
+
+#ifndef VCOS_GENERIC_QUICKSLOW_MUTEX_H
+#define VCOS_GENERIC_QUICKSLOW_MUTEX_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+
+/**
+ * \file
+ *
+ * Quickslow Mutexes implemented as regular ones (i.e. quick and slow modes are the same).
+ *
+ */
+
+typedef VCOS_MUTEX_T VCOS_QUICKSLOW_MUTEX_T;
+
+#if defined(VCOS_INLINE_BODIES)
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_quickslow_mutex_create(VCOS_QUICKSLOW_MUTEX_T *m, const char *name)
+{
+   return vcos_mutex_create(m, name);
+}
+
+VCOS_INLINE_IMPL
+void vcos_quickslow_mutex_delete(VCOS_QUICKSLOW_MUTEX_T *m)
+{
+   vcos_mutex_delete(m);
+}
+
+VCOS_INLINE_IMPL
+void vcos_quickslow_mutex_lock(VCOS_QUICKSLOW_MUTEX_T *m)
+{
+   while (vcos_mutex_lock(m) == VCOS_EAGAIN);
+}
+
+VCOS_INLINE_IMPL
+void vcos_quickslow_mutex_unlock(VCOS_QUICKSLOW_MUTEX_T *m)
+{
+   vcos_mutex_unlock(m);
+}
+
+VCOS_INLINE_IMPL
+void vcos_quickslow_mutex_lock_quick(VCOS_QUICKSLOW_MUTEX_T *m)
+{
+   while (vcos_mutex_lock(m) == VCOS_EAGAIN);
+}
+
+VCOS_INLINE_IMPL
+void vcos_quickslow_mutex_unlock_quick(VCOS_QUICKSLOW_MUTEX_T *m)
+{
+   vcos_mutex_unlock(m);
+}
+
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_reentrant_mtx.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_reentrant_mtx.h
new file mode 100644
index 0000000..ffb0b27
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_reentrant_mtx.h
@@ -0,0 +1,75 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - reentrant mutexes created from regular ones.
+=============================================================================*/
+
+#ifndef VCOS_GENERIC_REENTRANT_MUTEX_H
+#define VCOS_GENERIC_REENTRANT_MUTEX_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+
+/**
+ * \file
+ *
+ * Reentrant Mutexes from regular ones.
+ *
+ */
+
+typedef struct VCOS_REENTRANT_MUTEX_T
+{
+   VCOS_MUTEX_T mutex;
+   VCOS_THREAD_T *owner;
+   unsigned count;
+} VCOS_REENTRANT_MUTEX_T;
+
+/* Extern definitions of functions that do the actual work */
+
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_generic_reentrant_mutex_create(VCOS_REENTRANT_MUTEX_T *m, const char *name);
+
+VCOSPRE_ void VCOSPOST_ vcos_generic_reentrant_mutex_delete(VCOS_REENTRANT_MUTEX_T *m);
+
+VCOSPRE_ void VCOSPOST_ vcos_generic_reentrant_mutex_lock(VCOS_REENTRANT_MUTEX_T *m);
+
+VCOSPRE_ void VCOSPOST_ vcos_generic_reentrant_mutex_unlock(VCOS_REENTRANT_MUTEX_T *m);
+
+/* Inline forwarding functions */
+
+#if defined(VCOS_INLINE_BODIES)
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_reentrant_mutex_create(VCOS_REENTRANT_MUTEX_T *m, const char *name) {
+   return vcos_generic_reentrant_mutex_create(m,name);
+}
+
+VCOS_INLINE_IMPL
+void vcos_reentrant_mutex_delete(VCOS_REENTRANT_MUTEX_T *m) {
+   vcos_generic_reentrant_mutex_delete(m);
+}
+
+VCOS_INLINE_IMPL
+void vcos_reentrant_mutex_lock(VCOS_REENTRANT_MUTEX_T *m) {
+   vcos_generic_reentrant_mutex_lock(m);
+}
+
+VCOS_INLINE_IMPL
+void vcos_reentrant_mutex_unlock(VCOS_REENTRANT_MUTEX_T *m) {
+   vcos_generic_reentrant_mutex_unlock(m);
+}
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_tls.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_tls.h
new file mode 100644
index 0000000..3af975f
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_tls.h
@@ -0,0 +1,144 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - generic thread local storage
+=============================================================================*/
+
+#ifndef VCOS_GENERIC_TLS_H
+#define VCOS_GENERIC_TLS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+
+/**
+  * \file
+  *
+  * Do an emulation of Thread Local Storage. The platform needs to
+  * provide a way to set and get a per-thread pointer which is
+  * where the TLS data itself is stored.
+  *
+  *
+  * Each thread that wants to join in this scheme needs to call
+  * vcos_tls_thread_register().
+  *
+  * The platform needs to support the macros/functions
+  * _vcos_tls_thread_ptr_set() and _vcos_tls_thread_ptr_get().
+  */
+
+#ifndef VCOS_WANT_TLS_EMULATION
+#error Should not be included unless TLS emulation is defined
+#endif
+
+/** Number of slots to reserve per thread. This results in an overhead
+  * of this many words per thread.
+  */
+#define VCOS_TLS_MAX_SLOTS 4
+
+/** TLS key. Allocating one of these reserves the client one of the 
+  * available slots.
+  */
+typedef VCOS_UNSIGNED VCOS_TLS_KEY_T;
+
+/** TLS per-thread structure. Each thread gets one of these
+  * if TLS emulation (rather than native TLS support) is
+  * being used.
+  */
+typedef struct VCOS_TLS_THREAD_T
+{
+   void *slots[VCOS_TLS_MAX_SLOTS];
+} VCOS_TLS_THREAD_T;
+
+/*
+ * Internal APIs 
+ */
+
+/** Register this thread's TLS storage area. */
+VCOSPRE_ void VCOSPOST_ vcos_tls_thread_register(VCOS_TLS_THREAD_T *);
+
+/** Create a new TLS key */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_generic_tls_create(VCOS_TLS_KEY_T *key);
+
+/** Delete a TLS key */
+VCOSPRE_ void VCOSPOST_ vcos_generic_tls_delete(VCOS_TLS_KEY_T tls);
+
+/** Initialise the TLS library */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_tls_init(void);
+
+/** Deinitialise the TLS library */
+VCOSPRE_ void VCOSPOST_ vcos_tls_deinit(void);
+
+#if defined(VCOS_INLINE_BODIES)
+
+#undef VCOS_ASSERT_LOGGING_DISABLE
+#define VCOS_ASSERT_LOGGING_DISABLE 1
+
+/*
+ * Implementations of public API functions
+ */
+
+/** Set the given value. Since everything is per-thread, there is no need
+  * for any locking.
+  */
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_tls_set(VCOS_TLS_KEY_T tls, void *v) {
+   VCOS_TLS_THREAD_T *tlsdata = _vcos_tls_thread_ptr_get();
+   vcos_assert(tlsdata); /* Fires if this thread has not been registered */
+   if (tls<VCOS_TLS_MAX_SLOTS)
+   {
+      tlsdata->slots[tls] = v;
+      return VCOS_SUCCESS;
+   }
+   else
+   {
+      vcos_assert(0);
+      return VCOS_EINVAL;
+   }
+}
+
+/** Get the given value. No locking required.
+  */
+VCOS_INLINE_IMPL
+void *vcos_tls_get(VCOS_TLS_KEY_T tls) {
+   VCOS_TLS_THREAD_T *tlsdata = _vcos_tls_thread_ptr_get();
+   vcos_assert(tlsdata); /* Fires if this thread has not been registered */
+   if (tls<VCOS_TLS_MAX_SLOTS)
+   {
+      return tlsdata->slots[tls];
+   }
+   else
+   {
+      vcos_assert(0);
+      return NULL;
+   }
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_tls_create(VCOS_TLS_KEY_T *key) {
+   return vcos_generic_tls_create(key);
+}
+
+VCOS_INLINE_IMPL
+void vcos_tls_delete(VCOS_TLS_KEY_T tls) {
+   vcos_generic_tls_delete(tls);
+}
+
+#undef VCOS_ASSERT_LOGGING_DISABLE
+#define VCOS_ASSERT_LOGGING_DISABLE 0
+
+#endif /* VCOS_INLINE_BODIES */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_joinable_thread_from_plain.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_joinable_thread_from_plain.h
new file mode 100644
index 0000000..5387b9e
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_joinable_thread_from_plain.h
@@ -0,0 +1,202 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Module   :  vcos
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - implementation: joinable thread from plain
+=============================================================================*/
+
+/** \file
+  *
+  * Header file for platforms creating the joinable thread from a lowlevel
+  * thread.
+  *
+  * In addition to the actual thread, the following are also created:
+  *
+  * - a semaphore to wait on when joining the thread
+  * - a semaphore to support counted suspend/resume (used by event group)
+  * - a per-thread timer (used by event group, but could be removed)
+  */
+
+#ifndef VCOS_JOINABLE_THREAD_FROM_PLAIN_H
+#define VCOS_JOINABLE_THREAD_FROM_PLAIN_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_semaphore.h"
+#include "interface/vcos/vcos_lowlevel_thread.h"
+#include "interface/vcos/vcos_timer.h"
+
+#ifdef VCOS_WANT_TLS_EMULATION
+#include "interface/vcos/generic/vcos_generic_tls.h"
+#endif
+
+#define VCOS_THREAD_MAGIC 0x56436a74
+
+#define VCOS_THREAD_VALID(t) (t->magic == VCOS_THREAD_MAGIC)
+#define VCOS_HAVE_THREAD_AT_EXIT        1
+
+/** Thread attribute structure. Clients should not manipulate this directly, but
+  * should instead use the provided functions.
+  */
+typedef struct VCOS_THREAD_ATTR_T
+{
+   void *ta_stackaddr;
+   VCOS_UNSIGNED ta_stacksz;
+   VCOS_UNSIGNED ta_priority;
+   VCOS_UNSIGNED ta_affinity;
+   VCOS_UNSIGNED ta_timeslice;
+   VCOS_UNSIGNED legacy;
+   VCOS_UNSIGNED ta_autostart;
+} VCOS_THREAD_ATTR_T;
+
+/** Each thread gets a timer, which is for internal VCOS use.
+  */
+typedef struct _VCOS_THREAD_TIMER_T
+{
+   VCOS_TIMER_T timer;
+   void (*pfn)(void *);
+   void *cxt;
+} _VCOS_THREAD_TIMER_T;
+
+typedef void (*VCOS_THREAD_EXIT_HANDLER_T)(void *);
+/** Called at thread exit.
+  */
+typedef struct VCOS_THREAD_EXIT_T
+{
+   VCOS_THREAD_EXIT_HANDLER_T pfn;
+   void *cxt;
+} VCOS_THREAD_EXIT_T;
+#define VCOS_MAX_EXIT_HANDLERS  8
+
+/* The name field isn't used for anything, so we can just copy the
+ * the pointer. Nucleus makes its own copy.
+ */
+typedef const char *          VCOS_LLTHREAD_T_NAME;
+#define _VCOS_LLTHREAD_NAME(dst,src) (dst)=(src)
+
+/*
+ * Simulated TLS support
+ */
+
+
+/** Thread structure.
+  *
+  * \warning Do not access the members of this structure directly!
+  */
+typedef struct VCOS_THREAD_T
+{
+   VCOS_LLTHREAD_T  thread;      /**< The underlying thread */
+   char name[16];                /**< The name */
+   unsigned int     magic;       /**< For debug */
+   void            *exit_data;   /**< Exit data passed out in vcos_joinable_thread_exit() */
+   void            *stack;       /**< Stack, if not supplied by caller */
+   VCOS_SEMAPHORE_T wait;        /**< Semaphore to wait on at join */
+   VCOS_SEMAPHORE_T suspend;     /**< Semaphore to wait on for counted suspend */
+   int16_t          joined;      /**< Joined yet? For debug. */
+   VCOS_UNSIGNED    legacy;      /**< Use (argc,argv) for entry point arguments */
+   void *(*entry)(void*);        /**< Entry point */
+   void             *arg;        /**< Argument passed to entry point */
+   void *(*term)(void*);         /**< Termination function, used by reaper */
+   void             *term_arg;   /**< Argument passed to termination function */
+   _VCOS_THREAD_TIMER_T _timer;  /**< Internal timer, mainly for event groups */
+#ifdef VCOS_WANT_TLS_EMULATION
+   VCOS_TLS_THREAD_T   _tls;     /**< TLS data when native TLS not available, or NULL */
+#endif
+   /** Array of functions to call at thread exit */
+   VCOS_THREAD_EXIT_T at_exit[VCOS_MAX_EXIT_HANDLERS];
+
+   struct VCOS_THREAD_T *next;   /**< For linked lists of threads */
+} VCOS_THREAD_T;
+
+#if defined(VCOS_INLINE_BODIES)
+
+VCOS_INLINE_IMPL
+void vcos_thread_attr_setstack(VCOS_THREAD_ATTR_T *attrs, void *addr, VCOS_UNSIGNED stacksz) {
+   attrs->ta_stackaddr = addr;
+   attrs->ta_stacksz = stacksz;
+}
+
+VCOS_INLINE_IMPL
+void vcos_thread_attr_setstacksize(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED stacksz) {
+   attrs->ta_stacksz = stacksz;
+}
+
+VCOS_INLINE_IMPL
+void vcos_thread_attr_setpriority(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED pri) {
+   attrs->ta_priority = pri;
+}
+
+VCOS_INLINE_IMPL
+void vcos_thread_attr_setaffinity(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED affinity) {
+   attrs->ta_affinity = affinity;
+}
+
+VCOS_INLINE_IMPL
+void vcos_thread_attr_settimeslice(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED ts) {
+   attrs->ta_timeslice = ts;
+}
+
+VCOS_INLINE_IMPL
+void _vcos_thread_attr_setlegacyapi(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED legacy) {
+   attrs->legacy = legacy;
+}
+
+VCOS_INLINE_IMPL
+void vcos_thread_attr_setautostart(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED autostart) {
+   attrs->ta_autostart = autostart;
+}
+
+VCOS_INLINE_IMPL
+VCOS_THREAD_T *vcos_thread_current(void) {
+   VCOS_THREAD_T *ret =  (VCOS_THREAD_T*)vcos_llthread_current();
+   /*If we're called from a non-vcos thread, this assert will fail.
+    *XXX FIXME why is this commented out?
+    *vcos_assert(ret->magic == VCOS_THREAD_MAGIC);
+    */
+   return ret;
+}
+
+VCOS_INLINE_IMPL
+int vcos_thread_running(VCOS_THREAD_T *thread) {
+   return vcos_llthread_running(&thread->thread);
+}
+
+VCOS_INLINE_IMPL
+void vcos_thread_resume(VCOS_THREAD_T *thread) {
+   vcos_llthread_resume(&thread->thread);
+}
+
+#endif /* VCOS_INLINE_BODIES */
+
+/**
+  * \brief Create a VCOS_THREAD_T for the current thread. This is so we can have
+  * VCOS_THREAD_Ts even for threads not originally created by VCOS (eg the
+  * thread that calls vcos_init)
+  */
+extern VCOS_STATUS_T _vcos_thread_create_attach(VCOS_THREAD_T *thread,
+                                                const char *name);
+
+/**
+  * \brief Deletes the VCOS_THREAD_T, but does not wait for the underlying
+  * thread to exit. This will cleanup everything created by
+  * _vcos_thread_create_attach
+  */
+extern void _vcos_thread_delete(VCOS_THREAD_T *thread);
+
+/** Register a function to be called when the current thread exits.
+  */
+extern VCOS_STATUS_T vcos_thread_at_exit(void (*pfn)(void*), void *cxt);
+
+/** Deregister a previously registered at-exit function.
+  */
+extern void vcos_thread_deregister_at_exit(void (*pfn)(void*), void *cxt);
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* VCOS_JOINABLE_THREAD_FROM_PLAIN_H */
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_latch_from_sem.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_latch_from_sem.h
new file mode 100644
index 0000000..9ee5b4b
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_latch_from_sem.h
@@ -0,0 +1,48 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  vcos
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - Construct a latch from a semaphore
+=============================================================================*/
+
+/** FIXME: rename to vcos_mutex_from_sem.c
+  */
+
+typedef struct VCOS_MUTEX_T {
+   VCOS_SEMAPHORE_T sem;
+   struct VCOS_THREAD_T *owner;
+} VCOS_MUTEX_T;
+
+extern VCOS_STATUS_T vcos_generic_mutex_create(VCOS_MUTEX_T *latch, const char *name);
+extern void vcos_generic_mutex_delete(VCOS_MUTEX_T *latch);
+extern VCOS_STATUS_T vcos_generic_mutex_lock(VCOS_MUTEX_T *latch);
+extern void vcos_generic_mutex_unlock(VCOS_MUTEX_T *latch);
+
+#if defined(VCOS_INLINE_BODIES)
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_mutex_create(VCOS_MUTEX_T *latch, const char *name) {
+   return vcos_generic_mutex_create(latch,name);
+}
+
+VCOS_INLINE_IMPL
+void vcos_mutex_delete(VCOS_MUTEX_T *latch) {
+   vcos_generic_mutex_delete(latch);
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_mutex_lock(VCOS_MUTEX_T *latch) {
+   return vcos_generic_mutex_lock(latch);
+}
+
+VCOS_INLINE_IMPL
+void vcos_mutex_unlock(VCOS_MUTEX_T *latch) {
+   vcos_generic_mutex_unlock(latch);
+}
+
+#endif /* VCOS_INLINE_BODIES */
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_logcat.c b/drivers/misc/vc04_services/interface/vcos/generic/vcos_logcat.c
new file mode 100644
index 0000000..d513fab
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_logcat.c
@@ -0,0 +1,549 @@
+/*=============================================================================
+Copyright (c) 2010 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  vcos
+
+FILE DESCRIPTION
+Categorized logging for VCOS - a generic implementation.
+=============================================================================*/
+
+#include "interface/vcos/vcos.h"
+#include "interface/vcos/vcos_ctype.h"
+#include "interface/vcos/vcos_string.h"
+
+static VCOS_MUTEX_T lock;
+static int warned_loglevel;             /* only warn about invalid log level once */
+static VCOS_VLOG_IMPL_FUNC_T vcos_vlog_impl_func = vcos_vlog_default_impl;
+
+#define  VCOS_LOG_CATEGORY (&dflt_log_category)
+static VCOS_LOG_CAT_T dflt_log_category;
+VCOS_LOG_CAT_T *vcos_logging_categories = NULL;
+static int inited;
+
+#if VCOS_HAVE_CMD
+
+/*
+ * For kernel or videocore purposes, we generally want the log command. For 
+ * user-space apps, they might want to provide their own log command, so we 
+ * don't include the built in on. 
+ *  
+ * So pthreads/vcos_platform.h defines VCOS_WANT_LOG_CMD to be 0. It is 
+ * undefined elsewhere. 
+ */
+
+#  if !defined( VCOS_WANT_LOG_CMD )
+#     define  VCOS_WANT_LOG_CMD 1
+#  endif
+#else
+#  define VCOS_WANT_LOG_CMD   0
+#endif
+
+#if VCOS_WANT_LOG_CMD
+
+/*****************************************************************************
+*
+*   Does a vcos_assert(0), which is useful to test logging.
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_log_assert_cmd( VCOS_CMD_PARAM_T *param )
+{
+   (void)param;
+
+#if defined( NDEBUG ) && !defined( VCOS_RELEASE_ASSERTS )
+   vcos_log_error( "vcos_asserts have been compiled out" );
+   vcos_cmd_printf( param, "vcos_asserts have been compiled out - did a vcos_log_error instead\n" );
+#else
+   vcos_assert(0);
+   vcos_cmd_printf( param, "Executed vcos_assert(0)\n" );
+#endif
+
+   return VCOS_SUCCESS;
+}
+
+/*****************************************************************************
+*
+*   Sets a vcos logging level
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_log_set_cmd( VCOS_CMD_PARAM_T *param )
+{
+   VCOS_LOG_CAT_T   *cat;
+   char             *name;
+   char             *levelStr;
+   VCOS_LOG_LEVEL_T  level;
+   VCOS_STATUS_T     status;
+
+   if ( param->argc != 3 )
+   {
+      vcos_cmd_usage( param );
+      return VCOS_EINVAL;
+   }
+
+   name = param->argv[1];
+   levelStr = param->argv[2];
+
+   if ( vcos_string_to_log_level( levelStr, &level ) != VCOS_SUCCESS )
+   {
+      vcos_cmd_printf( param, "Unrecognized logging level: '%s'\n", levelStr );
+      return VCOS_EINVAL;
+   }
+
+   vcos_mutex_lock(&lock);
+
+   status = VCOS_SUCCESS;
+   for ( cat = vcos_logging_categories; cat != NULL; cat = cat->next )
+   {
+      if ( vcos_strcmp( name, cat->name ) == 0 )
+      {
+         cat->level = level;
+         vcos_cmd_printf( param, "Category %s level set to %s\n", name, levelStr );
+         break;
+      }
+   }
+   if ( cat == NULL )
+   {
+      vcos_cmd_printf( param, "Unrecognized category: '%s'\n", name );
+      status = VCOS_ENOENT;
+   }
+
+   vcos_mutex_unlock(&lock);
+
+   return status;
+}
+
+/*****************************************************************************
+*
+*   Prints out the current settings for a given category (or all cvategories)
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_log_status_cmd( VCOS_CMD_PARAM_T *param )
+{
+   VCOS_LOG_CAT_T   *cat;
+   VCOS_STATUS_T     status;
+
+   vcos_mutex_lock(&lock);
+
+   if ( param->argc == 1)
+   {
+      int   nw;
+      int   nameWidth = 0;
+
+      /* Print information about all of the categories. */
+
+      for ( cat = vcos_logging_categories; cat != NULL; cat = cat->next )
+      {
+         nw = (int)strlen( cat->name );
+
+         if ( nw > nameWidth )
+         {
+            nameWidth = nw;
+         }
+      }
+
+      for ( cat = vcos_logging_categories; cat != NULL; cat = cat->next )
+      {
+         vcos_cmd_printf( param, "%-*s - %s\n", nameWidth, cat->name, vcos_log_level_to_string( cat->level ));
+      }
+   }
+   else
+   {
+      /* Print information about a particular category */
+
+      for ( cat = vcos_logging_categories; cat != NULL; cat = cat->next )
+      {
+         if ( vcos_strcmp( cat->name, param->argv[1] ) == 0 )
+         {
+            vcos_cmd_printf( param, "%s - %s\n", cat->name, vcos_log_level_to_string( cat->level ));
+            break;
+         }
+      }
+      if ( cat == NULL )
+      {
+         vcos_cmd_printf( param, "Unrecognized logging category: '%s'\n", param->argv[1] );
+         status = VCOS_ENOENT;
+         goto out;
+      }
+   }
+
+   status = VCOS_SUCCESS;
+out:
+   vcos_mutex_unlock(&lock);
+
+   return status;
+}
+
+/*****************************************************************************
+*
+*   Prints out the current settings for a given category (or all cvategories)
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_log_test_cmd( VCOS_CMD_PARAM_T *param )
+{
+   if ( param->argc == 1 )
+   {
+      static   int seq_num = 100;
+
+      /* No additional arguments - generate a message with an incrementing number */
+
+      vcos_log_error( "Test message %d", seq_num );
+
+      seq_num++;
+      vcos_cmd_printf( param, "Logged 'Test message %d'\n", seq_num );
+   }
+   else
+   {
+      int   arg_idx;
+
+      /* Arguments supplied - log these */
+
+      for ( arg_idx = 0; arg_idx < param->argc; arg_idx++ )
+      {
+         vcos_log_error( "argv[%d] = '%s'", arg_idx, param->argv[arg_idx] );
+      }
+      vcos_cmd_printf( param, "Logged %d line(s) of test data\n", param->argc );
+   }
+   return VCOS_SUCCESS;
+}
+
+/*****************************************************************************
+*
+*   Internal commands
+*
+*****************************************************************************/
+
+static VCOS_CMD_T log_cmd_entry[] =
+{
+    { "assert",   "",                  vcos_log_assert_cmd, NULL,    "Does a vcos_assert(0) to test logging" },
+    { "set",      "category level",    vcos_log_set_cmd,    NULL,    "Sets the vcos logging level for a category" },
+    { "status",   "[category]",        vcos_log_status_cmd, NULL,    "Prints the vcos log status for a (or all) categories" },
+    { "test",     "[arbitrary text]",  vcos_log_test_cmd,   NULL,    "Does a vcos_log to test logging" },
+
+    { NULL,       NULL,                NULL,                NULL,    NULL }
+};
+
+static VCOS_CMD_T cmd_log =
+    { "log",        "command [args]",  NULL,    log_cmd_entry, "Commands related to vcos logging" };
+
+#endif
+
+void vcos_logging_init(void)
+{
+   if (inited)
+   {
+      /* FIXME: should print a warning or something here */
+      return;
+   }
+   vcos_mutex_create(&lock, "vcos_log");
+
+   vcos_log_platform_init();
+
+   vcos_log_register("default", &dflt_log_category);
+
+#if VCOS_WANT_LOG_CMD
+   vcos_cmd_register( &cmd_log );
+#endif
+
+   vcos_assert(!inited);
+   inited = 1;
+}
+
+/** Read an alphanumeric token, returning True if we succeeded.
+  */
+
+static int read_tok(char *tok, size_t toklen, const char **pstr, char sep)
+{
+   const char *str = *pstr;
+   size_t n = 0;
+   char ch;
+
+   /* skip past any whitespace */
+   while (str[0] && isspace((int)(str[0])))
+      str++;
+
+   while ((ch = *str) != '\0' &&
+          ch != sep &&
+          (isalnum((int)ch) || (ch == '_')) &&
+          n != toklen-1)
+   {
+      tok[n++] = ch;
+      str++;
+   }
+
+   /* did it work out? */
+   if (ch == '\0' || ch == sep)
+   {
+      if (ch) str++; /* move to next token if not at end */
+      /* yes */
+      tok[n] = '\0';
+      *pstr = str;
+      return 1;
+   }
+   else
+   {
+      /* no */
+      return 0;
+   }
+}
+
+const char *vcos_log_level_to_string( VCOS_LOG_LEVEL_T level )
+{
+   switch (level)
+   {
+      case VCOS_LOG_UNINITIALIZED:  return "uninit";
+      case VCOS_LOG_NEVER:          return "never";
+      case VCOS_LOG_ERROR:          return "error";
+      case VCOS_LOG_WARN:           return "warn";
+      case VCOS_LOG_INFO:           return "info";
+      case VCOS_LOG_TRACE:          return "trace";
+   }
+   return "???";
+}
+
+VCOS_STATUS_T vcos_string_to_log_level( const char *str, VCOS_LOG_LEVEL_T *level )
+{
+   if (strcmp(str,"error") == 0)
+      *level = VCOS_LOG_ERROR;
+   else if (strcmp(str,"never") == 0)
+      *level = VCOS_LOG_NEVER;
+   else if (strcmp(str,"warn") == 0)
+      *level = VCOS_LOG_WARN;
+   else if (strcmp(str,"warning") == 0)
+      *level = VCOS_LOG_WARN;
+   else if (strcmp(str,"info") == 0)
+      *level = VCOS_LOG_INFO;
+   else if (strcmp(str,"trace") == 0)
+      *level = VCOS_LOG_TRACE;
+   else
+      return VCOS_EINVAL;
+
+   return VCOS_SUCCESS;
+}
+
+static int read_level(VCOS_LOG_LEVEL_T *level, const char **pstr, char sep)
+{
+   char buf[16];
+   int ret = 1;
+   if (read_tok(buf,sizeof(buf),pstr,sep))
+   {
+      if (vcos_string_to_log_level(buf,level) != VCOS_SUCCESS)
+      {
+         vcos_log("Invalid trace level '%s'\n", buf);
+         ret = 0;
+      }
+   }
+   else
+   {
+      ret = 0;
+   }
+   return ret;
+}
+
+void vcos_log_register(const char *name, VCOS_LOG_CAT_T *category)
+{
+   const char *env;
+   VCOS_LOG_CAT_T *i;
+
+   category->name  = name;
+   if ( category->level == VCOS_LOG_UNINITIALIZED )
+   {
+      category->level = VCOS_LOG_ERROR;
+   }
+   category->flags.want_prefix = (category != &dflt_log_category );
+
+   vcos_mutex_lock(&lock);
+
+   /* is it already registered? */
+   for (i = vcos_logging_categories; i ; i = i->next )
+   {
+      if (i == category)
+      {
+         i->refcount++;
+         break;
+      }
+   }
+
+   if (!i)
+   {
+      /* not yet registered */
+      category->next = vcos_logging_categories;
+      vcos_logging_categories = category;
+      category->refcount++;
+
+      vcos_log_platform_register(category);
+   }
+
+   vcos_mutex_unlock(&lock);
+
+   /* Check to see if this log level has been enabled. Look for
+    * (<category:level>,)*
+    *
+    * VC_LOGLEVEL=ilcs:info,vchiq:warn
+    */
+
+   env = _VCOS_LOG_LEVEL();
+   if (env)
+   {
+      do
+      {
+         char env_name[64];
+         VCOS_LOG_LEVEL_T level;
+         if (read_tok(env_name, sizeof(env_name), &env, ':') &&
+             read_level(&level, &env, ','))
+         {
+            if (strcmp(env_name, name) == 0)
+            {
+               category->level = level;
+               break;
+            }
+         }
+         else
+         {
+            if (!warned_loglevel)
+            {
+                vcos_log("VC_LOGLEVEL format invalid at %s\n", env);
+                warned_loglevel = 1;
+            }
+            return;
+         }
+      } while (env[0] != '\0');
+   }
+
+   vcos_log_info( "Registered log category '%s' with level %s",
+                  category->name,
+                  vcos_log_level_to_string( category->level ));
+}
+
+void vcos_log_unregister(VCOS_LOG_CAT_T *category)
+{
+   VCOS_LOG_CAT_T **pcat;
+   vcos_mutex_lock(&lock);
+   category->refcount--;
+   if (category->refcount == 0)
+   {
+      pcat = &vcos_logging_categories;
+      while (*pcat != category)
+      {
+         if (!*pcat)
+            break;   /* possibly deregistered twice? */
+         if ((*pcat)->next == NULL)
+         {
+            vcos_assert(0); /* already removed! */
+            vcos_mutex_unlock(&lock);
+            return;
+         }
+         pcat = &(*pcat)->next;
+      }
+      if (*pcat)
+         *pcat = category->next;
+
+      vcos_log_platform_unregister(category);
+   }
+   vcos_mutex_unlock(&lock);
+}
+
+VCOSPRE_ const VCOS_LOG_CAT_T * VCOSPOST_ vcos_log_get_default_category(void)
+{
+   return &dflt_log_category;
+}
+
+void vcos_set_log_options(const char *opt)
+{
+   (void)opt;
+}
+
+void vcos_log_dump_mem_impl( const VCOS_LOG_CAT_T *cat,
+                             const char           *label,
+                             uint32_t              addr,
+                             const void           *voidMem,
+                             size_t                numBytes )
+{
+   const uint8_t  *mem = (const uint8_t *)voidMem;
+   size_t          offset;
+   char            lineBuf[ 100 ];
+   char           *s;
+
+   while ( numBytes > 0 )
+   {
+       s = lineBuf;
+
+       for ( offset = 0; offset < 16; offset++ )
+       {
+           if ( offset < numBytes )
+           {
+               s += vcos_snprintf( s, 4, "%02x ", mem[ offset ]);
+           }
+           else
+           {
+               s += vcos_snprintf( s, 4, "   " );
+           }
+       }
+
+       for ( offset = 0; offset < 16; offset++ )
+       {
+           if ( offset < numBytes )
+           {
+               uint8_t ch = mem[ offset ];
+
+               if (( ch < ' ' ) || ( ch > '~' ))
+               {
+                   ch = '.';
+               }
+               *s++ = (char)ch;
+           }
+       }
+       *s++ = '\0';
+
+       if (( label != NULL ) && ( *label != '\0' ))
+       {
+          vcos_log_impl( cat, VCOS_LOG_INFO, "%s: %08x: %s", label, addr, lineBuf );
+       }
+       else
+       {
+          vcos_log_impl( cat, VCOS_LOG_INFO, "%08x: %s", addr, lineBuf );
+       }
+
+       addr += 16;
+       mem += 16;
+       if ( numBytes > 16 )
+       {
+           numBytes -= 16;
+       }
+       else
+       {
+           numBytes = 0;
+       }
+   }
+
+}
+
+void vcos_log_impl(const VCOS_LOG_CAT_T *cat, VCOS_LOG_LEVEL_T _level, const char *fmt, ...)
+{
+   va_list ap;
+   va_start(ap,fmt);
+   vcos_vlog_impl( cat, _level, fmt, ap );
+   va_end(ap);
+}
+
+void vcos_vlog_impl(const VCOS_LOG_CAT_T *cat, VCOS_LOG_LEVEL_T _level, const char *fmt, va_list args)
+{
+   vcos_vlog_impl_func( cat, _level, fmt, args );
+}
+
+void vcos_set_vlog_impl( VCOS_VLOG_IMPL_FUNC_T vlog_impl_func )
+{
+   if ( vlog_impl_func == NULL )
+   {
+      vcos_vlog_impl_func = vcos_vlog_default_impl;
+   }
+   else
+   {
+      vcos_vlog_impl_func = vlog_impl_func;
+   }
+}
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.c b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.c
new file mode 100644
index 0000000..7f22051
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.c
@@ -0,0 +1,73 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  vcos
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - memory alloc implementation
+=============================================================================*/
+
+#include "interface/vcos/vcos.h"
+
+#ifndef _vcos_platform_malloc
+#include <stdlib.h>
+#define _vcos_platform_malloc malloc
+#define _vcos_platform_free   free
+#endif
+
+typedef struct malloc_header_s {
+   uint32_t guardword;
+   uint32_t size;
+   const char *description;
+   void *ptr;
+} MALLOC_HEADER_T;
+
+
+#define MIN_ALIGN sizeof(MALLOC_HEADER_T)
+
+#define GUARDWORDHEAP  0xa55a5aa5
+
+void *vcos_generic_mem_alloc_aligned(VCOS_UNSIGNED size, VCOS_UNSIGNED align, const char *desc)
+{
+   int local_align = align == 0 ? 1 : align;
+   int required_size = size + local_align + sizeof(MALLOC_HEADER_T);
+   void *ptr = _vcos_platform_malloc(required_size);
+   void *ret = (void *)VCOS_ALIGN_UP(((char *)ptr)+sizeof(MALLOC_HEADER_T), local_align);
+   MALLOC_HEADER_T *h = ((MALLOC_HEADER_T *)ret)-1;
+
+   h->size = size;
+   h->description = desc;
+   h->guardword = GUARDWORDHEAP;
+   h->ptr = ptr;
+
+   return ret;
+}
+
+void *vcos_generic_mem_alloc(VCOS_UNSIGNED size, const char *desc)
+{
+   return vcos_generic_mem_alloc_aligned(size,MIN_ALIGN,desc);
+}
+
+void *vcos_generic_mem_calloc(VCOS_UNSIGNED count, VCOS_UNSIGNED sz, const char *desc)
+{
+   uint32_t size = count*sz;
+   void *ptr = vcos_generic_mem_alloc_aligned(size,MIN_ALIGN,desc);
+   if (ptr)
+   {
+      memset(ptr, 0, size);
+   }
+   return ptr;
+}
+
+void vcos_generic_mem_free(void *ptr)
+{
+   MALLOC_HEADER_T *h;
+   if (! ptr) return;
+
+   h = ((MALLOC_HEADER_T *)ptr)-1;
+   vcos_assert(h->guardword == GUARDWORDHEAP);
+   _vcos_platform_free(h->ptr);
+}
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.h
new file mode 100644
index 0000000..1074dcc
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.h
@@ -0,0 +1,54 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  VMCS Host Apps
+Module   :  Framework - VMCS
+
+FILE DESCRIPTION
+Create the vcos_malloc API from the regular system malloc/free
+=============================================================================*/
+
+/**
+  * \file
+  *
+  * Create the vcos malloc API from a regular system malloc/free library.
+  *
+  * The API lets callers specify an alignment.
+  *
+  * Under VideoCore this is not needed, as we can simply use the rtos_malloc routines.
+  * But on host platforms that won't be the case.
+  *
+  */
+
+VCOSPRE_ void * VCOSPOST_  vcos_generic_mem_alloc(VCOS_UNSIGNED sz, const char *desc);
+VCOSPRE_  void * VCOSPOST_ vcos_generic_mem_calloc(VCOS_UNSIGNED count, VCOS_UNSIGNED sz, const char *descr);
+VCOSPRE_  void VCOSPOST_   vcos_generic_mem_free(void *ptr);
+VCOSPRE_  void * VCOSPOST_ vcos_generic_mem_alloc_aligned(VCOS_UNSIGNED sz, VCOS_UNSIGNED align, const char *desc);
+
+#ifdef VCOS_INLINE_BODIES
+
+VCOS_INLINE_IMPL
+void *vcos_malloc(VCOS_UNSIGNED size, const char *description) {
+   return vcos_generic_mem_alloc(size, description);
+}
+
+VCOS_INLINE_IMPL
+void *vcos_calloc(VCOS_UNSIGNED num, VCOS_UNSIGNED size, const char *description) {
+   return vcos_generic_mem_calloc(num, size, description);
+}
+
+VCOS_INLINE_IMPL
+void vcos_free(void *ptr) {
+   vcos_generic_mem_free(ptr);
+}
+
+VCOS_INLINE_IMPL
+void * vcos_malloc_aligned(VCOS_UNSIGNED size, VCOS_UNSIGNED align, const char *description) {
+   return vcos_generic_mem_alloc_aligned(size, align, description);
+}
+
+
+#endif /* VCOS_INLINE_BODIES */
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_mutexes_are_reentrant.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mutexes_are_reentrant.h
new file mode 100644
index 0000000..d10352c9
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mutexes_are_reentrant.h
@@ -0,0 +1,68 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - reentrant mutexes mapped directly to regular ones
+=============================================================================*/
+
+#ifndef VCOS_GENERIC_REENTRANT_MUTEX_H
+#define VCOS_GENERIC_REENTRANT_MUTEX_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "interface/vcos/vcos_mutex.h"
+
+/**
+ * \file
+ *
+ * Reentrant Mutexes directly using the native re-entrant mutex.
+ *
+ */
+
+typedef VCOS_MUTEX_T VCOS_REENTRANT_MUTEX_T;
+
+/* Inline forwarding functions */
+
+#if defined(VCOS_INLINE_BODIES)
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_reentrant_mutex_create(VCOS_REENTRANT_MUTEX_T *m, const char *name) {
+   return vcos_mutex_create(m,name);
+}
+
+VCOS_INLINE_IMPL
+void vcos_reentrant_mutex_delete(VCOS_REENTRANT_MUTEX_T *m) {
+   vcos_mutex_delete(m);
+}
+
+VCOS_INLINE_IMPL
+void vcos_reentrant_mutex_lock(VCOS_REENTRANT_MUTEX_T *m) {
+   vcos_mutex_lock(m);
+}
+
+VCOS_INLINE_IMPL
+void vcos_reentrant_mutex_unlock(VCOS_REENTRANT_MUTEX_T *m) {
+   vcos_mutex_unlock(m);
+}
+
+VCOS_INLINE_IMPL
+int vcos_reentrant_mutex_is_locked(VCOS_REENTRANT_MUTEX_T *m) {
+   return vcos_mutex_is_locked(m);
+}
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/generic/vcos_thread_reaper.h b/drivers/misc/vc04_services/interface/vcos/generic/vcos_thread_reaper.h
new file mode 100644
index 0000000..a8fc3da
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_thread_reaper.h
@@ -0,0 +1,35 @@
+/*=============================================================================
+Copyright (c) 2010 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  vcos
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - thread reaping
+=============================================================================*/
+
+#ifndef VCOS_THREAD_REAPER_H
+#define VCOS_THREAD_REAPER_H
+
+#define VCOS_HAVE_THREAD_REAPER
+
+/** Initialise the thread reaper.
+  */
+VCOS_STATUS_T vcos_thread_reaper_init(void);
+
+/** Reap a thread. Arranges for the thread to be automatically
+  * joined.
+  *
+  * @sa vcos_thread_join().
+  *
+  * @param thread           the thread to terminate
+  * @param on_terminated    called after the thread has exited
+  * @param cxt              pass back to the callback
+  *
+  */
+void vcos_thread_reap(VCOS_THREAD_T *thread, void (*on_terminated)(void*), void *cxt);
+
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/stdint.h b/drivers/misc/vc04_services/interface/vcos/linuxkernel/stdint.h
new file mode 100644
index 0000000..d3469af
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/stdint.h
@@ -0,0 +1,17 @@
+/*=============================================================================
+Copyright (c) 2010 Broadcom Europe Limited.
+All rights reserved.
+
+FILE DESCRIPTION
+VideoCore OS fAbstraction Layer - stdint.h C standard header
+=============================================================================*/
+
+#ifndef _VCOS_PLATFORM_LINUX_STDINT_H
+#define _VCOS_PLATFORM_LINUX_STDINT_H
+
+/* The Linux kernel does not have a <stdint.h> so we have to provide one of
+   our own. */
+
+#include <linux/types.h> /* includes integer types */
+
+#endif /* _VCOS_PLATFORM_LINUX_STDINT_H */
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel.c b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel.c
new file mode 100644
index 0000000..154f4a1
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel.c
@@ -0,0 +1,616 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  vcos
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - pthreads types
+=============================================================================*/
+
+#define  VCOS_INLINE_BODIES
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/time.h>
+#include <linux/pid.h>
+#include <linux/mm.h>
+#include <linux/version.h>
+
+#if defined( CONFIG_BCM_KNLLOG_SUPPORT )
+#include <linux/broadcom/knllog.h>
+#endif
+#include "interface/vcos/vcos.h"
+#ifdef HAVE_VCOS_VERSION
+#include "interface/vcos/vcos_build_info.h"
+#endif
+
+VCOS_CFG_ENTRY_T  vcos_cfg_dir;
+VCOS_CFG_ENTRY_T  vcos_logging_cfg_dir;
+VCOS_CFG_ENTRY_T  vcos_version_cfg;
+
+#ifndef VCOS_DEFAULT_STACK_SIZE
+#define VCOS_DEFAULT_STACK_SIZE 4096
+#endif
+
+static VCOS_THREAD_ATTR_T default_attrs = {
+   0,
+   VCOS_DEFAULT_STACK_SIZE,
+};
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,36)
+static DEFINE_SEMAPHORE(lock);
+#else
+static DECLARE_MUTEX(lock);
+#endif
+
+typedef void (*LEGACY_ENTRY_FN_T)(int, void *);
+
+/** Wrapper function around the real thread function. Posts the semaphore
+  * when completed.
+  */
+static int vcos_thread_wrapper(void *arg)
+{
+   void *ret;
+   VCOS_THREAD_T *thread = arg;
+
+   vcos_assert(thread->magic == VCOS_THREAD_MAGIC);
+
+   thread->thread.thread = current;
+
+   vcos_add_thread(thread);
+
+#ifdef VCOS_WANT_TLS_EMULATION
+   vcos_tls_thread_register(&thread->_tls);
+#endif
+
+   if (thread->legacy)
+   {
+      LEGACY_ENTRY_FN_T fn = (LEGACY_ENTRY_FN_T)thread->entry;
+      fn(0,thread->arg);
+      ret = 0;
+   }
+   else
+   {
+      ret = thread->entry(thread->arg);
+   }
+
+   thread->exit_data = ret;
+
+   vcos_remove_thread(current);
+
+   /* For join and cleanup */
+   vcos_semaphore_post(&thread->wait);
+
+   return 0;
+}
+
+VCOS_STATUS_T vcos_thread_create(VCOS_THREAD_T *thread,
+                                 const char *name,
+                                 VCOS_THREAD_ATTR_T *attrs,
+                                 VCOS_THREAD_ENTRY_FN_T entry,
+                                 void *arg)
+{
+   VCOS_STATUS_T st;
+   struct task_struct *kthread;
+
+   memset(thread, 0, sizeof(*thread));
+   thread->magic     = VCOS_THREAD_MAGIC;
+   strlcpy( thread->name, name, sizeof( thread->name ));
+   thread->legacy    = attrs ? attrs->legacy : 0;
+   thread->entry = entry;
+   thread->arg = arg;
+
+   if (!name)
+   {
+      vcos_assert(0);
+      return VCOS_EINVAL;
+   }
+
+   st = vcos_semaphore_create(&thread->wait, NULL, 0);
+   if (st != VCOS_SUCCESS)
+   {
+      return st;
+   }
+
+   st = vcos_semaphore_create(&thread->suspend, NULL, 0);
+   if (st != VCOS_SUCCESS)
+   {
+      return st;
+   }
+
+   /*required for event groups */
+   vcos_timer_create(&thread->_timer.timer, thread->name, NULL, NULL);
+
+   kthread = kthread_create((int (*)(void *))vcos_thread_wrapper, (void*)thread, name);
+   vcos_assert(kthread != NULL);
+   set_user_nice(kthread, attrs->ta_priority);
+   thread->thread.thread = kthread;
+   wake_up_process(kthread);
+   return VCOS_SUCCESS;
+}
+
+void vcos_thread_join(VCOS_THREAD_T *thread,
+                             void **pData)
+{
+   vcos_assert(thread);
+   vcos_assert(thread->magic == VCOS_THREAD_MAGIC);
+
+   thread->joined = 1;
+
+   vcos_semaphore_wait(&thread->wait);
+
+   if (pData)
+   {
+      *pData = thread->exit_data;
+   }
+
+   /* Clean up */
+   if (thread->stack)
+      vcos_free(thread->stack);
+
+   vcos_semaphore_delete(&thread->wait);
+   vcos_semaphore_delete(&thread->suspend);
+
+}
+
+uint32_t vcos_getmicrosecs( void )
+{
+   struct timeval tv;
+/*XXX FIX ME! switch to ktime_get_ts to use MONOTONIC clock */
+   do_gettimeofday(&tv);
+   return (tv.tv_sec*1000000) + tv.tv_usec;
+}
+
+VCOS_STATUS_T vcos_timer_init(void)
+{
+    return VCOS_SUCCESS;
+}
+
+static const char *log_prefix[] =
+{
+   "",            /* VCOS_LOG_UNINITIALIZED */
+   "",            /* VCOS_LOG_NEVER */
+   KERN_ERR,      /* VCOS_LOG_ERROR */
+   KERN_WARNING,  /* VCOS_LOG_WARN */
+   KERN_INFO,     /* VCOS_LOG_INFO */
+   KERN_INFO      /* VCOS_LOG_TRACE */
+};
+
+void vcos_vlog_default_impl(const VCOS_LOG_CAT_T *cat, VCOS_LOG_LEVEL_T _level, const char *fmt, va_list args)
+{
+   char *newline = strchr( fmt, '\n' );
+   const char  *prefix;
+   const char  *real_fmt;
+
+   preempt_disable();
+   {
+       if ( *fmt == '<' )
+       {
+           prefix = fmt;
+           real_fmt= &fmt[3];
+       }
+       else
+       {
+          prefix = log_prefix[_level];
+          real_fmt = fmt;
+       }
+#if defined( CONFIG_BCM_KNLLOG_SUPPORT )
+       knllog_ventry( "vcos", real_fmt, args );
+#endif
+       printk( "%.3svcos: [%d]: ", prefix, current->pid );
+       vprintk( real_fmt, args );
+
+       if ( newline == NULL )
+       {
+          printk("\n");
+       }
+   }
+   preempt_enable();
+}
+
+
+const char * _vcos_log_level(void)
+{
+   return NULL;
+}
+
+/*****************************************************************************
+*
+*    Displays the version information in /proc/vcos/version
+*
+*****************************************************************************/
+
+#ifdef HAVE_VCOS_VERSION
+
+static void show_version( VCOS_CFG_BUF_T buf, void *data )
+{
+   static const char* copyright = "Copyright (c) 2011 Broadcom";
+
+   vcos_cfg_buf_printf( buf, "Built %s %s on %s\n%s\nversion %s\n",
+                        vcos_get_build_date(),
+                        vcos_get_build_time(),
+                        vcos_get_build_hostname(),
+                        copyright,
+                        vcos_get_build_version() );
+}
+
+#endif
+
+/*****************************************************************************
+*
+*    Initialises vcos
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_init(void)
+{
+   if ( vcos_cfg_mkdir( &vcos_cfg_dir, NULL, "vcos" ) != VCOS_SUCCESS )
+   {
+      printk( KERN_ERR "%s: Unable to create vcos cfg entry\n", __func__ );
+   }
+   vcos_logging_init();
+
+#ifdef HAVE_VCOS_VERSION
+   if ( vcos_cfg_create_entry( &vcos_version_cfg, &vcos_cfg_dir, "version",
+                               show_version, NULL, NULL ) != VCOS_SUCCESS )
+   {
+      printk( KERN_ERR "%s: Unable to create vcos cfg entry 'version'\n", __func__ );
+   }
+#endif
+
+   return VCOS_SUCCESS;
+}
+
+/*****************************************************************************
+*
+*    Deinitializes vcos
+*
+*****************************************************************************/
+
+void vcos_deinit(void)
+{
+#ifdef HAVE_VCOS_VERSION
+   vcos_cfg_remove_entry( &vcos_version_cfg );
+#endif
+   vcos_cfg_remove_entry( &vcos_cfg_dir );
+}
+
+void vcos_global_lock(void)
+{
+   down(&lock);
+}
+
+void vcos_global_unlock(void)
+{
+   up(&lock);
+}
+
+/* vcos_thread_exit() doesn't really stop this thread here
+ *
+ * At the moment, call to do_exit() will leak task_struct for
+ * current thread, so we let the vcos_thread_wrapper() do the
+ * cleanup and exit job, and we return w/o actually stopping the thread.
+ *
+ * ToDo: Kernel v2.6.31 onwards, it is considered safe to call do_exit()
+ * from kthread, the implementation of which is combined in 2 patches
+ * with commit-ids "63706172" and "cdd140bd" in oss Linux kernel tree
+ */
+
+void vcos_thread_exit(void *arg)
+{
+   VCOS_THREAD_T *thread = vcos_thread_current();
+
+   vcos_assert(thread);
+   vcos_assert(thread->magic == VCOS_THREAD_MAGIC);
+
+   thread->exit_data = arg;
+}
+
+void vcos_thread_attr_init(VCOS_THREAD_ATTR_T *attrs)
+{
+   *attrs = default_attrs;
+}
+
+void _vcos_task_timer_set(void (*pfn)(void *), void *cxt, VCOS_UNSIGNED ms)
+{
+   VCOS_THREAD_T *self = vcos_thread_current();
+   vcos_assert(self);
+   vcos_assert(self->_timer.pfn == NULL);
+
+   vcos_timer_create( &self->_timer.timer, "TaskTimer", pfn, cxt );
+   vcos_timer_set(&self->_timer.timer, ms);
+}
+
+void _vcos_task_timer_cancel(void)
+{
+   VCOS_THREAD_T *self = vcos_thread_current();
+   if (self->_timer.timer.linux_timer.function)
+   {
+      vcos_timer_cancel(&self->_timer.timer);
+      vcos_timer_delete(&self->_timer.timer);
+   }
+}
+
+int vcos_vsnprintf( char *buf, size_t buflen, const char *fmt, va_list ap )
+{
+   return vsnprintf( buf, buflen, fmt, ap );
+}
+
+int vcos_snprintf(char *buf, size_t buflen, const char *fmt, ...)
+{
+   int ret;
+   va_list ap;
+   va_start(ap,fmt);
+   ret = vsnprintf(buf, buflen, fmt, ap);
+   va_end(ap);
+   return ret;
+}
+
+int vcos_llthread_running(VCOS_LLTHREAD_T *t) {
+   vcos_assert(0);   /* this function only exists as a nasty hack for the video codecs! */
+   return 1;
+}
+
+static int vcos_verify_bkpts = 1;
+
+int vcos_verify_bkpts_enabled(void)
+{
+   return vcos_verify_bkpts;
+}
+
+/*****************************************************************************
+*
+*    _vcos_log_platform_init is called from vcos_logging_init
+*
+*****************************************************************************/
+
+void _vcos_log_platform_init(void)
+{
+   if ( vcos_cfg_mkdir( &vcos_logging_cfg_dir, &vcos_cfg_dir, "logging" ) != VCOS_SUCCESS )
+   {
+      printk( KERN_ERR "%s: Unable to create logging cfg entry\n", __func__ );
+   }
+}
+
+/*****************************************************************************
+*
+*    Called to display the contents of a logging category.
+*
+*****************************************************************************/
+
+static void logging_show_category( VCOS_CFG_BUF_T buf, void *data )
+{
+   VCOS_LOG_CAT_T *category = data;
+
+   vcos_cfg_buf_printf( buf, "%s\n", vcos_log_level_to_string( category->level ));
+}
+
+/*****************************************************************************
+*
+*    Called to parse content for a logging category.
+*
+*****************************************************************************/
+
+static void logging_parse_category( VCOS_CFG_BUF_T buf, void *data )
+{
+   VCOS_LOG_CAT_T *category = data;
+   const char *str = vcos_cfg_buf_get_str( buf );
+   VCOS_LOG_LEVEL_T  level;
+
+   if ( vcos_string_to_log_level( str, &level ) == VCOS_SUCCESS )
+   {
+      category->level = level;
+   }
+   else
+   {
+      printk( KERN_ERR "%s: Unrecognized logging level: '%s'\n",
+              __func__, str );
+   }
+}
+
+/*****************************************************************************
+*
+*    _vcos_log_platform_register is called from vcos_log_register whenever
+*    a new category is registered.
+*
+*****************************************************************************/
+
+void _vcos_log_platform_register(VCOS_LOG_CAT_T *category)
+{
+   VCOS_CFG_ENTRY_T  entry;
+
+   if ( vcos_cfg_create_entry( &entry, &vcos_logging_cfg_dir, category->name,
+                               logging_show_category, logging_parse_category,
+                               category ) != VCOS_SUCCESS )
+   {
+      printk( KERN_ERR "%s: Unable to create cfg entry for logging category '%s'\n",
+              __func__, category->name );
+      category->platform_data = NULL;
+   }
+   else
+   {
+      category->platform_data = entry;
+   }
+}
+
+/*****************************************************************************
+*
+*    _vcos_log_platform_unregister is called from vcos_log_unregister whenever
+*    a new category is unregistered.
+*
+*****************************************************************************/
+
+void _vcos_log_platform_unregister(VCOS_LOG_CAT_T *category)
+{
+   VCOS_CFG_ENTRY_T  entry;
+
+   entry = category->platform_data;
+   if ( entry != NULL )
+   {
+      if ( vcos_cfg_remove_entry( &entry ) != VCOS_SUCCESS )
+      {
+         printk( KERN_ERR "%s: Unable to remove cfg entry for logging category '%s'\n",
+                 __func__, category->name );
+      }
+   }
+}
+
+/*****************************************************************************
+*
+*    Allocate memory.
+*
+*****************************************************************************/
+
+void *vcos_platform_malloc( VCOS_UNSIGNED required_size )
+{
+   if ( required_size >= ( 2 * PAGE_SIZE ))
+   {
+      /* For larger allocations, use vmalloc, whose underlying allocator
+       * returns pages
+       */
+
+      return vmalloc( required_size );
+   }
+
+   /* For smaller allocation, use kmalloc */
+
+   return kmalloc( required_size, GFP_KERNEL );
+}
+
+/*****************************************************************************
+*
+*    Free previously allocated memory
+*
+*****************************************************************************/
+
+void  vcos_platform_free( void *ptr )
+{
+   if (((unsigned long)ptr >= VMALLOC_START )
+   &&  ((unsigned long)ptr < VMALLOC_END ))
+   {
+      vfree( ptr );
+   }
+   else
+   {
+      kfree( ptr );
+   }
+}
+
+/*****************************************************************************
+*
+*    Execute a routine exactly once.
+*
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_once(VCOS_ONCE_T *once_control,
+                        void (*init_routine)(void))
+{
+   /* In order to be thread-safe we need to re-test *once_control
+    * inside the lock. The outer test is basically an optimization
+    * so that once it is initialized we don't need to waste time
+    * trying to acquire the lock.
+    */
+
+   if ( *once_control == 0 )
+   {
+       vcos_global_lock();
+       if ( *once_control == 0 )
+       {
+           init_routine();
+           *once_control = 1;
+       }
+       vcos_global_unlock();
+   }
+
+   return VCOS_SUCCESS;
+}
+
+/*****************************************************************************
+*
+*    String duplication routine.
+*
+*****************************************************************************/
+
+char *vcos_strdup(const char *str)
+{
+    return kstrdup(str, GFP_KERNEL);
+}
+
+
+/* Export functions for modules to use */
+EXPORT_SYMBOL( vcos_init );
+
+EXPORT_SYMBOL( vcos_semaphore_trywait );
+EXPORT_SYMBOL( vcos_semaphore_post );
+EXPORT_SYMBOL( vcos_semaphore_create );
+EXPORT_SYMBOL( vcos_semaphore_wait );
+EXPORT_SYMBOL( vcos_semaphore_delete );
+
+EXPORT_SYMBOL( vcos_log_impl );
+EXPORT_SYMBOL( vcos_vlog_impl );
+EXPORT_SYMBOL( vcos_vlog_default_impl );
+EXPORT_SYMBOL( vcos_log_get_default_category );
+EXPORT_SYMBOL( vcos_log_register );
+EXPORT_SYMBOL( vcos_log_unregister );
+EXPORT_SYMBOL( vcos_logging_init );
+EXPORT_SYMBOL( vcos_log_level_to_string );
+EXPORT_SYMBOL( vcos_string_to_log_level );
+EXPORT_SYMBOL( vcos_log_dump_mem_impl );
+
+EXPORT_SYMBOL( vcos_event_create );
+EXPORT_SYMBOL( vcos_event_delete );
+EXPORT_SYMBOL( vcos_event_flags_set );
+EXPORT_SYMBOL( vcos_event_signal );
+EXPORT_SYMBOL( vcos_event_wait );
+EXPORT_SYMBOL( vcos_event_try );
+
+EXPORT_SYMBOL( vcos_getmicrosecs );
+
+EXPORT_SYMBOL( vcos_strcasecmp );
+EXPORT_SYMBOL( vcos_snprintf );
+EXPORT_SYMBOL( vcos_vsnprintf );
+
+EXPORT_SYMBOL( vcos_thread_current );
+EXPORT_SYMBOL( vcos_thread_join );
+EXPORT_SYMBOL( vcos_thread_create );
+EXPORT_SYMBOL( vcos_thread_set_priority );
+EXPORT_SYMBOL( vcos_thread_exit );
+EXPORT_SYMBOL( vcos_once );
+
+EXPORT_SYMBOL( vcos_thread_attr_init );
+EXPORT_SYMBOL( vcos_thread_attr_setpriority );
+EXPORT_SYMBOL( vcos_thread_attr_settimeslice );
+EXPORT_SYMBOL( vcos_thread_attr_setstacksize );
+EXPORT_SYMBOL( _vcos_thread_attr_setlegacyapi );
+
+EXPORT_SYMBOL( vcos_event_flags_create );
+EXPORT_SYMBOL( vcos_event_flags_delete );
+EXPORT_SYMBOL( vcos_event_flags_get );
+
+EXPORT_SYMBOL( vcos_sleep );
+
+EXPORT_SYMBOL( vcos_calloc );
+EXPORT_SYMBOL( vcos_malloc );
+EXPORT_SYMBOL( vcos_malloc_aligned );
+EXPORT_SYMBOL( vcos_free );
+
+EXPORT_SYMBOL( vcos_mutex_create );
+EXPORT_SYMBOL( vcos_mutex_delete );
+EXPORT_SYMBOL( vcos_mutex_lock );
+EXPORT_SYMBOL( vcos_mutex_unlock );
+EXPORT_SYMBOL( vcos_mutex_trylock );
+
+EXPORT_SYMBOL( vcos_timer_cancel );
+EXPORT_SYMBOL( vcos_timer_create );
+EXPORT_SYMBOL( vcos_timer_delete );
+EXPORT_SYMBOL( vcos_timer_set );
+
+EXPORT_SYMBOL( vcos_atomic_flags_create );
+EXPORT_SYMBOL( vcos_atomic_flags_delete );
+EXPORT_SYMBOL( vcos_atomic_flags_or );
+EXPORT_SYMBOL( vcos_atomic_flags_get_and_clear );
+
+EXPORT_SYMBOL( vcos_verify_bkpts_enabled );
+
+EXPORT_SYMBOL( vcos_strdup );
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_cfg.c b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_cfg.c
new file mode 100644
index 0000000..e3f0840
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_cfg.c
@@ -0,0 +1,332 @@
+/*****************************************************************************
+* Copyright 2009 - 2010 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+#include "interface/vcos/vcos.h"
+#include <linux/module.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <asm/uaccess.h>
+
+struct opaque_vcos_cfg_buf_t
+{
+    struct seq_file *seq;
+    char            *charBuf;
+};
+
+struct opaque_vcos_cfg_entry_t
+{
+    struct proc_dir_entry *pde;
+    struct proc_dir_entry *parent_pde;
+    VCOS_CFG_SHOW_FPTR     showFunc;
+    VCOS_CFG_PARSE_FPTR    parseFunc;
+    void                  *data;
+    const char            *name;
+};
+
+/***************************************************************************** 
+* 
+*    cfg_proc_show
+*  
+*****************************************************************************/
+
+static int cfg_proc_show( struct seq_file *s, void *v )
+{
+    VCOS_CFG_ENTRY_T                entry;
+    struct opaque_vcos_cfg_buf_t    buf;
+
+    entry = s->private;
+
+    if ( entry->showFunc )
+    {
+        memset( &buf, 0, sizeof( buf ));
+        buf.seq = s;
+
+        entry->showFunc( &buf, entry->data );
+    }
+
+    return 0;
+}
+
+/***************************************************************************** 
+* 
+*    cfg_proc_write
+*  
+*****************************************************************************/
+
+static ssize_t cfg_proc_write( struct file *file, const char __user *buffer, size_t count, loff_t *ppos)
+{
+    VCOS_CFG_ENTRY_T                entry = PDE(file->f_path.dentry->d_inode)->data;
+    char                           *charBuf;
+    struct opaque_vcos_cfg_buf_t    buf;
+    size_t                          len;
+
+    if ( entry->parseFunc != NULL )
+    {
+        /* The number 4000 is rather arbitrary. It just needs to be bigger than any input
+         * string we expect to use.
+         */
+
+        len = count;
+        if ( count > 4000 )
+        {
+            len = 4000;
+        }
+
+        /* Allocate a kernel buffer to contain the string being written. */
+
+        charBuf = kmalloc( len + 1, GFP_KERNEL );
+        if ( copy_from_user( charBuf, buffer, len ))
+        {
+            kfree( charBuf );
+            return -EFAULT;
+        }
+
+        /* echo puts a trailing newline in the buffer - strip it out. */
+
+        if (( len > 0 ) && ( charBuf[ len - 1 ] == '\n' ))
+        {
+            len--;
+        }
+        charBuf[len] = '\0';
+
+        memset( &buf, 0, sizeof( buf ));
+        buf.charBuf = charBuf;
+
+        entry->parseFunc( &buf, entry->data );
+        kfree( charBuf );
+    }
+    return count;
+}
+
+/***************************************************************************** 
+* 
+*    cfg_proc_open
+*  
+*****************************************************************************/
+
+static int cfg_proc_open( struct inode *inode, struct file *file )
+{
+    return single_open( file, cfg_proc_show, PDE(inode)->data );
+}
+
+static const struct file_operations cfg_proc_fops = 
+{
+    .open       = cfg_proc_open,
+    .read       = seq_read,
+    .llseek     = seq_lseek,
+    .release    = single_release,
+    .write      = cfg_proc_write,
+};
+
+/***************************************************************************** 
+* 
+*    vcos_cfg_mkdir
+*  
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_cfg_mkdir( VCOS_CFG_ENTRY_T *entryp,
+                              VCOS_CFG_ENTRY_T *parent,
+                              const char *dirName )
+{
+    VCOS_CFG_ENTRY_T    entry;
+
+    if (( entry = kzalloc( sizeof( *entry ), GFP_KERNEL )) == NULL )
+    {
+        return VCOS_ENOMEM;
+    }
+
+    if ( parent == NULL )
+    {
+        entry->pde = proc_mkdir( dirName, NULL );
+    }
+    else
+    {
+        entry->pde = proc_mkdir( dirName, (*parent)->pde );
+        entry->parent_pde = (*parent)->pde;
+    }
+    if ( entry->pde == NULL )
+    {
+        kfree( entry );
+        return VCOS_ENOMEM;
+    }
+
+    entry->name = dirName;
+
+    *entryp = entry;
+    return VCOS_SUCCESS;
+}
+
+/***************************************************************************** 
+* 
+*    vcos_cfg_create_entry
+*  
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_cfg_create_entry( VCOS_CFG_ENTRY_T *entryp,
+                                     VCOS_CFG_ENTRY_T *parent,
+                                     const char *entryName,
+                                     VCOS_CFG_SHOW_FPTR showFunc,
+                                     VCOS_CFG_PARSE_FPTR parseFunc,
+                                     void *data )
+{
+    VCOS_CFG_ENTRY_T    entry;
+    mode_t              mode;
+
+    *entryp = NULL;
+
+    if (( entry = kzalloc( sizeof( *entry ), GFP_KERNEL )) == NULL )
+    {
+        return VCOS_ENOMEM;
+    }
+
+    mode = 0;
+    if ( showFunc != NULL )
+    {
+        mode |= 0444;
+    }
+    if ( parseFunc != NULL )
+    {
+        mode |= 0200;
+    }
+    
+    if ( parent == NULL )
+    {
+        entry->pde = create_proc_entry( entryName, mode, NULL );
+    }
+    else
+    {
+        entry->pde = create_proc_entry( entryName, mode, (*parent)->pde );
+        entry->parent_pde = (*parent)->pde;
+    }
+    if ( entry->pde == NULL )
+    {
+        kfree( entry );
+        return -ENOMEM;
+    }
+    entry->showFunc = showFunc;
+    entry->parseFunc = parseFunc;
+    entry->data = data;
+    entry->name = entryName;
+
+    entry->pde->data = entry;
+    entry->pde->proc_fops = &cfg_proc_fops;
+    
+    *entryp = entry;
+    return VCOS_SUCCESS;    
+}
+
+/***************************************************************************** 
+* 
+*    vcos_cfg_remove_entry
+*  
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_cfg_remove_entry( VCOS_CFG_ENTRY_T *entryp )
+{
+    if (( entryp != NULL ) && ( *entryp != NULL ))
+    {
+        remove_proc_entry( (*entryp)->name, (*entryp)->parent_pde );
+
+        kfree( *entryp );
+        *entryp = NULL;
+    }
+
+    return VCOS_SUCCESS;
+}
+
+/***************************************************************************** 
+* 
+*    vcos_cfg_is_entry_created
+*  
+*****************************************************************************/
+
+int vcos_cfg_is_entry_created( VCOS_CFG_ENTRY_T entry )
+{
+    return ( entry != NULL ) && ( entry->pde != NULL );
+}
+
+/***************************************************************************** 
+* 
+*    vcos_cfg_buf_printf
+*  
+*****************************************************************************/
+
+void vcos_cfg_buf_printf( VCOS_CFG_BUF_T buf, const char *fmt, ... )
+{
+    struct seq_file *m = buf->seq;
+
+    /* Bah - there is no seq_vprintf */
+
+    va_list args;
+    int len;
+
+    if (m->count < m->size) {
+        va_start(args, fmt);
+        len = vsnprintf(m->buf + m->count, m->size - m->count, fmt, args);
+        va_end(args);
+        if (m->count + len < m->size) {
+            m->count += len;
+            return;
+        }
+    }
+    m->count = m->size;
+}
+
+/***************************************************************************** 
+* 
+*    vcos_cfg_buf_get_str
+*  
+*****************************************************************************/
+
+char *vcos_cfg_buf_get_str( VCOS_CFG_BUF_T buf )
+{
+    return buf->charBuf;
+}
+
+/***************************************************************************** 
+* 
+*    vcos_cfg_get_proc_entry
+*  
+*    This function is only created for a couple of backwards compatibility '
+*    issues and shouldn't normally be used.
+*  
+*****************************************************************************/
+
+void *vcos_cfg_get_proc_entry( VCOS_CFG_ENTRY_T entry )
+{
+    return entry->pde;
+}
+
+/***************************************************************************** 
+* 
+*    vcos_cfg_get_entry_name
+*  
+*****************************************************************************/
+
+const char *vcos_cfg_get_entry_name( VCOS_CFG_ENTRY_T entry )
+{
+   return entry->pde->name;
+}
+
+
+EXPORT_SYMBOL( vcos_cfg_mkdir );
+EXPORT_SYMBOL( vcos_cfg_create_entry );
+EXPORT_SYMBOL( vcos_cfg_remove_entry );
+EXPORT_SYMBOL( vcos_cfg_get_entry_name );
+EXPORT_SYMBOL( vcos_cfg_is_entry_created );
+EXPORT_SYMBOL( vcos_cfg_buf_printf );
+EXPORT_SYMBOL( vcos_cfg_buf_get_str );
+
+EXPORT_SYMBOL_GPL( vcos_cfg_get_proc_entry );
+
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_misc.c b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_misc.c
new file mode 100644
index 0000000..4d934c38
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_misc.c
@@ -0,0 +1,113 @@
+// #############################################################################
+// START #######################################################################
+/*****************************************************************************
+* Copyright 2009 - 2010 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+#include "interface/vcos/vcos.h"
+#include <linux/sched.h>
+#include <linux/module.h>
+#include <linux/freezer.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+
+/***************************************************************************** 
+* 
+*        vcos_semaphore_wait_freezable
+*  
+*****************************************************************************/
+
+VCOS_STATUS_T vcos_semaphore_wait_freezable(VCOS_SEMAPHORE_T *sem)
+{
+    int rval, sig_pended = 0;
+    unsigned long flags;
+    struct task_struct *task = current;
+
+    while (1) {
+       rval = down_interruptible((struct semaphore *)sem);
+       if (rval == 0) { /* down now */
+          break;
+       } else {
+          if (freezing(current)) {
+             try_to_freeze();
+          } else {
+             spin_lock_irqsave(&task->sighand->siglock, flags);
+             if (test_tsk_thread_flag(task, TIF_SIGPENDING)) {
+                clear_tsk_thread_flag(task, TIF_SIGPENDING);
+                sig_pended = 1;
+             }
+             spin_unlock_irqrestore(&task->sighand->siglock, flags);
+          }
+       }
+    }
+
+    if (sig_pended) {
+       spin_lock_irqsave(&task->sighand->siglock, flags);
+       set_tsk_thread_flag(task, TIF_SIGPENDING);
+       spin_unlock_irqrestore(&task->sighand->siglock, flags);
+    }
+
+    return 0;
+}
+
+EXPORT_SYMBOL( vcos_semaphore_wait_freezable );
+
+/***************************************************************************** 
+* 
+*  vcos_kmalloc
+*  
+*  We really need to convert malloc to do kmalloc or vmalloc based on the
+*  size, but for now we'll add a separate function.
+*  
+*****************************************************************************/
+
+void *vcos_kmalloc(VCOS_UNSIGNED size, const char *description)
+{
+   (void)description;
+
+   return kmalloc( size, GFP_KERNEL );
+}
+
+/***************************************************************************** 
+* 
+*  vcos_kmalloc
+*  
+*  We really need to convert malloc to do kmalloc or vmalloc based on the
+*  size, but for now we'll add a separate function.
+*  
+*****************************************************************************/
+
+void *vcos_kcalloc(VCOS_UNSIGNED num, VCOS_UNSIGNED size, const char *description)
+{
+   (void)description;
+
+   return kzalloc( num * size, GFP_KERNEL );
+}
+
+/***************************************************************************** 
+* 
+*  vcos_kfree
+*  
+*****************************************************************************/
+
+void vcos_kfree(void *ptr)
+{
+   kfree( ptr );
+}
+
+EXPORT_SYMBOL( vcos_kmalloc );
+EXPORT_SYMBOL( vcos_kcalloc );
+EXPORT_SYMBOL( vcos_kfree );
+
+// END #########################################################################
+// #############################################################################
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_mod_init.c b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_mod_init.c
new file mode 100644
index 0000000..6ffd755
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_mod_init.c
@@ -0,0 +1,64 @@
+/*****************************************************************************
+* Copyright 2006 - 2008 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL"). 
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+****************************************************************************/
+
+/* ---- Include Files ---------------------------------------------------- */
+
+#include "interface/vcos/vcos.h"
+#include <linux/module.h>
+
+/* ---- Public Variables ------------------------------------------------- */
+
+/* ---- Private Constants and Types -------------------------------------- */
+
+/* ---- Private Variables ------------------------------------------------ */
+
+/* ---- Private Function Prototypes -------------------------------------- */
+
+/* ---- Functions -------------------------------------------------------- */
+
+/****************************************************************************
+*
+*   Called to perform module initialization when the module is loaded
+*
+***************************************************************************/
+
+static int __init vcos_mod_init( void )
+{
+    printk( KERN_INFO "VCOS Module\n" );
+
+    vcos_init();
+    return 0;
+}
+
+/****************************************************************************
+*
+*   Called to perform module cleanup when the module is unloaded.
+*
+***************************************************************************/
+
+static void __exit vcos_mod_exit( void )
+{
+    vcos_deinit();
+}
+
+/****************************************************************************/
+
+module_init( vcos_mod_init );
+module_exit( vcos_mod_exit );
+
+MODULE_AUTHOR("Broadcom");
+MODULE_DESCRIPTION( "VCOS Module Functions" );
+MODULE_LICENSE( "GPL" );
+MODULE_VERSION( "1.0" );
+
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform.h b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform.h
new file mode 100644
index 0000000..854001d
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform.h
@@ -0,0 +1,496 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  vcos
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - Linux kernel (partial) implementation.
+=============================================================================*/
+
+/* Do not include this file directly - instead include it via vcos.h */
+
+/** @file
+  *
+  * Linux kernel (partial) implementation of VCOS.
+  *
+  */
+
+#ifndef VCOS_PLATFORM_H
+#define VCOS_PLATFORM_H
+
+#include <linux/types.h>
+#include <linux/semaphore.h>
+#include <linux/mutex.h>
+#include <asm/bitops.h>
+#include <linux/kthread.h>
+#include <linux/wait.h>
+#include <linux/vmalloc.h>
+#include <linux/jiffies.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/random.h>
+#include <linux/sched.h>
+#include <linux/ctype.h>
+#include <linux/uaccess.h>
+#include <linux/time.h>  /* for time_t */
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#define VCOS_HAVE_RTOS         1
+#define VCOS_HAVE_SEMAPHORE    1
+#define VCOS_HAVE_EVENT        1
+#define VCOS_HAVE_QUEUE        0
+#define VCOS_HAVE_LEGACY_ISR   0
+#define VCOS_HAVE_TIMER        1
+#define VCOS_HAVE_CANCELLATION_SAFE_TIMER 0
+#define VCOS_HAVE_MEMPOOL      0
+#define VCOS_HAVE_ISR          0
+#define VCOS_HAVE_ATOMIC_FLAGS 1
+#define VCOS_HAVE_BLOCK_POOL   0
+#define VCOS_HAVE_ONCE         1
+#define VCOS_HAVE_FILE         0
+#define VCOS_HAVE_USER_BUF     0
+#define VCOS_HAVE_CFG          1
+#define VCOS_HAVE_SPINLOCK     0
+#define VCOS_HAVE_CMD          1
+#define VCOS_HAVE_EVENT_FLAGS  1
+
+/* Exclude many VCOS classes which don't have predicates */
+#define VCOS_TLS_H
+#define VCOS_NAMED_MUTEX_H
+#define VCOS_REENTRANT_MUTEX_H
+#define VCOS_NAMED_SEMAPHORE_H
+#define VCOS_QUICKSLOW_MUTEX_H
+/*#define VCOS_INIT_H */
+/*#define VCOS_MEM_H */
+/*#define VCOS_STRING_H */
+
+typedef struct semaphore      VCOS_SEMAPHORE_T;
+typedef struct semaphore      VCOS_EVENT_T;
+typedef struct mutex          VCOS_MUTEX_T;
+typedef volatile int          VCOS_ONCE_T;
+
+typedef unsigned int          VCOS_UNSIGNED;
+typedef unsigned int          VCOS_OPTION;
+typedef atomic_t              VCOS_ATOMIC_FLAGS_T;
+
+typedef struct
+{
+    struct  timer_list      linux_timer;
+    void                   *context;
+    void                  (*expiration_routine)(void *context);
+
+} VCOS_TIMER_T;
+
+typedef struct VCOS_LLTHREAD_T
+{
+   struct task_struct *thread;             /**< The thread itself */
+   VCOS_SEMAPHORE_T suspend;     /**< For support event groups and similar - a per thread semaphore */
+} VCOS_LLTHREAD_T;
+
+typedef enum
+{
+    VCOS_O_RDONLY   = 00000000,
+    VCOS_O_WRONLY   = 00000001,
+    VCOS_O_RDWR     = 00000002,
+    VCOS_O_TRUNC    = 00001000,
+} VCOS_FILE_FLAGS_T;
+
+typedef struct file *VCOS_FILE_T;
+
+#define VCOS_SUSPEND          -1
+#define VCOS_NO_SUSPEND       0
+
+#define VCOS_START 1
+#define VCOS_NO_START 0
+
+#define VCOS_THREAD_PRI_MIN   -20
+#define VCOS_THREAD_PRI_MAX   19
+
+#define VCOS_THREAD_PRI_INCREASE -1
+#define VCOS_THREAD_PRI_HIGHEST  VCOS_THREAD_PRI_MIN
+#define VCOS_THREAD_PRI_LOWEST   VCOS_THREAD_PRI_MAX
+#define VCOS_THREAD_PRI_NORMAL ((VCOS_THREAD_PRI_MAX+VCOS_THREAD_PRI_MIN)/2)
+#define VCOS_THREAD_PRI_ABOVE_NORMAL (VCOS_THREAD_PRI_NORMAL + VCOS_THREAD_PRI_INCREASE)
+#define VCOS_THREAD_PRI_REALTIME VCOS_THREAD_PRI_HIGHEST
+
+#define _VCOS_AFFINITY_DEFAULT 0
+#define _VCOS_AFFINITY_CPU0 0
+#define _VCOS_AFFINITY_CPU1 0
+#define _VCOS_AFFINITY_MASK 0
+#define VCOS_CAN_SET_STACK_ADDR  0
+
+#define VCOS_TICKS_PER_SECOND HZ
+
+#include "interface/vcos/generic/vcos_generic_event_flags.h"
+#include "interface/vcos/generic/vcos_mem_from_malloc.h"
+#include "interface/vcos/generic/vcos_joinable_thread_from_plain.h"
+
+/***********************************************************
+ *
+ * Memory allcoation
+ *
+ ***********************************************************/
+
+#define  _vcos_platform_malloc   vcos_platform_malloc
+#define  _vcos_platform_free     vcos_platform_free
+
+void *vcos_platform_malloc( VCOS_UNSIGNED required_size );
+void  vcos_platform_free( void *ptr );
+
+#if defined(VCOS_INLINE_BODIES)
+
+#undef VCOS_ASSERT_LOGGING_DISABLE
+#define VCOS_ASSERT_LOGGING_DISABLE 1
+
+/***********************************************************
+ *
+ * Counted Semaphores
+ *
+ ***********************************************************/
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_semaphore_wait(VCOS_SEMAPHORE_T *sem) {
+   int ret = down_interruptible(sem);
+   if ( ret == 0 )
+      /* Success */
+      return VCOS_SUCCESS;
+   else if ( ret == -EINTR )
+      /* Interrupted */
+      return VCOS_EINTR;
+   else
+      /* Default (timeout) */
+      return VCOS_EAGAIN;
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_semaphore_trywait(VCOS_SEMAPHORE_T *sem) {
+   if (down_trylock(sem) != 0)
+      return VCOS_EAGAIN;
+   return VCOS_SUCCESS;
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_semaphore_create(VCOS_SEMAPHORE_T *sem,
+                                    const char *name,
+                                    VCOS_UNSIGNED initial_count) {
+   sema_init(sem, initial_count);
+   return VCOS_SUCCESS;
+}
+
+VCOS_INLINE_IMPL
+void vcos_semaphore_delete(VCOS_SEMAPHORE_T *sem) {
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_semaphore_post(VCOS_SEMAPHORE_T *sem) {
+   up(sem);
+   return VCOS_SUCCESS;
+}
+
+/***********************************************************
+ *
+ * Threads
+ *
+ ***********************************************************/
+
+#include "vcos_thread_map.h"
+
+VCOS_INLINE_IMPL
+VCOS_LLTHREAD_T *vcos_llthread_current(void) {
+        return &vcos_kthread_current()->thread;
+}
+
+VCOS_INLINE_IMPL
+void vcos_llthread_resume(VCOS_LLTHREAD_T *thread) {
+   vcos_assert(0);
+}
+
+VCOS_INLINE_IMPL
+void vcos_sleep(uint32_t ms) {
+   msleep(ms);
+}
+
+VCOS_INLINE_IMPL
+void vcos_thread_set_priority(VCOS_THREAD_T *thread, VCOS_UNSIGNED p) {
+   /* not implemented */
+}
+VCOS_INLINE_IMPL
+VCOS_UNSIGNED vcos_thread_get_priority(VCOS_THREAD_T *thread) {
+   /* not implemented */
+   return 0;
+}
+
+/***********************************************************
+ *
+ * Miscellaneous
+ *
+ ***********************************************************/
+
+VCOS_INLINE_IMPL
+int vcos_strcasecmp(const char *s1, const char *s2) {
+   return strcasecmp(s1,s2);
+}
+
+
+/***********************************************************
+ *
+ * Mutexes
+ *
+ ***********************************************************/
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_mutex_create(VCOS_MUTEX_T *m, const char *name) {
+   mutex_init(m);
+   return VCOS_SUCCESS;
+}
+
+VCOS_INLINE_IMPL
+void vcos_mutex_delete(VCOS_MUTEX_T *m) {
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_mutex_lock(VCOS_MUTEX_T *m) {
+   int ret = mutex_lock_interruptible(m);
+   if ( ret == 0 )
+      /* Success */
+      return VCOS_SUCCESS;
+   else if ( ret == -EINTR )
+      /* Interrupted */
+      return VCOS_EINTR;
+   else
+      /* Default */
+      return VCOS_EAGAIN;
+}
+
+VCOS_INLINE_IMPL
+void vcos_mutex_unlock(VCOS_MUTEX_T *m) {
+   mutex_unlock(m);
+}
+
+VCOS_INLINE_IMPL
+int vcos_mutex_is_locked(VCOS_MUTEX_T *m) {
+   if (mutex_trylock(m) != 0)
+      return 1; /* it was locked */
+   mutex_unlock(m);
+   /* it wasn't locked */
+   return 0;
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_mutex_trylock(VCOS_MUTEX_T *m) {
+   if (mutex_trylock(m) == 0)
+      return VCOS_SUCCESS;
+   else
+      return VCOS_EAGAIN;
+}
+
+/* For supporting event groups - per thread semaphore */
+VCOS_INLINE_IMPL
+void _vcos_thread_sem_wait(void) {
+   VCOS_THREAD_T *t = vcos_thread_current();
+   vcos_semaphore_wait(&t->suspend);
+}
+
+VCOS_INLINE_IMPL
+void _vcos_thread_sem_post(VCOS_THREAD_T *target) {
+   vcos_semaphore_post(&target->suspend);
+}
+
+/***********************************************************
+ *
+ * Events
+ *
+ ***********************************************************/
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_event_create(VCOS_EVENT_T *event, const char *debug_name)
+{
+   sema_init(event, 0);
+   return VCOS_SUCCESS;
+}
+
+VCOS_INLINE_IMPL
+void vcos_event_signal(VCOS_EVENT_T *event)
+{
+   up(event);
+}
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_event_wait(VCOS_EVENT_T *event)
+{
+   int ret = down_interruptible(event);
+   if ( ret == -EINTR )
+      /* Interrupted */
+      return VCOS_EINTR;
+   else if (ret != 0)
+      /* Default (timeout) */
+      return VCOS_EAGAIN;
+   /* Emulate a maximum count of 1 by removing any extra upness */
+   while (down_trylock(event) == 0) continue;
+   return VCOS_SUCCESS;
+}
+
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_event_try(VCOS_EVENT_T *event)
+{
+   return (down_trylock(event) == 0) ? VCOS_SUCCESS : VCOS_EAGAIN;
+}
+
+VCOS_INLINE_IMPL
+void vcos_event_delete(VCOS_EVENT_T *event)
+{
+}
+
+/***********************************************************
+ *
+ * Timers
+ *
+ ***********************************************************/
+
+VCOS_INLINE_DECL
+void vcos_timer_linux_func(unsigned long data)
+{
+    VCOS_TIMER_T    *vcos_timer = (VCOS_TIMER_T *)data;
+
+    vcos_timer->expiration_routine( vcos_timer->context );
+}
+
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_timer_create(VCOS_TIMER_T *timer,
+                                const char *name,
+                                void (*expiration_routine)(void *context),
+                                void *context) {
+	init_timer(&timer->linux_timer);
+	timer->linux_timer.data = (unsigned long)timer;
+	timer->linux_timer.function = vcos_timer_linux_func;
+
+    timer->context = context;
+    timer->expiration_routine = expiration_routine;
+
+    return VCOS_SUCCESS;
+}
+
+VCOS_INLINE_IMPL
+void vcos_timer_set(VCOS_TIMER_T *timer, VCOS_UNSIGNED delay_ms) {
+	timer->linux_timer.expires = jiffies + msecs_to_jiffies(delay_ms);
+       add_timer(&timer->linux_timer);
+}
+
+VCOS_INLINE_IMPL
+void vcos_timer_cancel(VCOS_TIMER_T *timer) {
+     del_timer(&timer->linux_timer);
+}
+
+VCOS_INLINE_IMPL
+void vcos_timer_reset(VCOS_TIMER_T *timer, VCOS_UNSIGNED delay_ms) {
+    del_timer_sync(&timer->linux_timer);
+    timer->linux_timer.expires = jiffies + msecs_to_jiffies(delay_ms);
+    add_timer(&timer->linux_timer);
+}
+
+VCOS_INLINE_IMPL
+void vcos_timer_delete(VCOS_TIMER_T *timer) {
+    timer->context = NULL;
+    timer->expiration_routine = NULL;
+    timer->linux_timer.function = NULL;
+    timer->linux_timer.data = 0;
+    return;
+}
+
+VCOS_INLINE_IMPL
+VCOS_UNSIGNED vcos_process_id_current(void) {
+   return (VCOS_UNSIGNED)current->pid;
+}
+
+
+VCOS_INLINE_IMPL
+int vcos_in_interrupt(void) {
+   return in_interrupt();
+}
+
+/***********************************************************
+ *
+ * Atomic flags
+ *
+ ***********************************************************/
+
+VCOS_INLINE_IMPL
+VCOS_STATUS_T vcos_atomic_flags_create(VCOS_ATOMIC_FLAGS_T *atomic_flags)
+{
+   atomic_set(atomic_flags, 0);
+   return VCOS_SUCCESS;
+}
+
+VCOS_INLINE_IMPL
+void vcos_atomic_flags_or(VCOS_ATOMIC_FLAGS_T *atomic_flags, uint32_t flags)
+{
+   uint32_t value;
+   do {
+      value = atomic_read(atomic_flags);
+   } while (atomic_cmpxchg(atomic_flags, value, value | flags) != value);
+}
+
+VCOS_INLINE_IMPL
+uint32_t vcos_atomic_flags_get_and_clear(VCOS_ATOMIC_FLAGS_T *atomic_flags)
+{
+   return atomic_xchg(atomic_flags, 0);
+}
+
+VCOS_INLINE_IMPL
+void vcos_atomic_flags_delete(VCOS_ATOMIC_FLAGS_T *atomic_flags)
+{
+}
+
+#undef VCOS_ASSERT_LOGGING_DISABLE
+#define VCOS_ASSERT_LOGGING_DISABLE 0
+
+#endif /* VCOS_INLINE_BODIES */
+
+VCOS_INLINE_DECL void _vcos_thread_sem_wait(void);
+VCOS_INLINE_DECL void _vcos_thread_sem_post(VCOS_THREAD_T *);
+
+/***********************************************************
+ *
+ * Misc
+ *
+ ***********************************************************/
+VCOS_INLINE_DECL char *vcos_strdup(const char *str);
+
+/***********************************************************
+ *
+ * Logging
+ *
+ ***********************************************************/
+
+VCOSPRE_ const char * VCOSPOST_ _vcos_log_level(void);
+#define _VCOS_LOG_LEVEL() _vcos_log_level()
+
+#define  vcos_log_platform_init()               _vcos_log_platform_init()
+#define  vcos_log_platform_register(category)   _vcos_log_platform_register(category)
+#define  vcos_log_platform_unregister(category) _vcos_log_platform_unregister(category)
+
+struct VCOS_LOG_CAT_T;  /* Forward declaration since vcos_logging.h hasn't been included yet */
+
+void _vcos_log_platform_init(void);
+void _vcos_log_platform_register(struct VCOS_LOG_CAT_T *category);
+void _vcos_log_platform_unregister(struct VCOS_LOG_CAT_T *category);
+
+/***********************************************************
+ *
+ * Memory barriers
+ *
+ ***********************************************************/
+
+#define vcos_wmb(x) wmb()
+#define vcos_rmb() rmb()
+
+#include "interface/vcos/generic/vcos_common.h"
+/*#include "interface/vcos/generic/vcos_generic_quickslow_mutex.h" */
+
+#endif /* VCOS_PLATFORM_H */
+
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform_types.h b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform_types.h
new file mode 100644
index 0000000..20d973e
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform_types.h
@@ -0,0 +1,47 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  osal
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - platform-specific types and defines
+=============================================================================*/
+
+#ifndef VCOS_PLATFORM_TYPES_H
+#define VCOS_PLATFORM_TYPES_H
+
+#include <stddef.h>
+#include <linux/types.h>
+#include <linux/bug.h>
+
+#define VCOSPRE_ extern
+#define VCOSPOST_
+
+#if defined(__GNUC__) && (( __GNUC__ > 2 ) || (( __GNUC__ == 2 ) && ( __GNUC_MINOR__ >= 3 )))
+#define VCOS_FORMAT_ATTR_(ARCHETYPE, STRING_INDEX, FIRST_TO_CHECK)  __attribute__ ((format (ARCHETYPE, STRING_INDEX, FIRST_TO_CHECK)))
+#else
+#define VCOS_FORMAT_ATTR_(ARCHETYPE, STRING_INDEX, FIRST_TO_CHECK)
+#endif
+
+#if !defined( __STDC_VERSION__ )
+#define __STDC_VERSION__ 199901L
+#endif
+
+#if !defined( __STDC_VERSION )
+#define __STDC_VERSION   __STDC_VERSION__
+#endif
+
+static inline void __vcos_bkpt( void ) { BUG(); }
+#define VCOS_BKPT __vcos_bkpt()
+
+#define VCOS_ASSERT_MSG(...) printk( KERN_ERR "vcos_assert: " __VA_ARGS__ )
+
+#define PRId64 "lld"
+#define PRIi64 "lli"
+#define PRIo64 "llo"
+#define PRIu64 "llu"
+#define PRIx64 "llx"
+
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_thread_map.c b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_thread_map.c
new file mode 100644
index 0000000..2d62589
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_thread_map.c
@@ -0,0 +1,129 @@
+/*****************************************************************************
+* Copyright 2009 - 2010 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+/** Support to allow VCOS thread-related functions to be called from
+  * threads that were not created by VCOS.
+  */
+
+#include <linux/semaphore.h>
+#include <linux/vmalloc.h>
+#include <linux/list.h>
+#include <linux/sched.h>
+
+#include "vcos_thread_map.h"
+#include "interface/vcos/vcos_logging.h"
+
+/*
+ * Store the vcos_thread pointer at the end of
+ * current kthread stack, right after the thread_info
+ * structure.
+ *
+ * I belive we should be safe here to steal these 4 bytes
+ * from the stack, as long as the vcos thread does not use up
+ * all the stack available
+ *
+ * NOTE: This scheme will not work on architectures with stack growing up
+ */
+
+/* Shout, if we are not being compiled for ARM kernel */
+
+#ifndef CONFIG_ARM
+#error " **** The vcos kthread implementation may not work for non-ARM kernel ****"
+#endif
+
+static inline void *to_current_vcos_thread(void)
+{
+   unsigned long *vcos_data;
+
+   vcos_data = (unsigned long *)((char *)current_thread_info() + sizeof(struct thread_info));
+
+   return (void *)vcos_data;
+}
+
+
+static inline void *to_vcos_thread(struct task_struct *tsk)
+{
+   unsigned long *vcos_data;
+
+   vcos_data = (unsigned long *)((char *)tsk->stack + sizeof(struct thread_info));
+
+   return (void *)vcos_data;
+}
+
+/**
+   @fn uint32_t vcos_add_thread(THREAD_MAP_T *vcos_thread);
+*/
+uint32_t vcos_add_thread(VCOS_THREAD_T *vcos_thread)
+{
+   VCOS_THREAD_T **vcos_thread_storage = (VCOS_THREAD_T **)to_current_vcos_thread();
+
+   *vcos_thread_storage = vcos_thread;
+
+   return(0);
+}
+
+
+/**
+   @fn uint32_t vcos_remove_thread(struct task_struct * thread_id);
+*/
+uint32_t vcos_remove_thread(struct task_struct *thread_id)
+{
+   /* Remove thread_id -> VCOS_THREAD_T relationship */
+   VCOS_THREAD_T **vcos_thread_storage;
+
+   /*
+    * We want to be able to build vcos as a loadable module, which
+    * means that we can't call get_task_struct. So we assert if we're
+    * ever called with thread_id != current.
+    */
+
+   BUG_ON( thread_id != current );
+
+   vcos_thread_storage = (VCOS_THREAD_T **)to_vcos_thread(thread_id);
+
+   *(unsigned long *)vcos_thread_storage = 0xCAFEBABE;
+
+   return(0);
+}
+
+
+VCOS_THREAD_T *vcos_kthread_current(void)
+{
+   VCOS_THREAD_T **vcos_thread_storage = (VCOS_THREAD_T **)to_current_vcos_thread();
+
+   /* If we find this, either the thread is already dead or stack pages of a
+    * dead vcos thread are re-allocated to this one.
+    *
+    * Since there's no way to differentiate between these 2 cases, we just dump
+    * the current task name to the log.
+    *
+    * If the current thread is created using VCOS API, you should *never* see this
+    * print.
+    * 
+    * If its a non-VCOS thread, just let it go ...
+    *
+    * To debug VCOS, uncomment printk's under the "if" condition below
+    *
+    */
+   if (*vcos_thread_storage == (void *)0xCAFEBABE)
+   {
+     #if 0
+      printk(KERN_DEBUG"****************************************************\n");
+      printk(KERN_DEBUG"%s : You have a problem, if \"%s\" is a VCOS thread\n",__func__, current->comm);
+      printk(KERN_DEBUG"****************************************************\n");
+     #endif
+   }
+
+   return *vcos_thread_storage;
+}
diff --git a/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_thread_map.h b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_thread_map.h
new file mode 100644
index 0000000..62963c1
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_thread_map.h
@@ -0,0 +1,39 @@
+/*****************************************************************************
+* Copyright 2009 - 2010 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+
+#ifndef VCOS_THREAD_MAP_H
+#define VCOS_THREAD_MAP_H
+
+#include <linux/string.h>
+
+#include "vcos_platform.h"
+
+static inline void vcos_thread_map_init(void)
+{
+   return;
+}
+
+static inline void vcos_thread_map_cleanup(void)
+{
+   return;
+}
+
+uint32_t vcos_add_thread(VCOS_THREAD_T *vcos_thread);
+
+uint32_t vcos_remove_thread(struct task_struct *thread_id);
+
+VCOS_THREAD_T *vcos_kthread_current(void);
+
+#endif /*VCOS_THREAD_MAP_H */
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos.h b/drivers/misc/vc04_services/interface/vcos/vcos.h
new file mode 100644
index 0000000..4ff5cc8
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos.h
@@ -0,0 +1,201 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - public header file
+=============================================================================*/
+
+/**
+  * \mainpage OS Abstraction Layer
+  *
+  * \section intro Introduction
+  *
+  * This abstraction layer is here to allow the underlying OS to be easily changed (e.g. from
+  * Nucleus to ThreadX) and to aid in porting host applications to new targets.
+  *
+  * \subsection error Error handling
+  *
+  * Wherever possible, VCOS functions assert internally and return void. The only exceptions
+  * are creation functions (which might fail due to lack of resources) and functions that
+  * might timeout or fail due to lack of space. Errors that might be reported by the underlying
+  * OS API (e.g. invalid mutex) are treated as a programming error, and are merely asserted on.
+  *
+  * \section thread_synch Threads and synchronisation
+  *
+  * \subsection thread Threads
+  *
+  * The thread API is somewhat different to that found in Nucleus. In particular, threads
+  * cannot just be destroyed at arbitrary times and nor can they merely exit. This is so
+  * that the same API can be implemented across all interesting platforms without too much
+  * difficulty. See vcos_thread.h for details. Thread attributes are configured via
+  * the VCOS_THREAD_ATTR_T structure, found in vcos_thread_attr.h.
+  *
+  * \subsection sema Semaphores
+  *
+  * Counted semaphores (c.f. Nucleus NU_SEMAPHORE) are created with VCOS_SEMAPHORE_T.
+  * Under ThreadX on VideoCore, semaphores are implemented using VideoCore spinlocks, and
+  * so are quite a lot faster than ordinary ThreadX semaphores. See vcos_semaphore.h.
+  *
+  * \subsection mtx Mutexes
+  *
+  * Mutexes are used for locking. Attempts to take a mutex twice, or to unlock it
+  * in a different thread to the one in which it was locked should be expected to fail.
+  * Mutexes are not re-entrant (see vcos_reentrant_mutex.h for a slightly slower
+  * re-entrant mutex).
+  *
+  * \subsection evflags Event flags
+  *
+  * Event flags (the ThreadX name - also known as event groups under Nucleus) provide
+  * 32 flags which can be waited on by multiple clients, and signalled by multiple clients.
+  * A timeout can be specified. See vcos_event_flags.h. An alternative to this is the
+  * VCOS_EVENT_T (see vcos_event.h) which is akin to the Win32 auto-reset event, or a
+  * saturating counted semaphore.
+  *
+  * \subsection event Events
+  *
+  * A VCOS_EVENT_T is a bit like a saturating semaphore. No matter how many times it
+  * is signalled, the waiter will only wake up once. See vcos_event.h. You might think this
+  * is useful if you suspect that the cost of reading the semaphore count (perhaps via a
+  * system call) is expensive on your platform.
+  *
+  * \subsection tls Thread local storage
+  *
+  * Thread local storage is supported using vcos_tls.h. This is emulated on Nucleus
+  * and ThreadX.
+  *
+  * \section int Interrupts
+  *
+  * The legacy LISR/HISR scheme found in Nucleus is supported via the legacy ISR API,
+  * which is also supported on ThreadX. New code should avoid this, and old code should
+  * be migrated away from it, since it is slow. See vcos_legacy_isr.h.
+  *
+  * Registering an interrupt handler, and disabling/restoring interrupts, is handled
+  * using the functions in vcos_isr.h.
+  *
+  */
+
+/**
+  * \file vcos.h
+  *
+  * This is the top level header file. Clients include this. It pulls in the platform-specific
+  * header file (vcos_platform.h) together with header files defining the expected APIs, such
+  * as vcos_mutex.h, vcos_semaphore.h, etc. It is also possible to include these header files
+  * directly.
+  *
+  */
+
+#ifndef VCOS_H
+#define VCOS_H
+
+#include "interface/vcos/vcos_assert.h"
+#include "vcos_types.h"
+#include "vcos_platform.h"
+
+#ifndef VCOS_INIT_H
+#include "interface/vcos/vcos_init.h"
+#endif
+
+#ifndef VCOS_SEMAPHORE_H
+#include "interface/vcos/vcos_semaphore.h"
+#endif
+
+#ifndef VCOS_THREAD_H
+#include "interface/vcos/vcos_thread.h"
+#endif
+
+#ifndef VCOS_MUTEX_H
+#include "interface/vcos/vcos_mutex.h"
+#endif
+
+#ifndef VCOS_MEM_H
+#include "interface/vcos/vcos_mem.h"
+#endif
+
+#ifndef VCOS_LOGGING_H
+#include "interface/vcos/vcos_logging.h"
+#endif
+
+#ifndef VCOS_STRING_H
+#include "interface/vcos/vcos_string.h"
+#endif
+
+#ifndef VCOS_EVENT_H
+#include "interface/vcos/vcos_event.h"
+#endif
+
+#ifndef VCOS_THREAD_ATTR_H
+#include "interface/vcos/vcos_thread_attr.h"
+#endif
+
+#ifndef VCOS_TLS_H
+#include "interface/vcos/vcos_tls.h"
+#endif
+
+#ifndef VCOS_REENTRANT_MUTEX_H
+#include "interface/vcos/vcos_reentrant_mutex.h"
+#endif
+
+#ifndef VCOS_NAMED_SEMAPHORE_H
+#include "interface/vcos/vcos_named_semaphore.h"
+#endif
+
+#ifndef VCOS_QUICKSLOW_MUTEX_H
+#include "interface/vcos/vcos_quickslow_mutex.h"
+#endif
+
+/* Headers with predicates */
+
+#if VCOS_HAVE_EVENT_FLAGS
+#include "interface/vcos/vcos_event_flags.h"
+#endif
+
+#if VCOS_HAVE_QUEUE
+#include "interface/vcos/vcos_queue.h"
+#endif
+
+#if VCOS_HAVE_LEGACY_ISR
+#include "interface/vcos/vcos_legacy_isr.h"
+#endif
+
+#if VCOS_HAVE_TIMER
+#include "interface/vcos/vcos_timer.h"
+#endif
+
+#if VCOS_HAVE_MEMPOOL
+#include "interface/vcos/vcos_mempool.h"
+#endif
+
+#if VCOS_HAVE_ISR
+#include "interface/vcos/vcos_isr.h"
+#endif
+
+#if VCOS_HAVE_ATOMIC_FLAGS
+#include "interface/vcos/vcos_atomic_flags.h"
+#endif
+
+#if VCOS_HAVE_ONCE
+#include "interface/vcos/vcos_once.h"
+#endif
+
+#if VCOS_HAVE_BLOCK_POOL
+#include "interface/vcos/vcos_blockpool.h"
+#endif
+
+#if VCOS_HAVE_FILE
+#include "interface/vcos/vcos_file.h"
+#endif
+
+#if VCOS_HAVE_CFG
+#include "interface/vcos/vcos_cfg.h"
+#endif
+
+#if VCOS_HAVE_CMD
+#include "interface/vcos/vcos_cmd.h"
+#endif
+
+#endif /* VCOS_H */
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_assert.h b/drivers/misc/vc04_services/interface/vcos/vcos_assert.h
new file mode 100644
index 0000000..5091621
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_assert.h
@@ -0,0 +1,269 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  osal
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - Assertion and error-handling macros.
+=============================================================================*/
+
+
+#ifndef VCOS_ASSERT_H
+#define VCOS_ASSERT_H
+
+/*
+ * Macro:
+ *    vcos_assert(cond)
+ *    vcos_assert_msg(cond, fmt, ...)
+ * Use:
+ *    Detecting programming errors by ensuring that assumptions are correct.
+ * On failure:
+ *    Performs a platform-dependent "breakpoint", usually with an assert-style
+ *    message. The '_msg' variant expects a printf-style format string and
+ *    parameters.
+ *    If a failure is detected, the code should be fixed and rebuilt.
+ * In release builds:
+ *    Generates no code, i.e. does not evaluate 'cond'.
+ * Returns:
+ *    Nothing.
+ *
+ * Macro:
+ *    vcos_demand(cond)
+ *    vcos_demand_msg(cond, fmt, ...)
+ * Use:
+ *    Detecting fatal system errors that require a reboot.
+ * On failure:
+ *    Performs a platform-dependent "breakpoint", usually with an assert-style
+ *    message, then calls vcos_abort (see below).
+ * In release builds:
+ *    Calls vcos_abort() if 'cond' is false.
+ * Returns:
+ *    Nothing (never, on failure).
+ *
+ * Macro:
+ *    vcos_verify(cond)
+ *    vcos_verify_msg(cond, fmt, ...)
+ * Use:
+ *    Detecting run-time errors and interesting conditions, normally within an
+ *    'if' statement to catch the failures, i.e.
+ *       if (!vcos_verify(cond)) handle_error();
+ * On failure:
+ *    Generates a message and optionally stops at a platform-dependent
+ *    "breakpoint" (usually disabled). See vcos_verify_bkpts_enable below.
+ * In release builds:
+ *    Just evaluates and returns 'cond'.
+ * Returns:
+ *    Non-zero if 'cond' is true, otherwise zero.
+ *
+ * Macro:
+ *    vcos_static_assert(cond)
+ * Use:
+ *    Detecting compile-time errors.
+ * On failure:
+ *    Generates a compiler error.
+ * In release builds:
+ *    Generates a compiler error.
+ *
+ * Function:
+ *    void vcos_abort(void)
+ * Use:
+ *    Invokes the fatal error handling mechanism, alerting the host where
+ *    applicable.
+ * Returns:
+ *    Never.
+ *
+ * Macro:
+ *    VCOS_VERIFY_BKPTS
+ * Use:
+ *    Define in a module (before including vcos.h) to specify an alternative
+ *    flag to control breakpoints on vcos_verify() failures.
+ * Returns:
+ *    Non-zero values enable breakpoints.
+ *
+ * Function:
+ *    int vcos_verify_bkpts_enable(int enable);
+ * Use:
+ *    Sets the global flag controlling breakpoints on vcos_verify failures,
+ *    enabling the breakpoints iff 'enable' is non-zero.
+ * Returns:
+ *    The previous state of the flag.
+ *
+ * Function:
+ *    int vcos_verify_bkpts_enabled(void);
+ * Use:
+ *    Queries the state of the global flag enabling breakpoints on vcos_verify
+ *    failures.
+ * Returns:
+ *    The current state of the flag.
+ *
+ * Examples:
+ *
+ * int my_breakpoint_enable_flag = 1;
+ *
+ * #define VCOS_VERIFY_BKPTS my_breakpoint_enable_flag
+ *
+ * #include "interface/vcos/vcos.h"
+ *
+ * vcos_static_assert((sizeof(object) % 32) == 0);
+ *
+ * // ...
+ *
+ *    vcos_assert_msg(postcondition_is_true, "Coding error");
+ *
+ *    if (!vcos_verify_msg(buf, "Buffer allocation failed (%d bytes)", size))
+ *    {
+ *       // Tidy up
+ *       // ...
+ *       return OUT_OF_MEMORY;
+ *    }
+ *
+ *    vcos_demand(*p++==GUARDWORDHEAP);
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+
+#ifdef __COVERITY__
+#undef VCOS_ASSERT_BKPT
+#define VCOS_ASSERT_BKPT __coverity_panic__()
+#endif
+
+#ifndef VCOS_VERIFY_BKPTS
+#define VCOS_VERIFY_BKPTS vcos_verify_bkpts_enabled()
+#endif
+
+#ifndef VCOS_BKPT
+#if defined(__VIDEOCORE__) && !defined(VCOS_ASSERT_NO_BKPTS)
+#define VCOS_BKPT _bkpt()
+#else
+#define VCOS_BKPT (void )0
+#endif
+#endif
+
+#ifndef VCOS_ASSERT_BKPT
+#define VCOS_ASSERT_BKPT VCOS_BKPT
+#endif
+
+#ifndef VCOS_VERIFY_BKPT
+#define VCOS_VERIFY_BKPT (VCOS_VERIFY_BKPTS ? VCOS_BKPT : (void)0)
+#endif
+
+VCOSPRE_ int VCOSPOST_ vcos_verify_bkpts_enabled(void);
+VCOSPRE_ int VCOSPOST_ vcos_verify_bkpts_enable(int enable);
+VCOSPRE_ void VCOSPOST_ vcos_abort(void);
+
+#ifndef VCOS_ASSERT_MSG
+#ifdef LOGGING
+extern void logging_assert(const char *file, const char *func, int line, const char *format, ...);
+#define VCOS_ASSERT_MSG(...) ((VCOS_ASSERT_LOGGING && !VCOS_ASSERT_LOGGING_DISABLE) ? logging_assert(__FILE__, __func__, __LINE__, __VA_ARGS__) : (void)0)
+#else
+#define VCOS_ASSERT_MSG(...) ((void)0)
+#endif
+#endif
+
+#ifndef VCOS_VERIFY_MSG
+#define VCOS_VERIFY_MSG(...) VCOS_ASSERT_MSG(__VA_ARGS__)
+#endif
+
+#ifndef VCOS_ASSERT_LOGGING
+#define VCOS_ASSERT_LOGGING 0
+#endif
+
+#ifndef VCOS_ASSERT_LOGGING_DISABLE
+#define VCOS_ASSERT_LOGGING_DISABLE 0
+#endif
+
+#if !defined(NDEBUG) || defined(VCOS_RELEASE_ASSERTS)
+
+#ifndef vcos_assert
+#define vcos_assert(cond) \
+   ( (cond) ? (void)0 : (VCOS_ASSERT_MSG("%s", #cond), VCOS_ASSERT_BKPT) )
+#endif
+
+#ifndef vcos_assert_msg
+#define vcos_assert_msg(cond, ...) \
+   ( (cond) ? (void)0 : (VCOS_ASSERT_MSG(__VA_ARGS__), VCOS_ASSERT_BKPT) )
+#endif
+
+#else  /* !defined(NDEBUG) || defined(VCOS_RELEASE_ASSERTS) */
+
+#ifndef vcos_assert
+#define vcos_assert(cond) (void)0
+#endif
+
+#ifndef vcos_assert_msg
+#define vcos_assert_msg(cond, ...) (void)0
+#endif
+
+#endif /* !defined(NDEBUG) || defined(VCOS_RELEASE_ASSERTS) */
+
+#if !defined(NDEBUG)
+
+#ifndef vcos_demand
+#define vcos_demand(cond) \
+   ( (cond) ? (void)0 : (VCOS_ASSERT_MSG("%s", #cond), VCOS_ASSERT_BKPT, vcos_abort()) )
+#endif
+
+#ifndef vcos_demand_msg
+#define vcos_demand_msg(cond, ...) \
+   ( (cond) ? (void)0 : (VCOS_ASSERT_MSG(__VA_ARGS__), VCOS_ASSERT_BKPT, vcos_abort()) )
+#endif
+
+#ifndef vcos_verify
+#define vcos_verify(cond) \
+   ( (cond) ? 1 : (VCOS_VERIFY_MSG("%s", #cond), VCOS_VERIFY_BKPT, 0) )
+#endif
+
+#ifndef vcos_verify_msg
+#define vcos_verify_msg(cond, ...) \
+   ( (cond) ? 1 : (VCOS_VERIFY_MSG(__VA_ARGS__), VCOS_VERIFY_BKPT, 0) )
+#endif
+
+#else  /* !defined(NDEBUG) */
+
+#ifndef vcos_demand
+#define vcos_demand(cond) \
+   ( (cond) ? (void)0 : vcos_abort() )
+#endif
+
+#ifndef vcos_demand_msg
+#define vcos_demand_msg(cond, ...) \
+   ( (cond) ? (void)0 : vcos_abort() )
+#endif
+
+#ifndef vcos_verify
+#define vcos_verify(cond) (cond)
+#endif
+
+#ifndef vcos_verify_msg
+#define vcos_verify_msg(cond, ...) (cond)
+#endif
+
+#endif /* !defined(NDEBUG) */
+
+#ifndef vcos_static_assert
+#if defined(__GNUC__)
+#define vcos_static_assert(cond) __attribute__((unused)) extern int vcos_static_assert[(cond)?1:-1]
+#else
+#define vcos_static_assert(cond) extern int vcos_static_assert[(cond)?1:-1]
+#endif
+#endif
+
+#ifndef vc_assert
+#define vc_assert(cond) vcos_assert(cond)
+#endif
+
+/** Print out a backtrace, on supported platforms.
+  */
+extern void vcos_backtrace_self(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* VCOS_ASSERT_H */
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_atomic_flags.h b/drivers/misc/vc04_services/interface/vcos/vcos_atomic_flags.h
new file mode 100644
index 0000000..bb8041e
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_atomic_flags.h
@@ -0,0 +1,72 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver (just for consistency with the rest of vcos ;)
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - public header file
+=============================================================================*/
+
+#ifndef VCOS_ATOMIC_FLAGS_H
+#define VCOS_ATOMIC_FLAGS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/**
+ * \file vcos_atomic_flags.h
+ *
+ * Defines atomic flags API.
+ *
+ * 32 flags. Atomic "or" and "get and clear" operations
+ */
+
+/**
+ * Create an atomic flags instance.
+ *
+ * @param atomic_flags Pointer to atomic flags instance, filled in on return
+ *
+ * @return VCOS_SUCCESS if succeeded.
+ */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_atomic_flags_create(VCOS_ATOMIC_FLAGS_T *atomic_flags);
+
+/**
+ * Atomically set the specified flags.
+ *
+ * @param atomic_flags Instance to set flags on
+ * @param flags        Mask of flags to set
+ */
+VCOS_INLINE_DECL
+void vcos_atomic_flags_or(VCOS_ATOMIC_FLAGS_T *atomic_flags, uint32_t flags);
+
+/**
+ * Retrieve the current flags and then clear them. The entire operation is
+ * atomic.
+ *
+ * @param atomic_flags Instance to get/clear flags from/on
+ *
+ * @return Mask of flags which were set (and we cleared)
+ */
+VCOS_INLINE_DECL
+uint32_t vcos_atomic_flags_get_and_clear(VCOS_ATOMIC_FLAGS_T *atomic_flags);
+
+/**
+ * Delete an atomic flags instance.
+ *
+ * @param atomic_flags Instance to delete
+ */
+VCOS_INLINE_DECL
+void vcos_atomic_flags_delete(VCOS_ATOMIC_FLAGS_T *atomic_flags);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_build_info.h b/drivers/misc/vc04_services/interface/vcos/vcos_build_info.h
new file mode 100644
index 0000000..7543a89
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_build_info.h
@@ -0,0 +1,5 @@
+const char *vcos_get_build_hostname( void );
+const char *vcos_get_build_version( void );
+const char *vcos_get_build_time( void );
+const char *vcos_get_build_date( void );
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_cfg.h b/drivers/misc/vc04_services/interface/vcos/vcos_cfg.h
new file mode 100644
index 0000000..4423051
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_cfg.h
@@ -0,0 +1,113 @@
+/*****************************************************************************
+* Copyright 2009 - 2011 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+#if !defined( VCOS_CFG_H )
+#define VCOS_CFG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+typedef struct opaque_vcos_cfg_buf_t    *VCOS_CFG_BUF_T;
+typedef struct opaque_vcos_cfg_entry_t  *VCOS_CFG_ENTRY_T;
+
+/** \file vcos_file.h
+  *
+  * API for accessing configuration/statistics information. This
+  * is loosely modelled on the linux proc entries.
+  */
+
+typedef void (*VCOS_CFG_SHOW_FPTR)( VCOS_CFG_BUF_T buf, void *data );
+typedef void (*VCOS_CFG_PARSE_FPTR)( VCOS_CFG_BUF_T buf, void *data );
+
+/** Create a configuration directory.
+  *
+  * @param entry        Place to store the created config entry.
+  * @param parent       Parent entry (for directory like config 
+  *                     options).
+  * @param entryName    Name of the directory.
+  */
+
+VCOS_STATUS_T vcos_cfg_mkdir( VCOS_CFG_ENTRY_T *entry,
+                              VCOS_CFG_ENTRY_T *parent,
+                              const char *dirName );           
+
+/** Create a configuration entry.
+  *
+  * @param entry        Place to store the created config entry.
+  * @param parent       Parent entry (for directory like config 
+  *                     options).
+  * @param entryName    Name of the configuration entry.
+  * @param showFunc     Function pointer to show configuration 
+  *                     data.
+  * @param parseFunc    Function pointer to parse new data. 
+  */
+
+VCOS_STATUS_T vcos_cfg_create_entry( VCOS_CFG_ENTRY_T *entry,
+                                     VCOS_CFG_ENTRY_T *parent,
+                                     const char *entryName,
+                                     VCOS_CFG_SHOW_FPTR showFunc,
+                                     VCOS_CFG_PARSE_FPTR parseFunc,
+                                     void *data );
+
+/** Determines if a configuration entry has been created or not.
+  *
+  * @param entry        Configuration entry to query.
+  */
+
+int vcos_cfg_is_entry_created( VCOS_CFG_ENTRY_T entry );
+
+/** Returns the name of a configuration entry.
+  *
+  * @param entry        Configuration entry to query.
+  */
+
+const char *vcos_cfg_get_entry_name( VCOS_CFG_ENTRY_T entry );
+
+/** Removes a configuration entry.
+  *
+  * @param entry        Configuration entry to remove.
+  */
+
+VCOS_STATUS_T vcos_cfg_remove_entry( VCOS_CFG_ENTRY_T *entry );
+
+
+/** Writes data into a configuration buffer. Only valid inside
+  * the show function. 
+  *
+  * @param buf      Buffer to write data into.
+  * @param fmt      printf style format string. 
+  */
+
+void vcos_cfg_buf_printf( VCOS_CFG_BUF_T buf, const char *fmt, ... );
+
+/** Retrieves a null terminated string of the data associated
+  * with the buffer. Only valid inside the parse function.
+  *
+  * @param buf      Buffer to get data from.
+  * @param fmt      printf style format string. 
+  */
+
+char *vcos_cfg_buf_get_str( VCOS_CFG_BUF_T buf );
+
+void *vcos_cfg_get_proc_entry( VCOS_CFG_ENTRY_T entry );
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_cmd.h b/drivers/misc/vc04_services/interface/vcos/vcos_cmd.h
new file mode 100644
index 0000000..29d5315
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_cmd.h
@@ -0,0 +1,98 @@
+/*****************************************************************************
+* Copyright 2009 - 2011 Broadcom Corporation.  All rights reserved.
+*
+* Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2, available at
+* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+*
+* Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a
+* license other than the GPL, without Broadcom's express prior written
+* consent.
+*****************************************************************************/
+
+#if !defined( VCOS_CMD_H )
+#define VCOS_CMD_H
+
+/* ---- Include Files ----------------------------------------------------- */
+
+#include "interface/vcos/vcos.h"
+#include "interface/vcos/vcos_stdint.h"
+
+
+/* ---- Constants and Types ---------------------------------------------- */
+
+struct VCOS_CMD_S;
+typedef struct VCOS_CMD_S VCOS_CMD_T;
+
+typedef struct
+{
+    int         argc;           /* Number of arguments (includes the command/sub-command) */
+    char      **argv;           /* Array of arguments */
+    char      **argv_orig;      /* Original array of arguments */
+
+    VCOS_CMD_T *cmd_entry;
+    VCOS_CMD_T *cmd_parent_entry;
+
+    int         use_log;        /* Output being logged? */
+    size_t      result_size;    /* Size of result buffer. */
+    char       *result_ptr;     /* Next place to put output. */
+    char       *result_buf;     /* Start of the buffer. */
+
+} VCOS_CMD_PARAM_T;
+
+typedef VCOS_STATUS_T (*VCOS_CMD_FUNC_T)( VCOS_CMD_PARAM_T *param );
+
+struct VCOS_CMD_S
+{
+    const char         *name;
+    const char         *args;
+    VCOS_CMD_FUNC_T     cmd_fn;
+    VCOS_CMD_T         *sub_cmd_entry;
+    const char         *descr;
+
+};
+
+/* ---- Variable Externs ------------------------------------------------- */
+
+/* ---- Function Prototypes ---------------------------------------------- */
+
+/*
+ * Common printing routine for generating command output.
+ */
+VCOSPRE_ void VCOSPOST_ vcos_cmd_error( VCOS_CMD_PARAM_T *param, const char *fmt, ... ) VCOS_FORMAT_ATTR_(printf, 2, 3);
+VCOSPRE_ void VCOSPOST_ vcos_cmd_printf( VCOS_CMD_PARAM_T *param, const char *fmt, ... ) VCOS_FORMAT_ATTR_(printf, 2, 3);
+VCOSPRE_ void VCOSPOST_ vcos_cmd_vprintf( VCOS_CMD_PARAM_T *param, const char *fmt, va_list args ) VCOS_FORMAT_ATTR_(printf, 2, 0);
+
+/*
+ * Cause vcos_cmd_error, printf and vprintf to always log to the provided
+ * category. When this call is made, the results buffer passed into
+ * vcos_cmd_execute is used as a line buffer and does not need to be
+ * output by the caller.
+ */
+VCOSPRE_ void VCOSPOST_ vcos_cmd_always_log_output( VCOS_LOG_CAT_T *log_category );
+
+/*
+ * Prints command usage for the current command.
+ */
+VCOSPRE_ void VCOSPOST_ vcos_cmd_usage( VCOS_CMD_PARAM_T *param );
+
+/*
+ * Register commands to be processed
+ */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_cmd_register( VCOS_CMD_T *cmd_entry );
+
+/*
+ * Registers multiple commands to be processed. The array should
+ * be terminated by an entry with all zeros.
+ */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_cmd_register_multiple( VCOS_CMD_T *cmd_entry );
+
+/*
+ * Executes a command based on a command line.
+ */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_cmd_execute( int argc, char **argv, size_t result_size, char *result_buf );
+
+#endif /* VCOS_CMD_H */
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_ctype.h b/drivers/misc/vc04_services/interface/vcos/vcos_ctype.h
new file mode 100644
index 0000000..131b982
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_ctype.h
@@ -0,0 +1,29 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - public header file
+=============================================================================*/
+
+#ifndef VCOS_CTYPE_H
+#define VCOS_CTYPE_H
+
+/**
+  * \file
+  *
+  * ctype functions.
+  *
+  */
+
+#ifdef __KERNEL__
+#include <linux/ctype.h>
+#else
+#include <ctype.h>
+#endif
+
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_dlfcn.h b/drivers/misc/vc04_services/interface/vcos/vcos_dlfcn.h
new file mode 100644
index 0000000..456b08e
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_dlfcn.h
@@ -0,0 +1,69 @@
+/*=============================================================================
+Copyright (c) 2010 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VCOS - abstraction over dynamic library opening
+=============================================================================*/
+
+#ifndef VCOS_DLFCN_H
+#define VCOS_DLFCN_H
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define VCOS_DL_LAZY 1
+#define VCOS_DL_NOW  2
+
+/**
+ * \file
+ *
+ * Loading dynamic libraries. See also dlfcn.h.
+ */
+
+/** Open a dynamic library.
+  *
+  * @param name  name of the library
+  * @param mode  Load lazily or immediately (VCOS_DL_LAZY, VCOS_DL_NOW).
+  *
+  * @return A handle for use in subsequent calls.
+  */
+VCOSPRE_ void * VCOSPOST_ vcos_dlopen(const char *name, int mode);
+
+/** Look up a symbol.
+  *
+  * @param handle Handle to open
+  * @param name   Name of function
+  *
+  * @return Function pointer, or NULL.
+  */
+VCOSPRE_ void VCOSPOST_ (*vcos_dlsym(void *handle, const char *name))(void);
+
+/** Close a library
+  *
+  * @param handle Handle to close
+  */
+VCOSPRE_ int VCOSPOST_ vcos_dlclose (void *handle);
+
+/** Return error message from library.
+  *
+  * @param err  On return, set to non-zero if an error has occurred
+  * @param buf  Buffer to write error to
+  * @param len  Size of buffer (including terminating NUL).
+  */
+VCOSPRE_ int VCOSPOST_ vcos_dlerror(int *err, char *buf, size_t buflen);
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_event.h b/drivers/misc/vc04_services/interface/vcos/vcos_event.h
new file mode 100644
index 0000000..f335059
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_event.h
@@ -0,0 +1,97 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - public header file for events
+=============================================================================*/
+
+#ifndef VCOS_EVENT_H
+#define VCOS_EVENT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/** 
+  * \file
+  *
+  * An event is akin to the Win32 auto-reset event.
+  *
+  *
+  * Signalling an event will wake up one waiting thread only. Once one
+  * thread has been woken the event atomically returns to the unsignalled
+  * state.
+  * 
+  * If no threads are waiting on the event when it is signalled it remains
+  * signalled.
+  *
+  * This is almost, but not quite, completely unlike the "event flags"
+  * object based on Nucleus event groups and ThreadX event flags.
+  *
+  * In particular, it should be similar in speed to a semaphore, unlike
+  * the event flags.
+  */
+
+/**
+  * Create an event instance.
+  *
+  * @param event  Filled in with constructed event.
+  * @param name   Name of the event (for debugging)
+  *
+  * @return VCOS_SUCCESS on success, or error code.
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_event_create(VCOS_EVENT_T *event, const char *name);
+
+#ifndef vcos_event_signal
+
+/**
+  * Signal the event. The event will return to being unsignalled
+  * after exactly one waiting thread has been woken up. If no
+  * threads are waiting it remains signalled.
+  *
+  * @param event The event to signal
+  */
+VCOS_INLINE_DECL
+void vcos_event_signal(VCOS_EVENT_T *event);
+
+/**
+  * Wait for the event.
+  *
+  * @param event The event to wait for
+  * @return VCOS_SUCCESS on success, VCOS_EAGAIN if the wait was interrupted.
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_event_wait(VCOS_EVENT_T *event);
+
+/**
+  * Try event, but don't block.
+  *
+  * @param event The event to try
+  * @return VCOS_SUCCESS on success, VCOS_EAGAIN if the event is not currently signalled
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_event_try(VCOS_EVENT_T *event);
+
+#endif
+
+/*
+ * Destroy an event.
+ */
+VCOS_INLINE_DECL
+void vcos_event_delete(VCOS_EVENT_T *event);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_event_flags.h b/drivers/misc/vc04_services/interface/vcos/vcos_event_flags.h
new file mode 100644
index 0000000..6223c48
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_event_flags.h
@@ -0,0 +1,98 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - public header file
+=============================================================================*/
+
+#ifndef VCOS_EVENT_FLAGS_H
+#define VCOS_EVENT_FLAGS_H
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+#define VCOS_EVENT_FLAGS_SUSPEND    VCOS_SUSPEND
+#define VCOS_EVENT_FLAGS_NO_SUSPEND VCOS_NO_SUSPEND
+typedef VCOS_OPTION VCOS_EVENTGROUP_OPERATION_T;
+
+/**
+ * \file vcos_event_flags.h
+ *
+ * Defines event flags API.
+ *
+ * Similar to Nucleus event groups.
+ *
+ * These have the same semantics as Nucleus event groups and ThreadX event
+ * flags. As such, they are quite complex internally; if speed is important
+ * they might not be your best choice.
+ *
+ */
+
+/**
+ * Create an event flags instance.
+ *
+ * @param flags   Pointer to event flags instance, filled in on return.
+ * @param name    Name for the event flags, used for debug.
+ *
+ * @return VCOS_SUCCESS if succeeded.
+ */
+
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_event_flags_create(VCOS_EVENT_FLAGS_T *flags, const char *name);
+
+/**
+  * Set some events.
+  *
+  * @param flags   Instance to set flags on
+  * @param events  Bitmask of the flags to actually set
+  * @param op      How the flags should be set. VCOS_OR will OR in the flags; VCOS_AND
+  *                will AND them in, possibly clearing existing flags.
+  */
+VCOS_INLINE_DECL
+void vcos_event_flags_set(VCOS_EVENT_FLAGS_T *flags,
+                          VCOS_UNSIGNED events,
+                          VCOS_OPTION op);
+
+/**
+ * Retrieve some events.
+ *
+ * Waits until the specified events have been set.
+ *
+ * @param flags            Instance to wait on
+ * @param requested_events The bitmask to wait for
+ * @param op               VCOS_OR - get any; VCOS_AND - get all.
+ * @param ms_suspend       How long to wait, in milliseconds
+ * @param retrieved_events the events actually retrieved.
+ *
+ * @return VCOS_SUCCESS if events were retrieved. VCOS_EAGAIN if the
+ * timeout expired.
+ */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_event_flags_get(VCOS_EVENT_FLAGS_T *flags,
+                                                     VCOS_UNSIGNED requested_events,
+                                                     VCOS_OPTION op,
+                                                     VCOS_UNSIGNED ms_suspend,
+                                                     VCOS_UNSIGNED *retrieved_events);
+
+
+/**
+ * Delete an event flags instance.
+ */
+VCOS_INLINE_DECL
+void vcos_event_flags_delete(VCOS_EVENT_FLAGS_T *);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_init.h b/drivers/misc/vc04_services/interface/vcos/vcos_init.h
new file mode 100644
index 0000000..e67327c
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_init.h
@@ -0,0 +1,43 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - initialization routines
+=============================================================================*/
+
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file
+  *
+  * Some OS support libraries need some initialization. To support this, call this
+  * function at the start of day.
+  */
+
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_init(void);
+VCOSPRE_ void VCOSPOST_ vcos_deinit(void);
+VCOSPRE_ void VCOSPOST_ vcos_global_lock(void);
+VCOSPRE_ void VCOSPOST_ vcos_global_unlock(void);
+
+/** Pass in the argv/argc arguments passed to main() */
+VCOSPRE_ void VCOSPOST_ vcos_set_args(int argc, const char **argv);
+
+/** Return argc. */
+VCOSPRE_ int VCOSPOST_ vcos_get_argc(void);
+
+/** Return argv. */
+VCOSPRE_ const char ** VCOSPOST_ vcos_get_argv(void);
+
+#ifdef __cplusplus
+}
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_logging.h b/drivers/misc/vc04_services/interface/vcos/vcos_logging.h
new file mode 100644
index 0000000..0c54781d
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_logging.h
@@ -0,0 +1,279 @@
+/*=============================================================================
+Copyright (c) 2009-2011 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - logging support
+=============================================================================*/
+
+#ifndef VCOS_LOGGING_H
+#define VCOS_LOGGING_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdarg.h>
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/**
+ * \file
+ *
+ * Logging support
+ *
+ * This provides categorised logging. Clients register
+ * a category, and then get a number of logging levels for
+ * that category.
+ *
+ * The logging level flag is tested using a flag *before* the
+ * function call, which makes logging very fast when disabled - there
+ * is no function call overhead just to find out that this log
+ * message is disabled.
+ *
+ * \section VCOS_LOG_CATEGORY
+ *
+ * As a convenience, clients define VCOS_LOG_CATEGORY to point to
+ * their category; the various vcos_log_xxx() macros then expand to
+ * use this.
+ *
+ * e.g.
+ *
+ *     #define VCOS_LOG_CATEGORY (&my_category)
+ *
+ *     #include <interface/vcos/vcos.h>
+ *
+ *     VCOS_LOG_CAT_T my_category;
+ *
+ *     ....
+ *
+ *     vcos_log_trace("Stuff happened: %d", n_stuff);
+ *
+ */
+
+/** Logging levels */
+typedef enum VCOS_LOG_LEVEL_T
+{
+   VCOS_LOG_UNINITIALIZED   = 0,
+   VCOS_LOG_NEVER,
+   VCOS_LOG_ERROR,
+   VCOS_LOG_WARN,
+   VCOS_LOG_INFO,
+   VCOS_LOG_TRACE,
+} VCOS_LOG_LEVEL_T;
+
+
+/** Initialize a logging category without going through vcos_log_register().
+ *
+ * This is useful for the case where there is no obvious point to do the
+ * registration (no initialization function for the module). However, it
+ * means that your logging category is not registered, so cannot be easily
+ * changed at run-time.
+ */
+#define VCOS_LOG_INIT(n,l) { l, n, 0, {0}, 0, 0 }
+
+/** A registered logging category.
+  */
+typedef struct VCOS_LOG_CAT_T
+{
+   VCOS_LOG_LEVEL_T level;      /** Which levels are enabled for this category */
+   const char *name;            /** Name for this category. */
+   struct VCOS_LOG_CAT_T *next;
+   struct {
+      unsigned int want_prefix:1;
+   } flags;
+   unsigned int refcount;
+   void *platform_data;         /** platform specific data */
+} VCOS_LOG_CAT_T;
+
+typedef void (*VCOS_VLOG_IMPL_FUNC_T)(const VCOS_LOG_CAT_T *cat, VCOS_LOG_LEVEL_T _level, const char *fmt, va_list args);
+
+/** Convert a VCOS_LOG_LEVEL_T into a printable string.
+  * The platform needs to implement this function.
+  */
+VCOSPRE_ const char * VCOSPOST_ vcos_log_level_to_string( VCOS_LOG_LEVEL_T level );
+
+/** Convert a string into a VCOS_LOG_LEVEL_T
+  * The platform needs to implement this function.
+  */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_string_to_log_level( const char *str, VCOS_LOG_LEVEL_T *level );
+
+/** Log a message. Basic API. Normal code should not use this.
+  * The platform needs to implement this function.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_log_impl(const VCOS_LOG_CAT_T *cat, VCOS_LOG_LEVEL_T _level, const char *fmt, ...) VCOS_FORMAT_ATTR_(printf, 3, 4);
+
+/** Log a message using a varargs parameter list. Normal code should
+  * not use this.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_vlog_impl(const VCOS_LOG_CAT_T *cat, VCOS_LOG_LEVEL_T _level, const char *fmt, va_list args) VCOS_FORMAT_ATTR_(printf, 3, 0);
+
+/** Set the function which does the actual logging output.
+ *  Passing in NULL causes the default logging function to be
+ *  used.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_set_vlog_impl( VCOS_VLOG_IMPL_FUNC_T vlog_impl_func );
+
+/** The default logging function, which is provided by each
+  * platform.
+  */
+
+VCOSPRE_ void VCOSPOST_ vcos_vlog_default_impl(const VCOS_LOG_CAT_T *cat, VCOS_LOG_LEVEL_T _level, const char *fmt, va_list args) VCOS_FORMAT_ATTR_(printf, 3, 0);
+
+/*
+ * Initialise the logging subsystem. This is called from
+ * vcos_init() so you don't normally need to call it.
+ */
+VCOSPRE_ void VCOSPOST_ vcos_logging_init(void);
+
+/** Register a logging category.
+  *
+  * @param name the name of this category.
+  * @param category the category to register.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_log_register(const char *name, VCOS_LOG_CAT_T *category);
+
+/** Unregister a logging category.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_log_unregister(VCOS_LOG_CAT_T *category);
+
+/** Return a default logging category, for people too lazy to create their own.
+  *
+  * Using the default category will be slow (there's an extra function
+  * call overhead). Don't do this in normal code.
+  */
+VCOSPRE_ const VCOS_LOG_CAT_T * VCOSPOST_ vcos_log_get_default_category(void);
+
+VCOSPRE_ void VCOSPOST_ vcos_set_log_options(const char *opt);
+
+/** Set the logging level for a category at run time. Without this, the level
+  * will be that set by vcos_log_register from a platform-specific source.
+  *
+  * @param category the category to modify.
+  * @param level the new logging level for this category.
+  */
+VCOS_STATIC_INLINE void vcos_log_set_level(VCOS_LOG_CAT_T *category, VCOS_LOG_LEVEL_T level)
+{
+   category->level = level;
+}
+
+#define vcos_log_dump_mem(cat,label,addr,voidMem,numBytes)  do { if (vcos_is_log_enabled(cat,VCOS_LOG_TRACE)) vcos_log_dump_mem_impl(cat,label,addr,voidMem,numBytes); } while (0)
+
+void vcos_log_dump_mem_impl( const VCOS_LOG_CAT_T *cat,
+                             const char           *label,
+                             uint32_t              addr,
+                             const void           *voidMem,
+                             size_t                numBytes );
+
+/*
+ * Platform specific hooks (optional).
+ */
+#ifndef vcos_log_platform_init
+#define vcos_log_platform_init()                (void)0
+#endif
+
+#ifndef vcos_log_platform_register
+#define vcos_log_platform_register(category)    (void)0
+#endif
+
+#ifndef vcos_log_platform_unregister
+#define vcos_log_platform_unregister(category)  (void)0
+#endif
+
+/* VCOS_TRACE() - deprecated macro which just outputs in a debug build and
+ * is a no-op in a release build.
+ *
+ * _VCOS_LOG_X() - internal macro which outputs if the current level for the
+ * particular category is higher than the supplied message level.
+ */
+
+#define VCOS_LOG_DFLT_CATEGORY vcos_log_get_default_category()
+
+#define _VCOS_LEVEL(x) (x)
+
+#define vcos_is_log_enabled(cat,_level)  (_VCOS_LEVEL((cat)->level) >= _VCOS_LEVEL(_level))
+
+#if defined(_VCOS_METAWARE) || defined(__GNUC__)
+
+# if !defined(NDEBUG) || defined(VCOS_ALWAYS_WANT_LOGGING)
+#  define VCOS_LOGGING_ENABLED
+#  define _VCOS_LOG_X(cat, _level, fmt...)   do { if (vcos_is_log_enabled(cat,_level)) vcos_log_impl(cat,_level,fmt); } while (0)
+#  define _VCOS_VLOG_X(cat, _level, fmt, ap) do { if (vcos_is_log_enabled(cat,_level)) vcos_vlog_impl(cat,_level,fmt,ap); } while (0)
+# else
+#  define _VCOS_LOG_X(cat, _level, fmt...) (void)0
+#  define _VCOS_VLOG_X(cat, _level, fmt, ap) (void)0
+# endif
+
+
+
+# define vcos_log_error(...)   _VCOS_LOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_ERROR, __VA_ARGS__)
+# define vcos_log_warn(...)    _VCOS_LOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_WARN, __VA_ARGS__)
+# define vcos_log_info(...)    _VCOS_LOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_INFO, __VA_ARGS__)
+# define vcos_log_trace(...)   _VCOS_LOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_TRACE, __VA_ARGS__)
+
+# define vcos_vlog_error(fmt,ap)  _VCOS_VLOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_ERROR, fmt, ap)
+# define vcos_vlog_warn(fmt,ap)   _VCOS_VLOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_WARN, fmt, ap)
+# define vcos_vlog_info(fmt,ap)   _VCOS_VLOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_INFO, fmt, ap)
+# define vcos_vlog_trace(fmt,ap)  _VCOS_VLOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_TRACE, fmt, ap)
+
+# define vcos_log(...)   _VCOS_LOG_X(VCOS_LOG_DFLT_CATEGORY, VCOS_LOG_INFO, __VA_ARGS__)
+# define vcos_vlog(fmt,ap)  _VCOS_VLOG_X(VCOS_LOG_DFLT_CATEGORY, VCOS_LOG_INFO, fmt, ap)
+# define VCOS_ALERT(...) _VCOS_LOG_X(VCOS_LOG_DFLT_CATEGORY, VCOS_LOG_ERROR, __VA_ARGS__)
+# define VCOS_TRACE(...) _VCOS_LOG_X(VCOS_LOG_DFLT_CATEGORY, VCOS_LOG_INFO, __VA_ARGS__)
+
+/*
+ * MS Visual Studio - pre 2005 does not grok variadic macros
+ */
+#elif defined(_MSC_VER)
+
+# if _MSC_VER >= 1400
+
+#  if !defined(NDEBUG) || defined(VCOS_ALWAYS_WANT_LOGGING)
+#   define VCOS_LOGGING_ENABLED
+#   define _VCOS_LOG_X(cat, _level, fmt,...) do { if (vcos_is_log_enabled(cat,_level)) vcos_log_impl(cat, _level, fmt, __VA_ARGS__); } while (0)
+#  else
+#   define _VCOS_LOG_X(cat, _level, fmt,...) (void)0
+#  endif
+
+# define vcos_log_error(fmt,...)   _VCOS_LOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_ERROR, fmt, __VA_ARGS__)
+# define vcos_log_warn(fmt,...)    _VCOS_LOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_WARN, fmt, __VA_ARGS__)
+# define vcos_log_info(fmt,...)    _VCOS_LOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_INFO, fmt, __VA_ARGS__)
+# define vcos_log_trace(fmt,...)   _VCOS_LOG_X(VCOS_LOG_CATEGORY, VCOS_LOG_TRACE, fmt, __VA_ARGS__)
+
+# define vcos_log(fmt,...)   _VCOS_LOG_X(VCOS_LOG_DFLT_CATEGORY, VCOS_LOG_INFO, fmt)
+# define VCOS_ALERT(fmt,...) _VCOS_LOG_X(VCOS_LOG_DFLT_CATEGORY, VCOS_LOG_ERROR, fmt)
+# define VCOS_TRACE(fmt,...) _VCOS_LOG_X(VCOS_LOG_DFLT_CATEGORY, VCOS_LOG_INFO, fmt)
+
+# else /* _MSC_VER >= 1400 */
+
+/* do not define these */
+
+# endif /* _MSC_VER >= 1400 */
+
+#endif
+
+#if VCOS_HAVE_CMD
+
+#include "interface/vcos/vcos_cmd.h"
+
+/*
+ * These are the log sub-commands. They're exported here for user-mode apps which 
+ * may want to call these, since the "log" command isn't registered for user-mode 
+ * apps (vcdbg for example, has its own log command). 
+ */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_log_assert_cmd( VCOS_CMD_PARAM_T *param );
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_log_set_cmd( VCOS_CMD_PARAM_T *param );
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_log_status_cmd( VCOS_CMD_PARAM_T *param );
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_log_test_cmd( VCOS_CMD_PARAM_T *param );
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* VCOS_LOGGING_H */
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_lowlevel_thread.h b/drivers/misc/vc04_services/interface/vcos/vcos_lowlevel_thread.h
new file mode 100644
index 0000000..658a30b
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_lowlevel_thread.h
@@ -0,0 +1,107 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - low level thread support
+=============================================================================*/
+
+#ifndef VCOS_LOWLEVEL_THREAD_H
+#define VCOS_LOWLEVEL_THREAD_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/**
+ * \file
+ *
+ * This defines a low level thread API that is supported by *some* operating systems
+ * and can be used to construct the regular "joinable thread" API on those operating
+ * systems.
+ *
+ * Most clients will not need to use this code.
+ *
+ * \sa vcos_joinable_thread.h
+ */
+
+/**
+  * \brief Create a thread.
+  *
+  * This creates a thread which can be stopped either by returning from the
+  * entry point function or by calling vcos_llthread_exit from within the entry
+  * point function. The thread must be cleaned up by calling
+  * vcos_llthread_delete. vcos_llthread_delete may or may not terminate the
+  * thread.
+  *
+  * The preemptible parameter familiar from Nucleus is removed, as it is unused in
+  *  VideoCore code. Affinity is added, since we do use this.
+  *
+  * @param thread       Filled in with thread instance
+  * @param name         An optional name for the thread. "" may be used (but
+  *                     a name will aid in debugging).
+  * @param entry        Entry point
+  * @param arg          A single argument passed to the entry point function
+  * @param stack        Pointer to stack address
+  * @param stacksz      Size of stack in bytes
+  * @param priority     Priority of task, between VCOS_PRI_LOW and VCOS_PRI_HIGH
+  * @param affinity     CPU affinity
+  *
+  * @sa vcos_llthread_terminate vcos_llthread_delete
+  */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_llthread_create(VCOS_LLTHREAD_T *thread,
+                                                      const char *name,
+                                                      VCOS_LLTHREAD_ENTRY_FN_T entry,
+                                                      void *arg,
+                                                      void *stack,
+                                                      VCOS_UNSIGNED stacksz,
+                                                      VCOS_UNSIGNED priority,
+                                                      VCOS_UNSIGNED affinity,
+                                                      VCOS_UNSIGNED timeslice,
+                                                      VCOS_UNSIGNED autostart);
+
+/**
+  * \brief Exits the current thread.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_llthread_exit(void);
+
+/**
+  * \brief Delete a thread. This must be called to cleanup after
+  * vcos_llthread_create. This may or may not terminate the thread.
+  * It does not clean up any resources that may have been
+  * allocated by the thread.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_llthread_delete(VCOS_LLTHREAD_T *thread);
+
+/**
+  * \brief Return current lowlevel thread pointer.
+  */
+VCOS_INLINE_DECL
+VCOS_LLTHREAD_T *vcos_llthread_current(void);
+
+/**
+  * Resume a thread.
+  */
+VCOS_INLINE_DECL
+void vcos_llthread_resume(VCOS_LLTHREAD_T *thread);
+
+VCOSPRE_ int VCOSPOST_ vcos_llthread_running(VCOS_LLTHREAD_T *thread);
+
+/**
+  * \brief Create a VCOS_LLTHREAD_T for the current thread. This is so we can
+  * have VCOS_LLTHREAD_Ts even for threads not originally created by VCOS (eg
+  * the thread that calls vcos_init).
+  */
+extern VCOS_STATUS_T _vcos_llthread_create_attach(VCOS_LLTHREAD_T *thread);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_mem.h b/drivers/misc/vc04_services/interface/vcos/vcos_mem.h
new file mode 100644
index 0000000..99860db5
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_mem.h
@@ -0,0 +1,81 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - memory support
+=============================================================================*/
+
+#ifndef VCOS_MEM_H
+#define VCOS_MEM_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/** \file
+  *
+  * Memory allocation api (malloc/free equivalents) is for benefit of host
+  * applications. VideoCore code should use rtos_XXX functions.
+  *
+  */
+
+
+/** Allocate memory
+  *
+  * @param size Size of memory to allocate
+  * @param description Description, to aid in debugging. May be ignored internally on some platforms.
+  */
+VCOS_INLINE_DECL
+void *vcos_malloc(VCOS_UNSIGNED size, const char *description);
+
+void *vcos_kmalloc(VCOS_UNSIGNED size, const char *description);
+void *vcos_kcalloc(VCOS_UNSIGNED num, VCOS_UNSIGNED size, const char *description);
+
+/** Allocate cleared memory
+  *
+  * @param num Number of items to allocate.
+  * @param size Size of each item in bytes.
+  * @param description Description, to aid in debugging. May be ignored internally on some platforms.
+  */
+VCOS_INLINE_DECL
+void *vcos_calloc(VCOS_UNSIGNED num, VCOS_UNSIGNED size, const char *description);
+
+/** Free memory
+  *
+  * Free memory that has been allocated.
+  */
+VCOS_INLINE_DECL
+void vcos_free(void *ptr);
+
+void vcos_kfree(void *ptr);
+
+/** Allocate aligned memory
+  *
+  * Allocate memory aligned on the specified boundary.
+  *
+  * @param size Size of memory to allocate
+  * @param description Description, to aid in debugging. May be ignored internally on some platforms.
+  */
+VCOS_INLINE_DECL
+void *vcos_malloc_aligned(VCOS_UNSIGNED size, VCOS_UNSIGNED align, const char *description);
+
+/** Return the amount of free heap memory
+  *
+  */
+VCOS_INLINE_DECL
+unsigned long vcos_get_free_mem(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_msgqueue.h b/drivers/misc/vc04_services/interface/vcos/vcos_msgqueue.h
new file mode 100644
index 0000000..31f77c1
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_msgqueue.h
@@ -0,0 +1,157 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VCOS - packet-like messages, based loosely on those found in TRIPOS.
+=============================================================================*/
+
+#ifndef VCOS_MSGQUEUE_H
+#define VCOS_MSGQUEUE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/**
+ * \file
+ *
+ * Packet-like messages, based loosely on those found in TRIPOS and
+ * derivatives thereof.
+ *
+ * A task can send a message *pointer* to another task, where it is
+ * queued on a linked list and the task woken up. The receiving task
+ * consumes all of the messages on its input queue, and optionally
+ * sends back replies using the original message memory.
+ *
+ * A caller can wait for the reply to a specific message - any other
+ * messages that arrive in the meantime are queued separately.
+ *
+ *
+ * All messages have a standard common layout, but the payload area can
+ * be used freely to extend this.
+ */
+
+/** Map the payload portion of a message to a structure pointer.
+  */
+#define VCOS_MSG_DATA(_msg) (void*)((_msg)->data)
+
+/** Standard message ids - FIXME - these need to be done properly! */
+#define VCOS_MSG_N_QUIT            1
+#define VCOS_MSG_N_OPEN            2
+#define VCOS_MSG_N_CLOSE           3
+#define VCOS_MSG_N_PRIVATE         (1<<20)
+
+#define VCOS_MSG_REPLY_BIT         (1<<31)
+
+/** Make gnuc compiler be happy about pointer punning */
+#ifdef __GNUC__
+#define __VCOS_MAY_ALIAS __attribute__((__may_alias__))
+#else
+#define __VCOS_MAY_ALIAS
+#endif
+
+/** A single message queue.
+  */
+typedef struct VCOS_MSGQUEUE_T
+{
+   struct VCOS_MSG_T *head;            /**< head of linked list of messages waiting on this queue */
+   struct VCOS_MSG_T *tail;            /**< tail of message queue */
+   VCOS_SEMAPHORE_T sem;               /**< thread waits on this for new messages */
+   VCOS_MUTEX_T lock;                  /**< locks the messages list */
+} VCOS_MSGQUEUE_T;
+
+/** A single message
+  */
+typedef struct VCOS_MSG_T
+{
+   uint32_t code;                      /**< message code */
+   int error;                          /**< error status signalled back to caller */
+   VCOS_MSGQUEUE_T *dst;               /**< destination queue */
+   VCOS_MSGQUEUE_T *src;               /**< source; replies go back to here */
+   struct VCOS_MSG_T *next;            /**< next in queue */
+   VCOS_THREAD_T *src_thread;          /**< for debug */
+   uint32_t data[25];                  /**< payload area */
+} VCOS_MSG_T;
+   
+/** An endpoint
+  */
+typedef struct VCOS_MSG_ENDPOINT_T
+{
+   VCOS_MSGQUEUE_T primary;            /**< incoming messages */
+   VCOS_MSGQUEUE_T secondary;          /**< this is used for waitspecific */
+   char name[32];                      /**< name of this endpoint, for find() */
+   struct VCOS_MSG_ENDPOINT_T *next;   /**< next in global list of endpoints */
+} VCOS_MSG_ENDPOINT_T;
+#define MSG_REPLY_BIT (1<<31)
+
+/** Initalise the library. Normally called from vcos_init().
+  */
+extern VCOS_STATUS_T vcos_msgq_init(void);
+
+/** Find a message queue by name and get a handle to it.
+  *
+  * @param name  the name of the queue to find
+  *
+  * @return The message queue, or NULL if not found.
+  */
+VCOSPRE_ VCOS_MSGQUEUE_T VCOSPOST_ *vcos_msgq_find(const char *name);
+
+/** Wait for a message queue to come into existence. If it already exists,
+  * return immediately, otherwise block.
+  *
+  * On the whole, if you find yourself using this, it is probably a sign
+  * of poor design, since you should create all the server threads first,
+  * and then the client threads. But it is sometimes useful.
+  *
+  * @param name  the name of the queue to find
+  * @return The message queue
+  */
+VCOSPRE_ VCOS_MSGQUEUE_T VCOSPOST_ *vcos_msgq_wait(const char *name);
+
+/** Send a message.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_msg_send(VCOS_MSGQUEUE_T *dest, uint32_t code, VCOS_MSG_T *msg);
+
+/** Send a message and wait for a reply.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_msg_sendwait(VCOS_MSGQUEUE_T *queue, uint32_t code, VCOS_MSG_T *msg);
+
+/** Wait for a message on this thread's endpoint.
+  */
+VCOSPRE_ VCOS_MSG_T * VCOSPOST_ vcos_msg_wait(void);
+
+/** Wait for a specific message.
+  */
+VCOS_MSG_T * vcos_msg_wait_specific(VCOS_MSGQUEUE_T *queue, VCOS_MSG_T *msg);
+
+/** Peek for a message on this thread's endpoint, if a message is not available, NULL is 
+    returned. If a message is available it will be removed from the endpoint and returned.
+  */
+VCOSPRE_ VCOS_MSG_T * VCOSPOST_ vcos_msg_peek(void);
+
+/** Send a reply to a message
+  */
+VCOSPRE_ void VCOSPOST_ vcos_msg_reply(VCOS_MSG_T *msg);
+
+/** Create an endpoint. Each thread should need no more than one of these - if you 
+  * find yourself needing a second one, you've done something wrong.
+  */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_msgq_endpoint_create(VCOS_MSG_ENDPOINT_T *ep, const char *name);
+
+/** Destroy an endpoint.
+  */
+VCOSPRE_ void  VCOSPOST_ vcos_msgq_endpoint_delete(VCOS_MSG_ENDPOINT_T *ep);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_mutex.h b/drivers/misc/vc04_services/interface/vcos/vcos_mutex.h
new file mode 100644
index 0000000..5347ab2
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_mutex.h
@@ -0,0 +1,92 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - mutex public header file
+=============================================================================*/
+
+#ifndef VCOS_MUTEX_H
+#define VCOS_MUTEX_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/**
+ * \file vcos_mutex.h
+ *
+ * Mutex API. Mutexes are not re-entrant, as supporting this adds extra code
+ * that slows down clients which have been written sensibly.
+ *
+ * \sa vcos_reentrant_mutex.h
+ *
+ */
+
+/** Create a mutex.
+  *
+  * @param m      Filled in with mutex on return
+  * @param name   A non-null name for the mutex, used for diagnostics.
+  *
+  * @return VCOS_SUCCESS if mutex was created, or error code.
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_mutex_create(VCOS_MUTEX_T *m, const char *name);
+
+/** Delete the mutex.
+  */
+VCOS_INLINE_DECL
+void vcos_mutex_delete(VCOS_MUTEX_T *m);
+
+/**
+  * \brief Wait to claim the mutex.
+  *
+  * On most platforms this always returns VCOS_SUCCESS, and so would ideally be
+  * a void function, however some platforms allow a wait to be interrupted so
+  * it remains non-void.
+  *
+  * Try to obtain the mutex.
+  * @param m   Mutex to wait on
+  * @return VCOS_SUCCESS - mutex was taken.
+  *         VCOS_EAGAIN  - could not take mutex.
+  */
+#ifndef vcos_mutex_lock
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_mutex_lock(VCOS_MUTEX_T *m);
+
+/** Release the mutex.
+  */
+VCOS_INLINE_DECL
+void vcos_mutex_unlock(VCOS_MUTEX_T *m);
+#endif
+
+/** Test if the mutex is already locked.
+  *
+  * @return 1 if mutex is locked, 0 if it is unlocked.
+  */
+VCOS_INLINE_DECL
+int vcos_mutex_is_locked(VCOS_MUTEX_T *m);
+
+/** Obtain the mutex if possible.
+  *
+  * @param m  the mutex to try to obtain
+  *
+  * @return VCOS_SUCCESS if mutex is succesfully obtained, or VCOS_EAGAIN
+  * if it is already in use by another thread.
+  */
+#ifndef vcos_mutex_trylock
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_mutex_trylock(VCOS_MUTEX_T *m);
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_once.h b/drivers/misc/vc04_services/interface/vcos/vcos_once.h
new file mode 100644
index 0000000..d12ac5e
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_once.h
@@ -0,0 +1,42 @@
+/*=============================================================================
+Copyright (c) 2011 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - 'once'
+=============================================================================*/
+
+#ifndef VCOS_ONCE_H
+#define VCOS_ONCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/**
+ * \file vcos_once.h
+ *
+ * Ensure something is called only once.
+ *
+ * Initialize once_control to VCOS_ONCE_INIT. The first
+ * time this is called, the init_routine will be called. Thereafter
+ * it won't.
+ *
+ * \sa pthread_once()
+ *
+ */
+
+VCOS_STATUS_T vcos_once(VCOS_ONCE_T *once_control,
+                        void (*init_routine)(void));
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_semaphore.h b/drivers/misc/vc04_services/interface/vcos/vcos_semaphore.h
new file mode 100644
index 0000000..5a32031
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_semaphore.h
@@ -0,0 +1,115 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - public header file
+=============================================================================*/
+
+#ifndef VCOS_SEMAPHORE_H
+#define VCOS_SEMAPHORE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/**
+ * \file vcos_semaphore.h
+ *
+ * \section sem Semaphores
+ *
+ * This provides counting semaphores. Semaphores are not re-entrant. On sensible
+ * operating systems a semaphore can always be posted but can only be taken in 
+ * thread (not interrupt) context. Under Nucleus, a LISR cannot post a semaphore,
+ * although it would not be hard to lift this restriction.
+ *
+ * \subsection timeout Timeout
+ *
+ * On both Nucleus and ThreadX a semaphore can be taken with a timeout. This is
+ * not supported by VCOS because it makes the non-timeout code considerably more
+ * complicated (and hence slower). In the unlikely event that you need a timeout
+ * with a semaphore, and you cannot simply redesign your code to avoid it, use
+ * an event flag (vcos_event_flags.h).
+ *
+ * \subsection sem_nucleus Changes from Nucleus:
+ *
+ *  Semaphores are always "FIFO" - i.e. sleeping threads are woken in FIFO order. That's
+ *  because:
+ *  \arg there's no support for NU_PRIORITY in threadx (though it can be emulated, slowly)
+ *  \arg we don't appear to actually consciously use it - for example, Dispmanx uses
+ *  it, but all threads waiting are the same priority.
+ *         
+ */
+
+/** 
+  * \brief Create a semaphore.
+  *
+  * Create a semaphore.
+  *
+  * @param sem   Pointer to memory to be initialized
+  * @param name  A name for this semaphore. The name may be truncated internally.
+  * @param count The initial count for the semaphore.
+  *
+  * @return VCOS_SUCCESS if the semaphore was created.
+  * 
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_semaphore_create(VCOS_SEMAPHORE_T *sem, const char *name, VCOS_UNSIGNED count);
+
+/**
+  * \brief Wait on a semaphore.
+  *
+  * There is no timeout option on a semaphore, as adding this will slow down
+  * implementations on some platforms. If you need that kind of behaviour, use
+  * an event group.
+  *
+  * On most platforms this always returns VCOS_SUCCESS, and so would ideally be
+  * a void function, however some platforms allow a wait to be interrupted so
+  * it remains non-void.
+  *
+  * @param sem Semaphore to wait on
+  * @return VCOS_SUCCESS - semaphore was taken.
+  *         VCOS_EAGAIN  - could not take semaphore
+  *
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_semaphore_wait(VCOS_SEMAPHORE_T *sem);
+
+/**
+  * \brief Try to wait for a semaphore.
+  *
+  * Try to obtain the semaphore. If it is already taken, return VCOS_TIMEOUT.
+  * @param sem Semaphore to wait on
+  * @return VCOS_SUCCESS - semaphore was taken.
+  *         VCOS_EAGAIN - could not take semaphore
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_semaphore_trywait(VCOS_SEMAPHORE_T *sem);
+
+/**
+  * \brief Post a semaphore.
+  *
+  * @param sem Semaphore to wait on
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_semaphore_post(VCOS_SEMAPHORE_T *sem);
+
+/**
+  * \brief Delete a semaphore, releasing any resources consumed by it.
+  *
+  * @param sem Semaphore to wait on
+  */
+VCOS_INLINE_DECL
+void vcos_semaphore_delete(VCOS_SEMAPHORE_T *sem);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_stdbool.h b/drivers/misc/vc04_services/interface/vcos/vcos_stdbool.h
new file mode 100644
index 0000000..f1de3cb
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_stdbool.h
@@ -0,0 +1,17 @@
+#ifndef VCOS_STDBOOL_H
+#define VCOS_STDBOOL_H
+
+#ifndef __cplusplus
+
+#if defined(__STDC__) && (__STDC_VERSION__ >= 199901L)
+#include <stdbool.h>
+#else
+typedef enum {
+   false,
+   true
+} bool;
+#endif
+
+#endif /* __cplusplus */
+
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_stdint.h b/drivers/misc/vc04_services/interface/vcos/vcos_stdint.h
new file mode 100644
index 0000000..537ec33
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_stdint.h
@@ -0,0 +1,193 @@
+/*=============================================================================
+Copyright (c) 2011 Broadcom Europe Limited.
+All rights reserved.
+
+FILE DESCRIPTION
+
+=============================================================================*/
+
+#ifndef VCOS_STDINT_H
+#define VCOS_STDINT_H
+
+/* Attempt to provide the types defined in stdint.h.
+ *
+ * Ideally this would either call out to a platform-specific
+ * header file (e.g. stdint.h) or define the types on a
+ * per-architecture/compiler basis. But for now we just
+ * use #ifdefs.
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef __SYMBIAN32__
+
+typedef signed   char      int8_t;
+typedef unsigned char      uint8_t;
+
+typedef signed   short     int16_t;
+typedef unsigned short     uint16_t;
+
+typedef int16_t            int_least16_t;
+
+typedef signed   long      int32_t;
+typedef unsigned long      uint32_t;
+
+typedef signed long long   int64_t;
+typedef unsigned long long uint64_t;
+
+typedef int32_t            intptr_t;
+typedef uint32_t           uintptr_t;
+
+typedef int64_t            intmax_t;
+typedef uint64_t           uintmax_t;
+
+#define INT8_MIN SCHAR_MIN
+#define INT8_MAX SCHAR_MAX
+#define UINT8_MAX UCHAR_MAX
+#define INT16_MIN SHRT_MIN
+#define INT16_MAX SHRT_MAX
+#define UINT16_MAX USHRT_MAX
+#define INT32_MIN LONG_MIN
+#define INT32_MAX LONG_MAX
+#define UINT32_MAX ULONG_MAX
+#define INT64_MIN LLONG_MIN
+#define INT64_MAX LLONG_MAX
+#define UINT64_MAX ULLONG_MAX
+
+#define INTPTR_MIN INT32_MIN
+#define INTPTR_MAX INT32_MAX
+#define UINTPTR_MAX UINT32_MAX
+#define INTMAX_MIN INT64_MIN
+#define INTMAX_MAX INT64_MAX
+#define INT_LEAST16_MAX INT16_MAX
+#define INT_LEAST16_MAX INT16_MAX
+
+/*{{{ C99 types  - THIS WHOLE SECTION IS INCOMPATIBLE WITH C99. IT SHOULD RESIDE IN A STDINT.H SINCE THIS FILE GETS USED ON HOST SIDE */
+
+#elif defined( __STDC__ ) && __STDC_VERSION__ >= 199901L
+
+#include <stdint.h>
+
+#elif defined( __GNUC__ )
+
+#include <stdint.h>
+
+#elif defined(_MSC_VER)                     /* Visual C define equivalent types */
+
+#include <stddef.h> /* Avoids intptr_t being defined in vadefs.h */
+
+typedef          __int8    int8_t;
+typedef unsigned __int8    uint8_t;
+
+typedef          __int16   int16_t;
+typedef unsigned __int16   uint16_t;
+
+typedef          __int32   int32_t;
+typedef unsigned __int32   uint32_t;
+
+typedef          __int64   int64_t;
+typedef unsigned __int64   uint64_t;
+typedef uint32_t           uintptr_t;
+typedef int64_t            intmax_t;
+typedef uint64_t           uintmax_t;
+typedef int16_t            int_least16_t;
+
+#elif defined (VCMODS_LCC)
+#include <limits.h>
+
+typedef signed   char      int8_t;
+typedef unsigned char      uint8_t;
+
+typedef signed   short     int16_t;
+typedef unsigned short     uint16_t;
+
+typedef signed   long      int32_t;
+typedef unsigned long      uint32_t;
+
+typedef signed   long      int64_t; /*!!!! PFCD, this means code using 64bit numbers will be broken on the VCE */
+typedef unsigned long      uint64_t; /* !!!! PFCD */
+
+typedef int32_t            intptr_t;
+typedef uint32_t           uintptr_t;
+typedef int64_t            intmax_t;
+typedef uint64_t           uintmax_t;
+typedef int16_t            int_least16_t;
+
+#define INT8_MIN SCHAR_MIN
+#define INT8_MAX SCHAR_MAX
+#define UINT8_MAX UCHAR_MAX
+#define INT16_MIN SHRT_MIN
+#define INT16_MAX SHRT_MAX
+#define UINT16_MAX USHRT_MAX
+#define INT32_MIN LONG_MIN
+#define INT32_MAX LONG_MAX
+#define UINT32_MAX ULONG_MAX
+#define INT64_MIN LONG_MIN /* !!!! PFCD */
+#define INT64_MAX LONG_MAX /* !!!! PFCD */
+#define UINT64_MAX ULONG_MAX /* !!!! PFCD */
+
+#define INTPTR_MIN INT32_MIN
+#define INTPTR_MAX INT32_MAX
+#define UINTPTR_MAX UINT32_MAX
+#define INTMAX_MIN INT64_MIN
+#define INTMAX_MIN INT64_MIN
+#define INT_LEAST16_MAX INT16_MAX
+#define INT_LEAST16_MAX INT16_MAX
+
+#elif defined(__VIDEOCORE__)
+
+typedef signed   char      int8_t;
+typedef unsigned char      uint8_t;
+
+typedef signed   short     int16_t;
+typedef unsigned short     uint16_t;
+
+typedef signed   long      int32_t;
+typedef unsigned long      uint32_t;
+
+typedef signed   long long int64_t;
+typedef unsigned long long uint64_t;
+
+typedef int32_t            intptr_t;
+typedef uint32_t           uintptr_t;
+typedef int64_t            intmax_t;
+typedef uint64_t           uintmax_t;
+typedef int16_t            int_least16_t;
+
+#define INT8_MIN SCHAR_MIN
+#define INT8_MAX SCHAR_MAX
+#define UINT8_MAX UCHAR_MAX
+#define INT16_MIN SHRT_MIN
+#define INT16_MAX SHRT_MAX
+#define UINT16_MAX USHRT_MAX
+#define INT32_MIN LONG_MIN
+#define INT32_MAX LONG_MAX
+#define UINT32_MAX ULONG_MAX
+#define INT64_MIN LLONG_MIN
+#define INT64_MAX LLONG_MAX
+#define UINT64_MAX ULLONG_MAX
+
+#define INTPTR_MIN INT32_MIN
+#define INTPTR_MAX INT32_MAX
+#define UINTPTR_MAX UINT32_MAX
+#define INTMAX_MIN INT64_MIN
+#define INTMAX_MAX INT64_MAX
+#define INT_LEAST16_MAX INT16_MAX
+#define INT_LEAST16_MAX INT16_MAX
+
+#elif defined (__HIGHC__) && defined(_I386)
+
+#include <stdint.h>
+
+#else
+#error Unknown platform
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* VCOS_STDINT_H */
+
+
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_string.h b/drivers/misc/vc04_services/interface/vcos/vcos_string.h
new file mode 100644
index 0000000..554b57d
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_string.h
@@ -0,0 +1,73 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - public header file
+=============================================================================*/
+
+#ifndef VCOS_STRING_H
+#define VCOS_STRING_H
+
+/**
+  * \file
+  *
+  * String functions.
+  *
+  */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+#ifdef __KERNEL__
+#include <linux/string.h>
+#else
+#include <string.h>
+#endif
+
+/** Case insensitive string comparison.
+  *
+  */
+
+VCOS_INLINE_DECL
+int vcos_strcasecmp(const char *s1, const char *s2);
+
+VCOS_INLINE_DECL
+int vcos_strncasecmp(const char *s1, const char *s2, size_t n);
+
+VCOSPRE_ int VCOSPOST_ vcos_vsnprintf( char *buf, size_t buflen, const char *fmt, va_list ap );
+
+VCOSPRE_ int VCOSPOST_ vcos_snprintf(char *buf, size_t buflen, const char *fmt, ...);
+
+VCOS_STATIC_INLINE
+int vcos_strlen(const char *s) { return (int)strlen(s); }
+
+VCOS_STATIC_INLINE
+int vcos_strcmp(const char *s1, const char *s2) { return strcmp(s1,s2); }
+
+VCOS_STATIC_INLINE
+int vcos_strncmp(const char *cs, const char *ct, size_t count) { return strncmp(cs, ct, count); }
+
+VCOS_STATIC_INLINE
+char *vcos_strcpy(char *dst, const char *src) { return strcpy(dst, src); }
+
+VCOS_STATIC_INLINE
+char *vcos_strncpy(char *dst, const char *src, size_t count) { return strncpy(dst, src, count); }
+
+VCOS_STATIC_INLINE
+void *vcos_memcpy(void *dst, const void *src, size_t n) {  memcpy(dst, src, n);  return dst;  }
+
+VCOS_STATIC_INLINE
+void *vcos_memset(void *p, int c, size_t n) { return memset(p, c, n); }
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_thread.h b/drivers/misc/vc04_services/interface/vcos/vcos_thread.h
new file mode 100644
index 0000000..08a76a0
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_thread.h
@@ -0,0 +1,259 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - public header file
+=============================================================================*/
+
+#ifndef VCOS_THREAD_H
+#define VCOS_THREAD_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/**
+ * \file vcos_thread.h
+ *
+ * \section thread Threads
+ *
+ * Under Nucleus, a thread is created by NU_Create_Task, passing in the stack
+ * and various other parameters. To stop the thread, NU_Terminate_Thread() and
+ * NU_Delete_Thread() are called.
+ *
+ * Unfortunately it's not possible to emulate this API under some fairly common
+ * operating systems. Under Windows you can't pass in the stack, and you can't
+ * safely terminate a thread.
+ *
+ * Therefore, an API which is similar to the pthreads API is used instead. This
+ * API can (mostly) be emulated under all interesting operating systems.
+ *
+ * Obviously this makes the code somewhat more complicated on VideoCore than it
+ * would otherwise be - we end up with an extra mutex per thread, and some code
+ * that waits for it. The benefit is that we have a single way of creating
+ * threads that works consistently on all platforms (apart from stack supplying).
+ *
+ * \subsection stack Stack
+ *
+ * It's still not possible to pass in the stack address, but this can be made
+ * much more obvious in the API: the relevant function is missing and the
+ * CPP symbol VCOS_CAN_SET_STACK_ADDR is zero rather than one.
+ *
+ * \subsection thr_create Creating a thread
+ *
+ * The simplest way to create a thread is with vcos_thread_create() passing in a
+ * NULL thread parameter argument. To wait for the thread to exit, call
+ * vcos_thread_join().
+ *
+ * \subsection back Backward compatibility
+ *
+ * To ease migration, a "classic" thread creation API is provided for code
+ * that used to make use of Nucleus, vcos_thread_create_classic(). The
+ * arguments are not exactly the same, as the PREEMPT parameter is dropped.
+ *
+ */
+
+#define VCOS_AFFINITY_CPU0    _VCOS_AFFINITY_CPU0
+#define VCOS_AFFINITY_CPU1    _VCOS_AFFINITY_CPU1
+#define VCOS_AFFINITY_MASK    _VCOS_AFFINITY_MASK
+#define VCOS_AFFINITY_DEFAULT _VCOS_AFFINITY_DEFAULT
+#define VCOS_AFFINITY_THISCPU _VCOS_AFFINITY_THISCPU
+
+/** Report whether or not we have an RTOS at all, and hence the ability to
+  * create threads.
+  */
+VCOSPRE_ int VCOSPOST_ vcos_have_rtos(void);
+
+/** Create a thread. It must be cleaned up by calling vcos_thread_join().
+  *
+  * @param thread   Filled in on return with thread
+  * @param name     A name for the thread. May be the empty string.
+  * @param attrs    Attributes; default attributes will be used if this is NULL.
+  * @param entry    Entry point.
+  * @param arg      Argument passed to the entry point.
+  */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_thread_create(VCOS_THREAD_T *thread,
+                                                    const char *name,
+                                                    VCOS_THREAD_ATTR_T *attrs,
+                                                    VCOS_THREAD_ENTRY_FN_T entry,
+                                                    void *arg);
+
+/** Exit the thread from within the thread function itself.
+  * Resources must still be cleaned up via a call to thread_join().
+  *
+  * The thread can also be terminated by simply exiting the thread function.
+  *
+  * @param data Data passed to thread_join. May be NULL.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_thread_exit(void *data);
+
+/** Wait for a thread to terminate and then clean up its resources.
+  *
+  * @param thread Thread to wait for
+  * @param pData  Updated to point at data provided in vcos_thread_exit or exit
+  * code of thread function.
+  */
+VCOSPRE_ void VCOSPOST_ vcos_thread_join(VCOS_THREAD_T *thread,
+                             void **pData);
+
+
+/**
+  * \brief Create a thread using an API similar to the one "traditionally"
+  * used under Nucleus.
+  *
+  * This creates a thread which must be cleaned up by calling vcos_thread_join().
+  * The thread cannot be simply terminated (as in Nucleus and ThreadX) as thread
+  * termination is not universally supported.
+  *
+  * @param thread       Filled in with thread instance
+  * @param name         An optional name for the thread. NULL or "" may be used (but
+  *                     a name will aid in debugging).
+  * @param entry        Entry point
+  * @param arg          A single argument passed to the entry point function
+  * @param stack        Pointer to stack address
+  * @param stacksz      Size of stack in bytes
+  * @param priaff       Priority of task, between VCOS_PRI_LOW and VCOS_PRI_HIGH, ORed with the CPU affinity
+  * @param autostart    If non-zero the thread will start immediately.
+  * @param timeslice    Timeslice (system ticks) for this thread.
+  *
+  * @sa vcos_thread_terminate vcos_thread_delete
+  */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_thread_create_classic(VCOS_THREAD_T *thread,
+                                                            const char *name,
+                                                            void *(*entry)(void *arg),
+                                                            void *arg,
+                                                            void *stack,
+                                                            VCOS_UNSIGNED stacksz,
+                                                            VCOS_UNSIGNED priaff,
+                                                            VCOS_UNSIGNED timeslice,
+                                                            VCOS_UNSIGNED autostart);
+
+/**
+  * \brief Set a thread's priority
+  *
+  * Set the priority for a thread.
+  *
+  * @param thread  The thread
+  * @param pri     Thread priority in VCOS_PRI_MASK bits; affinity in VCOS_AFFINITY_MASK bits.
+  */
+VCOS_INLINE_DECL
+void vcos_thread_set_priority(VCOS_THREAD_T *thread, VCOS_UNSIGNED pri);
+
+/**
+  * \brief Return the currently executing thread.
+  *
+  */
+VCOS_INLINE_DECL
+VCOS_THREAD_T *vcos_thread_current(void);
+
+/**
+  * \brief Return the thread's priority.
+  */
+VCOS_INLINE_DECL
+VCOS_UNSIGNED vcos_thread_get_priority(VCOS_THREAD_T *thread);
+
+/**
+  * \brief Return the thread's cpu affinity.
+  */
+VCOS_INLINE_DECL
+VCOS_UNSIGNED vcos_thread_get_affinity(VCOS_THREAD_T *thread);
+
+/**
+  * \brief Set the thread's cpu affinity.
+  */
+
+VCOS_INLINE_DECL
+void vcos_thread_set_affinity(VCOS_THREAD_T *thread, VCOS_UNSIGNED affinity);
+
+/**
+  * \brief Query whether we are in an interrupt.
+  *
+  * @return 1 if in interrupt context.
+  */
+VCOS_INLINE_DECL
+int vcos_in_interrupt(void);
+
+/**
+  * \brief Sleep a while.
+  *
+  * @param ms Number of milliseconds to sleep for
+  *
+  * This may actually sleep a whole number of ticks.
+  */
+VCOS_INLINE_DECL
+void vcos_sleep(uint32_t ms);
+
+/**
+  * \brief Return the value of the hardware microsecond counter.
+  *
+  */
+VCOS_INLINE_DECL
+uint32_t vcos_getmicrosecs(void);
+
+#define vcos_get_ms() (vcos_getmicrosecs()/1000)
+
+/**
+  * \brief Return a unique identifier for the current process
+  *
+  */
+VCOS_INLINE_DECL
+VCOS_UNSIGNED vcos_process_id_current(void);
+
+/** Relinquish this time slice. */
+VCOS_INLINE_DECL
+void vcos_thread_relinquish(void);
+
+/** Return the name of the given thread.
+  */
+VCOSPRE_ const char * VCOSPOST_ vcos_thread_get_name(const VCOS_THREAD_T *thread);
+
+/** Change preemption. This is almost certainly not what you want, as it won't
+  * work reliably in a multicore system: although you can affect the preemption
+  * on *this* core, you won't affect what's happening on the other core(s).
+  *
+  * It's mainly here to ease migration. If you're using it in new code, you
+  * probably need to think again.
+  *
+  * @param pe New preemption, VCOS_PREEMPT or VCOS_NO_PREEMPT
+  * @return Old value of preemption.
+  */
+VCOS_INLINE_DECL
+VCOS_UNSIGNED vcos_change_preemption(VCOS_UNSIGNED pe);
+
+/** Is a thread still running, or has it exited?
+  *
+  * Note: this exists for some fairly scary code in the video codec tests. Don't
+  * try to use it for anything else, as it may well not do what you expect.
+  *
+  * @param thread   thread to query
+  * @return non-zero if thread is running, or zero if it has exited.
+  */
+VCOS_INLINE_DECL
+int vcos_thread_running(VCOS_THREAD_T *thread);
+
+/** Resume a thread.
+  *
+  * @param thread thread to resume
+  */
+VCOS_INLINE_DECL
+void vcos_thread_resume(VCOS_THREAD_T *thread);
+
+/*
+ * Internal APIs - may not always be present and should not be used in
+ * client code.
+ */
+
+extern void _vcos_task_timer_set(void (*pfn)(void*), void *, VCOS_UNSIGNED ms);
+extern void _vcos_task_timer_cancel(void);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_thread_attr.h b/drivers/misc/vc04_services/interface/vcos/vcos_thread_attr.h
new file mode 100644
index 0000000..510991f
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_thread_attr.h
@@ -0,0 +1,73 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - thread attributes
+=============================================================================*/
+
+#ifndef VCOS_THREAD_ATTR_H
+#define VCOS_THREAD_ATTR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * \file
+ *
+ * Attributes for thread creation.
+ *
+ */
+
+/** Initialize thread attribute struct. This call does not allocate memory,
+  * and so cannot fail.
+  *
+  */
+VCOSPRE_ void VCOSPOST_ vcos_thread_attr_init(VCOS_THREAD_ATTR_T *attrs);
+
+/** Set the stack address and size. If not set, a stack will be allocated automatically.
+  *
+  * This can only be set on some platforms. It will always be possible to set the stack
+  * address on VideoCore, but on host platforms, support may well not be available.
+  */
+#if VCOS_CAN_SET_STACK_ADDR
+VCOS_INLINE_DECL
+void vcos_thread_attr_setstack(VCOS_THREAD_ATTR_T *attrs, void *addr, VCOS_UNSIGNED sz);
+#endif
+
+/** Set the stack size. If not set, a default size will be used. Attempting to call this after having
+  * set the stack location with vcos_thread_attr_setstack() will result in undefined behaviour.
+  */
+VCOS_INLINE_DECL
+void vcos_thread_attr_setstacksize(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED sz);
+
+/** Set the task priority. If not set, a default value will be used.
+  */
+VCOS_INLINE_DECL
+void vcos_thread_attr_setpriority(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED pri);
+
+/** Set the task cpu affinity. If not set, the default will be used.
+  */
+VCOS_INLINE_DECL
+void vcos_thread_attr_setaffinity(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED aff);
+
+/** Set the timeslice. If not set the default will be used.
+  */
+VCOS_INLINE_DECL
+void vcos_thread_attr_settimeslice(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED ts);
+
+/** The thread entry function takes (argc,argv), as per Nucleus, with
+  * argc being 0. This may be withdrawn in a future release and should not
+  * be used in new code.
+  */
+VCOS_INLINE_DECL
+void _vcos_thread_attr_setlegacyapi(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED legacy);
+
+VCOS_INLINE_DECL
+void vcos_thread_attr_setautostart(VCOS_THREAD_ATTR_T *attrs, VCOS_UNSIGNED autostart);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_timer.h b/drivers/misc/vc04_services/interface/vcos/vcos_timer.h
new file mode 100644
index 0000000..499adce
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_timer.h
@@ -0,0 +1,95 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+Project  :  vcfw
+Module   :  chip driver
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - timer support
+=============================================================================*/
+
+#ifndef VCOS_TIMER_H
+#define VCOS_TIMER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "interface/vcos/vcos_types.h"
+#include "vcos_platform.h"
+
+/** \file vcos_timer.h
+  *
+  * Timers are single shot.
+  *
+  * Timer times are in milliseconds.
+  *
+  * \note that timer callback functions are called from an arbitrary thread
+  * context. The expiration function should do its work as quickly as possible;
+  * blocking should be avoided.
+  *
+  * \note On Windows, the separate function vcos_timer_init() must be called
+  * as timer initialization from DllMain is not possible.
+  */
+
+/** Perform timer subsystem initialization. This function is not needed
+  * on non-Windows platforms but is still present so that it can be
+  * called. On Windows it is needed because vcos_init() gets called
+  * from DLL initialization where it is not possible to create a
+  * time queue (deadlock occurs if you try).
+  *
+  * @return VCOS_SUCCESS on success. VCOS_EEXIST if this has already been called
+  * once. VCOS_ENOMEM if resource allocation failed.
+  */
+VCOSPRE_ VCOS_STATUS_T VCOSPOST_ vcos_timer_init(void);
+
+/** Create a timer in a disabled state.
+  *
+  * The timer is initially disabled.
+  *
+  * @param timer     timer handle
+  * @param name      name for timer
+  * @param expiration_routine function to call when timer expires
+  * @param context   context passed to expiration routine
+  *
+  */
+VCOS_INLINE_DECL
+VCOS_STATUS_T vcos_timer_create(VCOS_TIMER_T *timer,
+                                const char *name,
+                                void (*expiration_routine)(void *context),
+                                void *context);
+
+
+
+/** Start a timer running.
+  *
+  * Timer must be stopped.
+  *
+  * @param timer     timer handle
+  * @param delay     Delay to wait for, in ms
+  */
+VCOS_INLINE_DECL
+void vcos_timer_set(VCOS_TIMER_T *timer, VCOS_UNSIGNED delay);
+
+/** Stop an already running timer.
+  *
+  * @param timer     timer handle
+  */
+VCOS_INLINE_DECL
+void vcos_timer_cancel(VCOS_TIMER_T *timer);
+
+/** Stop a timer and restart it.
+  * @param timer     timer handle
+  * @param delay     delay in ms
+  */
+VCOS_INLINE_DECL
+void vcos_timer_reset(VCOS_TIMER_T *timer, VCOS_UNSIGNED delay);
+
+VCOS_INLINE_DECL
+void vcos_timer_delete(VCOS_TIMER_T *timer);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/drivers/misc/vc04_services/interface/vcos/vcos_types.h b/drivers/misc/vc04_services/interface/vcos/vcos_types.h
new file mode 100644
index 0000000..ab16f36
--- /dev/null
+++ b/drivers/misc/vc04_services/interface/vcos/vcos_types.h
@@ -0,0 +1,197 @@
+/*=============================================================================
+Copyright (c) 2009 Broadcom Europe Limited.
+All rights reserved.
+
+FILE DESCRIPTION
+VideoCore OS Abstraction Layer - basic types
+=============================================================================*/
+
+#ifndef VCOS_TYPES_H
+#define VCOS_TYPES_H
+
+#define VCOS_VERSION   1
+
+#include "vcos_platform_types.h"
+
+#if !defined(VCOSPRE_) || !defined(VCOSPOST_)
+#error VCOSPRE_ and VCOSPOST_ not defined!
+#endif
+
+/* Redefine these here; this means that existing header files can carry on
+ * using the VCHPOST/VCHPRE macros rather than having huge changes, which
+ * could cause nasty merge problems.
+ */
+#ifndef VCHPOST_
+#define VCHPOST_ VCOSPOST_
+#endif
+#ifndef VCHPRE_
+#define VCHPRE_  VCOSPRE_
+#endif
+
+/** Entry function for a lowlevel thread.
+  *
+  * Returns void for consistency with Nucleus/ThreadX.
+  */
+typedef void (*VCOS_LLTHREAD_ENTRY_FN_T)(void *);
+
+/** Thread entry point. Returns a void* for consistency
+  * with pthreads.
+  */
+typedef void *(*VCOS_THREAD_ENTRY_FN_T)(void*);
+
+
+/* Error return codes - chosen to be similar to errno values */
+typedef enum
+{
+   VCOS_SUCCESS,
+   VCOS_EAGAIN,
+   VCOS_ENOENT,
+   VCOS_ENOSPC,
+   VCOS_EINVAL,
+   VCOS_EACCESS,
+   VCOS_ENOMEM,
+   VCOS_ENOSYS,
+   VCOS_EEXIST,
+   VCOS_ENXIO,
+   VCOS_EINTR
+} VCOS_STATUS_T;
+
+/* Some compilers (MetaWare) won't inline with -g turned on, which then results
+ * in a lot of code bloat. To overcome this, inline functions are forward declared
+ * with the prefix VCOS_INLINE_DECL, and implemented with the prefix VCOS_INLINE_IMPL.
+ *
+ * That then means that in a release build, "static inline" can be used in the obvious
+ * way, but in a debug build the implementations can be skipped in all but one file,
+ * by using VCOS_INLINE_BODIES.
+ *
+ * VCOS_INLINE_DECL - put this at the start of an inline forward declaration of a VCOS
+ * function.
+ *
+ * VCOS_INLINE_IMPL - put this at the start of an inlined implementation of a VCOS
+ * function.
+ *
+ */
+
+/* VCOS_EXPORT - it turns out that in some circumstances we need the implementation of
+ * a function even if it is usually inlined.
+ *
+ * In particular, if we have a codec that is usually provided in object form, if it
+ * was built for a debug build it will be full of calls to vcos_XXX(). If this is used
+ * in a *release* build, then there won't be any of these calls around in the main image
+ * as they will all have been inlined. The problem also exists for vcos functions called
+ * from assembler.
+ *
+ * VCOS_EXPORT ensures that the named function will be emitted as a regular (not static-inline)
+ * function inside vcos_<platform>.c so that it can be linked against. Doing this for every
+ * VCOS function would be a bit code-bloat-tastic, so it is only done for those that need it.
+ *
+ */
+
+#ifdef __cplusplus
+#define _VCOS_INLINE inline
+#else
+#define _VCOS_INLINE __inline
+#endif
+
+#if defined(NDEBUG)
+
+#ifdef __GNUC__
+# define VCOS_INLINE_DECL extern __inline__
+# define VCOS_INLINE_IMPL static __inline__
+#else
+# define VCOS_INLINE_DECL static _VCOS_INLINE   /* declare a func */
+# define VCOS_INLINE_IMPL static _VCOS_INLINE   /* implement a func inline */
+#endif
+
+# if defined(VCOS_WANT_IMPL)
+#  define VCOS_EXPORT
+# else
+#  define VCOS_EXPORT VCOS_INLINE_IMPL
+# endif /* VCOS_WANT_IMPL */
+
+#define VCOS_INLINE_BODIES
+
+#else /* NDEBUG */
+
+#if !defined(VCOS_INLINE_DECL)
+   #define VCOS_INLINE_DECL extern
+#endif
+#if !defined(VCOS_INLINE_IMPL)
+   #define VCOS_INLINE_IMPL
+#endif
+#define VCOS_EXPORT VCOS_INLINE_IMPL
+#endif
+
+#define VCOS_STATIC_INLINE static _VCOS_INLINE
+
+#if defined(__HIGHC__) || defined(__HIGHC_ANSI__)
+#define _VCOS_METAWARE
+#endif
+
+/** It seems that __FUNCTION__ isn't standard!
+  */
+#if __STDC_VERSION__ < 199901L
+# if __GNUC__ >= 2 || defined(__VIDEOCORE__)
+#  define VCOS_FUNCTION __FUNCTION__
+# else
+#  define VCOS_FUNCTION "<unknown>"
+# endif
+#else
+# define VCOS_FUNCTION __func__
+#endif
+
+#define _VCOS_MS_PER_TICK (1000/VCOS_TICKS_PER_SECOND)
+
+/* Convert a number of milliseconds to a tick count. Internal use only - fails to
+ * convert VCOS_SUSPEND correctly.
+ */
+#define _VCOS_MS_TO_TICKS(ms) (((ms)+_VCOS_MS_PER_TICK-1)/_VCOS_MS_PER_TICK)
+
+#define VCOS_TICKS_TO_MS(ticks) ((ticks) * _VCOS_MS_PER_TICK)
+
+/** VCOS version of DATESTR, from pcdisk.h. Used by the hostreq service.
+ */ 
+typedef struct vcos_datestr
+{
+   uint8_t       cmsec;              /**< Centesimal mili second */
+   uint16_t      date;               /**< Date */
+   uint16_t      time;               /**< Time */
+
+} VCOS_DATESTR;
+
+/* Compile-time assert - declares invalid array length if condition
+ * not met, or array of length one if OK.
+ */
+#define VCOS_CASSERT(e) extern char vcos_compile_time_check[1/(e)]
+
+#define vcos_min(x,y) ((x) < (y) ? (x) : (y))
+#define vcos_max(x,y) ((x) > (y) ? (x) : (y))
+
+/** Return the count of an array. FIXME: under gcc we could make
+ * this report an error for pointers using __builtin_types_compatible().
+ */
+#define vcos_countof(x) (sizeof((x)) / sizeof((x)[0]))
+
+/* for backward compatibility */
+#define countof(x) (sizeof((x)) / sizeof((x)[0]))
+
+#define VCOS_ALIGN_DOWN(p,n) (((ptrdiff_t)(p)) & ~((n)-1))
+#define VCOS_ALIGN_UP(p,n) VCOS_ALIGN_DOWN((ptrdiff_t)(p)+(n)-1,(n))
+
+/** bool_t is not a POSIX type so cannot rely on it. Define it here.
+  * It's not even defined in stdbool.h.
+  */
+typedef int32_t vcos_bool_t;
+typedef int32_t vcos_fourcc_t;
+
+#define VCOS_FALSE   0
+#define VCOS_TRUE    (!VCOS_FALSE)
+
+/** Mark unused arguments to keep compilers quiet */
+#define vcos_unused(x) (void)(x)
+
+/** For backward compatibility */
+typedef vcos_fourcc_t fourcc_t;
+typedef vcos_fourcc_t FOURCC_T;
+
+#endif