From 3fdcec5bc717c987d05441349aa327035d4ac3d4 Mon Sep 17 00:00:00 2001 From: Stephan Raue Date: Thu, 18 Oct 2012 15:03:11 +0200 Subject: [PATCH] linux-3.2: update RPi patch Signed-off-by: Stephan Raue --- ...inux-3.2.31-601-RPi_support-10182a3.patch} | 22598 +++++++++++----- 1 file changed, 15126 insertions(+), 7472 deletions(-) rename packages/linux/patches/{linux-3.2.31-601-RPi_support-8bccc7a.patch => linux-3.2.31-601-RPi_support-10182a3.patch} (83%) diff --git a/packages/linux/patches/linux-3.2.31-601-RPi_support-8bccc7a.patch b/packages/linux/patches/linux-3.2.31-601-RPi_support-10182a3.patch similarity index 83% rename from packages/linux/patches/linux-3.2.31-601-RPi_support-8bccc7a.patch rename to packages/linux/patches/linux-3.2.31-601-RPi_support-10182a3.patch index 49edf5bf1c..527dcd56bd 100644 --- a/packages/linux/patches/linux-3.2.31-601-RPi_support-8bccc7a.patch +++ b/packages/linux/patches/linux-3.2.31-601-RPi_support-10182a3.patch @@ -6083,10 +6083,10 @@ index 0000000..5c43412 + diff --git a/arch/arm/mach-bcm2708/include/mach/vc_mem.h b/arch/arm/mach-bcm2708/include/mach/vc_mem.h new file mode 100644 -index 0000000..d29125b +index 0000000..9d41c3a --- /dev/null +++ b/arch/arm/mach-bcm2708/include/mach/vc_mem.h -@@ -0,0 +1,36 @@ +@@ -0,0 +1,35 @@ +/***************************************************************************** +* Copyright 2010 - 2011 Broadcom Corporation. All rights reserved. +* @@ -6111,7 +6111,6 @@ index 0000000..d29125b +#define VC_MEM_IOC_MEM_PHYS_ADDR _IOR( VC_MEM_IOC_MAGIC, 0, unsigned long ) +#define VC_MEM_IOC_MEM_SIZE _IOR( VC_MEM_IOC_MAGIC, 1, unsigned int ) +#define VC_MEM_IOC_MEM_BASE _IOR( VC_MEM_IOC_MAGIC, 2, unsigned int ) -+#define VC_MEM_IOC_MEM_LOAD _IOR( VC_MEM_IOC_MAGIC, 3, unsigned int ) + +#if defined( __KERNEL__ ) +#define VC_MEM_TO_ARM_ADDR_MASK 0x3FFFFFFF @@ -6465,10 +6464,10 @@ index 0000000..256bf1a +MODULE_LICENSE("GPL"); diff --git a/arch/arm/mach-bcm2708/vc_mem.c b/arch/arm/mach-bcm2708/vc_mem.c new file mode 100644 -index 0000000..f12cb49 +index 0000000..5c56b31 --- /dev/null +++ b/arch/arm/mach-bcm2708/vc_mem.c -@@ -0,0 +1,458 @@ +@@ -0,0 +1,444 @@ +/***************************************************************************** +* Copyright 2010 - 2011 Broadcom Corporation. All rights reserved. +* @@ -6679,20 +6678,6 @@ index 0000000..f12cb49 + } + break; + } -+ case VC_MEM_IOC_MEM_LOAD: -+ { -+ // Get the videocore memory base -+ vc_mem_get_base(); -+ -+ LOG_DBG("%s: VC_MEM_IOC_MEM_LOAD=%u", __func__, -+ mm_vc_mem_base); -+ -+ if (copy_to_user((void *) arg, &mm_vc_mem_base, -+ sizeof (mm_vc_mem_base)) != 0) { -+ rc = -EFAULT; -+ } -+ break; -+ } + default: + { + return -ENOTTY; @@ -8327,55 +8312,55 @@ index b26495a..a06b534 100644 obj-y += carma/ obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o obj-$(CONFIG_ALTERA_STAPL) +=altera-stapl/ -+obj-$(CONFIG_BCM2708_VCHIQ) += vc04_services/ ++obj-y += vc04_services/ diff --git a/drivers/misc/vc04_services/Kconfig b/drivers/misc/vc04_services/Kconfig new file mode 100644 -index 0000000..b48a3f3 +index 0000000..d97a1e2 --- /dev/null +++ b/drivers/misc/vc04_services/Kconfig -@@ -0,0 +1,10 @@ +@@ -0,0 +1,7 @@ +config BCM2708_VCHIQ + tristate "Videocore VCHIQ" + depends on MACH_BCM2708 -+ default y -+ help -+ Kernel to VideoCore communication interface for the -+ BCM2708 family of products. -+ Defaults to Y when the Broadcom Videocore services -+ are included in the build, N otherwise. ++ 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..1aeb20a +index 0000000..ee53af7 --- /dev/null +++ b/drivers/misc/vc04_services/Makefile -@@ -0,0 +1,18 @@ -+ifeq ($(CONFIG_MACH_BCM2708),y) -+ +@@ -0,0 +1,21 @@ +obj-$(CONFIG_BCM2708_VCHIQ) += vchiq.o + +vchiq-objs := \ + interface/vchiq_arm/vchiq_core.o \ ++ interface/vchiq_arm/vchiq_shim.o \ ++ interface/vchiq_arm/vchiq_util.o \ + interface/vchiq_arm/vchiq_arm.o \ + interface/vchiq_arm/vchiq_kern_lib.o \ + interface/vchiq_arm/vchiq_2835_arm.o \ -+ interface/vchiq_arm/vchiq_proc.o \ -+ interface/vchiq_arm/vchiq_shim.o \ -+ interface/vchiq_arm/vchiq_util.o \ -+ interface/vchiq_arm/vchiq_connected.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 -DUSE_VCHIQ_ARM -D__VCCOREVER__=0x04000000 -Idrivers/misc/vc04_services -Idrivers/misc/vc04_services/interface/vcos/linuxkernel + -+EXTRA_CFLAGS += -DVCOS_VERIFY_BKPTS=1 -Idrivers/misc/vc04_services -DUSE_VCHIQ_ARM -D__VCCOREVER__=0x04000000 + -+endif + diff --git a/drivers/misc/vc04_services/interface/vchi/connections/connection.h b/drivers/misc/vc04_services/interface/vchi/connections/connection.h new file mode 100644 -index 0000000..f4e8225 +index 0000000..2fe5742 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchi/connections/connection.h -@@ -0,0 +1,313 @@ +@@ -0,0 +1,309 @@ +/* -+ * Copyright (c) 2010-2012 Broadcom. All rights reserved. ++ * 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 @@ -8395,10 +8380,6 @@ index 0000000..f4e8225 +#ifndef CONNECTION_H_ +#define CONNECTION_H_ + -+#include -+#include -+#include -+ +#include "interface/vchi/vchi_cfg_internal.h" +#include "interface/vchi/vchi_common.h" +#include "interface/vchi/message_drivers/message.h" @@ -8430,15 +8411,15 @@ index 0000000..f4e8225 + +// Routine to create a service +typedef int32_t (*VCHI_CONNECTION_SERVICE_CONNECT_T)( VCHI_CONNECTION_STATE_T *state_handle, -+ int32_t service_id, ++ vcos_fourcc_t service_id, + uint32_t rx_fifo_size, + uint32_t tx_fifo_size, + int server, + VCHI_CALLBACK_T callback, + void *callback_param, -+ int32_t want_crc, -+ int32_t want_unaligned_bulk_rx, -+ int32_t want_unaligned_bulk_tx, ++ vcos_bool_t want_crc, ++ vcos_bool_t want_unaligned_bulk_rx, ++ vcos_bool_t want_unaligned_bulk_tx, + VCHI_CONNECTION_SERVICE_HANDLE_T *service_handle ); + +// Routine to close a service @@ -8496,7 +8477,7 @@ index 0000000..f4e8225 + uint32_t *rx_timestamp ); + +// Routine to check whether the iterator has a next message -+typedef int32_t (*VCHI_CONNECTION_MSG_ITER_HAS_NEXT_T)( VCHI_CONNECTION_SERVICE_HANDLE_T service, ++typedef vcos_bool_t (*VCHI_CONNECTION_MSG_ITER_HAS_NEXT_T)( VCHI_CONNECTION_SERVICE_HANDLE_T service, + const VCHI_MSG_ITER_T *iter ); + +// Routine to advance the iterator @@ -8529,7 +8510,7 @@ index 0000000..f4e8225 + void *bulk_handle ); + +// Routine to report if a server is available -+typedef int32_t (*VCHI_CONNECTION_SERVER_PRESENT)( VCHI_CONNECTION_STATE_T *state, int32_t service_id, int32_t peer_flags ); ++typedef int32_t (*VCHI_CONNECTION_SERVER_PRESENT)( VCHI_CONNECTION_STATE_T *state, vcos_fourcc_t service_id, int32_t peer_flags ); + +// Routine to report the number of RX slots available +typedef int (*VCHI_CONNECTION_RX_SLOTS_AVAILABLE)( const VCHI_CONNECTION_STATE_T *state ); @@ -8539,7 +8520,7 @@ index 0000000..f4e8225 + +// Callback to indicate that the other side has added a buffer to the rx bulk DMA FIFO +typedef void (*VCHI_CONNECTION_RX_BULK_BUFFER_ADDED)(VCHI_CONNECTION_STATE_T *state, -+ int32_t service, ++ vcos_fourcc_t service, + uint32_t length, + MESSAGE_TX_CHANNEL_T channel, + uint32_t channel_params, @@ -8547,10 +8528,10 @@ index 0000000..f4e8225 + uint32_t data_offset); + +// Callback to inform a service that a Xon or Xoff message has been received -+typedef void (*VCHI_CONNECTION_FLOW_CONTROL)(VCHI_CONNECTION_STATE_T *state, int32_t service_id, int32_t xoff); ++typedef void (*VCHI_CONNECTION_FLOW_CONTROL)(VCHI_CONNECTION_STATE_T *state, vcos_fourcc_t service_id, int32_t xoff); + +// Callback to inform a service that a server available reply message has been received -+typedef void (*VCHI_CONNECTION_SERVER_AVAILABLE_REPLY)(VCHI_CONNECTION_STATE_T *state, int32_t service_id, uint32_t flags); ++typedef void (*VCHI_CONNECTION_SERVER_AVAILABLE_REPLY)(VCHI_CONNECTION_STATE_T *state, vcos_fourcc_t service_id, uint32_t flags); + +// Callback to indicate that bulk auxiliary messages have arrived +typedef void (*VCHI_CONNECTION_BULK_AUX_RECEIVED)(VCHI_CONNECTION_STATE_T *state); @@ -8565,7 +8546,7 @@ index 0000000..f4e8225 +typedef void (*VCHI_CONNECTION_DISCONNECT)(VCHI_CONNECTION_STATE_T *state, uint32_t flags); + +// Callback to inform of a power control request -+typedef void (*VCHI_CONNECTION_POWER_CONTROL)(VCHI_CONNECTION_STATE_T *state, MESSAGE_TX_CHANNEL_T channel, int32_t enable); ++typedef void (*VCHI_CONNECTION_POWER_CONTROL)(VCHI_CONNECTION_STATE_T *state, MESSAGE_TX_CHANNEL_T channel, vcos_bool_t enable); + +// allocate memory suitably aligned for this connection +typedef void * (*VCHI_BUFFER_ALLOCATE)(VCHI_CONNECTION_SERVICE_HANDLE_T service_handle, uint32_t * length); @@ -8679,7 +8660,7 @@ index 0000000..f4e8225 + const VCHI_CONNECTION_API_T *api; + VCHI_CONNECTION_STATE_T *state; +#ifdef VCHI_COARSE_LOCKING -+ struct semaphore sem; ++ VCOS_SEMAPHORE_T sem; +#endif +}; + @@ -8689,12 +8670,12 @@ index 0000000..f4e8225 +/****************************** End of file **********************************/ diff --git a/drivers/misc/vc04_services/interface/vchi/message_drivers/message.h b/drivers/misc/vc04_services/interface/vchi/message_drivers/message.h new file mode 100644 -index 0000000..1e2da83 +index 0000000..7701b15 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchi/message_drivers/message.h -@@ -0,0 +1,189 @@ +@@ -0,0 +1,186 @@ +/* -+ * Copyright (c) 2010-2012 Broadcom. All rights reserved. ++ * 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 @@ -8714,11 +8695,8 @@ index 0000000..1e2da83 +#ifndef _VCHI_MESSAGE_H_ +#define _VCHI_MESSAGE_H_ + -+#include -+#include -+#include -+ +#include "interface/vchi/vchi_cfg_internal.h" ++#include "interface/vcos/vcos.h" +#include "interface/vchi/vchi_common.h" + + @@ -8761,7 +8739,7 @@ index 0000000..1e2da83 + struct rx_msg_slot_info *next; + //struct slot_info *prev; +#if !defined VCHI_COARSE_LOCKING -+ struct semaphore sem; ++ VCOS_SEMAPHORE_T sem; +#endif + + uint8_t *addr; // base address of slot @@ -8782,7 +8760,7 @@ index 0000000..1e2da83 +typedef struct rx_bulk_slotinfo_t { + struct rx_bulk_slotinfo_t *next; + -+ struct semaphore *blocking; ++ VCOS_SEMAPHORE_T *blocking; + + // needed by DMA + void *addr; @@ -8825,7 +8803,7 @@ index 0000000..1e2da83 + uint16_t slot_delta; // whether this message indicated slot delta + uint32_t len; // length of message + RX_MSG_SLOTINFO_T *slot; // slot this message is in -+ int32_t service; // service id this message is destined for ++ vcos_fourcc_t service; // service id this message is destined for + uint32_t tx_timestamp; // timestamp from the header + uint32_t rx_timestamp; // timestamp when we parsed it + } message; @@ -8854,14 +8832,14 @@ index 0000000..1e2da83 + VCHI_MDRIVER_HANDLE_T *(*open)( VCHI_MESSAGE_DRIVER_OPEN_T *params, void *state ); + int32_t (*suspending)( VCHI_MDRIVER_HANDLE_T *handle ); + int32_t (*resumed)( VCHI_MDRIVER_HANDLE_T *handle ); -+ int32_t (*power_control)( VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_TX_CHANNEL_T, int32_t enable ); ++ int32_t (*power_control)( VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_TX_CHANNEL_T, vcos_bool_t enable ); + int32_t (*add_msg_rx_slot)( VCHI_MDRIVER_HANDLE_T *handle, RX_MSG_SLOTINFO_T *slot ); // rx message + int32_t (*add_bulk_rx)( VCHI_MDRIVER_HANDLE_T *handle, void *data, uint32_t len, RX_BULK_SLOTINFO_T *slot ); // rx data (bulk) + int32_t (*send)( VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_TX_CHANNEL_T channel, const void *data, uint32_t len, VCHI_MSG_FLAGS_T flags, void *send_handle ); // tx (message & bulk) + void (*next_event)( VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_EVENT_T *event ); // get the next event from message_driver + int32_t (*enable)( VCHI_MDRIVER_HANDLE_T *handle ); -+ int32_t (*form_message)( VCHI_MDRIVER_HANDLE_T *handle, int32_t service_id, VCHI_MSG_VECTOR_T *vector, uint32_t count, void -+ *address, uint32_t length_avail, uint32_t max_total_length, int32_t pad_to_fill, int32_t allow_partial ); ++ int32_t (*form_message)( VCHI_MDRIVER_HANDLE_T *handle, vcos_fourcc_t service_id, VCHI_MSG_VECTOR_T *vector, uint32_t count, void ++ *address, uint32_t length_avail, uint32_t max_total_length, vcos_bool_t pad_to_fill, vcos_bool_t allow_partial ); + + int32_t (*update_message)( VCHI_MDRIVER_HANDLE_T *handle, void *dest, int16_t *slot_count ); + int32_t (*buffer_aligned)( VCHI_MDRIVER_HANDLE_T *handle, int tx, int uncached, const void *address, const uint32_t length ); @@ -8870,7 +8848,7 @@ index 0000000..1e2da83 + int (*rx_slot_size)( VCHI_MDRIVER_HANDLE_T *handle, int msg_size ); + int (*tx_slot_size)( VCHI_MDRIVER_HANDLE_T *handle, int msg_size ); + -+ int32_t (*tx_supports_terminate)( const VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_TX_CHANNEL_T channel ); ++ vcos_bool_t (*tx_supports_terminate)( const VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_TX_CHANNEL_T channel ); + uint32_t (*tx_bulk_chunk_size)( const VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_TX_CHANNEL_T channel ); + int (*tx_alignment)( const VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_TX_CHANNEL_T channel ); + int (*rx_alignment)( const VCHI_MDRIVER_HANDLE_T *handle, MESSAGE_RX_CHANNEL_T channel ); @@ -8884,12 +8862,12 @@ index 0000000..1e2da83 +/****************************** End of file ***********************************/ diff --git a/drivers/misc/vc04_services/interface/vchi/vchi.h b/drivers/misc/vc04_services/interface/vchi/vchi.h new file mode 100644 -index 0000000..7a7612d +index 0000000..e441d8c --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchi/vchi.h -@@ -0,0 +1,355 @@ +@@ -0,0 +1,347 @@ +/* -+ * Copyright (c) 2010-2012 Broadcom. All rights reserved. ++ * 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 @@ -8906,9 +8884,14 @@ index 0000000..7a7612d + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + ++/*============================================================================= ++Contains the protypes for the vchi functions. ++=============================================================================*/ ++ +#ifndef VCHI_H_ +#define VCHI_H_ + ++#include "interface/vcos/vcos.h" +#include "interface/vchi/vchi_cfg.h" +#include "interface/vchi/vchi_common.h" +#include "interface/vchi/connections/connection.h" @@ -8929,12 +8912,6 @@ index 0000000..7a7612d +#define VCHI_BULK_ALIGNED(x) (((unsigned long)(x) & (VCHI_BULK_ALIGN-1)) == 0) +#endif + -+struct vchi_version { -+ uint32_t version; -+ uint32_t version_min; -+}; -+#define VCHI_VERSION(v_) { v_, v_ } -+#define VCHI_VERSION_EX(v_, m_) { v_, m_ } + +typedef enum +{ @@ -8979,8 +8956,8 @@ index 0000000..7a7612d +// Construct an entry in a msg vector for a message handle (h), starting at offset (o) of length (l) +#define VCHI_VEC_HANDLE(h,o,l) VCHI_VEC_HANDLE, { { (h), (o), (l) } } + -+// Macros to manipulate 'FOURCC' values -+#define MAKE_FOURCC(x) ((int32_t)( (x[0] << 24) | (x[1] << 16) | (x[2] << 8) | x[3] )) ++// Macros to manipulate fourcc_t values ++#define MAKE_FOURCC(x) ((fourcc_t)( (x[0] << 24) | (x[1] << 16) | (x[2] << 8) | x[3] )) +#define FOURCC_TO_CHAR(x) (x >> 24) & 0xFF,(x >> 16) & 0xFF,(x >> 8) & 0xFF, x & 0xFF + + @@ -8999,22 +8976,15 @@ index 0000000..7a7612d + +// structure used to provide the information needed to open a server or a client +typedef struct { -+ struct vchi_version version; -+ int32_t service_id; -+ VCHI_CONNECTION_T *connection; -+ uint32_t rx_fifo_size; -+ uint32_t tx_fifo_size; -+ VCHI_CALLBACK_T callback; -+ void *callback_param; -+ /* client intends to receive bulk transfers of -+ odd lengths or into unaligned buffers */ -+ int32_t want_unaligned_bulk_rx; -+ /* client intends to transmit bulk transfers of -+ odd lengths or out of unaligned buffers */ -+ int32_t want_unaligned_bulk_tx; -+ /* client wants to check CRCs on (bulk) xfers. -+ Only needs to be set at 1 end - will do both directions. */ -+ int32_t want_crc; ++ vcos_fourcc_t service_id; ++ VCHI_CONNECTION_T *connection; ++ uint32_t rx_fifo_size; ++ uint32_t tx_fifo_size; ++ VCHI_CALLBACK_T callback; ++ void *callback_param; ++ vcos_bool_t want_unaligned_bulk_rx; // client intends to receive bulk transfers of odd lengths or into unaligned buffers ++ vcos_bool_t want_unaligned_bulk_tx; // client intends to transmit bulk transfers of odd lengths or out of unaligned buffers ++ vcos_bool_t want_crc; // client wants to check CRCs on (bulk) transfers. Only needs to be set at 1 end - will do both directions. +} SERVICE_CREATION_T; + +// Opaque handle for a VCHI instance @@ -9167,7 +9137,7 @@ index 0000000..7a7612d +extern int32_t vchi_held_msg_release( VCHI_HELD_MSG_T *message ); + +// Indicates whether the iterator has a next message. -+extern int32_t vchi_msg_iter_has_next( const VCHI_MSG_ITER_T *iter ); ++extern vcos_bool_t vchi_msg_iter_has_next( const VCHI_MSG_ITER_T *iter ); + +// Return the pointer and length for the next message and advance the iterator. +extern int32_t vchi_msg_iter_next( VCHI_MSG_ITER_T *iter, @@ -9245,12 +9215,12 @@ index 0000000..7a7612d +/****************************** End of file **********************************/ diff --git a/drivers/misc/vc04_services/interface/vchi/vchi_cfg.h b/drivers/misc/vc04_services/interface/vchi/vchi_cfg.h new file mode 100644 -index 0000000..df02282 +index 0000000..a66e489 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchi/vchi_cfg.h -@@ -0,0 +1,209 @@ +@@ -0,0 +1,214 @@ +/* -+ * Copyright (c) 2010-2012 Broadcom. All rights reserved. ++ * 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 @@ -9267,6 +9237,11 @@ index 0000000..df02282 + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + ++/*============================================================================= ++Contains the #defines for the number of servers / clients etc, these can be ++over-ridden from the platform makefile if needed ++=============================================================================*/ ++ +#ifndef VCHI_CFG_H_ +#define VCHI_CFG_H_ + @@ -9460,12 +9435,12 @@ index 0000000..df02282 +/****************************** End of file **********************************/ diff --git a/drivers/misc/vc04_services/interface/vchi/vchi_cfg_internal.h b/drivers/misc/vc04_services/interface/vchi/vchi_cfg_internal.h new file mode 100644 -index 0000000..f334e4c +index 0000000..958cc55 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchi/vchi_cfg_internal.h @@ -0,0 +1,56 @@ +/* -+ * Copyright (c) 2010-2012 Broadcom. All rights reserved. ++ * 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 @@ -9522,12 +9497,12 @@ index 0000000..f334e4c +#endif /*VCHI_CFG_INTERNAL_H_*/ diff --git a/drivers/misc/vc04_services/interface/vchi/vchi_common.h b/drivers/misc/vc04_services/interface/vchi/vchi_common.h new file mode 100644 -index 0000000..b3aa5e7 +index 0000000..4057878 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchi/vchi_common.h -@@ -0,0 +1,148 @@ +@@ -0,0 +1,152 @@ +/* -+ * Copyright (c) 2010-2012 Broadcom. All rights reserved. ++ * 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 @@ -9544,6 +9519,10 @@ index 0000000..b3aa5e7 + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + ++/*============================================================================= ++Contains global defs used by submodules within vchi. ++=============================================================================*/ ++ +#ifndef VCHI_COMMON_H_ +#define VCHI_COMMON_H_ + @@ -9676,7 +9655,7 @@ index 0000000..b3aa5e7 +#endif // VCHI_COMMON_H_ 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..9dc3a37 +index 0000000..9bcf12e --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchi/vchi_mh.h @@ -0,0 +1,27 @@ @@ -9701,7 +9680,7 @@ index 0000000..9dc3a37 +#ifndef VCHI_MH_H_ +#define VCHI_MH_H_ + -+#include ++#include + +typedef int32_t VCHI_MEM_HANDLE_T; +#define VCHI_MEM_HANDLE_INVALID 0 @@ -9709,10 +9688,10 @@ index 0000000..9dc3a37 +#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..9c359c0 +index 0000000..49d3087 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq.h -@@ -0,0 +1,26 @@ +@@ -0,0 +1,27 @@ +/* + * Copyright (c) 2010-2011 Broadcom. All rights reserved. + * @@ -9736,6 +9715,7 @@ index 0000000..9c359c0 + +#include "vchiq_if.h" +#include "vchiq_util.h" ++#include "interface/vcos/vcos.h" + +#endif + @@ -9774,10 +9754,10 @@ index 0000000..959405e +#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..93d4c3f +index 0000000..35f6afe --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c -@@ -0,0 +1,518 @@ +@@ -0,0 +1,512 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -9804,9 +9784,9 @@ index 0000000..93d4c3f +#include +#include +#include -+#include -+#include +#include ++#include ++#include + +#include + @@ -9820,14 +9800,10 @@ index 0000000..93d4c3f + +#include "vchiq_arm.h" +#include "vchiq_2835.h" -+#include "vchiq_connected.h" + +#define MAX_FRAGMENTS (VCHIQ_NUM_CURRENT_BULKS * 2) + -+typedef struct vchiq_2835_state_struct { -+ int inited; -+ VCHIQ_ARM_STATE_T arm_state; -+} VCHIQ_2835_ARM_STATE_T; ++#define VCOS_LOG_CATEGORY (&vchiq_arm_log_category) + +static char *g_slot_mem; +static int g_slot_mem_size; @@ -9836,21 +9812,29 @@ index 0000000..93d4c3f +static FRAGMENTS_T *g_free_fragments; +struct semaphore g_free_fragments_sema; + -+extern int vchiq_arm_log_level; -+ ++#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); ++ 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; @@ -9866,49 +9850,48 @@ index 0000000..93d4c3f + &g_slot_phys, GFP_ATOMIC); + + if (!g_slot_mem) { -+ vchiq_log_error(vchiq_arm_log_level, -+ "Unable to allocate channel memory"); ++ vcos_log_error("Unable to allocate channel memory"); + err = -ENOMEM; + goto failed_alloc; + } + -+ WARN_ON(((int)g_slot_mem & (PAGE_SIZE - 1)) != 0); ++ 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; ++ 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; ++ 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] = ++ *(FRAGMENTS_T **) & g_fragments_base[i] = + &g_fragments_base[i + 1]; + } -+ *(FRAGMENTS_T **)&g_fragments_base[i] = NULL; ++ *(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) { ++ VCHIQ_SUCCESS) ++ { + err = -EINVAL; + goto failed_vchiq_init; + } + + err = request_irq(VCHIQ_DOORBELL_IRQ, vchiq_doorbell_irq, -+ IRQF_IRQPOLL, "VCHIQ doorbell", ++ IRQF_SAMPLE_RANDOM | IRQF_IRQPOLL, "VCHIQ doorbell", + state); -+ if (err < 0) { -+ vchiq_log_error(vchiq_arm_log_level, "%s: failed to register " -+ "irq=%d err=%d", __func__, -+ VCHIQ_DOORBELL_IRQ, err); ++ if (err < 0) ++ { ++ printk( KERN_ERR "%s: failed to register irq=%d err=%d\n", __func__, ++ VCHIQ_DOORBELL_IRQ, err ); + goto failed_request_irq; + } + @@ -9918,67 +9901,39 @@ index 0000000..93d4c3f + + bcm_mailbox_write(MBOX_CHAN_VCHIQ, (unsigned int)g_slot_phys); + -+ vchiq_log_info(vchiq_arm_log_level, -+ "vchiq_init - done (slots %x, phys %x)", ++ vcos_log_info("vchiq_init - done (slots %x, phys %x)", + (unsigned int)vchiq_slot_zero, g_slot_phys); + -+ vchiq_call_connected_callbacks(); -+ -+ return 0; ++ 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); ++ dma_free_coherent(NULL, g_slot_mem_size, g_slot_mem, g_slot_phys); + +failed_alloc: -+ return err; ++ 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); -+} -+ -+ -+VCHIQ_STATUS_T -+vchiq_platform_init_state(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; -+ state->platform_state = kzalloc(sizeof(VCHIQ_2835_ARM_STATE_T), GFP_KERNEL); -+ ((VCHIQ_2835_ARM_STATE_T*)state->platform_state)->inited = 1; -+ status = vchiq_arm_init_state(state, &((VCHIQ_2835_ARM_STATE_T*)state->platform_state)->arm_state); -+ if(status != VCHIQ_SUCCESS) -+ { -+ ((VCHIQ_2835_ARM_STATE_T*)state->platform_state)->inited = 0; -+ } -+ return status; -+} -+ -+VCHIQ_ARM_STATE_T* -+vchiq_platform_get_arm_state(VCHIQ_STATE_T *state) -+{ -+ if(!((VCHIQ_2835_ARM_STATE_T*)state->platform_state)->inited) -+ { -+ BUG(); -+ } -+ return &((VCHIQ_2835_ARM_STATE_T*)state->platform_state)->arm_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) +{ -+ wmb(); -+ + event->fired = 1; + -+ dsb(); /* data barrier operation */ ++ /* 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)); + } @@ -9987,10 +9942,13 @@ index 0000000..93d4c3f +int +vchiq_copy_from_user(void *dst, const void *src, int size) +{ -+ if ((uint32_t)src < TASK_SIZE) { ++ if ( (uint32_t)src < TASK_SIZE) ++ { + return copy_from_user(dst, src, size); -+ } else { -+ memcpy(dst, src, size); ++ } ++ else ++ { ++ memcpy( dst, src, size ); + return 0; + } +} @@ -10002,7 +9960,7 @@ index 0000000..93d4c3f + PAGELIST_T *pagelist; + int ret; + -+ WARN_ON(memhandle != VCHI_MEM_HANDLE_INVALID); ++ vcos_assert(memhandle == VCHI_MEM_HANDLE_INVALID); + + ret = create_pagelist((char __user *)offset, size, + (dir == VCHIQ_BULK_RECEIVE) @@ -10026,8 +9984,7 @@ index 0000000..93d4c3f +void +vchiq_complete_bulk(VCHIQ_BULK_T *bulk) +{ -+ if (bulk && bulk->remote_data && bulk->actual) -+ free_pagelist((PAGELIST_T *)bulk->remote_data, bulk->actual); ++ free_pagelist((PAGELIST_T *)bulk->remote_data, bulk->actual); +} + +void @@ -10037,57 +9994,83 @@ index 0000000..93d4c3f + * This should only be called on the master (VideoCore) side, but + * provide an implementation to avoid the need for ifdefery. + */ -+ BUG(); ++ 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 = snprintf(buf, sizeof(buf), -+ " Platform: 2835 (VC master)"); -+ vchiq_dump(dump_context, buf, len + 1); ++ char buf[80]; ++ int len; ++ len = vcos_snprintf(buf, sizeof(buf), ++ " Platform: 2835 (VC master)"); ++ vchiq_dump(dump_context, buf, len + 1); +} + +VCHIQ_STATUS_T +vchiq_platform_suspend(VCHIQ_STATE_T *state) +{ ++ vcos_unused(state); ++ vcos_assert_msg(0, "Suspend/resume not supported"); + return VCHIQ_ERROR; +} + +VCHIQ_STATUS_T +vchiq_platform_resume(VCHIQ_STATE_T *state) +{ -+ return VCHIQ_SUCCESS; ++ vcos_unused(state); ++ vcos_assert_msg(0, "Suspend/resume not supported"); ++ return VCHIQ_ERROR; +} + +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"); +} + +int +vchiq_platform_videocore_wanted(VCHIQ_STATE_T* state) +{ ++ vcos_unused(state); + return 1; // autosuspend not supported - videocore always wanted +} + ++#if VCOS_HAVE_TIMER +int +vchiq_platform_use_suspend_timer(void) +{ + return 0; +} ++#endif +void +vchiq_dump_platform_use_state(VCHIQ_STATE_T *state) +{ -+ vchiq_log_info((vchiq_arm_log_level>=VCHIQ_LOG_INFO),"Suspend timer not in use"); ++ vcos_unused(state); +} ++ ++VCHIQ_STATUS_T ++vchiq_platform_init_state(VCHIQ_STATE_T *state) ++{ ++ vcos_unused(state); ++ return VCHIQ_SUCCESS; ++} ++ ++VCHIQ_ARM_STATE_T* ++vchiq_platform_get_arm_state(VCHIQ_STATE_T *state) ++{ ++ vcos_unused(state); ++ return NULL; ++} ++ +/* + * Local functions + */ @@ -10095,7 +10078,7 @@ index 0000000..93d4c3f +static irqreturn_t +vchiq_doorbell_irq(int irq, void *dev_id) +{ -+ VCHIQ_STATE_T *state = dev_id; ++ VCHIQ_STATE_T *state = dev_id; + irqreturn_t ret = IRQ_NONE; + unsigned int status; + @@ -10111,17 +10094,17 @@ index 0000000..93d4c3f +} + +/* 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. ++ 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. -+*/ ++ 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, @@ -10140,16 +10123,13 @@ index 0000000..93d4c3f + + *ppagelist = NULL; + -+ /* Allocate enough storage to hold the page pointers and the page -+ ** list -+ */ -+ pagelist = kmalloc(sizeof(PAGELIST_T) + ++ /* 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); + -+ vchiq_log_trace(vchiq_arm_log_level, -+ "create_pagelist - %x", (unsigned int)pagelist); ++ vcos_log_trace("create_pagelist - %x", (unsigned int)pagelist); + if (!pagelist) + return -ENOMEM; + @@ -10160,22 +10140,17 @@ index 0000000..93d4c3f + 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 */); ++ pages, NULL /*vmas */ ); + up_read(&task->mm->mmap_sem); + -+ if (actual_pages != num_pages) -+ { -+ /* This is probably due to the process being killed */ -+ while (actual_pages > 0) -+ { -+ actual_pages--; -+ page_cache_release(pages[actual_pages]); -+ } -+ kfree(pagelist); -+ if (actual_pages == 0) -+ actual_pages = -ENOMEM; -+ return actual_pages; -+ } ++ 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; @@ -10208,8 +10183,7 @@ index 0000000..93d4c3f + /* 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)))) { ++ ((pagelist->offset + pagelist->length) & (CACHE_LINE_SIZE - 1)))) { + FRAGMENTS_T *fragments; + + if (down_interruptible(&g_free_fragments_sema) != 0) { @@ -10217,11 +10191,11 @@ index 0000000..93d4c3f + return -EINTR; + } + -+ WARN_ON(g_free_fragments == NULL); ++ vcos_assert(g_free_fragments != NULL); + + down(&g_free_fragments_mutex); + fragments = (FRAGMENTS_T *) g_free_fragments; -+ WARN_ON(fragments == NULL); ++ vcos_assert(fragments != NULL); + g_free_fragments = *(FRAGMENTS_T **) g_free_fragments; + up(&g_free_fragments_mutex); + pagelist->type = @@ -10245,40 +10219,39 @@ index 0000000..93d4c3f + struct page **pages; + unsigned int num_pages, i; + -+ vchiq_log_trace(vchiq_arm_log_level, -+ "free_pagelist - %x, %d", (unsigned int)pagelist, actual); ++ vcos_log_trace("free_pagelist - %x, %d", (unsigned int)pagelist, actual); + + num_pages = -+ (pagelist->length + pagelist->offset + PAGE_SIZE - 1) / -+ PAGE_SIZE; ++ (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); ++ FRAGMENTS_T *fragments = ++ g_fragments_base + (pagelist->type - ++ PAGELIST_READ_WITH_FRAGMENTS); + int head_bytes, tail_bytes; -+ head_bytes = (CACHE_LINE_SIZE - pagelist->offset) & -+ (CACHE_LINE_SIZE - 1); -+ tail_bytes = (pagelist->offset + actual) & -+ (CACHE_LINE_SIZE - 1); + -+ if ((actual >= 0) && (head_bytes != 0)) { -+ if (head_bytes > actual) -+ head_bytes = actual; ++ 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 ((actual >= 0) && (head_bytes < actual) && -+ (tail_bytes != 0)) { -+ memcpy((char *)page_address(pages[num_pages - 1]) + -+ ((pagelist->offset + actual) & -+ (PAGE_SIZE - 1) & ~(CACHE_LINE_SIZE - 1)), -+ fragments->tailbuf, tail_bytes); ++ 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); @@ -10296,12 +10269,13 @@ index 0000000..93d4c3f + + kfree(pagelist); +} ++ 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 100755 -index 0000000..81daf3e +new file mode 100644 +index 0000000..49a53ce --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.c -@@ -0,0 +1,2740 @@ +@@ -0,0 +1,1912 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -10330,10 +10304,6 @@ index 0000000..81daf3e +#include +#include +#include -+#include -+#include -+#include -+#include + +#include "vchiq_core.h" +#include "vchiq_ioctl.h" @@ -10353,128 +10323,139 @@ index 0000000..81daf3e +#define MAX_ELEMENTS 8 +#define MSG_QUEUE_SIZE 64 + -+#define KEEPALIVE_VER 1 -+#define KEEPALIVE_VER_MIN KEEPALIVE_VER ++#define VCOS_LOG_CATEGORY (&vchiq_arm_log_category) + -+/* Run time control of log level, based on KERN_XXX level. */ -+int vchiq_arm_log_level = VCHIQ_LOG_DEFAULT; -+int vchiq_susp_log_level = VCHIQ_LOG_DEFAULT; ++#define VCHIQ_ARM_VCSUSPEND_TASK_STACK 4096 + ++#if VCOS_HAVE_TIMER +#define SUSPEND_TIMER_TIMEOUT_MS 100 -+#define SUSPEND_RETRY_TIMER_TIMEOUT_MS 1000 -+ -+#define VC_SUSPEND_NUM_OFFSET 3 /* number of values before idle which are -ve */ -+static const char *const suspend_state_names[] = { -+ "VC_SUSPEND_FORCE_CANCELED", -+ "VC_SUSPEND_REJECTED", -+ "VC_SUSPEND_FAILED", -+ "VC_SUSPEND_IDLE", -+ "VC_SUSPEND_REQUESTED", -+ "VC_SUSPEND_IN_PROGRESS", -+ "VC_SUSPEND_SUSPENDED" -+}; -+#define VC_RESUME_NUM_OFFSET 1 /* number of values before idle which are -ve */ -+static const char *const resume_state_names[] = { -+ "VC_RESUME_FAILED", -+ "VC_RESUME_IDLE", -+ "VC_RESUME_REQUESTED", -+ "VC_RESUME_IN_PROGRESS", -+ "VC_RESUME_RESUMED" -+}; -+/* The number of times we allow force suspend to timeout before actually -+** _forcing_ suspend. This is to cater for SW which fails to release vchiq -+** correctly - we don't want to prevent ARM suspend indefinitely in this case. -+*/ -+#define FORCE_SUSPEND_FAIL_MAX 8 -+ -+/* The time in ms allowed for videocore to go idle when force suspend has been -+ * requested */ -+#define FORCE_SUSPEND_TIMEOUT_MS 200 ++static VCOS_TIMER_T g_suspend_timer; ++static void suspend_timer_callback(void *context); ++#endif + + -+static void suspend_timer_callback(unsigned long context); -+static int vchiq_proc_add_instance(VCHIQ_INSTANCE_T instance); -+static void vchiq_proc_remove_instance(VCHIQ_INSTANCE_T instance); -+ -+ -+typedef struct user_service_struct { -+ VCHIQ_SERVICE_T *service; -+ void *userdata; -+ VCHIQ_INSTANCE_T instance; -+ int is_vchi; -+ int dequeue_pending; -+ int message_available_pos; -+ int msg_insert; -+ int msg_remove; -+ struct semaphore insert_event; -+ struct semaphore remove_event; -+ VCHIQ_HEADER_T * msg_queue[MSG_QUEUE_SIZE]; ++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 bulk_waiter_node { -+ struct bulk_waiter bulk_waiter; -+ int pid; -+ struct list_head list; -+}; -+ +struct vchiq_instance_struct { -+ VCHIQ_STATE_T *state; -+ VCHIQ_COMPLETION_DATA_T completions[MAX_COMPLETIONS]; -+ int completion_insert; -+ int completion_remove; -+ struct semaphore insert_event; -+ struct semaphore remove_event; -+ struct mutex completion_mutex; ++ 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; + -+ int connected; -+ int closing; -+ int pid; -+ int mark; ++ USER_SERVICE_T services[MAX_SERVICES]; + -+ struct list_head bulk_waiter_list; -+ struct mutex bulk_waiter_list_mutex; -+ -+ struct proc_dir_entry *proc_entry; ++ 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; ++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 DEFINE_SPINLOCK(msg_queue_spinlock); + -+static const char *const 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_SERVICE", -+ "SET_SERVICE_OPTION", -+ "DUMP_PHYS_MEM" ++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_SERVICE", ++ "SET_SERVICE_OPTION", ++ "DUMP_PHYS_MEM" +}; + -+vchiq_static_assert((sizeof(ioctl_names)/sizeof(ioctl_names[0])) == -+ (VCHIQ_IOC_MAX + 1)); ++vcos_static_assert(vcos_countof(ioctl_names) == (VCHIQ_IOC_MAX + 1)); ++ ++VCOS_LOG_LEVEL_T vchiq_default_arm_log_level = VCOS_LOG_ERROR; + +static void -+dump_phys_mem(void *virt_addr, uint32_t num_bytes); ++dump_phys_mem( void *virt_addr, uint32_t num_bytes ); ++ ++/**************************************************************************** ++* ++* 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; ++} + +/**************************************************************************** +* @@ -10484,58 +10465,47 @@ index 0000000..81daf3e + +static VCHIQ_STATUS_T +add_completion(VCHIQ_INSTANCE_T instance, VCHIQ_REASON_T reason, -+ VCHIQ_HEADER_T *header, USER_SERVICE_T *user_service, -+ void *bulk_userdata) ++ VCHIQ_HEADER_T *header, USER_SERVICE_T *service, void *bulk_userdata) +{ -+ VCHIQ_COMPLETION_DATA_T *completion; -+ DEBUG_INITIALISE(g_state.local) ++ 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); -+ vchiq_log_trace(vchiq_arm_log_level, -+ "add_completion - completion queue full"); -+ DEBUG_COUNT(COMPLETION_QUEUE_FULL_COUNT); -+ if (down_interruptible(&instance->remove_event) != 0) { -+ vchiq_log_info(vchiq_arm_log_level, -+ "service_callback interrupted"); -+ return VCHIQ_RETRY; -+ } else if (instance->closing) { -+ vchiq_log_info(vchiq_arm_log_level, -+ "service_callback closing"); -+ return VCHIQ_ERROR; -+ } -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ } ++ 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 = ++ &instance-> ++ completions[instance->completion_insert & (MAX_COMPLETIONS - 1)]; + -+ completion->header = header; -+ completion->reason = reason; -+ /* N.B. service_userdata is updated while processing AWAIT_COMPLETION */ -+ completion->service_userdata = user_service->service; -+ completion->bulk_userdata = bulk_userdata; ++ completion->header = header; ++ completion->reason = reason; ++ completion->service_userdata = service; ++ completion->bulk_userdata = bulk_userdata; + -+ if (reason == VCHIQ_SERVICE_CLOSED) -+ /* Take an extra reference, to be held until -+ this CLOSED notification is delivered. */ -+ lock_service(user_service->service); ++ /* 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); + -+ /* A write barrier is needed here to ensure that the entire completion -+ record is written out before the insert point. */ -+ wmb(); ++ if (reason == VCHIQ_MESSAGE_AVAILABLE) ++ service->message_available_pos = instance->completion_insert; ++ instance->completion_insert++; + -+ if (reason == VCHIQ_MESSAGE_AVAILABLE) -+ user_service->message_available_pos = -+ instance->completion_insert; -+ instance->completion_insert++; ++ vcos_event_signal(&instance->insert_event); + -+ up(&instance->insert_event); -+ -+ return VCHIQ_SUCCESS; ++ return VCHIQ_SUCCESS; +} + +/**************************************************************************** @@ -10546,104 +10516,90 @@ index 0000000..81daf3e + +static VCHIQ_STATUS_T +service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header, -+ VCHIQ_SERVICE_HANDLE_T handle, void *bulk_userdata) ++ 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 *user_service; -+ VCHIQ_SERVICE_T *service; -+ VCHIQ_INSTANCE_T instance; -+ DEBUG_INITIALISE(g_state.local) ++ /* 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); ++ 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); + -+ service = handle_to_service(handle); -+ BUG_ON(!service); -+ user_service = (USER_SERVICE_T *)service->base.userdata; -+ instance = user_service->instance; ++ if (!instance || instance->closing) { ++ return VCHIQ_SUCCESS; ++ } + -+ 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); ++ } + -+ vchiq_log_trace(vchiq_arm_log_level, -+ "service_callback - service %lx(%d), reason %d, header %lx, " -+ "instance %lx, bulk_userdata %lx", -+ (unsigned long)user_service, -+ service->localport, -+ reason, (unsigned long)header, -+ (unsigned long)instance, (unsigned long)bulk_userdata); ++ service->msg_queue[service->msg_insert & (MSG_QUEUE_SIZE - 1)] = ++ header; + -+ if (header && user_service->is_vchi) { -+ spin_lock(&msg_queue_spinlock); -+ while (user_service->msg_insert == -+ (user_service->msg_remove + MSG_QUEUE_SIZE)) { -+ spin_unlock(&msg_queue_spinlock); -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ DEBUG_COUNT(MSG_QUEUE_FULL_COUNT); -+ vchiq_log_trace(vchiq_arm_log_level, -+ "service_callback - msg queue full"); -+ /* If there is no MESSAGE_AVAILABLE in the completion -+ ** queue, add one -+ */ -+ if ((user_service->message_available_pos - -+ instance->completion_remove) < 0) { -+ VCHIQ_STATUS_T status; -+ vchiq_log_info(vchiq_arm_log_level, -+ "Inserting extra MESSAGE_AVAILABLE"); -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ status = add_completion(instance, reason, -+ NULL, user_service, bulk_userdata); -+ if (status != VCHIQ_SUCCESS) { -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ return status; -+ } -+ } ++ /* 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)]); + -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ if (down_interruptible(&user_service->remove_event) -+ != 0) { -+ vchiq_log_info(vchiq_arm_log_level, -+ "service_callback interrupted"); -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ return VCHIQ_RETRY; -+ } else if (instance->closing) { -+ vchiq_log_info(vchiq_arm_log_level, -+ "service_callback closing"); -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ return VCHIQ_ERROR; -+ } -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ spin_lock(&msg_queue_spinlock); -+ } ++ service->msg_insert++; ++ vcos_event_signal(&service->insert_event); + -+ user_service->msg_queue[user_service->msg_insert & -+ (MSG_QUEUE_SIZE - 1)] = header; -+ user_service->msg_insert++; -+ spin_unlock(&msg_queue_spinlock); ++ /* 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; ++ } + -+ up(&user_service->insert_event); ++ header = NULL; ++ } ++ DEBUG_TRACE(SERVICE_CALLBACK_LINE); + -+ /* 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 (((user_service->message_available_pos - -+ instance->completion_remove) >= 0) || -+ user_service->dequeue_pending) { -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ user_service->dequeue_pending = 0; -+ return VCHIQ_SUCCESS; -+ } -+ -+ header = NULL; -+ } -+ DEBUG_TRACE(SERVICE_CALLBACK_LINE); -+ -+ return add_completion(instance, reason, header, user_service, -+ bulk_userdata); ++ return add_completion(instance, reason, header, service, bulk_userdata); +} + +/**************************************************************************** @@ -10655,649 +10611,603 @@ index 0000000..81daf3e +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; -+ VCHIQ_SERVICE_T *service = NULL; -+ long ret = 0; -+ int i, rc; -+ DEBUG_INITIALISE(g_state.local) -+ -+ vchiq_log_trace(vchiq_arm_log_level, -+ "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)] : "", arg); -+ -+ switch (cmd) { -+ case VCHIQ_IOC_SHUTDOWN: -+ if (!instance->connected) -+ break; -+ -+ /* Remove all services */ -+ i = 0; -+ while ((service = next_service_by_instance(instance->state, -+ instance, &i)) != NULL) { -+ status = vchiq_remove_service(service->handle); -+ unlock_service(service); -+ if (status != VCHIQ_SUCCESS) -+ break; -+ } -+ service = NULL; -+ -+ if (status == VCHIQ_SUCCESS) { -+ /* Wake the completion thread and ask it to exit */ -+ instance->closing = 1; -+ up(&instance->insert_event); -+ } -+ -+ break; -+ -+ case VCHIQ_IOC_CONNECT: -+ if (instance->connected) { -+ ret = -EINVAL; -+ break; -+ } -+ rc = mutex_lock_interruptible(&instance->state->mutex); -+ if (rc != 0) { -+ vchiq_log_error(vchiq_arm_log_level, -+ "vchiq: connect: could not lock mutex for " -+ "state %d: %d", -+ instance->state->id, rc); -+ ret = -EINTR; -+ break; -+ } -+ status = vchiq_connect_internal(instance->state, instance); -+ mutex_unlock(&instance->state->mutex); -+ -+ if (status == VCHIQ_SUCCESS) -+ instance->connected = 1; -+ else -+ vchiq_log_error(vchiq_arm_log_level, -+ "vchiq: could not connect: %d", status); -+ break; -+ -+ case VCHIQ_IOC_CREATE_SERVICE: { -+ VCHIQ_CREATE_SERVICE_T args; -+ 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; -+ } -+ -+ user_service = kmalloc(sizeof(USER_SERVICE_T), GFP_KERNEL); -+ if (!user_service) { -+ ret = -ENOMEM; -+ break; -+ } -+ -+ if (args.is_open) { -+ if (!instance->connected) { -+ ret = -ENOTCONN; -+ break; -+ } -+ srvstate = VCHIQ_SRVSTATE_OPENING; -+ } else { -+ srvstate = -+ instance->connected ? -+ VCHIQ_SRVSTATE_LISTENING : -+ VCHIQ_SRVSTATE_HIDDEN; -+ } -+ -+ 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); -+ -+ if (service != NULL) { -+ user_service->service = service; -+ user_service->userdata = userdata; -+ user_service->instance = instance; -+ 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; -+ sema_init(&user_service->insert_event, 0); -+ sema_init(&user_service->remove_event, 0); -+ -+ if (args.is_open) { -+ status = vchiq_open_service_internal -+ (service, instance->pid); -+ if (status != VCHIQ_SUCCESS) { -+ vchiq_remove_service(service->handle); -+ service = NULL; -+ ret = (status == VCHIQ_RETRY) ? -+ -EINTR : -EIO; -+ user_service->service = NULL; -+ user_service->instance = NULL; -+ break; -+ } -+ } -+ -+ if (copy_to_user((void __user *) -+ &(((VCHIQ_CREATE_SERVICE_T __user *) -+ arg)->handle), -+ (const void *)&service->handle, -+ sizeof(service->handle)) != 0) { -+ ret = -EFAULT; -+ vchiq_remove_service(service->handle); -+ kfree(user_service); -+ } -+ -+ service = NULL; -+ } else { -+ ret = -EEXIST; -+ kfree(user_service); -+ } -+ } break; -+ -+ case VCHIQ_IOC_CLOSE_SERVICE: { -+ VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg; -+ -+ service = find_service_for_instance(instance, handle); -+ if (service != NULL) -+ status = vchiq_close_service(service->handle); -+ else -+ ret = -EINVAL; -+ } break; -+ -+ case VCHIQ_IOC_REMOVE_SERVICE: { -+ VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg; -+ -+ service = find_service_for_instance(instance, handle); -+ if (service != NULL) -+ status = vchiq_remove_service(service->handle); -+ else -+ ret = -EINVAL; -+ } break; -+ -+ case VCHIQ_IOC_USE_SERVICE: -+ case VCHIQ_IOC_RELEASE_SERVICE: { -+ VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg; -+ -+ service = find_service_for_instance(instance, handle); -+ if (service != NULL) { -+ status = (cmd == VCHIQ_IOC_USE_SERVICE) ? -+ vchiq_use_service_internal(service) : -+ vchiq_release_service_internal(service); -+ if (status != VCHIQ_SUCCESS) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s: cmd %s returned error %d for " -+ "service %c%c%c%c:%03d", -+ __func__, -+ (cmd == VCHIQ_IOC_USE_SERVICE) ? -+ "VCHIQ_IOC_USE_SERVICE" : -+ "VCHIQ_IOC_RELEASE_SERVICE", -+ status, -+ VCHIQ_FOURCC_AS_4CHARS( -+ service->base.fourcc), -+ service->client_id); -+ ret = -EINVAL; -+ } -+ } else -+ ret = -EINVAL; -+ } break; -+ -+ case VCHIQ_IOC_QUEUE_MESSAGE: { -+ VCHIQ_QUEUE_MESSAGE_T args; -+ if (copy_from_user -+ (&args, (const void __user *)arg, -+ sizeof(args)) != 0) { -+ ret = -EFAULT; -+ break; -+ } -+ -+ service = find_service_for_instance(instance, args.handle); -+ -+ if ((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 -+ (args.handle, -+ 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; -+ struct bulk_waiter_node *waiter = NULL; -+ 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; -+ } -+ -+ service = find_service_for_instance(instance, args.handle); -+ if (!service) { -+ ret = -EINVAL; -+ break; -+ } -+ -+ if (args.mode == VCHIQ_BULK_MODE_BLOCKING) { -+ waiter = kzalloc(sizeof(struct bulk_waiter_node), -+ GFP_KERNEL); -+ if (!waiter) { -+ ret = -ENOMEM; -+ break; -+ } -+ args.userdata = &waiter->bulk_waiter; -+ } else if (args.mode == VCHIQ_BULK_MODE_WAITING) { -+ struct list_head *pos; -+ mutex_lock(&instance->bulk_waiter_list_mutex); -+ list_for_each(pos, &instance->bulk_waiter_list) { -+ if (list_entry(pos, struct bulk_waiter_node, -+ list)->pid == current->pid) { -+ waiter = list_entry(pos, -+ struct bulk_waiter_node, -+ list); -+ list_del(pos); -+ break; -+ } -+ -+ } -+ mutex_unlock(&instance->bulk_waiter_list_mutex); -+ if (!waiter) { -+ vchiq_log_error(vchiq_arm_log_level, -+ "no bulk_waiter found for pid %d", -+ current->pid); -+ ret = -ESRCH; -+ break; -+ } -+ vchiq_log_info(vchiq_arm_log_level, -+ "found bulk_waiter %x for pid %d", -+ (unsigned int)waiter, current->pid); -+ args.userdata = &waiter->bulk_waiter; -+ } -+ status = vchiq_bulk_transfer -+ (args.handle, -+ VCHI_MEM_HANDLE_INVALID, -+ args.data, args.size, -+ args.userdata, args.mode, -+ dir); -+ if (!waiter) -+ break; -+ if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) || -+ !waiter->bulk_waiter.bulk) { -+ if (waiter->bulk_waiter.bulk) { -+ /* Cancel the signal when the transfer -+ ** completes. */ -+ spin_lock(&bulk_waiter_spinlock); -+ waiter->bulk_waiter.bulk->userdata = NULL; -+ spin_unlock(&bulk_waiter_spinlock); -+ } -+ kfree(waiter); -+ } else { -+ const VCHIQ_BULK_MODE_T mode_waiting = -+ VCHIQ_BULK_MODE_WAITING; -+ waiter->pid = current->pid; -+ mutex_lock(&instance->bulk_waiter_list_mutex); -+ list_add(&waiter->list, &instance->bulk_waiter_list); -+ mutex_unlock(&instance->bulk_waiter_list_mutex); -+ vchiq_log_info(vchiq_arm_log_level, -+ "saved bulk_waiter %x for pid %d", -+ (unsigned int)waiter, current->pid); -+ -+ if (copy_to_user((void __user *) -+ &(((VCHIQ_QUEUE_BULK_TRANSFER_T __user *) -+ arg)->mode), -+ (const void *)&mode_waiting, -+ sizeof(mode_waiting)) != 0) -+ ret = -EFAULT; -+ } -+ } 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; -+ } -+ -+ mutex_lock(&instance->completion_mutex); -+ -+ DEBUG_TRACE(AWAIT_COMPLETION_LINE); -+ while ((instance->completion_remove == -+ instance->completion_insert) -+ && !instance->closing) { -+ int rc; -+ DEBUG_TRACE(AWAIT_COMPLETION_LINE); -+ mutex_unlock(&instance->completion_mutex); -+ rc = down_interruptible(&instance->insert_event); -+ mutex_lock(&instance->completion_mutex); -+ if (rc != 0) { -+ DEBUG_TRACE(AWAIT_COMPLETION_LINE); -+ vchiq_log_info(vchiq_arm_log_level, -+ "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 -+ */ -+ rmb(); -+ -+ if ((ret == 0) && !instance->closing) { -+ int msgbufcount = args.msgbufcount; -+ for (ret = 0; ret < args.count; ret++) { -+ VCHIQ_COMPLETION_DATA_T *completion; -+ VCHIQ_SERVICE_T *service; -+ USER_SERVICE_T *user_service; -+ VCHIQ_HEADER_T *header; -+ if (instance->completion_remove == -+ instance->completion_insert) -+ break; -+ completion = &instance->completions[ -+ instance->completion_remove & -+ (MAX_COMPLETIONS - 1)]; -+ -+ service = completion->service_userdata; -+ user_service = service->base.userdata; -+ completion->service_userdata = -+ user_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) { -+ vchiq_log_error( -+ vchiq_arm_log_level, -+ "header %x: msgbufsize" -+ " %x < msglen %x", -+ (unsigned int)header, -+ args.msgbufsize, -+ msglen); -+ WARN(1, "invalid message " -+ "size\n"); -+ 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->handle, -+ header); -+ -+ /* The completion must point to the -+ ** msgbuf. */ -+ completion->header = msgbuf; -+ } -+ -+ if (completion->reason == -+ VCHIQ_SERVICE_CLOSED) { -+ unlock_service(service); -+ kfree(user_service); -+ } -+ -+ 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; -+ } -+ } -+ } -+ -+ if (ret != 0) -+ up(&instance->remove_event); -+ mutex_unlock(&instance->completion_mutex); -+ 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; -+ } -+ service = find_service_for_instance(instance, args.handle); -+ if (!service) { -+ ret = -EINVAL; -+ break; -+ } -+ user_service = (USER_SERVICE_T *)service->base.userdata; -+ if (user_service->is_vchi == 0) { -+ ret = -EINVAL; -+ break; -+ } -+ -+ spin_lock(&msg_queue_spinlock); -+ if (user_service->msg_remove == user_service->msg_insert) { -+ if (!args.blocking) { -+ spin_unlock(&msg_queue_spinlock); -+ DEBUG_TRACE(DEQUEUE_MESSAGE_LINE); -+ ret = -EWOULDBLOCK; -+ break; -+ } -+ user_service->dequeue_pending = 1; -+ do { -+ spin_unlock(&msg_queue_spinlock); -+ DEBUG_TRACE(DEQUEUE_MESSAGE_LINE); -+ if (down_interruptible( -+ &user_service->insert_event) != 0) { -+ vchiq_log_info(vchiq_arm_log_level, -+ "DEQUEUE_MESSAGE interrupted"); -+ ret = -EINTR; -+ break; -+ } -+ spin_lock(&msg_queue_spinlock); -+ } while (user_service->msg_remove == -+ user_service->msg_insert); -+ -+ if (ret) -+ break; -+ } -+ -+ BUG_ON((int)(user_service->msg_insert - -+ user_service->msg_remove) < 0); -+ -+ header = user_service->msg_queue[user_service->msg_remove & -+ (MSG_QUEUE_SIZE - 1)]; -+ user_service->msg_remove++; -+ spin_unlock(&msg_queue_spinlock); -+ -+ up(&user_service->remove_event); -+ 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( -+ service->handle, -+ header); -+ } else -+ ret = -EFAULT; -+ } else { -+ vchiq_log_error(vchiq_arm_log_level, -+ "header %x: bufsize %x < size %x", -+ (unsigned int)header, args.bufsize, -+ header->size); -+ WARN(1, "invalid size\n"); -+ ret = -EMSGSIZE; -+ } -+ DEBUG_TRACE(DEQUEUE_MESSAGE_LINE); -+ } break; -+ -+ case VCHIQ_IOC_GET_CLIENT_ID: { -+ VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg; -+ -+ ret = vchiq_get_client_id(handle); -+ } 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; -+ -+ if (copy_from_user( -+ &args, (const void __user *)arg, -+ sizeof(args)) != 0) { -+ ret = -EFAULT; -+ break; -+ } -+ -+ service = find_service_for_instance(instance, args.handle); -+ if (!service) { -+ ret = -EINVAL; -+ break; -+ } -+ -+ status = vchiq_set_service_option( -+ args.handle, args.option, args.value); -+ } break; -+ -+ case VCHIQ_IOC_DUMP_PHYS_MEM: { -+ VCHIQ_DUMP_MEM_T args; -+ -+ if (copy_from_user -+ (&args, (const void __user *)arg, -+ sizeof(args)) != 0) { -+ ret = -EFAULT; -+ break; -+ } -+ dump_phys_mem(args.virt_addr, args.num_bytes); -+ } break; -+ -+ default: -+ ret = -ENOTTY; -+ break; -+ } -+ -+ if (service) -+ unlock_service(service); -+ -+ if (ret == 0) { -+ if (status == VCHIQ_ERROR) -+ ret = -EIO; -+ else if (status == VCHIQ_RETRY) -+ ret = -EINTR; -+ } -+ -+ if ((status == VCHIQ_SUCCESS) && (ret < 0) && (ret != -EINTR) && -+ (ret != -EWOULDBLOCK)) -+ vchiq_log_info(vchiq_arm_log_level, -+ " ioctl instance %lx, cmd %s -> status %d, %ld", -+ (unsigned long)instance, -+ (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ? -+ ioctl_names[_IOC_NR(cmd)] : -+ "", -+ status, ret); -+ else -+ vchiq_log_trace(vchiq_arm_log_level, -+ " ioctl instance %lx, cmd %s -> status %d, %ld", -+ (unsigned long)instance, -+ (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ? -+ ioctl_names[_IOC_NR(cmd)] : -+ "", -+ status, ret); -+ -+ return ret; ++ 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)] : "", 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; ++ ++ case VCHIQ_IOC_DUMP_PHYS_MEM: ++ { ++ VCHIQ_DUMP_MEM_T args; ++ ++ if (copy_from_user ++ (&args, (const void __user *)arg, ++ sizeof(args)) != 0) { ++ ret = -EFAULT; ++ break; ++ } ++ dump_phys_mem( args.virt_addr, args.num_bytes ); ++ } ++ 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)] : ++ "", 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)] : ++ "", status, ret); ++ ++ return ret; +} + +/**************************************************************************** @@ -11309,49 +11219,39 @@ index 0000000..81daf3e +static int +vchiq_open(struct inode *inode, struct file *file) +{ -+ int dev = iminor(inode) & 0x0f; -+ vchiq_log_info(vchiq_arm_log_level, "vchiq_open"); -+ switch (dev) { -+ case VCHIQ_MINOR: { -+ int ret; -+ VCHIQ_STATE_T *state = vchiq_get_state(); -+ VCHIQ_INSTANCE_T instance; ++ 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) { -+ vchiq_log_error(vchiq_arm_log_level, -+ "vchiq has no connection to VideoCore"); -+ return -ENOTCONN; -+ } ++ if (!state) ++ { ++ vcos_log_error( "vchiq has no connection to VideoCore"); ++ return -ENOTCONN; ++ } + -+ instance = kzalloc(sizeof(*instance), GFP_KERNEL); -+ if (!instance) -+ return -ENOMEM; ++ instance = kzalloc(sizeof(*instance), GFP_KERNEL); ++ if (!instance) ++ return -ENOMEM; + -+ instance->state = state; -+ instance->pid = current->tgid; ++ instance->state = state; ++ instance->pid = current->tgid; ++ vcos_event_create(&instance->insert_event, DEVICE_NAME); ++ vcos_event_create(&instance->remove_event, DEVICE_NAME); + -+ ret = vchiq_proc_add_instance(instance); -+ if (ret != 0) { -+ kfree(instance); -+ return ret; -+ } ++ file->private_data = instance; ++ } ++ break; + -+ sema_init(&instance->insert_event, 0); -+ sema_init(&instance->remove_event, 0); -+ mutex_init(&instance->completion_mutex); -+ mutex_init(&instance->bulk_waiter_list_mutex); -+ INIT_LIST_HEAD(&instance->bulk_waiter_list); ++ default: ++ vcos_log_error("Unknown minor device: %d", dev); ++ return -ENXIO; ++ } + -+ file->private_data = instance; -+ } break; -+ -+ default: -+ vchiq_log_error(vchiq_arm_log_level, -+ "Unknown minor device: %d", dev); -+ return -ENXIO; -+ } -+ -+ return 0; ++ return 0; +} + +/**************************************************************************** @@ -11363,131 +11263,74 @@ index 0000000..81daf3e +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; -+ VCHIQ_STATE_T *state = vchiq_get_state(); -+ VCHIQ_SERVICE_T *service; -+ int i; ++ int dev = iminor(inode) & 0x0f; ++ int ret = 0; ++ switch (dev) { ++ case VCHIQ_MINOR: ++ { ++ VCHIQ_INSTANCE_T instance = file->private_data; ++ int i; + -+ vchiq_log_info(vchiq_arm_log_level, -+ "vchiq_release: instance=%lx", -+ (unsigned long)instance); ++ vcos_log_info("vchiq_release: instance=%lx", ++ (unsigned long)instance); + -+ if (state) { -+ /* Ensure videocore is awake to allow termination. */ -+ vchiq_use_internal(instance->state, NULL, -+ USE_TYPE_VCHIQ); -+ } ++ instance->closing = 1; + -+ mutex_lock(&instance->completion_mutex); ++ /* Wake the slot handler if the completion queue is full */ ++ vcos_event_signal(&instance->remove_event); + -+ /* Wake the completion thread and ask it to exit */ -+ instance->closing = 1; -+ up(&instance->insert_event); ++ /* Mark all services for termination... */ + -+ mutex_unlock(&instance->completion_mutex); ++ 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); + -+ /* Wake the slot handler if the completion queue is full. */ -+ up(&instance->remove_event); ++ if ((user_service->service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) && ++ (user_service->service->srvstate != VCHIQ_SRVSTATE_LISTENING)) ++ { ++ vchiq_terminate_service_internal(user_service->service); ++ } ++ } ++ } + -+ /* Mark all services for termination... */ -+ i = 0; -+ while ((service = next_service_by_instance(state, instance, -+ &i)) != NULL) { -+ USER_SERVICE_T *user_service = service->base.userdata; ++ /* ...and wait for them to die */ + -+ /* Wake the slot handler if the msg queue is full. */ -+ up(&user_service->remove_event); ++ 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_terminate_service_internal(service); -+ unlock_service(service); -+ } ++ vchiq_free_service_internal ++ (user_service->service); ++ } ++ } + -+ /* ...and wait for them to die */ -+ i = 0; -+ while ((service = next_service_by_instance(state, instance, &i)) -+ != NULL) { -+ USER_SERVICE_T *user_service = service->base.userdata; ++ vcos_event_delete(&instance->insert_event); ++ vcos_event_delete(&instance->remove_event); + -+ down(&service->remove_event); ++ kfree(instance); ++ file->private_data = NULL; ++ } ++ break; + -+ BUG_ON(service->srvstate != VCHIQ_SRVSTATE_FREE); ++ default: ++ vcos_log_error("Unknown minor device: %d", dev); ++ ret = -ENXIO; ++ } + -+ spin_lock(&msg_queue_spinlock); -+ -+ while (user_service->msg_remove != -+ user_service->msg_insert) { -+ VCHIQ_HEADER_T *header = user_service-> -+ msg_queue[user_service->msg_remove & -+ (MSG_QUEUE_SIZE - 1)]; -+ user_service->msg_remove++; -+ spin_unlock(&msg_queue_spinlock); -+ -+ if (header) -+ vchiq_release_message( -+ service->handle, -+ header); -+ spin_lock(&msg_queue_spinlock); -+ } -+ -+ spin_unlock(&msg_queue_spinlock); -+ -+ unlock_service(service); -+ kfree(user_service); -+ } -+ -+ /* Release any closed services */ -+ while (instance->completion_remove != -+ instance->completion_insert) { -+ VCHIQ_COMPLETION_DATA_T *completion; -+ VCHIQ_SERVICE_T *service; -+ completion = &instance->completions[ -+ instance->completion_remove & -+ (MAX_COMPLETIONS - 1)]; -+ service = completion->service_userdata; -+ if (completion->reason == VCHIQ_SERVICE_CLOSED) -+ unlock_service(service); -+ instance->completion_remove++; -+ } -+ -+ if (state) { -+ /* Release the PEER service count. */ -+ vchiq_release_internal(instance->state, NULL); -+ } -+ -+ { -+ struct list_head *pos, *next; -+ list_for_each_safe(pos, next, -+ &instance->bulk_waiter_list) { -+ struct bulk_waiter_node *waiter; -+ waiter = list_entry(pos, -+ struct bulk_waiter_node, -+ list); -+ list_del(pos); -+ vchiq_log_info(vchiq_arm_log_level, -+ "bulk_waiter - cleaned up %x " -+ "for pid %d", -+ (unsigned int)waiter, waiter->pid); -+ kfree(waiter); -+ } -+ } -+ -+ vchiq_proc_remove_instance(instance); -+ -+ kfree(instance); -+ file->private_data = NULL; -+ } break; -+ -+ default: -+ vchiq_log_error(vchiq_arm_log_level, -+ "Unknown minor device: %d", dev); -+ ret = -ENXIO; -+ } -+ -+ return ret; ++ return ret; +} + +/**************************************************************************** @@ -11499,37 +11342,40 @@ index 0000000..81daf3e +void +vchiq_dump(void *dump_context, const char *str, int len) +{ -+ DUMP_CONTEXT_T *context = (DUMP_CONTEXT_T *)dump_context; ++ 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 = min(len, (int)context->offset); -+ str += skip_bytes; -+ len -= skip_bytes; -+ context->offset -= skip_bytes; -+ if (context->offset > 0) -+ return; -+ } -+ copy_bytes = min(len, (int)(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 ((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; -+ } -+ } ++ /* 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; ++ } ++ } ++ } +} + +/**************************************************************************** @@ -11541,48 +11387,49 @@ index 0000000..81daf3e +void +vchiq_dump_platform_instances(void *dump_context) +{ -+ VCHIQ_STATE_T *state = vchiq_get_state(); -+ char buf[80]; -+ int len; -+ int i; ++ 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. */ ++ /* 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; ++ for (i = 0; i < state->unused_service; i++) ++ { ++ VCHIQ_SERVICE_T *service = state->services[i]; ++ VCHIQ_INSTANCE_T instance; + -+ if (service && (service->base.callback == service_callback)) { -+ instance = service->instance; -+ if (instance) -+ instance->mark = 0; -+ } -+ } ++ 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; ++ for (i = 0; i < state->unused_service; i++) ++ { ++ VCHIQ_SERVICE_T *service = state->services[i]; ++ VCHIQ_INSTANCE_T instance; + -+ if (service && (service->base.callback == service_callback)) { -+ instance = service->instance; -+ if (instance && !instance->mark) { -+ len = 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); ++ 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); ++ vchiq_dump(dump_context, buf, len + 1); + -+ instance->mark = 1; -+ } -+ } -+ } ++ instance->mark = 1; ++ } ++ } ++ } +} + +/**************************************************************************** @@ -11594,26 +11441,26 @@ index 0000000..81daf3e +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; ++ USER_SERVICE_T *user_service = (USER_SERVICE_T *)service->base.userdata; ++ char buf[80]; ++ int len; + -+ len = snprintf(buf, sizeof(buf), " instance %x", -+ (unsigned int)service->instance); ++ len = vcos_snprintf(buf, sizeof(buf), " instance %x", ++ service->instance); + -+ if ((service->base.callback == service_callback) && -+ user_service->is_vchi) { -+ len += snprintf(buf + len, sizeof(buf) - len, -+ ", %d/%d messages", -+ user_service->msg_insert - user_service->msg_remove, -+ MSG_QUEUE_SIZE); ++ 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 += snprintf(buf + len, sizeof(buf) - len, -+ " (dequeue pending)"); -+ } ++ if (user_service->dequeue_pending) ++ len += vcos_snprintf(buf + len, sizeof(buf) - len, ++ " (dequeue pending)"); ++ } + -+ vchiq_dump(dump_context, buf, len + 1); ++ vchiq_dump(dump_context, buf, len + 1); +} + +/**************************************************************************** @@ -11623,82 +11470,79 @@ index 0000000..81daf3e +***************************************************************************/ + +static void -+dump_phys_mem(void *virt_addr, uint32_t num_bytes) ++dump_phys_mem( void *virt_addr, uint32_t num_bytes ) +{ -+ int rc; -+ uint8_t *end_virt_addr = virt_addr + num_bytes; -+ int num_pages; -+ int offset; -+ int end_offset; -+ int page_idx; -+ int prev_idx; -+ struct page *page; -+ struct page **pages; -+ uint8_t *kmapped_virt_ptr; ++ int rc; ++ uint8_t *end_virt_addr = virt_addr + num_bytes; ++ int num_pages; ++ int offset; ++ int end_offset; ++ int page_idx; ++ int prev_idx; ++ struct page *page; ++ struct page **pages; ++ uint8_t *kmapped_virt_ptr; + -+ /* Align virtAddr and endVirtAddr to 16 byte boundaries. */ ++ // Align virtAddr and endVirtAddr to 16 byte boundaries. + -+ virt_addr = (void *)((unsigned long)virt_addr & ~0x0fuL); -+ end_virt_addr = (void *)(((unsigned long)end_virt_addr + 15uL) & -+ ~0x0fuL); ++ virt_addr = (void *)((unsigned long)virt_addr & ~0x0fuL ); ++ end_virt_addr = (void *)(( (unsigned long)end_virt_addr + 15uL ) & ~0x0fuL); + -+ offset = (int)(long)virt_addr & (PAGE_SIZE - 1); -+ end_offset = (int)(long)end_virt_addr & (PAGE_SIZE - 1); ++ offset = (int)(long)virt_addr & ( PAGE_SIZE - 1 ); ++ end_offset = (int)(long)end_virt_addr & ( PAGE_SIZE - 1 ); + -+ num_pages = (offset + num_bytes + PAGE_SIZE - 1) / PAGE_SIZE; ++ num_pages = (offset + num_bytes + PAGE_SIZE - 1) / PAGE_SIZE; + -+ pages = kmalloc(sizeof(struct page *) * num_pages, GFP_KERNEL); -+ if (pages == NULL) { -+ vchiq_log_error(vchiq_arm_log_level, -+ "Unable to allocation memory for %d pages\n", -+ num_pages); -+ return; -+ } ++ if (( pages = kmalloc( sizeof( struct page *) * num_pages, GFP_KERNEL )) == NULL ) ++ { ++ printk( KERN_ERR "Unable to allocation memory for %d pages\n", num_pages ); ++ return; ++ } + -+ down_read(¤t->mm->mmap_sem); -+ rc = get_user_pages(current, /* task */ -+ current->mm, /* mm */ -+ (unsigned long)virt_addr, /* start */ -+ num_pages, /* len */ -+ 0, /* write */ -+ 0, /* force */ -+ pages, /* pages (array of page pointers) */ -+ NULL); /* vmas */ -+ up_read(¤t->mm->mmap_sem); ++ down_read( ¤t->mm->mmap_sem ); ++ rc = get_user_pages( current, /* task */ ++ current->mm, /* mm */ ++ (unsigned long)virt_addr, /* start */ ++ num_pages, /* len */ ++ 0, /* write */ ++ 0, /* force */ ++ pages, /* pages (array of pointers to page) */ ++ NULL ); /* vmas */ ++ up_read( ¤t->mm->mmap_sem ); + -+ prev_idx = -1; -+ page = NULL; ++ prev_idx = -1; ++ page = NULL; + -+ while (offset < end_offset) { ++ while ( offset < end_offset ) { + -+ page_idx = offset / PAGE_SIZE; -+ int page_offset = offset % PAGE_SIZE; ++ int page_offset = offset % PAGE_SIZE; ++ page_idx = offset / PAGE_SIZE; + -+ if (page_idx != prev_idx) { ++ if ( page_idx != prev_idx ) { + -+ if (page != NULL) -+ kunmap(page); -+ page = pages[page_idx]; -+ kmapped_virt_ptr = kmap(page); ++ if (page != NULL) { ++ kunmap( page ); ++ } ++ page = pages[page_idx]; ++ kmapped_virt_ptr = kmap( page ); + -+ prev_idx = page_idx; -+ } ++ prev_idx = page_idx; ++ } + -+ if (vchiq_arm_log_level >= VCHIQ_LOG_TRACE) -+ vchiq_log_dump_mem("ph", -+ (uint32_t)(unsigned long)&kmapped_virt_ptr[ -+ page_offset], -+ &kmapped_virt_ptr[page_offset], 16); ++ vcos_log_dump_mem_impl( &vchiq_arm_log_category, "ph", ++ (uint32_t)(unsigned long)&kmapped_virt_ptr[page_offset], ++ &kmapped_virt_ptr[page_offset], 16 ); + -+ offset += 16; -+ } -+ if (page != NULL) -+ kunmap(page); ++ offset += 16; ++ } ++ if (page != NULL) { ++ kunmap( page ); ++ } + -+ for (page_idx = 0; page_idx < num_pages; page_idx++) -+ page_cache_release(pages[page_idx]); -+ -+ kfree(pages); ++ for ( page_idx = 0; page_idx < num_pages; page_idx++ ) { ++ page_cache_release( pages[page_idx] ); ++ } ++ kfree( pages ); +} + +/**************************************************************************** @@ -11708,1182 +11552,548 @@ index 0000000..81daf3e +***************************************************************************/ + +static ssize_t -+vchiq_read(struct file *file, char __user *buf, -+ size_t count, loff_t *ppos) ++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; ++ DUMP_CONTEXT_T context; ++ context.buf = buf; ++ context.actual = 0; ++ context.space = count; ++ context.offset = *ppos; + -+ vchiq_dump_state(&context, &g_state); ++ vchiq_dump_state(&context, &g_state); + -+ if (context.actual >= 0) -+ *ppos += context.actual; ++ if (context.actual >= 0) ++ *ppos += context.actual; + -+ return context.actual; ++ return context.actual; +} + +VCHIQ_STATE_T * +vchiq_get_state(void) +{ + -+ if (g_state.remote == NULL) -+ printk(KERN_ERR "%s: g_state.remote == NULL\n", __func__); -+ else if (g_state.remote->initialised != 1) -+ printk(KERN_ERR "%s: g_state.remote->initialised != 1 (%d)\n", -+ __func__, g_state.remote->initialised); ++ 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; ++ 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 ++ .owner = THIS_MODULE, ++ .unlocked_ioctl = vchiq_ioctl, ++ .open = vchiq_open, ++ .release = vchiq_release, ++ .read = vchiq_read +}; + +/* + * Autosuspend related functionality + */ + -+int -+vchiq_videocore_wanted(VCHIQ_STATE_T *state) ++static int vchiq_videocore_wanted(VCHIQ_STATE_T* state) +{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ if (!arm_state) -+ /* autosuspend not supported - always return wanted */ -+ return 1; -+ else if (arm_state->blocked_count) -+ return 1; -+ else if (!arm_state->videocore_use_count) -+ /* usage count zero - check for override unless we're forcing */ -+ if (arm_state->resume_blocked) -+ return 0; -+ else -+ return vchiq_platform_videocore_wanted(state); -+ else -+ /* non-zero usage count - videocore still required */ -+ return 1; -+} -+static VCHIQ_STATUS_T vchiq_keepalive_vchiq_callback(VCHIQ_REASON_T reason, -+ VCHIQ_HEADER_T *header, -+ VCHIQ_SERVICE_HANDLE_T service_user, -+ void *bulk_user) -+{ -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s callback reason %d", __func__, reason); -+ return 0; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ if(!arm_state) ++ { // autosuspend not supported - always return wanted ++ return 1; ++ } ++ else if(!arm_state->videocore_use_count) ++ { // usage count zero - check for override ++ return vchiq_platform_videocore_wanted(state); ++ } ++ else ++ { // non-zero usage count - videocore still required ++ return 1; ++ } +} + -+static int -+vchiq_keepalive_thread_func(void *v) ++ ++/* Called by the lp thread */ ++static void * ++lp_func(void *v) +{ -+ VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v; -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); + -+ VCHIQ_STATUS_T status; -+ VCHIQ_INSTANCE_T instance; -+ VCHIQ_SERVICE_HANDLE_T ka_handle; ++ while (1) { ++ vcos_event_wait(&arm_state->lp_evt); + -+ VCHIQ_SERVICE_PARAMS_T params = { -+ .fourcc = VCHIQ_MAKE_FOURCC('K', 'E', 'E', 'P'), -+ .callback = vchiq_keepalive_vchiq_callback, -+ .version = KEEPALIVE_VER, -+ .version_min = KEEPALIVE_VER_MIN -+ }; ++ vcos_mutex_lock(&arm_state->use_count_mutex); ++ if (!vchiq_videocore_wanted(state)) ++ { ++ arm_state->suspend_pending = 1; ++ } ++ vcos_mutex_unlock(&arm_state->use_count_mutex); + -+ status = vchiq_initialise(&instance); -+ if (status != VCHIQ_SUCCESS) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s vchiq_initialise failed %d", __func__, status); -+ goto exit; -+ } -+ -+ status = vchiq_connect(instance); -+ if (status != VCHIQ_SUCCESS) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s vchiq_connect failed %d", __func__, status); -+ goto shutdown; -+ } -+ -+ status = vchiq_add_service(instance, ¶ms, &ka_handle); -+ if (status != VCHIQ_SUCCESS) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s vchiq_open_service failed %d", __func__, status); -+ goto shutdown; -+ } -+ -+ while (1) { -+ long rc = 0, uc = 0; -+ if (wait_for_completion_interruptible(&arm_state->ka_evt) -+ != 0) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s interrupted", __func__); -+ flush_signals(current); -+ continue; -+ } -+ -+ /* read and clear counters. Do release_count then use_count to -+ * prevent getting more releases than uses */ -+ rc = atomic_xchg(&arm_state->ka_release_count, 0); -+ uc = atomic_xchg(&arm_state->ka_use_count, 0); -+ -+ /* Call use/release service the requisite number of times. -+ * Process use before release so use counts don't go negative */ -+ while (uc--) { -+ atomic_inc(&arm_state->ka_use_ack_count); -+ status = vchiq_use_service(ka_handle); -+ if (status != VCHIQ_SUCCESS) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s vchiq_use_service error %d", -+ __func__, status); -+ } -+ } -+ while (rc--) { -+ status = vchiq_release_service(ka_handle); -+ if (status != VCHIQ_SUCCESS) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s vchiq_release_service error %d", -+ __func__, status); -+ } -+ } -+ } -+ -+shutdown: -+ vchiq_shutdown(instance); -+exit: -+ return 0; ++ vchiq_arm_vcsuspend(state); ++ } ++ return NULL; +} ++/* Called by the hp thread */ ++static void * ++hp_func(void *v) ++{ ++ VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ int send_pending; + ++ while (1) { ++ vcos_event_wait(&arm_state->hp_evt); ++ ++ send_pending = 0; ++ ++ vcos_mutex_lock(&arm_state->use_count_mutex); ++ if (vchiq_videocore_wanted(state)) ++ { ++ vchiq_arm_vcresume(state); ++ } ++ if(arm_state->use_notify_pending) ++ { ++ send_pending = arm_state->use_notify_pending; ++ arm_state->use_notify_pending=0; ++ } ++ vcos_mutex_unlock(&arm_state->use_count_mutex); ++ while(send_pending--) ++ { ++ vcos_log_info( "%s sending VCHIQ_MSG_REMOTE_USE_ACTIVE", __func__); ++ if ( vchiq_send_remote_use_active(state) != VCHIQ_SUCCESS) ++ { ++ BUG(); /* vc should be resumed, so shouldn't be a problem sending message */ ++ } ++ } ++ } ++ return NULL; ++} + +VCHIQ_STATUS_T -+vchiq_arm_init_state(VCHIQ_STATE_T *state, VCHIQ_ARM_STATE_T *arm_state) ++vchiq_arm_init_state(VCHIQ_STATE_T* state, VCHIQ_ARM_STATE_T *arm_state) +{ -+ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; -+ char threadname[10]; ++ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ VCOS_THREAD_ATTR_T attrs; ++ char threadname[10]; + -+ if (arm_state) { -+ mutex_init(&arm_state->vc_use_cnt_mutex); -+ rwlock_init(&arm_state->susp_res_lock); ++ if(arm_state) ++ { ++ vcos_mutex_create(&arm_state->use_count_mutex, "v.use_count_mutex"); ++ vcos_mutex_create(&arm_state->suspend_resume_mutex, "v.susp_res_mutex"); + -+ init_completion(&arm_state->ka_evt); -+ atomic_set(&arm_state->ka_use_count, 0); -+ atomic_set(&arm_state->ka_use_ack_count, 0); -+ atomic_set(&arm_state->ka_release_count, 0); ++ vcos_event_create(&arm_state->lp_evt, "LP_EVT"); ++ vcos_event_create(&arm_state->hp_evt, "HP_EVT"); + -+ init_completion(&arm_state->vc_suspend_complete); ++ vcos_thread_attr_init(&attrs); ++ vcos_thread_attr_setstacksize(&attrs, VCHIQ_ARM_VCSUSPEND_TASK_STACK); ++ vcos_thread_attr_setpriority(&attrs, VCOS_THREAD_PRI_LOWEST); ++ vcos_snprintf(threadname, sizeof(threadname), "VCHIQl-%d", state->id); ++ if(vcos_thread_create(&arm_state->lp_thread, threadname, &attrs, lp_func, state) != VCOS_SUCCESS) ++ { ++ vcos_log_error("vchiq: FATAL: couldn't create thread %s", threadname); ++ status = VCHIQ_ERROR; ++ } ++ else ++ { ++ vcos_thread_attr_init(&attrs); ++ vcos_thread_attr_setstacksize(&attrs, VCHIQ_ARM_VCSUSPEND_TASK_STACK); ++ vcos_thread_attr_setpriority(&attrs, VCOS_THREAD_PRI_HIGHEST); ++ vcos_snprintf(threadname, sizeof(threadname), "VCHIQh-%d", state->id); + -+ init_completion(&arm_state->vc_resume_complete); -+ /* Initialise to 'done' state. We only want to block on resume -+ * completion while videocore is suspended. */ -+ set_resume_state(arm_state, VC_RESUME_RESUMED); -+ -+ init_completion(&arm_state->resume_blocker); -+ /* Initialise to 'done' state. We only want to block on this -+ * completion while resume is blocked */ -+ complete_all(&arm_state->resume_blocker); -+ -+ init_completion(&arm_state->blocked_blocker); -+ /* Initialise to 'done' state. We only want to block on this -+ * completion while things are waiting on the resume blocker */ -+ complete_all(&arm_state->blocked_blocker); -+ -+ snprintf(threadname, sizeof(threadname), "VCHIQka-%d", -+ state->id); -+ arm_state->ka_thread = kthread_create( -+ &vchiq_keepalive_thread_func, -+ (void *)state, -+ threadname); -+ if (arm_state->ka_thread == NULL) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "vchiq: FATAL: couldn't create thread %s", -+ threadname); -+ status = VCHIQ_ERROR; -+ } -+ -+ arm_state->suspend_timer_timeout = SUSPEND_TIMER_TIMEOUT_MS; -+ arm_state->suspend_timer_running = 0; -+ init_timer(&arm_state->suspend_timer); -+ arm_state->suspend_timer.data = (unsigned long)(state); -+ arm_state->suspend_timer.function = suspend_timer_callback; -+ -+ arm_state->first_connect = 0; -+ -+ } -+ return status; -+} -+ -+/* -+** Functions to modify the state variables; -+** set_suspend_state -+** set_resume_state -+** -+** There are more state variables than we might like, so ensure they remain in -+** step. Suspend and resume state are maintained separately, since most of -+** these state machines can operate independently. However, there are a few -+** states where state transitions in one state machine cause a reset to the -+** other state machine. In addition, there are some completion events which -+** need to occur on state machine reset and end-state(s), so these are also -+** dealt with in these functions. -+** -+** In all states we set the state variable according to the input, but in some -+** cases we perform additional steps outlined below; -+** -+** VC_SUSPEND_IDLE - Initialise the suspend completion at the same time. -+** The suspend completion is completed after any suspend -+** attempt. When we reset the state machine we also reset -+** the completion. This reset occurs when videocore is -+** resumed, and also if we initiate suspend after a suspend -+** failure. -+** -+** VC_SUSPEND_IN_PROGRESS - This state is considered the point of no return for -+** suspend - ie from this point on we must try to suspend -+** before resuming can occur. We therefore also reset the -+** resume state machine to VC_RESUME_IDLE in this state. -+** -+** VC_SUSPEND_SUSPENDED - Suspend has completed successfully. Also call -+** complete_all on the suspend completion to notify -+** anything waiting for suspend to happen. -+** -+** VC_SUSPEND_REJECTED - Videocore rejected suspend. Videocore will also -+** initiate resume, so no need to alter resume state. -+** We call complete_all on the suspend completion to notify -+** of suspend rejection. -+** -+** VC_SUSPEND_FAILED - We failed to initiate videocore suspend. We notify the -+** suspend completion and reset the resume state machine. -+** -+** VC_RESUME_IDLE - Initialise the resume completion at the same time. The -+** resume completion is in it's 'done' state whenever -+** videcore is running. Therfore, the VC_RESUME_IDLE state -+** implies that videocore is suspended. -+** Hence, any thread which needs to wait until videocore is -+** running can wait on this completion - it will only block -+** if videocore is suspended. -+** -+** VC_RESUME_RESUMED - Resume has completed successfully. Videocore is running. -+** Call complete_all on the resume completion to unblock -+** any threads waiting for resume. Also reset the suspend -+** state machine to it's idle state. -+** -+** VC_RESUME_FAILED - Currently unused - no mechanism to fail resume exists. -+*/ -+ -+inline void -+set_suspend_state(VCHIQ_ARM_STATE_T *arm_state, -+ enum vc_suspend_status new_state) -+{ -+ /* set the state in all cases */ -+ arm_state->vc_suspend_state = new_state; -+ -+ /* state specific additional actions */ -+ switch (new_state) { -+ case VC_SUSPEND_FORCE_CANCELED: -+ complete_all(&arm_state->vc_suspend_complete); -+ break; -+ case VC_SUSPEND_REJECTED: -+ complete_all(&arm_state->vc_suspend_complete); -+ break; -+ case VC_SUSPEND_FAILED: -+ complete_all(&arm_state->vc_suspend_complete); -+ arm_state->vc_resume_state = VC_RESUME_RESUMED; -+ complete_all(&arm_state->vc_resume_complete); -+ break; -+ case VC_SUSPEND_IDLE: -+ INIT_COMPLETION(arm_state->vc_suspend_complete); -+ break; -+ case VC_SUSPEND_REQUESTED: -+ break; -+ case VC_SUSPEND_IN_PROGRESS: -+ set_resume_state(arm_state, VC_RESUME_IDLE); -+ break; -+ case VC_SUSPEND_SUSPENDED: -+ complete_all(&arm_state->vc_suspend_complete); -+ break; -+ default: -+ BUG(); -+ break; -+ } -+} -+ -+inline void -+set_resume_state(VCHIQ_ARM_STATE_T *arm_state, -+ enum vc_resume_status new_state) -+{ -+ /* set the state in all cases */ -+ arm_state->vc_resume_state = new_state; -+ -+ /* state specific additional actions */ -+ switch (new_state) { -+ case VC_RESUME_FAILED: -+ break; -+ case VC_RESUME_IDLE: -+ INIT_COMPLETION(arm_state->vc_resume_complete); -+ break; -+ case VC_RESUME_REQUESTED: -+ break; -+ case VC_RESUME_IN_PROGRESS: -+ break; -+ case VC_RESUME_RESUMED: -+ complete_all(&arm_state->vc_resume_complete); -+ set_suspend_state(arm_state, VC_SUSPEND_IDLE); -+ break; -+ default: -+ BUG(); -+ break; -+ } ++ if(vcos_thread_create(&arm_state->hp_thread, threadname, &attrs, hp_func, state) != VCOS_SUCCESS) ++ { ++ vcos_log_error("vchiq: FATAL: couldn't create thread %s", threadname); ++ status = VCHIQ_ERROR; ++ } ++ } ++ } ++ return status; +} + + -+/* should be called with the write lock held */ -+inline void -+start_suspend_timer(VCHIQ_ARM_STATE_T *arm_state) -+{ -+ del_timer(&arm_state->suspend_timer); -+ arm_state->suspend_timer.expires = jiffies + -+ msecs_to_jiffies(arm_state-> -+ suspend_timer_timeout); -+ add_timer(&arm_state->suspend_timer); -+ arm_state->suspend_timer_running = 1; -+} -+ -+/* should be called with the write lock held */ -+static inline void -+stop_suspend_timer(VCHIQ_ARM_STATE_T *arm_state) -+{ -+ if (arm_state->suspend_timer_running) { -+ del_timer(&arm_state->suspend_timer); -+ arm_state->suspend_timer_running = 0; -+ } -+} -+ -+static inline int -+need_resume(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ return (arm_state->vc_suspend_state > VC_SUSPEND_IDLE) && -+ (arm_state->vc_resume_state < VC_RESUME_REQUESTED) && -+ vchiq_videocore_wanted(state); -+} -+ -+static int -+block_resume(VCHIQ_ARM_STATE_T *arm_state) -+{ -+ int status = VCHIQ_SUCCESS; -+ while (arm_state->blocked_count) { -+ INIT_COMPLETION(arm_state->blocked_blocker); -+ write_unlock_bh(&arm_state->susp_res_lock); -+ vchiq_log_info(vchiq_susp_log_level, "%s wait for previously " -+ "blocked clients", __func__); -+ if (wait_for_completion_interruptible_timeout( -+ &arm_state->blocked_blocker, -+ msecs_to_jiffies(FORCE_SUSPEND_TIMEOUT_MS)) -+ <= 0) { -+ vchiq_log_error(vchiq_susp_log_level, "%s wait for " -+ "previously blocked clients failed" , __func__); -+ status = VCHIQ_ERROR; -+ write_lock_bh(&arm_state->susp_res_lock); -+ goto out; -+ } -+ vchiq_log_info(vchiq_susp_log_level, "%s previously blocked " -+ "clients resumed", __func__); -+ write_lock_bh(&arm_state->susp_res_lock); -+ } -+ while (arm_state->vc_resume_state != VC_RESUME_RESUMED && -+ arm_state->vc_resume_state > VC_RESUME_IDLE) { -+ if (!try_wait_for_completion(&arm_state->vc_resume_complete)) { -+ write_unlock_bh(&arm_state->susp_res_lock); -+ vchiq_log_info(vchiq_susp_log_level, "%s wait for " -+ "resume", __func__); -+ if (wait_for_completion_interruptible_timeout( -+ &arm_state->vc_resume_complete, -+ msecs_to_jiffies( -+ FORCE_SUSPEND_TIMEOUT_MS)) -+ <= 0) { -+ vchiq_log_error(vchiq_susp_log_level, "%s wait " -+ "for resume failed (%s)", __func__, -+ resume_state_names[ -+ arm_state->vc_resume_state + -+ VC_RESUME_NUM_OFFSET]); -+ status = VCHIQ_ERROR; -+ write_lock_bh(&arm_state->susp_res_lock); -+ goto out; -+ } -+ vchiq_log_info(vchiq_susp_log_level, "%s resumed", -+ __func__); -+ write_lock_bh(&arm_state->susp_res_lock); -+ } -+ } -+ INIT_COMPLETION(arm_state->resume_blocker); -+ arm_state->resume_blocked = 1; -+ -+out: -+ return status; -+} -+ -+inline static void -+unblock_resume(VCHIQ_ARM_STATE_T *arm_state) -+{ -+ complete_all(&arm_state->resume_blocker); -+ arm_state->resume_blocked = 0; -+} -+ -+/* Initiate suspend via slot handler. Should be called with the write lock -+ * held */ +VCHIQ_STATUS_T +vchiq_arm_vcsuspend(VCHIQ_STATE_T *state) +{ -+ VCHIQ_STATUS_T status = VCHIQ_ERROR; -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); + -+ if (!arm_state) -+ goto out; ++ if (state->conn_state != VCHIQ_CONNSTATE_CONNECTED) ++ return VCHIQ_ERROR; + -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ status = VCHIQ_SUCCESS; ++ if(arm_state->suspend_pending) ++ { ++ vcos_mutex_lock(&arm_state->suspend_resume_mutex); ++ if(arm_state->videocore_suspended) ++ { ++ vcos_log_info("%s - already suspended", __func__); ++ } ++ else ++ { ++ vcos_log_info("%s - suspending", __func__); + ++ status = vchiq_platform_suspend(state); ++ arm_state->videocore_suspended = (status == VCHIQ_SUCCESS) ? 1 : 0; + -+ switch (arm_state->vc_suspend_state) { -+ case VC_SUSPEND_REQUESTED: -+ vchiq_log_info(vchiq_susp_log_level, "%s: suspend already " -+ "requested", __func__); -+ break; -+ case VC_SUSPEND_IN_PROGRESS: -+ vchiq_log_info(vchiq_susp_log_level, "%s: suspend already in " -+ "progress", __func__); -+ break; ++ vcos_mutex_unlock(&arm_state->suspend_resume_mutex); + -+ default: -+ /* We don't expect to be in other states, so log but continue -+ * anyway */ -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s unexpected suspend state %s", __func__, -+ suspend_state_names[arm_state->vc_suspend_state + -+ VC_SUSPEND_NUM_OFFSET]); -+ /* fall through */ -+ case VC_SUSPEND_REJECTED: -+ case VC_SUSPEND_FAILED: -+ INIT_COMPLETION(arm_state->vc_suspend_complete); -+ /* fall through */ -+ case VC_SUSPEND_IDLE: -+ vchiq_log_info(vchiq_susp_log_level, -+ "%s: suspending", __func__); -+ set_suspend_state(arm_state, VC_SUSPEND_REQUESTED); -+ /* kick the slot handler thread to initiate suspend */ -+ request_poll(state, NULL, 0); -+ break; -+ } -+ -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, status); -+ return status; ++ vcos_mutex_lock(&arm_state->use_count_mutex); ++ if(!arm_state->suspend_pending) ++ { /* Something has changed the suspend_pending state while we were suspending. ++ Run the HP task to check if we need to resume */ ++ vcos_log_info( "%s trigger HP task to check resume", __func__); ++ vcos_event_signal(&arm_state->hp_evt); ++ } ++ arm_state->suspend_pending = 0; ++ vcos_mutex_unlock(&arm_state->use_count_mutex); ++ } ++ } ++ else ++ { ++ vchiq_check_resume(state); ++ } ++ return status; +} + -+void -+vchiq_platform_check_suspend(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ int susp = 0; -+ -+ if (!arm_state) -+ goto out; -+ -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ if (arm_state->vc_suspend_state == VC_SUSPEND_REQUESTED && -+ arm_state->vc_resume_state == VC_RESUME_RESUMED) { -+ set_suspend_state(arm_state, VC_SUSPEND_IN_PROGRESS); -+ susp = 1; -+ } -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+ if (susp) -+ vchiq_platform_suspend(state); -+ -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__); -+ return; -+} -+ -+ -+ -+/* Try to get videocore into suspended state, regardless of autosuspend state. -+** We don't actually force suspend, since videocore may get into a bad state -+** if we force suspend at a bad time. Instead, we wait for autosuspend to -+** determine a good point to suspend. If this doesn't happen within 100ms we -+** report failure. -+** -+** Returns VCHIQ_SUCCESS if videocore suspended successfully, VCHIQ_RETRY if -+** videocore failed to suspend in time or VCHIQ_ERROR if interrupted. -+*/ -+VCHIQ_STATUS_T -+vchiq_arm_force_suspend(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ VCHIQ_STATUS_T status = VCHIQ_ERROR; -+ long rc = 0; -+ int repeat = -1; -+ static int autosuspend_override; -+ -+ if (!arm_state) -+ goto out; -+ -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ -+ status = block_resume(arm_state); -+ if (status != VCHIQ_SUCCESS) -+ goto unlock; -+ if (arm_state->vc_suspend_state == VC_SUSPEND_SUSPENDED) { -+ /* Already suspended - just block resume and exit */ -+ vchiq_log_info(vchiq_susp_log_level, "%s already " -+ "suspended", __func__); -+ status = VCHIQ_SUCCESS; -+ goto unlock; -+ } else if (arm_state->vc_suspend_state <= VC_SUSPEND_IDLE) { -+ /* initiate suspend immediately in the case that we're waiting -+ * for the timeout */ -+ stop_suspend_timer(arm_state); -+ if (!vchiq_videocore_wanted(state)) { -+ vchiq_log_info(vchiq_susp_log_level, "%s videocore " -+ "idle, initiating suspend", __func__); -+ status = vchiq_arm_vcsuspend(state); -+ } else if (autosuspend_override < FORCE_SUSPEND_FAIL_MAX) { -+ vchiq_log_info(vchiq_susp_log_level, "%s letting " -+ "videocore go idle", __func__); -+ status = VCHIQ_SUCCESS; -+ } else { -+ vchiq_log_warning(vchiq_susp_log_level, "%s failed too " -+ "many times - no more Mr Nice Guy... " -+ "initiating suspend", __func__); -+ status = vchiq_arm_vcsuspend(state); -+ } -+ } else { -+ vchiq_log_info(vchiq_susp_log_level, "%s videocore suspend " -+ "in progress - wait for completion", __func__); -+ status = VCHIQ_SUCCESS; -+ } -+ -+ /* Wait for suspend to happen due to system idle (not forced..) */ -+ if (status == VCHIQ_SUCCESS) { -+ do { -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+ rc = wait_for_completion_interruptible_timeout( -+ &arm_state->vc_suspend_complete, -+ msecs_to_jiffies(FORCE_SUSPEND_TIMEOUT_MS)); -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ if (rc < 0) { -+ vchiq_log_warning(vchiq_susp_log_level, "%s " -+ "interrupted waiting for suspend", -+ __func__); -+ status = VCHIQ_ERROR; -+ goto unblock_resume; -+ } else if (rc == 0) { -+ if (arm_state->vc_suspend_state > -+ VC_SUSPEND_IDLE) { -+ /* Repeat timeout once if in progress */ -+ if (repeat < 0) { -+ repeat = 1; -+ continue; -+ } -+ } -+ autosuspend_override++; -+ vchiq_log_warning(vchiq_susp_log_level, -+ "%s timed out waiting for vc suspend " -+ "(%d)", __func__, autosuspend_override); -+ status = VCHIQ_RETRY; -+ goto unblock_resume; -+ } -+ } while (0 < (repeat--)); -+ } else -+ goto unblock_resume; -+ -+ /* Check and report state in case we need to abort ARM suspend */ -+ if (arm_state->vc_suspend_state != VC_SUSPEND_SUSPENDED) { -+ status = VCHIQ_RETRY; -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s videocore suspend failed (state %s)", __func__, -+ suspend_state_names[arm_state->vc_suspend_state + -+ VC_SUSPEND_NUM_OFFSET]); -+ /* Reset the state only if it's still in an error state. -+ * Something could have already initiated another suspend. */ -+ if (arm_state->vc_suspend_state < VC_SUSPEND_IDLE) -+ set_suspend_state(arm_state, VC_SUSPEND_IDLE); -+ -+ goto unblock_resume; -+ } -+ -+ /* successfully suspended - reset override counter, unlock and exit */ -+ autosuspend_override = 0; -+ goto unlock; -+ -+unblock_resume: -+ /* all error states need to unblock resume before exit */ -+ unblock_resume(arm_state); -+ -+unlock: -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, status); -+ return status; -+} -+ -+void -+vchiq_check_suspend(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ -+ if (!arm_state) -+ goto out; -+ -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ if (arm_state->vc_suspend_state != VC_SUSPEND_SUSPENDED && -+ arm_state->first_connect && -+ !vchiq_videocore_wanted(state)) { -+ vchiq_arm_vcsuspend(state); -+ } -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__); -+ return; -+} -+ -+ -+int -+vchiq_arm_allow_resume(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ int resume = 0; -+ int ret = -1; -+ -+ if (!arm_state) -+ goto out; -+ -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ unblock_resume(arm_state); -+ resume = vchiq_check_resume(state); -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+ if (resume) { -+ wait_for_completion_interruptible( -+ &arm_state->vc_resume_complete); -+ } -+ -+ read_lock_bh(&arm_state->susp_res_lock); -+ if (arm_state->vc_suspend_state == VC_SUSPEND_SUSPENDED) { -+ vchiq_log_info(vchiq_susp_log_level, -+ "%s: Videocore remains suspended", __func__); -+ } else { -+ vchiq_log_info(vchiq_susp_log_level, -+ "%s: Videocore resumed", __func__); -+ ret = 0; -+ } -+ read_unlock_bh(&arm_state->susp_res_lock); -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret); -+ return ret; -+} -+ -+/* This function should be called with the write lock held */ -+int -+vchiq_check_resume(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ int resume = 0; -+ -+ if (!arm_state) -+ goto out; -+ -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ -+ if (need_resume(state)) { -+ set_resume_state(arm_state, VC_RESUME_REQUESTED); -+ request_poll(state, NULL, 0); -+ resume = 1; -+ } -+ -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__); -+ return resume; -+} -+ -+void -+vchiq_platform_check_resume(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ int res = 0; -+ -+ if (!arm_state) -+ goto out; -+ -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ if (arm_state->wake_address == 0) { -+ vchiq_log_info(vchiq_susp_log_level, -+ "%s: already awake", __func__); -+ goto unlock; -+ } -+ if (arm_state->vc_resume_state == VC_RESUME_IN_PROGRESS) { -+ vchiq_log_info(vchiq_susp_log_level, -+ "%s: already resuming", __func__); -+ goto unlock; -+ } -+ -+ if (arm_state->vc_resume_state == VC_RESUME_REQUESTED) { -+ set_resume_state(arm_state, VC_RESUME_IN_PROGRESS); -+ res = 1; -+ } else -+ vchiq_log_trace(vchiq_susp_log_level, -+ "%s: not resuming (resume state %s)", __func__, -+ resume_state_names[arm_state->vc_resume_state + -+ VC_RESUME_NUM_OFFSET]); -+ -+unlock: -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+ if (res) -+ vchiq_platform_resume(state); -+ -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__); -+ return; -+ -+} -+ -+ + +VCHIQ_STATUS_T -+vchiq_use_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service, -+ enum USE_TYPE_E use_type) ++vchiq_arm_vcresume(VCHIQ_STATE_T *state) +{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ VCHIQ_STATUS_T ret = VCHIQ_SUCCESS; -+ char entity[16]; -+ int *entity_uc; -+ int local_uc, local_entity_uc; ++ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ vcos_mutex_lock(&arm_state->suspend_resume_mutex); + -+ if (!arm_state) -+ goto out; ++ status = vchiq_platform_resume(state); ++ arm_state->videocore_suspended = (status == VCHIQ_RETRY) ? 1 : 0; + -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); ++ vcos_mutex_unlock(&arm_state->suspend_resume_mutex); + -+ if (use_type != USE_TYPE_SERVICE) { -+ sprintf(entity, "PEER: "); -+ entity_uc = &arm_state->peer_use_count; -+ } else if (service) { -+ sprintf(entity, "%c%c%c%c:%03d", -+ VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), -+ service->client_id); -+ entity_uc = &service->service_use_count; -+ } else { -+ vchiq_log_error(vchiq_susp_log_level, "%s null service " -+ "ptr", __func__); -+ ret = VCHIQ_ERROR; -+ goto out; -+ } -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ while (arm_state->resume_blocked) { -+ /* If we call 'use' while force suspend is waiting for suspend, -+ * then we're about to block the thread which the force is -+ * waiting to complete, so we're bound to just time out. In this -+ * case, set the suspend state such that the wait will be -+ * canceled, so we can complete as quickly as possible. */ -+ if (arm_state->resume_blocked && arm_state->vc_suspend_state == -+ VC_SUSPEND_IDLE) { -+ set_suspend_state(arm_state, VC_SUSPEND_FORCE_CANCELED); -+ break; -+ } -+ /* If suspend is already in progress then we need to block */ -+ if (!try_wait_for_completion(&arm_state->resume_blocker)) { -+ /* Indicate that there are threads waiting on the resume -+ * blocker. These need to be allowed to complete before -+ * a _second_ call to force suspend can complete, -+ * otherwise low priority threads might never actually -+ * continue */ -+ arm_state->blocked_count++; -+ write_unlock_bh(&arm_state->susp_res_lock); -+ vchiq_log_info(vchiq_susp_log_level, "%s %s resume " -+ "blocked - waiting...", __func__, entity); -+ if (wait_for_completion_killable( -+ &arm_state->resume_blocker) != 0) { -+ vchiq_log_error(vchiq_susp_log_level, "%s %s " -+ "wait for resume blocker interrupted", -+ __func__, entity); -+ ret = VCHIQ_ERROR; -+ write_lock_bh(&arm_state->susp_res_lock); -+ arm_state->blocked_count--; -+ write_unlock_bh(&arm_state->susp_res_lock); -+ goto out; -+ } -+ vchiq_log_info(vchiq_susp_log_level, "%s %s resume " -+ "unblocked", __func__, entity); -+ write_lock_bh(&arm_state->susp_res_lock); -+ if (--arm_state->blocked_count == 0) -+ complete_all(&arm_state->blocked_blocker); -+ } -+ } -+ -+ stop_suspend_timer(arm_state); -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+ mutex_lock(&arm_state->vc_use_cnt_mutex); -+ if (!arm_state->videocore_use_count) -+ pm_qos_add_request(&arm_state->qos_request, -+ PM_QOS_CPU_DMA_LATENCY, 100); -+ -+ local_uc = ++arm_state->videocore_use_count; -+ local_entity_uc = ++(*entity_uc); -+ mutex_unlock(&arm_state->vc_use_cnt_mutex); -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ -+ /* If there's a pending request which hasn't yet been serviced then -+ * just clear it. If we're past VC_SUSPEND_REQUESTED state then -+ * vc_resume_complete will block until we either resume or fail to -+ * suspend */ -+ if (arm_state->vc_suspend_state <= VC_SUSPEND_REQUESTED) -+ set_suspend_state(arm_state, VC_SUSPEND_IDLE); -+ -+ if (need_resume(state)) { -+ set_resume_state(arm_state, VC_RESUME_REQUESTED); -+ vchiq_log_info(vchiq_susp_log_level, -+ "%s %s count %d, state count %d", -+ __func__, entity, local_entity_uc, local_uc); -+ request_poll(state, NULL, 0); -+ } else -+ vchiq_log_trace(vchiq_susp_log_level, -+ "%s %s count %d, state count %d", -+ __func__, entity, *entity_uc, local_uc); -+ -+ -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+ /* Completion is in a done state when we're not suspended, so this won't -+ * block for the non-suspended case. */ -+ if (!try_wait_for_completion(&arm_state->vc_resume_complete)) { -+ vchiq_log_info(vchiq_susp_log_level, "%s %s wait for resume", -+ __func__, entity); -+ if (wait_for_completion_killable( -+ &arm_state->vc_resume_complete) != 0) { -+ vchiq_log_error(vchiq_susp_log_level, "%s %s wait for " -+ "resume interrupted", __func__, entity); -+ ret = VCHIQ_ERROR; -+ goto out; -+ } -+ vchiq_log_info(vchiq_susp_log_level, "%s %s resumed", __func__, -+ entity); -+ } -+ -+ if (ret == VCHIQ_SUCCESS) { -+ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; -+ long ack_cnt = atomic_xchg(&arm_state->ka_use_ack_count, 0); -+ while (ack_cnt && (status == VCHIQ_SUCCESS)) { -+ /* Send the use notify to videocore */ -+ status = vchiq_send_remote_use_active(state); -+ if (status == VCHIQ_SUCCESS) -+ ack_cnt--; -+ else -+ atomic_add(ack_cnt, -+ &arm_state->ka_use_ack_count); -+ } -+ } -+ -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret); -+ return ret; ++ return status; +} + -+VCHIQ_STATUS_T ++void ++vchiq_check_resume(VCHIQ_STATE_T* state) ++{ ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ vcos_mutex_lock(&arm_state->use_count_mutex); ++ ++ if (arm_state->videocore_suspended && vchiq_videocore_wanted(state)) ++ { /* signal high priority task to resume vc */ ++ vcos_event_signal(&arm_state->hp_evt); ++ } ++ ++ vcos_mutex_unlock(&arm_state->use_count_mutex); ++} ++ ++void ++vchiq_check_suspend(VCHIQ_STATE_T* state) ++{ ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ ++ vcos_mutex_lock(&arm_state->use_count_mutex); ++ ++ if (!arm_state->videocore_suspended && !vchiq_videocore_wanted(state)) ++ { /* signal low priority task to suspend vc */ ++ vcos_event_signal(&arm_state->lp_evt); ++ } ++ ++ vcos_mutex_unlock(&arm_state->use_count_mutex); ++} ++ ++ ++ ++static VCHIQ_STATUS_T ++vchiq_use_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service, int block_while_resume) ++{ ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ VCHIQ_STATUS_T ret = VCHIQ_SUCCESS; ++ char entity[10]; ++ int* entity_uc; ++ ++ if(arm_state) ++ { ++ vcos_mutex_lock(&arm_state->use_count_mutex); ++ ++ if (service) ++ { ++ sprintf(entity, "%c%c%c%c:%03d",VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), service->client_id); ++ entity_uc = &service->service_use_count; ++ } ++ else ++ { ++ sprintf(entity, "PEER: "); ++ entity_uc = &arm_state->peer_use_count; ++ } ++ ++ if (!arm_state->videocore_suspended && !vchiq_videocore_wanted(state)) ++ { ++#if VCOS_HAVE_TIMER ++ if (vchiq_platform_use_suspend_timer()) ++ { ++ vcos_log_trace( "%s %s - cancel suspend timer", __func__, entity); ++ } ++ vcos_timer_cancel(&g_suspend_timer); ++#endif ++ } ++ ++ arm_state->videocore_use_count++; ++ (*entity_uc)++; ++ arm_state->suspend_pending = 0; ++ ++ if (arm_state->videocore_suspended && vchiq_videocore_wanted(state)) ++ { ++ vcos_log_info( "%s %s count %d, state count %d", __func__, entity, *entity_uc, arm_state->videocore_use_count); ++ if(block_while_resume) ++ { ++ ret = vchiq_arm_vcresume(state); ++ } ++ else ++ { ++ vcos_log_info( "%s trigger HP task to do resume", __func__); /* triggering is done below */ ++ } ++ } ++ else ++ { ++ vcos_log_trace( "%s %s count %d, state count %d", __func__, entity, *entity_uc, arm_state->videocore_use_count); ++ } ++ if(!block_while_resume) ++ { ++ arm_state->use_notify_pending++; ++ vcos_event_signal(&arm_state->hp_evt); /* hp task will check if we need to resume and also send use notify */ ++ } ++ ++ if (ret == VCHIQ_RETRY) ++ { /* if we're told to retry, decrement the counters. VCHIQ_ERROR probably means we're already resumed. */ ++ (*entity_uc)--; ++ arm_state->videocore_use_count--; ++ } ++ ++ vcos_mutex_unlock(&arm_state->use_count_mutex); ++ } ++ return ret; ++} ++ ++static VCHIQ_STATUS_T +vchiq_release_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service) +{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ VCHIQ_STATUS_T ret = VCHIQ_SUCCESS; -+ char entity[16]; -+ int *entity_uc; -+ int local_uc, local_entity_uc; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ VCHIQ_STATUS_T ret = VCHIQ_SUCCESS; ++ char entity[10]; ++ int* entity_uc; + -+ if (!arm_state) -+ goto out; ++ if(arm_state) ++ { ++ vcos_mutex_lock(&arm_state->use_count_mutex); + -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); ++ if (service) ++ { ++ sprintf(entity, "%c%c%c%c:%03d",VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), service->client_id); ++ entity_uc = &service->service_use_count; ++ } ++ else ++ { ++ sprintf(entity, "PEER: "); ++ entity_uc = &arm_state->peer_use_count; ++ } + -+ if (service) { -+ sprintf(entity, "%c%c%c%c:%03d", -+ VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), -+ service->client_id); -+ entity_uc = &service->service_use_count; -+ } else { -+ sprintf(entity, "PEER: "); -+ entity_uc = &arm_state->peer_use_count; -+ } ++ if (*entity_uc && arm_state->videocore_use_count) ++ { ++ arm_state->videocore_use_count--; ++ (*entity_uc)--; + -+ write_lock_bh(&arm_state->susp_res_lock); -+ if (!arm_state->videocore_use_count || !(*entity_uc)) { -+ /* Don't use BUG_ON - don't allow user thread to crash kernel */ -+ WARN_ON(!arm_state->videocore_use_count); -+ WARN_ON(!(*entity_uc)); -+ ret = VCHIQ_ERROR; -+ goto unlock; -+ } -+ write_unlock_bh(&arm_state->susp_res_lock); ++ if (!vchiq_videocore_wanted(state)) ++ { ++#if VCOS_HAVE_TIMER ++ if (vchiq_platform_use_suspend_timer()) ++ { ++ vcos_log_trace( "%s %s count %d, state count %d - starting suspend timer", __func__, entity, *entity_uc, arm_state->videocore_use_count); ++ vcos_timer_cancel(&g_suspend_timer); ++ vcos_timer_set(&g_suspend_timer, SUSPEND_TIMER_TIMEOUT_MS); ++ } ++ else ++#endif ++ { ++ vcos_log_info( "%s %s count %d, state count %d - suspend pending", __func__, entity, *entity_uc, arm_state->videocore_use_count); ++ vcos_event_signal(&arm_state->lp_evt); /* kick the lp thread to do the suspend */ ++ } ++ } ++ else ++ { ++ vcos_log_trace( "%s %s count %d, state count %d", __func__, entity, *entity_uc, arm_state->videocore_use_count); ++ } ++ } ++ else ++ { ++ vcos_log_error( "%s %s ERROR releasing service; count %d, state count %d", __func__, entity, *entity_uc, arm_state->videocore_use_count); ++ ret = VCHIQ_ERROR; ++ } + -+ mutex_lock(&arm_state->vc_use_cnt_mutex); -+ local_uc = --arm_state->videocore_use_count; -+ local_entity_uc = --(*entity_uc); -+ -+ if (!arm_state->videocore_use_count) -+ pm_qos_remove_request(&arm_state->qos_request); -+ mutex_unlock(&arm_state->vc_use_cnt_mutex); -+ -+ write_lock_bh(&arm_state->susp_res_lock); -+ if (!vchiq_videocore_wanted(state)) { -+ if (vchiq_platform_use_suspend_timer() && -+ !arm_state->resume_blocked) { -+ /* Only use the timer if we're not trying to force -+ * suspend (=> resume_blocked) */ -+ start_suspend_timer(arm_state); -+ } else { -+ vchiq_log_info(vchiq_susp_log_level, -+ "%s %s count %d, state count %d - suspending", -+ __func__, entity, *entity_uc, -+ arm_state->videocore_use_count); -+ vchiq_arm_vcsuspend(state); -+ } -+ } else -+ vchiq_log_trace(vchiq_susp_log_level, -+ "%s %s count %d, state count %d", -+ __func__, entity, *entity_uc, -+ arm_state->videocore_use_count); -+ -+unlock: -+ write_unlock_bh(&arm_state->susp_res_lock); -+ -+out: -+ vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret); -+ return ret; ++ vcos_mutex_unlock(&arm_state->use_count_mutex); ++ } ++ return ret; +} + -+void ++VCHIQ_STATUS_T +vchiq_on_remote_use(VCHIQ_STATE_T *state) +{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ atomic_inc(&arm_state->ka_use_count); -+ complete(&arm_state->ka_evt); ++ vcos_log_info("%s state %p", __func__, state); ++ return state ? vchiq_use_internal(state, NULL, 0) : VCHIQ_ERROR; +} + -+void ++VCHIQ_STATUS_T +vchiq_on_remote_release(VCHIQ_STATE_T *state) +{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ atomic_inc(&arm_state->ka_release_count); -+ complete(&arm_state->ka_evt); ++ vcos_log_info("%s state %p", __func__, state); ++ return state ? vchiq_release_internal(state, NULL) : VCHIQ_ERROR; +} + +VCHIQ_STATUS_T +vchiq_use_service_internal(VCHIQ_SERVICE_T *service) +{ -+ return vchiq_use_internal(service->state, service, USE_TYPE_SERVICE); ++ VCHIQ_STATE_T* state = NULL; ++ ++ if (service) ++ { ++ state = service->state; ++ } ++ ++ if (!service || !state) ++ { ++ return VCHIQ_ERROR; ++ } ++ return vchiq_use_internal(state, service, 1); +} + +VCHIQ_STATUS_T +vchiq_release_service_internal(VCHIQ_SERVICE_T *service) +{ -+ return vchiq_release_internal(service->state, service); ++ VCHIQ_STATE_T* state = NULL; ++ ++ if (service) ++ { ++ state = service->state; ++ } ++ ++ if (!service || !state) ++ { ++ return VCHIQ_ERROR; ++ } ++ return vchiq_release_internal(state, service); +} + -+static void suspend_timer_callback(unsigned long context) ++ ++#if VCOS_HAVE_TIMER ++static void suspend_timer_callback(void* context) +{ -+ VCHIQ_STATE_T *state = (VCHIQ_STATE_T *)context; -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ if (!arm_state) -+ goto out; -+ vchiq_log_info(vchiq_susp_log_level, -+ "%s - suspend timer expired - check suspend", __func__); -+ vchiq_check_suspend(state); -+out: -+ return; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state((VCHIQ_STATE_T*)context); ++ vcos_log_info( "%s - suspend pending", __func__); ++ vcos_event_signal(&arm_state->lp_evt); +} ++#endif + +VCHIQ_STATUS_T +vchiq_use_service(VCHIQ_SERVICE_HANDLE_T handle) +{ -+ VCHIQ_STATUS_T ret = VCHIQ_ERROR; -+ VCHIQ_SERVICE_T *service = find_service_by_handle(handle); -+ if (service) { -+ ret = vchiq_use_internal(service->state, service, -+ USE_TYPE_SERVICE); -+ unlock_service(service); -+ } -+ return ret; ++ VCHIQ_STATUS_T ret = VCHIQ_ERROR; ++ VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *) handle; ++ if (service) ++ { ++ ret = vchiq_use_service_internal(service); ++ } ++ return ret; +} + +VCHIQ_STATUS_T +vchiq_release_service(VCHIQ_SERVICE_HANDLE_T handle) +{ -+ VCHIQ_STATUS_T ret = VCHIQ_ERROR; -+ VCHIQ_SERVICE_T *service = find_service_by_handle(handle); -+ if (service) { -+ ret = vchiq_release_internal(service->state, service); -+ unlock_service(service); -+ } -+ return ret; ++ VCHIQ_STATUS_T ret = VCHIQ_ERROR; ++ VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *) handle; ++ if (service) ++ { ++ ret = vchiq_release_service_internal(service); ++ } ++ return ret; +} + +void +vchiq_dump_service_use_state(VCHIQ_STATE_T *state) +{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ int i, j = 0; -+ /* Only dump 64 services */ -+ static const int local_max_services = 64; -+ /* If there's more than 64 services, only dump ones with -+ * non-zero counts */ -+ int only_nonzero = 0; -+ static const char *nz = "<-- preventing suspend"; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); ++ int i; ++ if(arm_state) ++ { ++ vcos_mutex_lock(&arm_state->suspend_resume_mutex); ++ if (arm_state->videocore_suspended) ++ { ++ vcos_log_warn("--VIDEOCORE SUSPENDED--"); ++ } ++ else ++ { ++ vcos_log_warn("--VIDEOCORE AWAKE--"); ++ } ++ for (i = 0; i < state->unused_service; i++) { ++ VCHIQ_SERVICE_T *service_ptr = state->services[i]; ++ if (service_ptr && (service_ptr->srvstate != VCHIQ_SRVSTATE_FREE)) ++ { ++ if (service_ptr->service_use_count) ++ vcos_log_error("----- %c%c%c%c:%d service count %d <-- preventing suspend", VCHIQ_FOURCC_AS_4CHARS(service_ptr->base.fourcc), service_ptr->client_id, service_ptr->service_use_count); ++ else ++ vcos_log_warn("----- %c%c%c%c:%d service count 0", VCHIQ_FOURCC_AS_4CHARS(service_ptr->base.fourcc), service_ptr->client_id); ++ } ++ } ++ vcos_log_warn("----- PEER use count count %d", arm_state->peer_use_count); ++ vcos_log_warn("--- Overall vchiq instance use count %d", arm_state->videocore_use_count); + -+ enum vc_suspend_status vc_suspend_state; -+ enum vc_resume_status vc_resume_state; -+ int peer_count; -+ int vc_use_count; -+ int active_services; -+ struct service_data_struct { -+ int fourcc; -+ int clientid; -+ int use_count; -+ } service_data[local_max_services]; ++ vchiq_dump_platform_use_state(state); + -+ if (!arm_state) -+ return; -+ -+ read_lock_bh(&arm_state->susp_res_lock); -+ vc_suspend_state = arm_state->vc_suspend_state; -+ vc_resume_state = arm_state->vc_resume_state; -+ peer_count = arm_state->peer_use_count; -+ vc_use_count = arm_state->videocore_use_count; -+ active_services = state->unused_service; -+ if (active_services > local_max_services) -+ only_nonzero = 1; -+ -+ for (i = 0; (i < active_services) && (j < local_max_services); i++) { -+ VCHIQ_SERVICE_T *service_ptr = state->services[i]; -+ if (!service_ptr) -+ continue; -+ -+ if (only_nonzero && !service_ptr->service_use_count) -+ continue; -+ -+ if (service_ptr->srvstate != VCHIQ_SRVSTATE_FREE) { -+ service_data[j].fourcc = service_ptr->base.fourcc; -+ service_data[j].clientid = service_ptr->client_id; -+ service_data[j++].use_count = service_ptr-> -+ service_use_count; -+ } -+ } -+ -+ read_unlock_bh(&arm_state->susp_res_lock); -+ -+ vchiq_log_warning(vchiq_susp_log_level, -+ "-- Videcore suspend state: %s --", -+ suspend_state_names[vc_suspend_state + VC_SUSPEND_NUM_OFFSET]); -+ vchiq_log_warning(vchiq_susp_log_level, -+ "-- Videcore resume state: %s --", -+ resume_state_names[vc_resume_state + VC_RESUME_NUM_OFFSET]); -+ -+ if (only_nonzero) -+ vchiq_log_warning(vchiq_susp_log_level, "Too many active " -+ "services (%d). Only dumping up to first %d services " -+ "with non-zero use-count", active_services, -+ local_max_services); -+ -+ for (i = 0; i < j; i++) { -+ vchiq_log_error(vchiq_arm_log_level, -+ "----- %c%c%c%c:%d service count %d %s", -+ VCHIQ_FOURCC_AS_4CHARS(service_data[i].fourcc), -+ service_data[i].clientid, -+ service_data[i].use_count, -+ service_data[i].use_count ? nz : ""); -+ } -+ vchiq_log_warning(vchiq_susp_log_level, -+ "----- PEER use count count %d", peer_count); -+ vchiq_log_warning(vchiq_susp_log_level, -+ "--- Overall vchiq instance use count %d", vc_use_count); -+ -+ vchiq_dump_platform_use_state(state); ++ vcos_mutex_unlock(&arm_state->suspend_resume_mutex); ++ } +} + +VCHIQ_STATUS_T -+vchiq_check_service(VCHIQ_SERVICE_T *service) ++vchiq_check_service(VCHIQ_SERVICE_T * service) +{ -+ VCHIQ_ARM_STATE_T *arm_state = -+ vchiq_platform_get_arm_state(service->state); -+ VCHIQ_STATUS_T ret = VCHIQ_ERROR; -+ -+ if (!arm_state || !service) -+ goto out; -+ -+ vchiq_log_trace(vchiq_susp_log_level, "%s", __func__); -+ -+ read_lock_bh(&arm_state->susp_res_lock); -+ if (service->service_use_count) -+ ret = VCHIQ_SUCCESS; -+ read_unlock_bh(&arm_state->susp_res_lock); -+ -+ if (ret == VCHIQ_ERROR) { -+ vchiq_log_error(vchiq_susp_log_level, -+ "%s ERROR - %c%c%c%c:%d service count %d, " -+ "state count %d, videocore suspend state %s", __func__, -+ VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), -+ service->client_id, service->service_use_count, -+ arm_state->videocore_use_count, -+ suspend_state_names[arm_state->vc_suspend_state + -+ VC_SUSPEND_NUM_OFFSET]); -+ vchiq_dump_service_use_state(service->state); -+ } -+out: -+ return ret; ++ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(service->state); ++ VCHIQ_STATUS_T ret = VCHIQ_ERROR; ++ /* on 2835 vchiq does not have an arm_state */ ++ if (!arm_state) ++ return VCHIQ_SUCCESS; ++ if (service && arm_state) ++ { ++ vcos_mutex_lock(&arm_state->use_count_mutex); ++ if (!service->service_use_count) ++ { ++ vcos_log_error( "%s ERROR - %c%c%c%c:%d service count %d, state count %d, videocore_suspended %d", __func__,VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc), service->client_id, service->service_use_count, arm_state->videocore_use_count, arm_state->videocore_suspended); ++ vchiq_dump_service_use_state(service->state); ++ vcos_assert(0); // vcos_assert should kill the calling thread, so a user thread shouldn't be able to kill the kernel. ++ } ++ else ++ { ++ ret = VCHIQ_SUCCESS; ++ } ++ vcos_mutex_unlock(&arm_state->use_count_mutex); ++ } ++ return ret; +} + +/* stub functions */ +void vchiq_on_remote_use_active(VCHIQ_STATE_T *state) +{ -+ (void)state; ++ vcos_unused(state); +} + -+void vchiq_platform_conn_state_changed(VCHIQ_STATE_T *state, -+ VCHIQ_CONNSTATE_T oldstate, VCHIQ_CONNSTATE_T newstate) ++void vchiq_platform_conn_state_changed(VCHIQ_STATE_T *state, VCHIQ_CONNSTATE_T oldstate, VCHIQ_CONNSTATE_T newstate) +{ -+ VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state); -+ vchiq_log_info(vchiq_susp_log_level, "%d: %s->%s", state->id, -+ get_conn_state_name(oldstate), get_conn_state_name(newstate)); -+ if (state->conn_state == VCHIQ_CONNSTATE_CONNECTED) { -+ write_lock_bh(&arm_state->susp_res_lock); -+ if (!arm_state->first_connect) { -+ arm_state->first_connect = 1; -+ write_unlock_bh(&arm_state->susp_res_lock); -+ wake_up_process(arm_state->ka_thread); -+ } else -+ write_unlock_bh(&arm_state->susp_res_lock); -+ } ++ vcos_unused(state); ++ vcos_unused(oldstate); ++ vcos_unused(oldstate); +} + + @@ -12896,132 +12106,67 @@ index 0000000..81daf3e +static int __init +vchiq_init(void) +{ -+ int err; -+ void *ptr_err; ++ int err; ++ void *ptr_err; + -+ /* create proc entries */ -+ err = vchiq_proc_init(); -+ if (err != 0) -+ goto failed_proc_init; ++ err = vchiq_platform_vcos_init(); ++ if (err != 0) ++ goto failed_platform_vcos_init; + -+ err = alloc_chrdev_region(&vchiq_devid, VCHIQ_MINOR, 1, DEVICE_NAME); -+ if (err != 0) { -+ vchiq_log_error(vchiq_arm_log_level, -+ "Unable to allocate device number"); -+ goto failed_alloc_chrdev; -+ } -+ cdev_init(&vchiq_cdev, &vchiq_fops); -+ vchiq_cdev.owner = THIS_MODULE; -+ err = cdev_add(&vchiq_cdev, vchiq_devid, 1); -+ if (err != 0) { -+ vchiq_log_error(vchiq_arm_log_level, -+ "Unable to register device"); -+ goto failed_cdev_add; -+ } ++ vcos_log_set_level(VCOS_LOG_CATEGORY, vchiq_default_arm_log_level); ++ vcos_log_register("vchiq_arm", VCOS_LOG_CATEGORY); + -+ /* create sysfs entries */ -+ vchiq_class = class_create(THIS_MODULE, DEVICE_NAME); -+ ptr_err = vchiq_class; -+ if (IS_ERR(ptr_err)) -+ goto failed_class_create; ++ 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; ++ } + -+ vchiq_dev = device_create(vchiq_class, NULL, -+ vchiq_devid, NULL, "vchiq"); -+ ptr_err = vchiq_dev; -+ if (IS_ERR(ptr_err)) -+ goto failed_device_create; ++ /* create sysfs entries */ ++ vchiq_class = class_create(THIS_MODULE, DEVICE_NAME); ++ if (IS_ERR(ptr_err = vchiq_class)) ++ goto failed_class_create; + -+ err = vchiq_platform_init(&g_state); -+ if (err != 0) -+ goto failed_platform_init; ++ vchiq_dev = device_create(vchiq_class, NULL, ++ vchiq_devid, NULL, "vchiq"); ++ if (IS_ERR(ptr_err = vchiq_dev)) ++ goto failed_device_create; + -+ vchiq_log_info(vchiq_arm_log_level, -+ "vchiq: initialised - version %d (min %d), device %d.%d", -+ VCHIQ_VERSION, VCHIQ_VERSION_MIN, -+ MAJOR(vchiq_devid), MINOR(vchiq_devid)); ++ err = vchiq_platform_init(&g_state); ++ if (err != 0) ++ goto failed_platform_init; + -+ return 0; ++#if VCOS_HAVE_TIMER ++ vcos_timer_create( &g_suspend_timer, "suspend_timer", suspend_timer_callback, (void*)(&g_state)); ++#endif ++ ++ 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); ++ device_destroy(vchiq_class, vchiq_devid); +failed_device_create: -+ class_destroy(vchiq_class); ++ class_destroy(vchiq_class); +failed_class_create: -+ cdev_del(&vchiq_cdev); -+ err = PTR_ERR(ptr_err); ++ cdev_del(&vchiq_cdev); ++ err = PTR_ERR(ptr_err); +failed_cdev_add: -+ unregister_chrdev_region(vchiq_devid, 1); ++ unregister_chrdev_region(vchiq_devid, 1); +failed_alloc_chrdev: -+ vchiq_proc_deinit(); -+failed_proc_init: -+ vchiq_log_warning(vchiq_arm_log_level, "could not load vchiq"); -+ return err; ++failed_platform_vcos_init: ++ printk(KERN_WARNING "could not load vchiq\n"); ++ return err; +} -+ -+static int vchiq_instance_get_use_count(VCHIQ_INSTANCE_T instance) -+{ -+ VCHIQ_SERVICE_T *service; -+ int use_count = 0, i; -+ i = 0; -+ while ((service = next_service_by_instance(instance->state, -+ instance, &i)) != NULL) { -+ use_count += service->service_use_count; -+ unlock_service(service); -+ } -+ return use_count; -+} -+ -+/* read the per-process use-count */ -+static int proc_read_use_count(char *page, char **start, -+ off_t off, int count, -+ int *eof, void *data) -+{ -+ VCHIQ_INSTANCE_T instance = data; -+ int len, use_count; -+ -+ use_count = vchiq_instance_get_use_count(instance); -+ len = snprintf(page+off, count, "%d\n", use_count); -+ -+ return len; -+} -+ -+/* add an instance (process) to the proc entries */ -+static int vchiq_proc_add_instance(VCHIQ_INSTANCE_T instance) -+{ -+ char pidstr[32]; -+ struct proc_dir_entry *top, *use_count; -+ struct proc_dir_entry *clients = vchiq_clients_top(); -+ int pid = instance->pid; -+ -+ snprintf(pidstr, sizeof(pidstr), "%d", pid); -+ top = proc_mkdir(pidstr, clients); -+ if (!top) -+ goto fail_top; -+ -+ use_count = create_proc_read_entry("use_count", -+ 0444, top, -+ proc_read_use_count, -+ instance); -+ if (!use_count) -+ goto fail_use_count; -+ -+ instance->proc_entry = top; -+ -+ return 0; -+ -+fail_use_count: -+ remove_proc_entry(top->name, clients); -+fail_top: -+ return -ENOMEM; -+} -+ -+static void vchiq_proc_remove_instance(VCHIQ_INSTANCE_T instance) -+{ -+ struct proc_dir_entry *clients = vchiq_clients_top(); -+ remove_proc_entry("use_count", instance->proc_entry); -+ remove_proc_entry(instance->proc_entry->name, clients); -+} -+ +/**************************************************************************** +* +* vchiq_exit - called when the module is unloaded. @@ -13031,11 +12176,12 @@ index 0000000..81daf3e +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); ++ 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); @@ -13044,10 +12190,10 @@ index 0000000..81daf3e +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..6db1074 +index 0000000..24b42ff --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_arm.h -@@ -0,0 +1,201 @@ +@@ -0,0 +1,118 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -13069,101 +12215,46 @@ index 0000000..6db1074 +#ifndef VCHIQ_ARM_H +#define VCHIQ_ARM_H + -+#include -+#include -+//#include -+#define pm_qos_add_request(a,b,c) do {} while (0) -+#define pm_qos_remove_request(a) do {} while (0) -+struct pm_qos_request_list {int dummy;}; -+#include +#include "vchiq_core.h" + + -+enum vc_suspend_status { -+ VC_SUSPEND_FORCE_CANCELED = -3, /* Force suspend canceled, too busy */ -+ VC_SUSPEND_REJECTED = -2, /* Videocore rejected suspend request */ -+ VC_SUSPEND_FAILED = -1, /* Videocore suspend failed */ -+ VC_SUSPEND_IDLE = 0, /* VC active, no suspend actions */ -+ VC_SUSPEND_REQUESTED, /* User has requested suspend */ -+ VC_SUSPEND_IN_PROGRESS, /* Slot handler has recvd suspend request */ -+ VC_SUSPEND_SUSPENDED /* Videocore suspend succeeded */ -+}; -+ -+enum vc_resume_status { -+ VC_RESUME_FAILED = -1, /* Videocore resume failed */ -+ VC_RESUME_IDLE = 0, /* VC suspended, no resume actions */ -+ VC_RESUME_REQUESTED, /* User has requested resume */ -+ VC_RESUME_IN_PROGRESS, /* Slot handler has received resume request */ -+ VC_RESUME_RESUMED /* Videocore resumed successfully (active) */ -+}; -+ -+ -+enum USE_TYPE_E { -+ USE_TYPE_SERVICE, -+ USE_TYPE_PEER, -+ USE_TYPE_VCHIQ -+}; -+ -+ +typedef struct vchiq_arm_state_struct { -+ /* Keepalive-related data */ -+ struct task_struct *ka_thread; -+ struct completion ka_evt; -+ atomic_t ka_use_count; -+ atomic_t ka_use_ack_count; -+ atomic_t ka_release_count; + -+ struct completion vc_suspend_complete; -+ struct completion vc_resume_complete; ++ VCOS_THREAD_T lp_thread; /* processes low priority messages (eg suspend) */ ++ VCOS_THREAD_T hp_thread; /* processes high priority messages (eg resume) */ + -+ rwlock_t susp_res_lock; -+ enum vc_suspend_status vc_suspend_state; -+ enum vc_resume_status vc_resume_state; ++ VCOS_EVENT_T lp_evt; ++ VCOS_EVENT_T hp_evt; + -+ struct mutex vc_use_cnt_mutex; -+ unsigned int wake_address; ++ VCOS_MUTEX_T use_count_mutex; ++ VCOS_MUTEX_T suspend_resume_mutex; + -+ struct timer_list suspend_timer; -+ int suspend_timer_timeout; -+ int suspend_timer_running; ++ int suspend_pending; + -+ /* 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; ++ /* 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; + -+ /* Use count to track requests from videocore peer. -+ ** This use count is not associated with a service, so needs to be -+ ** tracked separately with the state. -+ */ -+ int peer_use_count; ++ /* Use count to track requests from videocore peer. ++ * This use count is not associated with a service, so needs to be tracked separately ++ * with the state. ++ */ ++ int peer_use_count; + -+ /* Flag to indicate whether resume is blocked. This happens when the -+ ** ARM is suspending -+ */ -+ struct completion resume_blocker; -+ int resume_blocked; -+ struct completion blocked_blocker; -+ int blocked_count; -+ -+ /* Flag to indicate that the first vchiq connect has made it through. -+ ** This means that both sides should be fully ready, and we should -+ ** be able to suspend after this point. -+ */ -+ int first_connect; -+ -+ struct pm_qos_request_list qos_request; -+ -+ unsigned long long suspend_start_time; -+ unsigned long long sleep_start_time; -+ unsigned long long resume_start_time; -+ unsigned long long last_wake_time; ++ /* Flag to indicate whether videocore is currently suspended */ ++ int videocore_suspended; + ++ /* Flag to indicate whether a notification is pending back to videocore that it's ++ * "remote use request" has been actioned */ ++ int use_notify_pending; +} VCHIQ_ARM_STATE_T; + -+extern int vchiq_arm_log_level; -+extern int vchiq_susp_log_level; ++ ++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); @@ -13178,22 +12269,16 @@ index 0000000..6db1074 +vchiq_arm_vcsuspend(VCHIQ_STATE_T *state); + +extern VCHIQ_STATUS_T -+vchiq_arm_force_suspend(VCHIQ_STATE_T *state); -+ -+extern int -+vchiq_arm_allow_resume(VCHIQ_STATE_T *state); -+ -+extern VCHIQ_STATUS_T +vchiq_arm_vcresume(VCHIQ_STATE_T *state); + +extern VCHIQ_STATUS_T +vchiq_arm_init_state(VCHIQ_STATE_T *state, VCHIQ_ARM_STATE_T *arm_state); + -+extern int -+vchiq_check_resume(VCHIQ_STATE_T *state); ++extern void ++vchiq_check_resume(VCHIQ_STATE_T* state); + +extern void -+vchiq_check_suspend(VCHIQ_STATE_T *state); ++vchiq_check_suspend(VCHIQ_STATE_T* state); + +extern VCHIQ_STATUS_T +vchiq_use_service(VCHIQ_SERVICE_HANDLE_T handle); @@ -13202,13 +12287,16 @@ index 0000000..6db1074 +vchiq_release_service(VCHIQ_SERVICE_HANDLE_T handle); + +extern VCHIQ_STATUS_T -+vchiq_check_service(VCHIQ_SERVICE_T *service); ++vchiq_check_service(VCHIQ_SERVICE_T * service); + +extern VCHIQ_STATUS_T +vchiq_platform_suspend(VCHIQ_STATE_T *state); + ++extern VCHIQ_STATUS_T ++vchiq_platform_resume(VCHIQ_STATE_T *state); ++ +extern int -+vchiq_platform_videocore_wanted(VCHIQ_STATE_T *state); ++vchiq_platform_videocore_wanted(VCHIQ_STATE_T* state); + +extern int +vchiq_platform_use_suspend_timer(void); @@ -13222,67 +12310,14 @@ index 0000000..6db1074 +extern VCHIQ_ARM_STATE_T* +vchiq_platform_get_arm_state(VCHIQ_STATE_T *state); + -+extern int -+vchiq_videocore_wanted(VCHIQ_STATE_T *state); -+ -+extern VCHIQ_STATUS_T -+vchiq_use_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service, -+ enum USE_TYPE_E use_type); -+extern VCHIQ_STATUS_T -+vchiq_release_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service); -+ -+void -+set_suspend_state(VCHIQ_ARM_STATE_T *arm_state, -+ enum vc_suspend_status new_state); -+ -+void -+set_resume_state(VCHIQ_ARM_STATE_T *arm_state, -+ enum vc_resume_status new_state); -+ -+void -+start_suspend_timer(VCHIQ_ARM_STATE_T *arm_state); -+ -+extern int vchiq_proc_init(void); -+extern void vchiq_proc_deinit(void); -+extern struct proc_dir_entry *vchiq_proc_top(void); -+extern struct proc_dir_entry *vchiq_clients_top(void); -+ + +#endif /* VCHIQ_ARM_H */ -diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_build_info.h b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_build_info.h -new file mode 100644 -index 0000000..7c3ab94 ---- /dev/null -+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_build_info.h -@@ -0,0 +1,22 @@ -+/* -+ * 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 -+ */ -+ -+const char *vchiq_get_build_hostname(void); -+const char *vchiq_get_build_version(void); -+const char *vchiq_get_build_time(void); -+const char *vchiq_get_build_date(void); 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..bd7f793 +index 0000000..cd51d99 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_cfg.h -@@ -0,0 +1,45 @@ +@@ -0,0 +1,43 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -13304,14 +12339,12 @@ index 0000000..bd7f793 +#ifndef VCHIQ_CFG_H +#define VCHIQ_CFG_H + -+#define VCHIQ_MAGIC VCHIQ_MAKE_FOURCC('V', 'C', 'H', 'I') ++#define VCHIQ_MAGIC VCHIQ_MAKE_FOURCC('V','C','H','I') +/* The version of VCHIQ - change with any non-trivial change */ -+#define VCHIQ_VERSION 5 -+/* The minimum compatible version - update to match VCHIQ_VERSION with any -+** incompatible change */ -+#define VCHIQ_VERSION_MIN 3 ++#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_STATES 1 +#define VCHIQ_MAX_SERVICES 4096 +#define VCHIQ_MAX_SLOTS 128 +#define VCHIQ_MAX_SLOTS_PER_SIDE 64 @@ -13330,10 +12363,10 @@ index 0000000..bd7f793 +#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..2927d4b +index 0000000..62e3006 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.c -@@ -0,0 +1,100 @@ +@@ -0,0 +1,101 @@ +/***************************************************************************** +* Copyright 2001 - 2010 Broadcom Corporation. All rights reserved. +* @@ -13348,18 +12381,20 @@ index 0000000..2927d4b +* consent. +*****************************************************************************/ + ++#include "vcos.h" +#include "vchiq_connected.h" -+#include "vchiq_core.h" +#include -+#include + +#define MAX_CALLBACKS 10 + -+static int g_connected; ++static int g_connected = 0; +static int g_num_deferred_callbacks; -+static VCHIQ_CONNECTED_CALLBACK_T g_deferred_callback[MAX_CALLBACKS]; -+static int g_once_init; -+static struct mutex g_connected_mutex; ++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) + +/**************************************************************************** +* @@ -13367,12 +12402,9 @@ index 0000000..2927d4b +* +***************************************************************************/ + -+static void connected_init(void) ++static void connected_init( void ) +{ -+ if (!g_once_init) { -+ mutex_init(&g_connected_mutex); -+ g_once_init = 1; -+ } ++ vcos_mutex_create( &g_connected_mutex, "connected_mutex"); +} + +/**************************************************************************** @@ -13384,30 +12416,32 @@ index 0000000..2927d4b +* +***************************************************************************/ + -+void vchiq_add_connected_callback(VCHIQ_CONNECTED_CALLBACK_T callback) ++void vchiq_add_connected_callback( VCHIQ_CONNECTED_CALLBACK_T callback ) +{ -+ connected_init(); ++ vcos_once( &g_once_init, connected_init ); + -+ if (mutex_lock_interruptible(&g_connected_mutex) != 0) -+ return; ++ vcos_mutex_lock( &g_connected_mutex ); + -+ if (g_connected) -+ /* We're already connected. Call the callback immediately. */ ++ if ( g_connected ) ++ { ++ // We're already connected. Call the callback immediately. + -+ callback(); -+ else { -+ if (g_num_deferred_callbacks >= MAX_CALLBACKS) -+ vchiq_log_error(vchiq_core_log_level, -+ "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++; -+ } -+ } -+ mutex_unlock(&g_connected_mutex); ++ 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 ); +} + +/**************************************************************************** @@ -13417,26 +12451,26 @@ index 0000000..2927d4b +* +***************************************************************************/ + -+void vchiq_call_connected_callbacks(void) ++void vchiq_call_connected_callbacks( void ) +{ -+ int i; ++ int i; + -+ connected_init(); ++ vcos_once( &g_once_init, connected_init ); + -+ if (mutex_lock_interruptible(&g_connected_mutex) != 0) -+ return; -+ -+ for (i = 0; i < g_num_deferred_callbacks; i++) -+ g_deferred_callback[i](); -+ -+ g_num_deferred_callbacks = 0; -+ g_connected = 1; -+ mutex_unlock(&g_connected_mutex); ++ 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); ++ ++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..f62faf0 +index 0000000..88cfa58 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_connected.h @@ -0,0 +1,32 @@ @@ -13461,23 +12495,23 @@ index 0000000..f62faf0 + +/* ---- Constants and Types ---------------------------------------------- */ + -+typedef void (*VCHIQ_CONNECTED_CALLBACK_T)(void); ++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); ++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..b8675d8 +index 0000000..749e7d4 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.c -@@ -0,0 +1,3760 @@ +@@ -0,0 +1,2717 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -13500,3750 +12534,2707 @@ index 0000000..b8675d8 + +#define VCHIQ_SLOT_HANDLER_STACK 8192 + -+#define HANDLE_STATE_SHIFT 12 -+ +#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 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)) + + -+struct vchiq_open_payload { -+ int fourcc; -+ int client_id; -+ short version; -+ short version_min; -+}; -+ -+/* we require this for consistency between endpoints */ -+vchiq_static_assert(sizeof(VCHIQ_HEADER_T) == 8); -+vchiq_static_assert(IS_POW2(sizeof(VCHIQ_HEADER_T))); -+vchiq_static_assert(IS_POW2(VCHIQ_NUM_CURRENT_BULKS)); -+vchiq_static_assert(IS_POW2(VCHIQ_NUM_SERVICE_BULKS)); -+vchiq_static_assert(IS_POW2(VCHIQ_MAX_SERVICES)); -+vchiq_static_assert(VCHIQ_VERSION >= VCHIQ_VERSION_MIN); -+ -+/* Run time control of log level, based on KERN_XXX level. */ -+int vchiq_core_log_level = VCHIQ_LOG_DEFAULT; -+int vchiq_core_msg_log_level = VCHIQ_LOG_DEFAULT; -+int vchiq_sync_log_level = VCHIQ_LOG_DEFAULT; -+ -+static atomic_t pause_bulks_count = ATOMIC_INIT(0); -+ -+static DEFINE_SPINLOCK(service_spinlock); -+DEFINE_SPINLOCK(bulk_waiter_spinlock); -+DEFINE_SPINLOCK(quota_spinlock); -+ -+VCHIQ_STATE_T *vchiq_states[VCHIQ_MAX_STATES]; -+static unsigned int handle_seq; -+ -+static const char *const srvstate_names[] = { -+ "FREE", -+ "HIDDEN", -+ "LISTENING", -+ "OPENING", -+ "OPEN", -+ "OPENSYNC", -+ "CLOSESENT", -+ "CLOSERECVD", -+ "CLOSEWAIT", -+ "CLOSED" -+}; -+ -+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", -+ "CONNECTING", -+ "CONNECTED", -+ "PAUSING", -+ "PAUSE_SENT", -+ "PAUSED", -+ "RESUMING" -+}; ++/* Used to check use counts allow vchiq use. */ ++extern VCHIQ_STATUS_T vchiq_check_service(VCHIQ_SERVICE_T * service); + + -+static void -+release_message_sync(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header); -+ -+static const char *msg_type_str(unsigned int msg_type) ++typedef struct bulk_waiter_struct +{ -+ 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"; -+ case VCHIQ_MSG_REMOTE_USE: return "REMOTE_USE"; -+ case VCHIQ_MSG_REMOTE_RELEASE: return "REMOTE_RELEASE"; -+ case VCHIQ_MSG_REMOTE_USE_ACTIVE: return "REMOTE_USE_ACTIVE"; -+ } -+ return "???"; ++ 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) +{ -+ vchiq_log_info(vchiq_core_log_level, "%d: srv:%d %s->%s", -+ service->state->id, service->localport, -+ srvstate_names[service->srvstate], -+ srvstate_names[newstate]); -+ service->srvstate = newstate; ++ vcos_log_info("%d: srv:%d %s->%s", service->state->id, service->localport, ++ srvstate_names[service->srvstate], ++ srvstate_names[newstate]); ++ service->srvstate = newstate; +} + -+VCHIQ_SERVICE_T * -+find_service_by_handle(VCHIQ_SERVICE_HANDLE_T handle) ++static inline int ++is_valid_service(VCHIQ_SERVICE_T *service) +{ -+ VCHIQ_SERVICE_T *service; -+ -+ spin_lock(&service_spinlock); -+ service = handle_to_service(handle); -+ if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE) && -+ (service->handle == handle)) { -+ BUG_ON(service->ref_count == 0); -+ service->ref_count++; -+ } else -+ service = NULL; -+ spin_unlock(&service_spinlock); -+ -+ if (!service) -+ vchiq_log_info(vchiq_core_log_level, -+ "Invalid service handle 0x%x", handle); -+ -+ return service; -+} -+ -+VCHIQ_SERVICE_T * -+find_service_by_port(VCHIQ_STATE_T *state, int localport) -+{ -+ VCHIQ_SERVICE_T *service = NULL; -+ if ((unsigned int)localport <= VCHIQ_PORT_MAX) { -+ spin_lock(&service_spinlock); -+ service = state->services[localport]; -+ if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE)) { -+ BUG_ON(service->ref_count == 0); -+ service->ref_count++; -+ } else -+ service = NULL; -+ spin_unlock(&service_spinlock); -+ } -+ -+ if (!service) -+ vchiq_log_info(vchiq_core_log_level, -+ "Invalid port %d", localport); -+ -+ return service; -+} -+ -+VCHIQ_SERVICE_T * -+find_service_for_instance(VCHIQ_INSTANCE_T instance, -+ VCHIQ_SERVICE_HANDLE_T handle) { -+ VCHIQ_SERVICE_T *service; -+ -+ spin_lock(&service_spinlock); -+ service = handle_to_service(handle); -+ if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE) && -+ (service->handle == handle) && -+ (service->instance == instance)) { -+ BUG_ON(service->ref_count == 0); -+ service->ref_count++; -+ } else -+ service = NULL; -+ spin_unlock(&service_spinlock); -+ -+ if (!service) -+ vchiq_log_info(vchiq_core_log_level, -+ "Invalid service handle 0x%x", handle); -+ -+ return service; -+} -+ -+VCHIQ_SERVICE_T * -+next_service_by_instance(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance, -+ int *pidx) -+{ -+ VCHIQ_SERVICE_T *service = NULL; -+ int idx = *pidx; -+ -+ spin_lock(&service_spinlock); -+ while (idx < state->unused_service) { -+ VCHIQ_SERVICE_T *srv = state->services[idx++]; -+ if (srv && (srv->srvstate != VCHIQ_SRVSTATE_FREE) && -+ (srv->instance == instance)) { -+ service = srv; -+ BUG_ON(service->ref_count == 0); -+ service->ref_count++; -+ break; -+ } -+ } -+ spin_unlock(&service_spinlock); -+ -+ *pidx = idx; -+ -+ return service; -+} -+ -+void -+lock_service(VCHIQ_SERVICE_T *service) -+{ -+ spin_lock(&service_spinlock); -+ BUG_ON(!service || (service->ref_count == 0)); -+ if (service) -+ service->ref_count++; -+ spin_unlock(&service_spinlock); -+} -+ -+void -+unlock_service(VCHIQ_SERVICE_T *service) -+{ -+ VCHIQ_STATE_T *state = service->state; -+ spin_lock(&service_spinlock); -+ BUG_ON(!service || (service->ref_count == 0)); -+ if (service && service->ref_count) { -+ service->ref_count--; -+ if (!service->ref_count) { -+ BUG_ON(service->srvstate != VCHIQ_SRVSTATE_FREE); -+ state->services[service->localport] = NULL; -+ } -+ else -+ service = NULL; -+ } -+ spin_unlock(&service_spinlock); -+ -+ kfree(service); -+} -+ -+int -+vchiq_get_client_id(VCHIQ_SERVICE_HANDLE_T handle) -+{ -+ VCHIQ_SERVICE_T *service = find_service_by_handle(handle); -+ int id; -+ -+ id = service ? service->client_id : 0; -+ if (service) -+ unlock_service(service); -+ -+ return id; -+} -+ -+void * -+vchiq_get_service_userdata(VCHIQ_SERVICE_HANDLE_T handle) -+{ -+ VCHIQ_SERVICE_T *service = handle_to_service(handle); -+ -+ return service ? service->base.userdata : NULL; -+} -+ -+int -+vchiq_get_service_fourcc(VCHIQ_SERVICE_HANDLE_T handle) -+{ -+ VCHIQ_SERVICE_T *service = handle_to_service(handle); -+ -+ return service ? service->base.fourcc : 0; -+} -+ -+static void -+mark_service_closing(VCHIQ_SERVICE_T *service) -+{ -+ VCHIQ_STATE_T *state = service->state; -+ VCHIQ_SERVICE_QUOTA_T *service_quota; -+ -+ service->closing = 1; -+ -+ /* Synchronise with other threads. */ -+ mutex_lock(&state->recycle_mutex); -+ mutex_unlock(&state->recycle_mutex); -+ mutex_lock(&state->slot_mutex); -+ mutex_unlock(&state->slot_mutex); -+ -+ /* Unblock any sending thread. */ -+ service_quota = &state->service_quotas[service->localport]; -+ up(&service_quota->quota_event); ++ return ((service != NULL) && ++ (service->srvstate != VCHIQ_SRVSTATE_FREE)); +} + +static inline VCHIQ_STATUS_T +make_service_callback(VCHIQ_SERVICE_T *service, VCHIQ_REASON_T reason, -+ VCHIQ_HEADER_T *header, void *bulk_userdata) ++ VCHIQ_HEADER_T *header, void *bulk_userdata) +{ -+ vchiq_log_trace(vchiq_core_log_level, "%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->handle, -+ 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) +{ -+ VCHIQ_CONNSTATE_T oldstate = state->conn_state; -+ vchiq_log_info(vchiq_core_log_level, "%d: %s->%s", state->id, -+ conn_state_names[oldstate], -+ conn_state_names[newstate]); -+ state->conn_state = newstate; -+ vchiq_platform_conn_state_changed(state, oldstate, newstate); ++ VCHIQ_CONNSTATE_T oldstate = state->conn_state; ++ vcos_log_info("%d: %s->%s", state->id, ++ conn_state_names[oldstate], ++ conn_state_names[newstate]); ++ state->conn_state = newstate; ++ vchiq_platform_conn_state_changed(state, oldstate, 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. */ -+ sema_init(event->event, 0); ++ 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) +{ -+ (void)event; ++ vcos_event_delete(event->event); +} + +static inline int +remote_event_wait(REMOTE_EVENT_T *event) +{ -+ if (!event->fired) { -+ event->armed = 1; -+ dsb(); -+ if (!event->fired) { -+ if (down_interruptible(event->event) != 0) { -+ event->armed = 0; -+ return 0; -+ } -+ } -+ event->armed = 0; -+ wmb(); -+ } ++ 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; ++ event->fired = 0; ++ return 1; +} + +static inline void +remote_event_signal_local(REMOTE_EVENT_T *event) +{ -+ event->armed = 0; -+ up(event->event); ++ event->armed = 0; ++ vcos_event_signal(event->event); +} + +static inline void +remote_event_poll(REMOTE_EVENT_T *event) +{ -+ if (event->fired && event->armed) -+ remote_event_signal_local(event); ++ if (event->armed) ++ remote_event_signal_local(event); +} + +void +remote_event_pollall(VCHIQ_STATE_T *state) +{ -+ remote_event_poll(&state->local->sync_trigger); -+ remote_event_poll(&state->local->sync_release); -+ remote_event_poll(&state->local->trigger); -+ remote_event_poll(&state->local->recycle); ++ 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. */ ++ 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); ++ /* Allow room for the header */ ++ size += sizeof(VCHIQ_HEADER_T); + -+ /* Round up */ -+ return (size + sizeof(VCHIQ_HEADER_T) - 1) & ~(sizeof(VCHIQ_HEADER_T) -+ - 1); ++ /* Round up */ ++ return (size + sizeof(VCHIQ_HEADER_T) - 1) & ~(sizeof(VCHIQ_HEADER_T) - 1); +} + -+/* Called by the slot handler thread */ +static VCHIQ_SERVICE_T * +get_listening_service(VCHIQ_STATE_T *state, int fourcc) +{ -+ int i; ++ int i; + -+ WARN_ON(fourcc == VCHIQ_FOURCC_INVALID); ++ 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)))) { -+ lock_service(service); -+ return service; -+ } -+ } ++ 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; ++ return NULL; +} + -+/* Called by the slot handler thread */ +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)) { -+ lock_service(service); -+ return service; -+ } -+ } -+ return NULL; ++ 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; +} + -+inline void ++static inline void +request_poll(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service, int poll_type) +{ -+ uint32_t value; ++ 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))); ++ } + -+ if (service) { -+ do { -+ value = atomic_read(&service->poll_flags); -+ } while (atomic_cmpxchg(&service->poll_flags, value, -+ value | (1 << poll_type)) != value); ++ state->poll_needed = 1; ++ vcos_wmb(&state->poll_needed); + -+ do { -+ value = atomic_read(&state->poll_services[ -+ service->localport>>5]); -+ } while (atomic_cmpxchg( -+ &state->poll_services[service->localport>>5], -+ value, value | (1 << (service->localport & 0x1f))) -+ != value); -+ } -+ -+ state->poll_needed = 1; -+ wmb(); -+ -+ /* ... and ensure the slot handler runs. */ -+ remote_event_signal_local(&state->local->trigger); ++ /* ... 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 */ ++ 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); ++ 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 */ -+ WARN_ON(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); ++ 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; -+ } ++ tx_pos += slot_space; ++ } + -+ /* If necessary, get the next slot. */ -+ if ((tx_pos & VCHIQ_SLOT_MASK) == 0) { -+ int slot_index; ++ /* If necessary, get the next slot. */ ++ if ((tx_pos & VCHIQ_SLOT_MASK) == 0) ++ { ++ int slot_index; + -+ /* If there is no free slot... */ ++ /* 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); + -+ if (down_trylock(&state->slot_available_event) != 0) { -+ /* ...wait for one. */ ++ /* But first, flush through the last slot. */ ++ local->tx_pos = tx_pos; ++ remote_event_signal(&state->remote->trigger); + -+ VCHIQ_STATS_INC(state, slot_stalls); ++ 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)); ++ } + -+ /* But first, flush through the last slot. */ -+ state->local_tx_pos = tx_pos; -+ local->tx_pos = tx_pos; -+ remote_event_signal(&state->remote->trigger); ++ 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); ++ } + -+ if (!is_blocking || -+ (down_interruptible( -+ &state->slot_available_event) != 0)) -+ return NULL; /* No space available */ -+ } ++ state->local_tx_pos = tx_pos + space; + -+ BUG_ON(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)); ++ return (VCHIQ_HEADER_T *)(state->tx_data + (tx_pos & VCHIQ_SLOT_MASK)); +} + -+/* Called by the recycle thread. */ ++/* 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; ++ 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. */ -+ rmb(); ++ /* 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; ++ /* 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) { -+ unsigned 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); -+ int data_found = 0; ++ while (slot_queue_available != local->slot_queue_recycle) ++ { ++ unsigned 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); + -+ vchiq_log_trace(vchiq_core_log_level, "%d: pfq %d=%x %x %x", -+ state->id, slot_index, (unsigned int)data, -+ local->slot_queue_recycle, slot_queue_available); ++ 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); ++ /* Initialise the bitmask for services which have used this slot */ ++ BITSET_ZERO(service_found); + -+ pos = 0; ++ 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); -+ VCHIQ_SERVICE_QUOTA_T *service_quota = -+ &state->service_quotas[port]; -+ int count; -+ spin_lock("a_spinlock); -+ count = service_quota->message_use_count; -+ if (count > 0) -+ service_quota->message_use_count = -+ count - 1; -+ spin_unlock("a_spinlock); ++ 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); ++ VCHIQ_SERVICE_QUOTA_T *service_quota = ++ &state->service_quotas[port]; ++ int count; ++ count = service_quota->message_use_count; ++ if (count > 0) ++ { ++ service_quota->message_use_count = count - 1; ++ if (count == service_quota->message_quota) ++ { ++ /* Signal the service that it has dropped below its quota */ ++ vcos_event_signal(&service_quota->quota_event); ++ } ++ } ++ else ++ { ++ vcos_log_error("service %d message_use_count=%d (header %x," ++ " msgid %x, header->msgid %x, header->size %x)", ++ port, service_quota->message_use_count, ++ (unsigned int)header, msgid, header->msgid, ++ header->size); ++ vcos_assert(0); ++ } ++ if (!BITSET_IS_SET(service_found, port)) ++ { ++ /* Set the found bit for this service */ ++ BITSET_SET(service_found, port); + -+ if (count == service_quota->message_quota) -+ /* Signal the service that it -+ ** has dropped below its quota -+ */ -+ up(&service_quota->quota_event); -+ else if (count == 0) { -+ vchiq_log_error(vchiq_core_log_level, -+ "service %d " -+ "message_use_count=%d " -+ "(header %x, msgid %x, " -+ "header->msgid %x, " -+ "header->size %x)", -+ port, -+ service_quota-> -+ message_use_count, -+ (unsigned int)header, msgid, -+ header->msgid, -+ header->size); -+ WARN(1, "invalid message use count\n"); -+ } -+ if (!BITSET_IS_SET(service_found, port)) { -+ /* Set the found bit for this service */ -+ BITSET_SET(service_found, port); ++ count = service_quota->slot_use_count; ++ if (count > 0) ++ { ++ service_quota->slot_use_count = count - 1; ++ /* 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); ++ } ++ } ++ } + -+ spin_lock("a_spinlock); -+ count = service_quota->slot_use_count; -+ if (count > 0) -+ service_quota->slot_use_count = -+ count - 1; -+ spin_unlock("a_spinlock); ++ pos += calc_stride(header->size); ++ if (pos > VCHIQ_SLOT_SIZE) ++ { ++ vcos_log_error("pfq - 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 (count > 0) { -+ /* Signal the service in case -+ ** it has dropped below its -+ ** quota */ -+ up(&service_quota->quota_event); -+ vchiq_log_trace( -+ vchiq_core_log_level, -+ "%d: pfq:%d %x@%x - " -+ "slot_use->%d", -+ state->id, port, -+ header->size, -+ (unsigned int)header, -+ count - 1); -+ } else { -+ vchiq_log_error( -+ vchiq_core_log_level, -+ "service %d " -+ "slot_use_count" -+ "=%d (header %x" -+ ", msgid %x, " -+ "header->msgid" -+ " %x, header->" -+ "size %x)", -+ port, count, -+ (unsigned int)header, -+ msgid, -+ header->msgid, -+ header->size); -+ WARN(1, "bad slot use count\n"); -+ } -+ } -+ -+ data_found = 1; -+ } -+ -+ pos += calc_stride(header->size); -+ if (pos > VCHIQ_SLOT_SIZE) { -+ vchiq_log_error(vchiq_core_log_level, -+ "pfq - pos %x: header %x, msgid %x, " -+ "header->msgid %x, header->size %x", -+ pos, (unsigned int)header, msgid, -+ header->msgid, header->size); -+ WARN(1, "invalid slot position\n"); -+ } -+ } -+ -+ if (data_found) { -+ int count; -+ spin_lock("a_spinlock); -+ count = state->data_use_count; -+ if (count > 0) -+ state->data_use_count = -+ count - 1; -+ spin_unlock("a_spinlock); -+ if (count == state->data_quota) -+ up(&state->data_quota_event); -+ } -+ -+ state->slot_queue_available = slot_queue_available; -+ up(&state->slot_available_event); -+ } ++ 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) ++ 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; -+ int type = VCHIQ_MSG_TYPE(msgid); ++ VCHIQ_SHARED_STATE_T *local; ++ VCHIQ_SERVICE_QUOTA_T *service_quota = NULL; ++ VCHIQ_HEADER_T *header; + -+ unsigned int stride; ++ unsigned int stride; + -+ local = state->local; ++ local = state->local; + -+ stride = calc_stride(size); ++ stride = calc_stride(size); + -+ WARN_ON(!(stride <= VCHIQ_SLOT_SIZE)); ++ vcos_assert(stride <= VCHIQ_SLOT_SIZE); + -+ if ((type != VCHIQ_MSG_RESUME) && -+ (mutex_lock_interruptible(&state->slot_mutex) != 0)) -+ return VCHIQ_RETRY; ++ /* 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 (type == VCHIQ_MSG_DATA) { -+ int tx_end_index; ++ if (service) ++ { ++ int tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1); + -+ BUG_ON(!service); ++ 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; ++ } + -+ if (service->closing) { -+ /* The service has been closed */ -+ mutex_unlock(&state->slot_mutex); -+ return VCHIQ_ERROR; -+ } ++ service_quota = &state->service_quotas[service->localport]; + -+ service_quota = &state->service_quotas[service->localport]; ++ /* ...ensure it doesn't use more than its quota of messages or slots */ ++ while ((service_quota->message_use_count == service_quota->message_quota) || ++ ((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 (msg %d, slot %d)", ++ state->id, service->localport, ++ msg_type_str(VCHIQ_MSG_TYPE(msgid)), size, ++ service_quota->message_use_count, service_quota->slot_use_count); ++ 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; ++ tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos + stride - 1); ++ } ++ } + -+ spin_lock("a_spinlock); ++ header = reserve_space(state, stride, is_blocking); + -+ /* Ensure this service doesn't use more than its quota of -+ ** messages or slots */ -+ tx_end_index = SLOT_QUEUE_INDEX_FROM_POS( -+ state->local_tx_pos + stride - 1); ++ if (!header) { ++ if (service) ++ VCHIQ_SERVICE_STATS_INC(service, slot_stalls); ++ vcos_mutex_unlock(&state->slot_mutex); ++ return VCHIQ_RETRY; ++ } + -+ /* Ensure data messages don't use more than their quota of -+ ** slots */ -+ while ((tx_end_index != state->previous_data_index) && -+ (state->data_use_count == state->data_quota)) { -+ VCHIQ_STATS_INC(state, data_stalls); -+ spin_unlock("a_spinlock); -+ mutex_unlock(&state->slot_mutex); ++ if (service) { ++ int i, pos; ++ int tx_end_index; + -+ if (down_interruptible(&state->data_quota_event) -+ != 0) -+ return VCHIQ_RETRY; ++ 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)); + -+ mutex_lock(&state->slot_mutex); -+ spin_lock("a_spinlock); -+ tx_end_index = SLOT_QUEUE_INDEX_FROM_POS( -+ state->local_tx_pos + stride - 1); -+ if ((tx_end_index == state->previous_data_index) || -+ (state->data_use_count < state->data_quota)) { -+ /* Pass the signal on to other waiters */ -+ up(&state->data_quota_event); -+ break; -+ } -+ } ++ 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 )); ++ } ++ } + -+ while ((service_quota->message_use_count == -+ service_quota->message_quota) || -+ ((tx_end_index != service_quota->previous_tx_index) && -+ (service_quota->slot_use_count == -+ service_quota->slot_quota))) { -+ spin_unlock("a_spinlock); -+ vchiq_log_trace(vchiq_core_log_level, -+ "%d: qm:%d %s,%x - quota stall " -+ "(msg %d, slot %d)", -+ state->id, service->localport, -+ msg_type_str(type), size, -+ service_quota->message_use_count, -+ service_quota->slot_use_count); -+ VCHIQ_SERVICE_STATS_INC(service, quota_stalls); -+ mutex_unlock(&state->slot_mutex); -+ if (down_interruptible(&service_quota->quota_event) -+ != 0) -+ return VCHIQ_RETRY; -+ if (service->closing) -+ return VCHIQ_ERROR; -+ if (mutex_lock_interruptible(&state->slot_mutex) != 0) -+ return VCHIQ_RETRY; -+ if (service->srvstate != VCHIQ_SRVSTATE_OPEN) { -+ /* The service has been closed */ -+ mutex_unlock(&state->slot_mutex); -+ return VCHIQ_ERROR; -+ } -+ spin_lock("a_spinlock); -+ tx_end_index = SLOT_QUEUE_INDEX_FROM_POS( -+ state->local_tx_pos + stride - 1); -+ } ++ /* 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); ++ } + -+ spin_unlock("a_spinlock); -+ } ++ service_quota->previous_tx_index = tx_end_index; ++ service_quota->message_use_count++; ++ 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 = reserve_space(state, stride, is_blocking); ++ header->msgid = msgid; ++ header->size = size; + -+ if (!header) { -+ if (service) -+ VCHIQ_SERVICE_STATS_INC(service, slot_stalls); -+ mutex_unlock(&state->slot_mutex); -+ return VCHIQ_RETRY; -+ } ++ if (vcos_is_log_enabled( &vchiq_core_msg_log_category, VCOS_LOG_INFO)) ++ { ++ int svc_fourcc; + -+ if (type == VCHIQ_MSG_DATA) { -+ int i, pos; -+ int tx_end_index; -+ int slot_use_count; ++ svc_fourcc = service ++ ? service->base.fourcc ++ : VCHIQ_MAKE_FOURCC('?','?','?','?'); + -+ vchiq_log_info(vchiq_core_log_level, -+ "%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)); ++ 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 ); ++ } + -+ BUG_ON(!service); ++ /* Make the new tx_pos visible to the peer. */ ++ local->tx_pos = state->local_tx_pos; ++ vcos_wmb(&local->tx_pos); + -+ 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) { -+ mutex_unlock(&state->slot_mutex); -+ VCHIQ_SERVICE_STATS_INC(service, -+ error_count); -+ return VCHIQ_ERROR; -+ } -+ if (i == 0) { -+ if (vchiq_core_msg_log_level >= -+ VCHIQ_LOG_INFO) -+ vchiq_log_dump_mem("Sent", 0, -+ header->data + pos, -+ min(64, -+ elements[0].size)); -+ } -+ } ++ if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_PAUSE) ++ vcos_mutex_unlock(&state->slot_mutex); + -+ spin_lock("a_spinlock); -+ service_quota->message_use_count++; ++ remote_event_signal(&state->remote->trigger); + -+ tx_end_index = -+ SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos - 1); -+ -+ /* If this transmission can't fit in the last slot used by any -+ ** service, the data_use_count must be increased. */ -+ if (tx_end_index != state->previous_data_index) { -+ state->previous_data_index = tx_end_index; -+ state->data_use_count++; -+ } -+ -+ /* If this isn't the same slot last used by this service, -+ ** the service's slot_use_count must be increased. */ -+ if (tx_end_index != service_quota->previous_tx_index) { -+ service_quota->previous_tx_index = tx_end_index; -+ slot_use_count = ++service_quota->slot_use_count; -+ } else { -+ slot_use_count = 0; -+ } -+ -+ spin_unlock("a_spinlock); -+ -+ if (slot_use_count) -+ vchiq_log_trace(vchiq_core_log_level, -+ "%d: qm:%d %s,%x - slot_use->%d (hdr %p)", -+ state->id, service->localport, -+ msg_type_str(VCHIQ_MSG_TYPE(msgid)), size, -+ slot_use_count, header); -+ -+ VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count); -+ VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size); -+ } else { -+ vchiq_log_info(vchiq_core_log_level, -+ "%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) { -+ WARN_ON(!((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; -+ -+ { -+ int svc_fourcc; -+ -+ svc_fourcc = service -+ ? service->base.fourcc -+ : VCHIQ_MAKE_FOURCC('?', '?', '?', '?'); -+ -+ vchiq_log_info(vchiq_core_msg_log_level, -+ "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 sure the new header is visible to the peer. */ -+ wmb(); -+ -+ /* Make the new tx_pos visible to the peer. */ -+ local->tx_pos = state->local_tx_pos; -+ wmb(); -+ -+ if (service && (type == VCHIQ_MSG_CLOSE)) -+ vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT); -+ -+ if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_PAUSE) -+ mutex_unlock(&state->slot_mutex); -+ -+ remote_event_signal(&state->remote->trigger); -+ -+ return VCHIQ_SUCCESS; -+} -+ -+/* Called by the slot handler and application threads */ -+static VCHIQ_STATUS_T -+queue_message_sync(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_HEADER_T *header; -+ -+ local = state->local; -+ -+ if ((VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_RESUME) && -+ (mutex_lock_interruptible(&state->sync_mutex) != 0)) -+ return VCHIQ_RETRY; -+ -+ remote_event_wait(&local->sync_release); -+ -+ rmb(); -+ -+ header = (VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state, -+ local->slot_sync); -+ -+ { -+ int oldmsgid = header->msgid; -+ if (oldmsgid != VCHIQ_MSGID_PADDING) -+ vchiq_log_error(vchiq_core_log_level, -+ "%d: qms - msgid %x, not PADDING", -+ state->id, oldmsgid); -+ } -+ -+ if (service) { -+ int i, pos; -+ -+ vchiq_log_info(vchiq_sync_log_level, -+ "%d: qms %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) { -+ mutex_unlock(&state->sync_mutex); -+ VCHIQ_SERVICE_STATS_INC(service, -+ error_count); -+ return VCHIQ_ERROR; -+ } -+ if (i == 0) { -+ if (vchiq_sync_log_level >= -+ VCHIQ_LOG_TRACE) -+ vchiq_log_dump_mem("Sent Sync", -+ 0, header->data + pos, -+ min(64, -+ elements[0].size)); -+ } -+ } -+ -+ VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count); -+ VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size); -+ } else { -+ vchiq_log_info(vchiq_sync_log_level, -+ "%d: qms %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) { -+ WARN_ON(!((count == 1) && (size == elements[0].size))); -+ memcpy(header->data, elements[0].data, -+ elements[0].size); -+ } -+ VCHIQ_STATS_INC(state, ctrl_tx_count); -+ } -+ -+ header->size = size; -+ header->msgid = msgid; -+ -+ if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) { -+ int svc_fourcc; -+ -+ svc_fourcc = service -+ ? service->base.fourcc -+ : VCHIQ_MAKE_FOURCC('?', '?', '?', '?'); -+ -+ vchiq_log_trace(vchiq_sync_log_level, -+ "Sent Sync 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 sure the new header is visible to the peer. */ -+ wmb(); -+ -+ remote_event_signal(&state->remote->sync_trigger); -+ -+ if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_PAUSE) -+ mutex_unlock(&state->sync_mutex); -+ -+ return VCHIQ_SUCCESS; ++ return VCHIQ_SUCCESS; +} + +static inline void +claim_slot(VCHIQ_SLOT_INFO_T *slot) +{ -+ slot->use_count++; ++ slot->use_count++; +} + +static void -+release_slot(VCHIQ_STATE_T *state, VCHIQ_SLOT_INFO_T *slot_info, -+ VCHIQ_HEADER_T *header, VCHIQ_SERVICE_T *service) ++release_slot(VCHIQ_STATE_T *state, VCHIQ_SLOT_INFO_T *slot_info) +{ -+ int release_count; ++ int release_count; ++ vcos_mutex_lock(&state->recycle_mutex); + -+ mutex_lock(&state->recycle_mutex); ++ release_count = slot_info->release_count; ++ slot_info->release_count = ++release_count; + -+ if (header) { -+ int msgid = header->msgid; -+ if (((msgid & VCHIQ_MSGID_CLAIMED) == 0) || -+ (service && service->closing)) { -+ mutex_unlock(&state->recycle_mutex); -+ return; -+ } ++ if (release_count == slot_info->use_count) ++ { ++ int slot_queue_recycle; ++ /* Add to the freed queue */ + -+ /* Rewrite the message header to prevent a double -+ ** release */ -+ header->msgid = msgid & ~VCHIQ_MSGID_CLAIMED; -+ } ++ /* A read barrier is necessary here to prevent speculative fetches of ++ remote->slot_queue_recycle from overtaking the mutex. */ ++ vcos_rmb(); + -+ release_count = slot_info->release_count; -+ slot_info->release_count = ++release_count; ++ 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); + -+ if (release_count == slot_info->use_count) { -+ int slot_queue_recycle; -+ /* Add to the freed queue */ ++ /* A write barrier is necessary, but remote_event_signal contains one. */ ++ remote_event_signal(&state->remote->recycle); ++ } + -+ /* A read barrier is necessary here to prevent speculative -+ ** fetches of remote->slot_queue_recycle from overtaking the -+ ** mutex. */ -+ 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; -+ vchiq_log_info(vchiq_core_log_level, -+ "%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); -+ } -+ -+ mutex_unlock(&state->recycle_mutex); ++ 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, -+ int retry_poll) ++notify_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue) +{ -+ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%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); ++ 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 (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)]; ++ 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, and non-terminated services */ -+ if (bulk->data && service->instance) { -+ 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) { -+ struct bulk_waiter *waiter; -+ spin_lock(&bulk_waiter_spinlock); -+ waiter = bulk->userdata; -+ if (waiter) { -+ waiter->actual = bulk->actual; -+ up(&waiter->event); -+ } -+ spin_unlock(&bulk_waiter_spinlock); -+ } 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; -+ status = VCHIQ_SUCCESS; -+ } -+ } ++ /* 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++; -+ up(&service->bulk_remove_event); -+ } -+ if (!retry_poll) -+ status = VCHIQ_SUCCESS; -+ } ++ queue->remove++; ++ vcos_event_signal(&service->bulk_remove_event); ++ } ++ } + -+ if (status == VCHIQ_RETRY) -+ request_poll(service->state, service, -+ (queue == &service->bulk_tx) ? -+ VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY); ++ if (status != VCHIQ_SUCCESS) ++ request_poll(service->state, service, (queue == &service->bulk_tx) ? ++ VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY); + -+ return status; ++ return status; +} + +/* Called by the slot handler thread */ +static void +poll_services(VCHIQ_STATE_T *state) +{ -+ int group, i; ++ int group, i; + -+ for (group = 0; group < BITSET_SIZE(state->unused_service); group++) { -+ uint32_t flags; -+ flags = atomic_xchg(&state->poll_services[group], 0); -+ for (i = 0; flags; i++) { -+ if (flags & (1 << i)) { -+ VCHIQ_SERVICE_T *service = -+ find_service_by_port(state, -+ (group<<5) + i); -+ uint32_t service_flags; -+ flags &= ~(1 << i); -+ if (!service) -+ continue; -+ service_flags = -+ atomic_xchg(&service->poll_flags, 0); -+ if (service_flags & -+ (1 << VCHIQ_POLL_TERMINATE)) { -+ vchiq_log_info(vchiq_core_log_level, -+ "%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, -+ 1/*retry_poll*/); -+ if (service_flags & (1 << VCHIQ_POLL_RXNOTIFY)) -+ notify_bulks(service, -+ &service->bulk_rx, -+ 1/*retry_poll*/); -+ unlock_service(service); -+ } -+ } -+ } ++ 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; ++ 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)]; ++ while ((queue->process != queue->local_insert) && ++ (queue->process != queue->remote_insert)) ++ { ++ VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->process)]; + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%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_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); + -+ WARN_ON(!((int)(queue->local_insert - queue->process) > 0)); -+ WARN_ON(!((int)(queue->remote_insert - queue->process) > 0)); ++ vcos_assert((int)(queue->local_insert - queue->process) > 0); ++ vcos_assert((int)(queue->remote_insert - queue->process) > 0); ++ vchiq_transfer_bulk(bulk); + -+ mutex_lock(&state->bulk_transfer_mutex); -+ vchiq_transfer_bulk(bulk); -+ mutex_unlock(&state->bulk_transfer_mutex); ++ 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 ); ++ } + -+ if (vchiq_core_msg_log_level >= VCHIQ_LOG_INFO) { -+ const char *header = (queue == &service->bulk_tx) ? -+ "Send Bulk to" : "Recv Bulk from"; -+ if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED) -+ vchiq_log_info(vchiq_core_msg_log_level, -+ "%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 -+ vchiq_log_info(vchiq_core_msg_log_level, -+ "%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; ++ 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); -+ vchiq_log_trace(vchiq_core_log_level, -+ "%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); ++ 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); + -+ WARN_ON(!((int)(queue->local_insert - queue->process) >= 0)); -+ WARN_ON(!((int)(queue->remote_insert - queue->process) >= 0)); ++ 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)]; ++ 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->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 (queue->process != queue->local_insert) ++ { ++ vchiq_complete_bulk(bulk); + -+ vchiq_log_info(vchiq_core_msg_log_level, -+ "%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++; -+ } ++ 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++; -+ } ++ queue->process++; ++ } +} + -+/* Called from the slot handler thread */ +static void +pause_bulks(VCHIQ_STATE_T *state) +{ -+ if (unlikely(atomic_inc_return(&pause_bulks_count) != 1)) { -+ WARN_ON_ONCE(1); -+ atomic_set(&pause_bulks_count, 1); -+ return; -+ } ++ int i; + -+ /* Block bulk transfers from all services */ -+ mutex_lock(&state->bulk_transfer_mutex); ++ /* 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); ++ } +} + -+/* Called from the slot handler thread */ +static void +resume_bulks(VCHIQ_STATE_T *state) +{ -+ if (unlikely(atomic_dec_return(&pause_bulks_count) != 0)) { -+ WARN_ON_ONCE(1); -+ atomic_set(&pause_bulks_count, 0); -+ return; -+ } ++ int i; + -+ /* Allow bulk transfers from all services */ -+ mutex_unlock(&state->bulk_transfer_mutex); -+} ++ /* 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; + -+static int -+parse_open(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header) -+{ -+ VCHIQ_SERVICE_T *service = NULL; -+ int msgid, size; -+ int type; -+ unsigned int localport, remoteport; -+ -+ msgid = header->msgid; -+ size = header->size; -+ type = VCHIQ_MSG_TYPE(msgid); -+ localport = VCHIQ_MSG_DSTPORT(msgid); -+ remoteport = VCHIQ_MSG_SRCPORT(msgid); -+ if (size == sizeof(struct vchiq_open_payload)) { -+ const struct vchiq_open_payload *payload = -+ (struct vchiq_open_payload *)header->data; -+ unsigned int fourcc; -+ -+ fourcc = payload->fourcc; -+ vchiq_log_info(vchiq_core_log_level, -+ "%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 */ -+ vchiq_loud_error_header(); -+ vchiq_loud_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); -+ vchiq_loud_error_footer(); -+ goto fail_open; -+ } -+ if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) { -+ /* Acknowledge the OPEN */ -+ if (service->sync) { -+ if (queue_message_sync(state, NULL, -+ VCHIQ_MAKE_MSG( -+ VCHIQ_MSG_OPENACK, -+ service->localport, -+ remoteport), -+ NULL, 0, 0, 0) == VCHIQ_RETRY) -+ goto bail_not_ready; -+ } else { -+ if (queue_message(state, NULL, -+ VCHIQ_MAKE_MSG( -+ VCHIQ_MSG_OPENACK, -+ service->localport, -+ remoteport), -+ NULL, 0, 0, 0) == VCHIQ_RETRY) -+ goto bail_not_ready; -+ } -+ -+ /* The service is now open */ -+ vchiq_set_service_state(service, -+ service->sync ? VCHIQ_SRVSTATE_OPENSYNC -+ : 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; -+ goto bail_not_ready; -+ } -+ -+ /* Success - the message has been dealt with */ -+ unlock_service(service); -+ return 1; -+ } -+ } -+ -+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) -+ goto bail_not_ready; -+ -+ unlock_service(service); -+ -+ return 1; -+ -+bail_not_ready: -+ unlock_service(service); -+ -+ return 0; ++ 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; -+ VCHIQ_SERVICE_T *service = NULL; -+ int tx_pos; -+ DEBUG_INITIALISE(state->local) ++ VCHIQ_SHARED_STATE_T *remote = state->remote; ++ int tx_pos; ++ DEBUG_INITIALISE(state->local) + -+ tx_pos = remote->tx_pos; ++ tx_pos = remote->tx_pos; + -+ while (state->rx_pos != tx_pos) { -+ VCHIQ_HEADER_T *header; -+ int msgid, size; -+ int type; -+ unsigned int localport, remoteport; ++ 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; -+ WARN_ON(!((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); ++ 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; -+ } ++ /* 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); ++ 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); ++ 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: -+ service = find_service_by_port(state, localport); -+ if (!service) { -+ vchiq_log_error(vchiq_core_log_level, -+ "%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; -+ } ++ 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 (vchiq_core_msg_log_level >= VCHIQ_LOG_INFO) { -+ int svc_fourcc; ++ 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('?', '?', '?', '?'); -+ vchiq_log_info(vchiq_core_msg_log_level, -+ "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) -+ vchiq_log_dump_mem("Rcvd", 0, header->data, -+ min(64, size)); -+ } ++ 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) { -+ vchiq_log_error(vchiq_core_log_level, -+ "header %x (msgid %x) - size %x too big for " -+ "slot", -+ (unsigned int)header, (unsigned int)msgid, -+ (unsigned int)size); -+ WARN(1, "oversized for slot\n"); -+ } ++ 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: -+ WARN_ON(!(VCHIQ_MSG_DSTPORT(msgid) == 0)); -+ if (!parse_open(state, header)) -+ goto bail_not_ready; -+ break; -+ case VCHIQ_MSG_OPENACK: -+ vchiq_log_info(vchiq_core_log_level, -+ "%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); -+ up(&service->remove_event); -+ } else -+ vchiq_log_error(vchiq_core_log_level, -+ "OPENACK received in state %s", -+ srvstate_names[service->srvstate]); -+ break; -+ case VCHIQ_MSG_CLOSE: -+ WARN_ON(size != 0); /* There should be no data */ ++ 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; + -+ vchiq_log_info(vchiq_core_log_level, -+ "%d: prs CLOSE@%x (%d->%d)", -+ state->id, (unsigned int)header, -+ remoteport, localport); ++ 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)); + -+ 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; -+ } ++ service = get_listening_service(state, fourcc); + -+ mark_service_closing(service); ++ 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 */ + -+ if (vchiq_close_service_internal(service, -+ 1/*close_recvd*/) == VCHIQ_RETRY) -+ goto bail_not_ready; ++ /* The service is now open */ ++ vchiq_set_service_state(service, VCHIQ_SRVSTATE_OPEN); ++ } + -+ vchiq_log_info(vchiq_core_log_level, -+ "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: -+ vchiq_log_trace(vchiq_core_log_level, -+ "%d: prs DATA@%x,%x (%d->%d)", -+ state->id, (unsigned int)header, size, -+ remoteport, localport); ++ 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; ++ } + -+ 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); -+ goto bail_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: -+ vchiq_log_info(vchiq_core_log_level, -+ "%d: prs CONNECT@%x", -+ state->id, (unsigned int)header); -+ up(&state->connect); -+ break; -+ case VCHIQ_MSG_BULK_RX: -+ case VCHIQ_MSG_BULK_TX: { -+ VCHIQ_BULK_QUEUE_T *queue; -+ WARN_ON(!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; ++ /* 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 */ + -+ WARN_ON(!(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]; -+ wmb(); ++ vcos_log_info("%d: prs CLOSE@%x (%d->%d)", ++ state->id, (unsigned int)header, ++ remoteport, localport); + -+ vchiq_log_info(vchiq_core_log_level, -+ "%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); ++ 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; ++ } + -+ queue->remote_insert++; ++ if (vchiq_close_service_internal(service, ++ 1/*close_recvd*/) == VCHIQ_RETRY) ++ return; /* Bail out if not ready */ + -+ if (state->conn_state != -+ VCHIQ_CONNSTATE_CONNECTED) -+ break; ++ 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); + -+ DEBUG_TRACE(PARSE_LINE); -+ if (mutex_lock_interruptible( -+ &service->bulk_mutex) != 0) { -+ DEBUG_TRACE(PARSE_LINE); -+ goto bail_not_ready; -+ } -+ DEBUG_TRACE(PARSE_LINE); -+ resolved = resolve_bulks(service, queue); -+ mutex_unlock(&service->bulk_mutex); -+ if (resolved) -+ notify_bulks(service, queue, -+ 1/*retry_poll*/); -+ } -+ } break; -+ case VCHIQ_MSG_BULK_RX_DONE: -+ case VCHIQ_MSG_BULK_TX_DONE: -+ WARN_ON(state->is_master); -+ if ((service->remoteport == remoteport) -+ && (service->srvstate != -+ VCHIQ_SRVSTATE_FREE)) { -+ VCHIQ_BULK_QUEUE_T *queue; -+ VCHIQ_BULK_T *bulk; ++ 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; + -+ queue = (type == VCHIQ_MSG_BULK_RX_DONE) ? -+ &service->bulk_rx : &service->bulk_tx; ++ 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]; + -+ if ((int)(queue->remote_insert - -+ queue->local_insert) >= 0) { -+ vchiq_log_error(vchiq_core_log_level, -+ "%d: prs %s@%x (%d->%d) " -+ "unexpected", -+ state->id, msg_type_str(type), -+ (unsigned int)header, -+ remoteport, localport); -+ break; -+ } ++ 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); + -+ BUG_ON(queue->process == queue->local_insert); -+ BUG_ON(queue->process != queue->remote_insert); ++ queue->remote_insert++; + -+ bulk = &queue->bulks[ -+ BULK_INDEX(queue->remote_insert)]; -+ bulk->actual = *(int *)header->data; -+ queue->remote_insert++; ++ if (state->conn_state != VCHIQ_CONNSTATE_CONNECTED) ++ break; + -+ vchiq_log_info(vchiq_core_log_level, -+ "%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); ++ 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; + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%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); ++ queue = (type == VCHIQ_MSG_BULK_RX_DONE) ? ++ &service->bulk_rx : &service->bulk_tx; + -+ DEBUG_TRACE(PARSE_LINE); -+ if (mutex_lock_interruptible( -+ &service->bulk_mutex) != 0) { -+ DEBUG_TRACE(PARSE_LINE); -+ goto bail_not_ready; -+ } -+ DEBUG_TRACE(PARSE_LINE); -+ WARN_ON(queue->process == queue->local_insert); -+ vchiq_complete_bulk(bulk); -+ queue->process++; -+ mutex_unlock(&service->bulk_mutex); -+ DEBUG_TRACE(PARSE_LINE); -+ notify_bulks(service, queue, 1/*retry_poll*/); -+ DEBUG_TRACE(PARSE_LINE); -+ } -+ break; -+ case VCHIQ_MSG_PADDING: -+ vchiq_log_trace(vchiq_core_log_level, -+ "%d: prs PADDING@%x,%x", -+ state->id, (unsigned int)header, size); -+ break; -+ case VCHIQ_MSG_PAUSE: -+ /* If initiated, signal the application thread */ -+ vchiq_log_trace(vchiq_core_log_level, -+ "%d: prs PAUSE@%x,%x", -+ state->id, (unsigned int)header, size); -+ if (state->conn_state == VCHIQ_CONNSTATE_PAUSED) { -+ vchiq_log_error(vchiq_core_log_level, -+ "%d: PAUSE received in state PAUSED", -+ state->id); -+ break; -+ } -+ 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) -+ goto bail_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: -+ vchiq_log_trace(vchiq_core_log_level, -+ "%d: prs RESUME@%x,%x", -+ state->id, (unsigned int)header, size); -+ /* Release the slot mutex */ -+ 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; ++ bulk = &queue->bulks[BULK_INDEX(queue->process)]; ++ bulk->actual = *(int *)header->data; + -+ case VCHIQ_MSG_REMOTE_USE: -+ vchiq_on_remote_use(state); -+ break; -+ case VCHIQ_MSG_REMOTE_RELEASE: -+ vchiq_on_remote_release(state); -+ break; -+ case VCHIQ_MSG_REMOTE_USE_ACTIVE: -+ vchiq_on_remote_use_active(state); -+ break; ++ 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); + -+ default: -+ vchiq_log_error(vchiq_core_log_level, -+ "%d: prs invalid msgid %x@%x,%x", -+ state->id, msgid, (unsigned int)header, size); -+ WARN(1, "invalid message\n"); -+ break; -+ } ++ 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); + -+skip_message: -+ if (service) { -+ unlock_service(service); -+ service = NULL; -+ } ++ 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; + -+ state->rx_pos += calc_stride(size); ++ case VCHIQ_MSG_REMOTE_USE: ++ vchiq_on_remote_use(state); ++ break; ++ case VCHIQ_MSG_REMOTE_RELEASE: ++ vchiq_on_remote_release(state); ++ break; ++ case VCHIQ_MSG_REMOTE_USE_ACTIVE: ++ vchiq_on_remote_use_active(state); ++ break; + -+ 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, NULL, NULL); -+ state->rx_data = NULL; -+ } -+ } ++ default: ++ vcos_log_error("%d: prs invalid msgid %x@%x,%x", ++ state->id, msgid, (unsigned int)header, size); ++ vcos_assert(0); ++ break; ++ } + -+bail_not_ready: -+ if (service) -+ unlock_service(service); ++ 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 int ++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) ++ 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); ++ while (1) { ++ DEBUG_COUNT(SLOT_HANDLER_COUNT); ++ DEBUG_TRACE(SLOT_HANDLER_LINE); ++ remote_event_wait(&local->trigger); + -+ rmb(); ++ vcos_rmb(); + -+ DEBUG_TRACE(SLOT_HANDLER_LINE); -+ if (state->poll_needed) { -+ /* Check if we need to suspend - may change our -+ * conn_state */ -+ vchiq_platform_check_suspend(state); ++ DEBUG_TRACE(SLOT_HANDLER_LINE); ++ if (state->poll_needed) ++ { ++ state->poll_needed = 0; + -+ 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; + -+ /* 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_PAUSING: -+ if (state->is_master) -+ pause_bulks(state); -+ if (queue_message(state, NULL, -+ VCHIQ_MAKE_MSG(VCHIQ_MSG_PAUSE, 0, 0), -+ NULL, 0, 0, 0) != VCHIQ_RETRY) { -+ vchiq_set_conn_state(state, -+ VCHIQ_CONNSTATE_PAUSE_SENT); -+ } else { -+ if (state->is_master) -+ resume_bulks(state); -+ /* Retry later */ -+ state->poll_needed = 1; -+ } -+ 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; ++ } ++ } + -+ case VCHIQ_CONNSTATE_PAUSED: -+ vchiq_platform_resume(state); -+ 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. */ -+ vchiq_log_error(vchiq_core_log_level, -+ "Failed to send RESUME " -+ "message"); -+ BUG(); -+ } -+ break; -+ default: -+ break; -+ } -+ -+ -+ } -+ -+ DEBUG_TRACE(SLOT_HANDLER_LINE); -+ parse_rx_slots(state); -+ } -+ return 0; ++ DEBUG_TRACE(SLOT_HANDLER_LINE); ++ parse_rx_slots(state); ++ } ++ return NULL; +} + + +/* Called by the recycle thread */ -+static int ++static void * +recycle_func(void *v) +{ -+ VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v; -+ VCHIQ_SHARED_STATE_T *local = state->local; ++ VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v; ++ VCHIQ_SHARED_STATE_T *local = state->local; + -+ while (1) { -+ remote_event_wait(&local->recycle); ++ while (1) { ++ remote_event_wait(&local->recycle); + -+ process_free_queue(state); -+ } -+ return 0; -+} ++ vcos_mutex_lock(&state->slot_mutex); + ++ process_free_queue(state); + -+/* Called by the sync thread */ -+static int -+sync_func(void *v) -+{ -+ VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v; -+ VCHIQ_SHARED_STATE_T *local = state->local; -+ VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state, -+ state->remote->slot_sync); -+ -+ while (1) { -+ VCHIQ_SERVICE_T *service; -+ int msgid, size; -+ int type; -+ unsigned int localport, remoteport; -+ -+ remote_event_wait(&local->sync_trigger); -+ -+ rmb(); -+ -+ msgid = header->msgid; -+ size = header->size; -+ type = VCHIQ_MSG_TYPE(msgid); -+ localport = VCHIQ_MSG_DSTPORT(msgid); -+ remoteport = VCHIQ_MSG_SRCPORT(msgid); -+ -+ service = find_service_by_port(state, localport); -+ -+ if (!service) { -+ vchiq_log_error(vchiq_sync_log_level, -+ "%d: sf %s@%x (%d->%d) - " -+ "invalid/closed service %d", -+ state->id, msg_type_str(type), -+ (unsigned int)header, -+ remoteport, localport, localport); -+ release_message_sync(state, header); -+ continue; -+ } -+ -+ if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) { -+ int svc_fourcc; -+ -+ svc_fourcc = service -+ ? service->base.fourcc -+ : VCHIQ_MAKE_FOURCC('?', '?', '?', '?'); -+ vchiq_log_trace(vchiq_sync_log_level, -+ "Rcvd Msg %s from %c%c%c%c s:%d d:%d len:%d", -+ msg_type_str(type), -+ VCHIQ_FOURCC_AS_4CHARS(svc_fourcc), -+ remoteport, localport, size); -+ if (size > 0) -+ vchiq_log_dump_mem("Rcvd", 0, header->data, -+ min(64, size)); -+ } -+ -+ switch (type) { -+ case VCHIQ_MSG_OPENACK: -+ vchiq_log_info(vchiq_sync_log_level, -+ "%d: sf 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_OPENSYNC); -+ up(&service->remove_event); -+ } -+ release_message_sync(state, header); -+ break; -+ -+ case VCHIQ_MSG_DATA: -+ vchiq_log_trace(vchiq_sync_log_level, -+ "%d: sf DATA@%x,%x (%d->%d)", -+ state->id, (unsigned int)header, size, -+ remoteport, localport); -+ -+ if ((service->remoteport == remoteport) && -+ (service->srvstate == -+ VCHIQ_SRVSTATE_OPENSYNC)) { -+ if (make_service_callback(service, -+ VCHIQ_MESSAGE_AVAILABLE, header, -+ NULL) == VCHIQ_RETRY) -+ vchiq_log_error(vchiq_sync_log_level, -+ "synchronous callback to " -+ "service %d returns " -+ "VCHIQ_RETRY", -+ localport); -+ } -+ break; -+ -+ default: -+ vchiq_log_error(vchiq_sync_log_level, -+ "%d: sf unexpected msgid %x@%x,%x", -+ state->id, msgid, (unsigned int)header, size); -+ release_message_sync(state, header); -+ break; -+ } -+ -+ unlock_service(service); -+ } -+ -+ return 0; ++ vcos_mutex_unlock(&state->slot_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; ++ queue->local_insert = 0; ++ queue->remote_insert = 0; ++ queue->process = 0; ++ queue->remote_notify = 0; ++ queue->remove = 0; +} + -+ -+inline const char * -+get_conn_state_name(VCHIQ_CONNSTATE_T conn_state) -+{ -+ return conn_state_names[conn_state]; -+} -+ -+ +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; ++ 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; ++ /* Ensure there is enough memory to run an absolutely minimum system */ ++ num_slots -= first_data_slot; + -+ if (num_slots < 4) { -+ vchiq_log_error(vchiq_core_log_level, -+ "vchiq_init_slots - insufficient memory %x bytes", -+ mem_size); -+ return NULL; -+ } ++ 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)); ++ 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->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_sync = first_data_slot; -+ slot_zero->master.slot_first = first_data_slot + 1; -+ slot_zero->master.slot_last = first_data_slot + (num_slots/2) - 1; -+ slot_zero->slave.slot_sync = first_data_slot + (num_slots/2); -+ slot_zero->slave.slot_first = first_data_slot + (num_slots/2) + 1; -+ slot_zero->slave.slot_last = first_data_slot + num_slots - 1; ++ 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; ++ return slot_zero; +} + +VCHIQ_STATUS_T -+vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero, -+ int is_master) ++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; -+ VCHIQ_STATUS_T status; -+ char threadname[10]; -+ static int id; -+ int i; ++ VCHIQ_SHARED_STATE_T *local; ++ VCHIQ_SHARED_STATE_T *remote; ++ VCOS_THREAD_ATTR_T attrs; ++ VCHIQ_STATUS_T status; ++ char threadname[10]; ++ static int id = 0; ++ int i; + -+ vchiq_log_warning(vchiq_core_log_level, -+ "%s: slot_zero = 0x%08lx, is_master = %d", -+ __func__, (unsigned long)slot_zero, is_master); ++ 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); + -+ /* Check the input configuration */ ++ vcos_log_warn( "%s: slot_zero = 0x%08lx, is_master = %d", __func__, (unsigned long)slot_zero, is_master ); + -+ if (slot_zero->magic != VCHIQ_MAGIC) { -+ vchiq_loud_error_header(); -+ vchiq_loud_error("Invalid VCHIQ magic value found."); -+ vchiq_loud_error("slot_zero=%x: magic=%x (expected %x)", -+ (unsigned int)slot_zero, slot_zero->magic, VCHIQ_MAGIC); -+ vchiq_loud_error_footer(); -+ return VCHIQ_ERROR; -+ } ++ /* Check the input configuration */ + -+ if (slot_zero->version < VCHIQ_VERSION_MIN) { -+ vchiq_loud_error_header(); -+ vchiq_loud_error("Incompatible VCHIQ versions found."); -+ vchiq_loud_error("slot_zero=%x: VideoCore version=%d " -+ "(minimum %d)", -+ (unsigned int)slot_zero, slot_zero->version, -+ VCHIQ_VERSION_MIN); -+ vchiq_loud_error("Restart with a newer VideoCore image."); -+ vchiq_loud_error_footer(); -+ return VCHIQ_ERROR; -+ } ++ 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 (VCHIQ_VERSION < slot_zero->version_min) { -+ vchiq_loud_error_header(); -+ vchiq_loud_error("Incompatible VCHIQ versions found."); -+ vchiq_loud_error("slot_zero=%x: version=%d (VideoCore " -+ "minimum %d)", -+ (unsigned int)slot_zero, VCHIQ_VERSION, -+ slot_zero->version_min); -+ vchiq_loud_error("Restart with a newer kernel."); -+ vchiq_loud_error_footer(); -+ 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 ((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)) { -+ vchiq_loud_error_header(); -+ if (slot_zero->slot_zero_size != sizeof(VCHIQ_SLOT_ZERO_T)) -+ vchiq_loud_error("slot_zero=%x: slot_zero_size=%x " -+ "(expected %x)", -+ (unsigned int)slot_zero, -+ slot_zero->slot_zero_size, -+ sizeof(VCHIQ_SLOT_ZERO_T)); -+ if (slot_zero->slot_size != VCHIQ_SLOT_SIZE) -+ vchiq_loud_error("slot_zero=%x: slot_size=%d " -+ "(expected %d", -+ (unsigned int)slot_zero, slot_zero->slot_size, -+ VCHIQ_SLOT_SIZE); -+ if (slot_zero->max_slots != VCHIQ_MAX_SLOTS) -+ vchiq_loud_error("slot_zero=%x: max_slots=%d " -+ "(expected %d)", -+ (unsigned int)slot_zero, slot_zero->max_slots, -+ VCHIQ_MAX_SLOTS); -+ if (slot_zero->max_slots_per_side != VCHIQ_MAX_SLOTS_PER_SIDE) -+ vchiq_loud_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); -+ vchiq_loud_error_footer(); -+ 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 (is_master) { -+ local = &slot_zero->master; -+ remote = &slot_zero->slave; -+ } else { -+ local = &slot_zero->slave; -+ remote = &slot_zero->master; -+ } ++ 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 (local->initialised) { -+ vchiq_loud_error_header(); -+ if (remote->initialised) -+ vchiq_loud_error("local state has already been " -+ "initialised"); -+ else -+ vchiq_loud_error("master/slave mismatch - two %ss", -+ is_master ? "master" : "slave"); -+ vchiq_loud_error_footer(); -+ 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; ++ } + -+ memset(state, 0, sizeof(VCHIQ_STATE_T)); ++ 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; ++ } + -+ state->id = id++; -+ state->is_master = is_master; ++ 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; ++ } + -+ /* -+ initialize shared state pointers -+ */ ++ if (is_master) ++ { ++ local = &slot_zero->master; ++ remote = &slot_zero->slave; ++ } ++ else ++ { ++ local = &slot_zero->slave; ++ remote = &slot_zero->master; ++ } + -+ state->local = local; -+ state->remote = remote; -+ state->slot_data = (VCHIQ_SLOT_T *)slot_zero; ++ 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; ++ } + -+ /* -+ initialize events and mutexes -+ */ ++ memset(state, 0, sizeof(VCHIQ_STATE_T)); ++ vcos_log_warn( "%s: called", __func__); ++ state->id = id++; ++ state->is_master = is_master; + -+ sema_init(&state->connect, 0); -+ mutex_init(&state->mutex); -+ sema_init(&state->trigger_event, 0); -+ sema_init(&state->recycle_event, 0); -+ sema_init(&state->sync_trigger_event, 0); -+ sema_init(&state->sync_release_event, 0); ++ /* ++ initialize shared state pointers ++ */ + -+ mutex_init(&state->slot_mutex); -+ mutex_init(&state->recycle_mutex); -+ mutex_init(&state->sync_mutex); -+ mutex_init(&state->bulk_transfer_mutex); ++ state->local = local; ++ state->remote = remote; ++ state->slot_data = (VCHIQ_SLOT_T *)slot_zero; + -+ sema_init(&state->slot_available_event, 0); -+ sema_init(&state->slot_remove_event, 0); -+ sema_init(&state->data_quota_event, 0); ++ /* ++ initialize events and mutexes ++ */ + -+ state->slot_queue_available = 0; ++ 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"); + -+ for (i = 0; i < VCHIQ_MAX_SERVICES; i++) { -+ VCHIQ_SERVICE_QUOTA_T *service_quota = -+ &state->service_quotas[i]; -+ sema_init(&service_quota->quota_event, 0); -+ } ++ vcos_mutex_create(&state->slot_mutex, "v.slot_mutex"); ++ vcos_mutex_create(&state->recycle_mutex, "v.recycle_mutex"); + -+ for (i = local->slot_first; i <= local->slot_last; i++) { -+ local->slot_queue[state->slot_queue_available++] = i; -+ up(&state->slot_available_event); -+ } ++ vcos_event_create(&state->slot_available_event, "v.slot_available_event"); ++ vcos_event_create(&state->slot_remove_event, "v.slot_remove_event"); + -+ state->default_slot_quota = state->slot_queue_available/2; -+ state->default_message_quota = -+ min((unsigned short)(state->default_slot_quota * 256), -+ (unsigned short)~0); ++ state->slot_queue_available = 0; + -+ state->previous_data_index = -1; -+ state->data_use_count = 0; -+ state->data_quota = state->slot_queue_available - 1; ++ 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"); ++ } + -+ local->trigger.event = &state->trigger_event; -+ remote_event_create(&local->trigger); -+ local->tx_pos = 0; ++ for (i = local->slot_first; i <= local->slot_last; i++) ++ { ++ local->slot_queue[state->slot_queue_available++] = i; ++ } + -+ local->recycle.event = &state->recycle_event; -+ remote_event_create(&local->recycle); -+ local->slot_queue_recycle = state->slot_queue_available; ++ state->default_slot_quota = state->slot_queue_available/2; ++ state->default_message_quota = vcos_min(state->default_slot_quota * 256, (unsigned short)~0); + -+ local->sync_trigger.event = &state->sync_trigger_event; -+ remote_event_create(&local->sync_trigger); ++ local->trigger.event = &state->trigger_event; ++ remote_event_create(&local->trigger); ++ local->tx_pos = 0; + -+ local->sync_release.event = &state->sync_release_event; -+ remote_event_create(&local->sync_release); ++ local->recycle.event = &state->recycle_event; ++ remote_event_create(&local->recycle); ++ local->slot_queue_recycle = state->slot_queue_available; + -+ /* At start-of-day, the slot is empty and available */ -+ ((VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state, local->slot_sync))->msgid -+ = VCHIQ_MSGID_PADDING; -+ remote_event_signal_local(&local->sync_release); ++ local->debug[DEBUG_ENTRIES] = DEBUG_MAX; + -+ local->debug[DEBUG_ENTRIES] = DEBUG_MAX; ++ /* ++ bring up slot handler thread ++ */ + -+ status = vchiq_platform_init_state(state); ++ 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) ++ { ++ vcos_log_error("vchiq: FATAL: couldn't create thread %s", threadname); ++ return VCHIQ_ERROR; ++ } + -+ /* -+ bring up slot handler thread -+ */ -+ snprintf(threadname, sizeof(threadname), "VCHIQ-%d", state->id); -+ state->slot_handler_thread = kthread_create(&slot_handler_func, -+ (void *)state, -+ threadname); ++ 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) ++ { ++ vcos_log_error("vchiq: FATAL: couldn't create thread %s", threadname); ++ return VCHIQ_ERROR; ++ } + -+ if (state->slot_handler_thread == NULL) { -+ vchiq_loud_error_header(); -+ vchiq_loud_error("couldn't create thread %s", threadname); -+ vchiq_loud_error_footer(); -+ return VCHIQ_ERROR; -+ } -+ set_user_nice(state->slot_handler_thread, -19); -+ wake_up_process(state->slot_handler_thread); ++ status = vchiq_platform_init_state(state); + -+ snprintf(threadname, sizeof(threadname), "VCHIQr-%d", state->id); -+ state->recycle_thread = kthread_create(&recycle_func, -+ (void *)state, -+ threadname); -+ if (state->recycle_thread == NULL) { -+ vchiq_loud_error_header(); -+ vchiq_loud_error("couldn't create thread %s", threadname); -+ vchiq_loud_error_footer(); -+ return VCHIQ_ERROR; -+ } -+ set_user_nice(state->recycle_thread, -19); -+ wake_up_process(state->recycle_thread); ++ /* Indicate readiness to the other side */ ++ local->initialised = 1; + -+ snprintf(threadname, sizeof(threadname), "VCHIQs-%d", state->id); -+ state->sync_thread = kthread_create(&sync_func, -+ (void *)state, -+ threadname); -+ if (state->sync_thread == NULL) { -+ vchiq_loud_error_header(); -+ vchiq_loud_error("couldn't create thread %s", threadname); -+ vchiq_loud_error_footer(); -+ return VCHIQ_ERROR; -+ } -+ set_user_nice(state->sync_thread, -20); -+ wake_up_process(state->sync_thread); -+ -+ BUG_ON(state->id >= VCHIQ_MAX_STATES); -+ vchiq_states[state->id] = state; -+ -+ /* Indicate readiness to the other side */ -+ local->initialised = 1; -+ -+ return status; ++ return status; +} + +/* 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) ++ const VCHIQ_SERVICE_PARAMS_T *params, int srvstate, ++ VCHIQ_INSTANCE_T instance) +{ -+ VCHIQ_SERVICE_T *service; ++ VCHIQ_SERVICE_T **pservice = NULL; ++ VCHIQ_SERVICE_T *service = NULL; ++ int i; + -+ service = kmalloc(sizeof(VCHIQ_SERVICE_T), GFP_KERNEL); -+ if (service) { -+ service->base.fourcc = params->fourcc; -+ service->base.callback = params->callback; -+ service->base.userdata = params->userdata; -+ service->handle = VCHIQ_SERVICE_HANDLE_INVALID; -+ service->ref_count = 1; -+ service->srvstate = VCHIQ_SRVSTATE_FREE; -+ service->localport = VCHIQ_PORT_FREE; -+ service->remoteport = VCHIQ_PORT_FREE; ++ /* Prepare to use a previously unused service */ ++ if (state->unused_service < VCHIQ_MAX_SERVICES) ++ { ++ pservice = &state->services[state->unused_service]; ++ } + -+ service->public_fourcc = (srvstate == VCHIQ_SRVSTATE_OPENING) ? -+ VCHIQ_FOURCC_INVALID : params->fourcc; -+ service->client_id = 0; -+ service->auto_close = 1; -+ service->sync = 0; -+ service->closing = 0; -+ atomic_set(&service->poll_flags, 0); -+ service->version = params->version; -+ service->version_min = params->version_min; -+ service->state = state; -+ service->instance = instance; -+ service->service_use_count = 0; -+ init_bulk_queue(&service->bulk_tx); -+ init_bulk_queue(&service->bulk_rx); -+ sema_init(&service->remove_event, 0); -+ sema_init(&service->bulk_remove_event, 0); -+ mutex_init(&service->bulk_mutex); -+ memset(&service->stats, 0, sizeof(service->stats)); -+ } else { -+ vchiq_log_error(vchiq_core_log_level, -+ "Out of memory"); -+ } ++ 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 (service) { -+ VCHIQ_SERVICE_T **pservice = NULL; -+ int i; ++ 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"); ++ } ++ } + -+ /* Although it is perfectly possible to use service_spinlock -+ ** to protect the creation of services, it is overkill as it -+ ** disables interrupts while the array is searched. -+ ** The only danger is of another thread trying to create a -+ ** service - service deletion is safe. -+ ** Therefore it is preferable to use state->mutex which, -+ ** although slower to claim, doesn't block interrupts while -+ ** it is held. -+ */ ++ 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; ++ service_quota->message_quota = state->default_message_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); + -+ mutex_lock(&state->mutex); ++ /* 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; + -+ /* Prepare to use a previously unused service */ -+ if (state->unused_service < VCHIQ_MAX_SERVICES) -+ pservice = &state->services[state->unused_service]; ++ if (pservice == &state->services[state->unused_service]) ++ 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; -+ } -+ } -+ } 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->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; -+ break; -+ } -+ } -+ } -+ -+ if (pservice) { -+ service->localport = (pservice - state->services); -+ if (!handle_seq) -+ handle_seq = VCHIQ_MAX_STATES * -+ VCHIQ_MAX_SERVICES; -+ service->handle = handle_seq | -+ (state->id * VCHIQ_MAX_SERVICES) | -+ service->localport; -+ handle_seq += VCHIQ_MAX_STATES * VCHIQ_MAX_SERVICES; -+ *pservice = service; -+ if (pservice == &state->services[state->unused_service]) -+ state->unused_service++; -+ } -+ -+ mutex_unlock(&state->mutex); -+ -+ if (!pservice) { -+ kfree(service); -+ service = NULL; -+ } -+ } -+ -+ if (service) { -+ VCHIQ_SERVICE_QUOTA_T *service_quota = -+ &state->service_quotas[service->localport]; -+ service_quota->slot_quota = state->default_slot_quota; -+ service_quota->message_quota = state->default_message_quota; -+ if (service_quota->slot_use_count == 0) -+ service_quota->previous_tx_index = -+ SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos) -+ - 1; -+ -+ /* Bring this service online */ -+ vchiq_set_service_state(service, srvstate); -+ -+ vchiq_log_info(vchiq_core_msg_log_level, -+ "%s Service %c%c%c%c SrcPort:%d", -+ (srvstate == VCHIQ_SRVSTATE_OPENING) -+ ? "Open" : "Add", -+ VCHIQ_FOURCC_AS_4CHARS(params->fourcc), -+ service->localport); -+ } -+ -+ /* Don't unlock the service - leave it with a ref_count of 1. */ -+ -+ return service; ++ return service; +} + +VCHIQ_STATUS_T +vchiq_open_service_internal(VCHIQ_SERVICE_T *service, int client_id) +{ -+ struct vchiq_open_payload payload = { -+ service->base.fourcc, -+ client_id, -+ service->version, -+ service->version_min -+ }; -+ VCHIQ_ELEMENT_T body = { &payload, sizeof(payload) }; -+ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ 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_internal(service); -+ 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 (down_interruptible(&service->remove_event) != 0) { -+ status = VCHIQ_RETRY; -+ vchiq_release_service_internal(service); -+ } else if ((service->srvstate != VCHIQ_SRVSTATE_OPEN) && -+ (service->srvstate != VCHIQ_SRVSTATE_OPENSYNC)) { -+ if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) -+ vchiq_log_error(vchiq_core_log_level, -+ "%d: osi - srvstate = %s (ref %d)", -+ service->state->id, -+ srvstate_names[service->srvstate], -+ service->ref_count); -+ status = VCHIQ_ERROR; -+ VCHIQ_SERVICE_STATS_INC(service, error_count); -+ vchiq_release_service_internal(service); -+ } -+ } -+ return status; -+} -+ -+static void -+release_service_messages(VCHIQ_SERVICE_T *service) -+{ -+ VCHIQ_STATE_T *state = service->state; -+ int slot_last = state->remote->slot_last; -+ int i; -+ -+ /* 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); -+ unsigned 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) && -+ (msgid & VCHIQ_MSGID_CLAIMED)) { -+ vchiq_log_info(vchiq_core_log_level, -+ " fsi - hdr %x", -+ (unsigned int)header); -+ release_slot(state, slot_info, header, -+ NULL); -+ } -+ pos += calc_stride(header->size); -+ if (pos > VCHIQ_SLOT_SIZE) { -+ vchiq_log_error(vchiq_core_log_level, -+ "fsi - pos %x: header %x, " -+ "msgid %x, header->msgid %x, " -+ "header->size %x", -+ pos, (unsigned int)header, -+ msgid, header->msgid, -+ header->size); -+ WARN(1, "invalid slot position\n"); -+ } -+ } -+ } -+ } -+} -+ -+static int -+do_abort_bulks(VCHIQ_SERVICE_T *service) -+{ -+ VCHIQ_STATUS_T status; -+ -+ /* Abort any outstanding bulk transfers */ -+ if (mutex_lock_interruptible(&service->bulk_mutex) != 0) -+ return 0; -+ abort_outstanding_bulks(service, &service->bulk_tx); -+ abort_outstanding_bulks(service, &service->bulk_rx); -+ mutex_unlock(&service->bulk_mutex); -+ -+ status = notify_bulks(service, &service->bulk_tx, 0/*!retry_poll*/); -+ if (status == VCHIQ_SUCCESS) -+ status = notify_bulks(service, &service->bulk_rx, -+ 0/*!retry_poll*/); -+ return (status == VCHIQ_SUCCESS); -+} -+ -+static VCHIQ_STATUS_T -+close_service_complete(VCHIQ_SERVICE_T *service, int failstate) -+{ -+ VCHIQ_STATUS_T status; -+ int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID); -+ int newstate; -+ -+ switch (service->srvstate) { -+ case VCHIQ_SRVSTATE_OPEN: -+ case VCHIQ_SRVSTATE_CLOSESENT: -+ case VCHIQ_SRVSTATE_CLOSERECVD: -+ if (is_server) -+ newstate = (service->auto_close ? -+ VCHIQ_SRVSTATE_LISTENING : -+ VCHIQ_SRVSTATE_CLOSEWAIT); -+ else -+ newstate = VCHIQ_SRVSTATE_CLOSED; -+ vchiq_set_service_state(service, newstate); -+ break; -+ default: -+ break; -+ } -+ -+ status = make_service_callback(service, -+ VCHIQ_SERVICE_CLOSED, NULL, NULL); -+ -+ if (status != VCHIQ_RETRY) { -+ int uc = service->service_use_count; -+ int i; -+ /* Complete the close process */ -+ for (i = 0; i < uc; i++) -+ /* cater for cases where close is forced and the -+ ** client may not close all it's handles */ -+ vchiq_release_service_internal(service); -+ -+ service->client_id = 0; -+ 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 (service->srvstate == VCHIQ_SRVSTATE_CLOSED) -+ vchiq_free_service_internal(service); -+ else { -+ if (is_server) -+ service->closing = 0; -+ -+ up(&service->remove_event); -+ } -+ } else -+ vchiq_set_service_state(service, failstate); -+ -+ return status; ++ 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; ++ VCHIQ_STATE_T *state = service->state; ++ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; + -+ vchiq_log_info(vchiq_core_log_level, "%d: csi:%d,%d (%s)", -+ service->state->id, service->localport, close_recvd, -+ srvstate_names[service->srvstate]); ++ vcos_log_trace("%d: csi:%d (%s)", ++ service->state->id, service->localport, ++ srvstate_names[service->srvstate]); + -+ switch (service->srvstate) { -+ case VCHIQ_SRVSTATE_CLOSED: -+ case VCHIQ_SRVSTATE_HIDDEN: -+ case VCHIQ_SRVSTATE_LISTENING: -+ case VCHIQ_SRVSTATE_CLOSEWAIT: -+ if (close_recvd) -+ vchiq_log_error(vchiq_core_log_level, -+ "vchiq_close_service_internal(1) called " -+ "in state %s", -+ srvstate_names[service->srvstate]); -+ else -+ vchiq_free_service_internal(service); -+ break; -+ case VCHIQ_SRVSTATE_OPENING: -+ if (close_recvd) { -+ /* The open was rejected - tell the user */ -+ vchiq_set_service_state(service, -+ VCHIQ_SRVSTATE_CLOSEWAIT); -+ up(&service->remove_event); -+ } else { -+ /* Shutdown mid-open - let the other side know */ -+ status = queue_message(state, service, -+ VCHIQ_MAKE_MSG -+ (VCHIQ_MSG_CLOSE, -+ service->localport, -+ VCHIQ_MSG_DSTPORT(service->remoteport)), -+ NULL, 0, 0, 0); -+ } -+ break; ++ 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); + -+ case VCHIQ_SRVSTATE_OPENSYNC: -+ mutex_lock(&state->sync_mutex); -+ /* Drop through */ ++ if (status == VCHIQ_SUCCESS) ++ vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT); ++ } ++ break; + -+ case VCHIQ_SRVSTATE_OPEN: -+ if (state->is_master || close_recvd) { -+ if (!do_abort_bulks(service)) -+ status = VCHIQ_RETRY; -+ } ++ 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); ++ } + -+ release_service_messages(service); ++ 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) -+ status = queue_message(state, service, -+ 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; + -+ if (status == VCHIQ_SUCCESS) { -+ if (!close_recvd) -+ break; -+ } else if (service->srvstate == VCHIQ_SRVSTATE_OPENSYNC) { -+ mutex_unlock(&state->sync_mutex); -+ break; -+ } else -+ break; ++ case VCHIQ_SRVSTATE_CLOSESENT: ++ vcos_assert(close_recvd); + -+ status = close_service_complete(service, -+ VCHIQ_SRVSTATE_CLOSERECVD); -+ break; ++ 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); ++ } + -+ case VCHIQ_SRVSTATE_CLOSESENT: -+ if (!close_recvd) -+ /* This happens when a process is killed mid-close */ -+ break; ++ if (status == VCHIQ_SUCCESS) ++ vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSING); ++ break; + -+ if (!state->is_master) { -+ if (!do_abort_bulks(service)) { -+ status = VCHIQ_RETRY; -+ break; -+ } -+ } ++ case VCHIQ_SRVSTATE_CLOSING: ++ /* We may come here after a retry */ ++ vcos_assert(!close_recvd); ++ break; + -+ if (status == VCHIQ_SUCCESS) -+ status = close_service_complete(service, -+ VCHIQ_SRVSTATE_CLOSERECVD); -+ break; ++ default: ++ vcos_log_error("vchiq_close_service_internal(%d) called in state %s", ++ close_recvd, srvstate_names[service->srvstate]); ++ vcos_assert(0); ++ break; ++ } + -+ case VCHIQ_SRVSTATE_CLOSERECVD: -+ if (!close_recvd && -+ (service->public_fourcc != VCHIQ_FOURCC_INVALID)) -+ /* Force into LISTENING mode */ -+ vchiq_set_service_state(service, -+ VCHIQ_SRVSTATE_LISTENING); -+ status = close_service_complete(service, -+ VCHIQ_SRVSTATE_CLOSERECVD); -+ break; ++ if (service->srvstate == VCHIQ_SRVSTATE_CLOSING) ++ { ++ int i; ++ int uc = service->service_use_count; ++ /* Complete the close process */ ++ for( i=0; iclient_id = 0; + -+ default: -+ vchiq_log_error(vchiq_core_log_level, -+ "vchiq_close_service_internal(%d) called in state %s", -+ close_recvd, srvstate_names[service->srvstate]); -+ break; -+ } ++ /* Now tell the client that the services is closed */ ++ if (service->instance) ++ { ++ int oldstate = service->srvstate; + -+ return status; ++ /* 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; ++ VCHIQ_STATE_T *state = service->state; + -+ vchiq_log_info(vchiq_core_log_level, "%d: tsi - (%d<->%d)", -+ state->id, service->localport, service->remoteport); ++ vcos_log_info("%d: tsi - (%d<->%d)", state->id, service->localport, service->remoteport); + -+ /* Make it look like a client, because it must be removed and not -+ left in the LISTENING state. */ -+ service->public_fourcc = VCHIQ_FOURCC_INVALID; ++ /* Disconnect from the instance, to prevent any callbacks */ ++ service->instance = NULL; + -+ mark_service_closing(service); -+ -+ /* Mark the service for termination by the slot handler */ -+ request_poll(state, service, VCHIQ_POLL_TERMINATE); ++ /* Mark the service for termination by the slot handler */ ++ request_poll(state, service, VCHIQ_POLL_TERMINATE); +} + -+/* Called from the slot handler */ ++/* 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; ++ VCHIQ_STATE_T *state = service->state; ++ int slot_last = state->remote->slot_last; ++ int i; + -+ vchiq_log_info(vchiq_core_log_level, "%d: fsi - (%d)", -+ state->id, service->localport); ++ vcos_log_info("%d: fsi - (%d)", state->id, service->localport); + -+ switch (service->srvstate) { -+ case VCHIQ_SRVSTATE_OPENING: -+ case VCHIQ_SRVSTATE_CLOSED: -+ case VCHIQ_SRVSTATE_HIDDEN: -+ case VCHIQ_SRVSTATE_LISTENING: -+ case VCHIQ_SRVSTATE_CLOSEWAIT: -+ break; -+ default: -+ vchiq_log_error(vchiq_core_log_level, -+ "%d: fsi - (%d) in state %s", -+ state->id, service->localport, -+ srvstate_names[service->srvstate]); -+ return; -+ } ++ vcos_mutex_lock(&state->mutex); + -+ vchiq_set_service_state(service, VCHIQ_SRVSTATE_FREE); ++ /* 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); ++ unsigned int pos, end; + -+ up(&service->remove_event); ++ 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; ++ } + -+ /* Release the initial lock */ -+ unlock_service(service); ++ 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); ++ if (pos > VCHIQ_SLOT_SIZE) ++ { ++ vcos_log_error("fsi - pos %x: header %x, msgid %x, header->msgid %x, header->size %x", ++ pos, (unsigned int)header, msgid, header->msgid, header->size); ++ vcos_assert(0); ++ } ++ } ++ } ++ } ++ ++ 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) +{ -+ VCHIQ_SERVICE_T *service; -+ int i; ++ int i; + -+ /* Find all services registered to this client and enable them. */ -+ i = 0; -+ while ((service = next_service_by_instance(state, instance, -+ &i)) != NULL) { -+ if (service->srvstate == VCHIQ_SRVSTATE_HIDDEN) -+ vchiq_set_service_state(service, -+ VCHIQ_SRVSTATE_LISTENING); -+ unlock_service(service); -+ } ++ /* 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; ++ 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_CONNECTING); -+ } ++ vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED); ++ } + -+ if (state->conn_state == VCHIQ_CONNSTATE_CONNECTING) { -+ if (down_interruptible(&state->connect) != 0) -+ return VCHIQ_RETRY; -+ -+ vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED); -+ up(&state->connect); -+ } -+ -+ return VCHIQ_SUCCESS; ++ return VCHIQ_SUCCESS; +} + +VCHIQ_STATUS_T +vchiq_shutdown_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance) +{ -+ VCHIQ_SERVICE_T *service; -+ int i; ++ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ int i; + -+ /* Find all services registered to this client and enable them. */ -+ i = 0; -+ while ((service = next_service_by_instance(state, instance, -+ &i)) != NULL) { -+ (void)vchiq_remove_service(service->handle); -+ unlock_service(service); -+ } ++ /* 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 VCHIQ_SUCCESS; ++ return status; +} + +VCHIQ_STATUS_T +vchiq_pause_internal(VCHIQ_STATE_T *state) +{ -+ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ 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; -+ default: -+ vchiq_log_error(vchiq_core_log_level, -+ "vchiq_pause_internal in state %s\n", -+ conn_state_names[state->conn_state]); -+ status = VCHIQ_ERROR; -+ VCHIQ_STATS_INC(state, error_count); -+ break; -+ } ++ 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; ++ return status; +} + +VCHIQ_STATUS_T +vchiq_resume_internal(VCHIQ_STATE_T *state) +{ -+ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ 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); -+ } ++ 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; ++ return status; +} + +VCHIQ_STATUS_T +vchiq_close_service(VCHIQ_SERVICE_HANDLE_T handle) +{ -+ /* Unregister the service */ -+ VCHIQ_SERVICE_T *service = find_service_by_handle(handle); -+ VCHIQ_STATUS_T status = VCHIQ_ERROR; ++ /* Unregister the service */ ++ VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *) handle; ++ VCHIQ_STATUS_T status = VCHIQ_ERROR; + -+ if (!service) -+ return VCHIQ_ERROR; ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; + -+ vchiq_log_info(vchiq_core_log_level, -+ "%d: close_service:%d", -+ service->state->id, service->localport); ++ vcos_log_info("%d: close_service:%d", service->state->id, service->localport); + -+ mark_service_closing(service); ++ 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); ++ } + -+ /* Mark the service for termination by the slot handler */ -+ request_poll(service->state, service, VCHIQ_POLL_TERMINATE); -+ -+ while (1) { -+ if (down_interruptible(&service->remove_event) != 0) { -+ status = VCHIQ_RETRY; -+ break; -+ } -+ -+ if ((service->srvstate == VCHIQ_SRVSTATE_FREE) || -+ (service->srvstate == VCHIQ_SRVSTATE_LISTENING) || -+ (service->srvstate == VCHIQ_SRVSTATE_OPEN)) -+ break; -+ -+ vchiq_log_warning(vchiq_core_log_level, -+ "%d: close_service:%d - waiting in state %s", -+ service->state->id, service->localport, -+ srvstate_names[service->srvstate]); -+ } -+ -+ if ((status == VCHIQ_SUCCESS) && -+ (service->srvstate != VCHIQ_SRVSTATE_FREE) && -+ (service->srvstate != VCHIQ_SRVSTATE_LISTENING)) -+ status = VCHIQ_ERROR; -+ -+ unlock_service(service); -+ -+ return status; ++ return status; +} + +VCHIQ_STATUS_T +vchiq_remove_service(VCHIQ_SERVICE_HANDLE_T handle) +{ -+ /* Unregister the service */ -+ VCHIQ_SERVICE_T *service = find_service_by_handle(handle); -+ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; ++ /* Unregister the service */ ++ VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *) handle; ++ VCHIQ_STATUS_T status = VCHIQ_SUCCESS; + -+ if (!service) -+ return VCHIQ_ERROR; ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; + -+ vchiq_log_info(vchiq_core_log_level, -+ "%d: remove_service:%d", -+ service->state->id, service->localport); ++ vcos_log_info("%d: remove_service:%d", service->state->id, service->localport); + -+ /* Make it look like a client, because it must be removed and not -+ left in the LISTENING state. */ -+ service->public_fourcc = VCHIQ_FOURCC_INVALID; ++ 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); + -+ mark_service_closing(service); ++ /* 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; + -+ /* Mark the service for termination by the slot handler */ -+ request_poll(service->state, service, VCHIQ_POLL_TERMINATE); ++ default: ++ break; ++ } + -+ while (1) { -+ if (down_interruptible(&service->remove_event) != 0) { -+ status = VCHIQ_RETRY; -+ break; -+ } ++ if (status == VCHIQ_SUCCESS) { ++ if (service->srvstate == VCHIQ_SRVSTATE_OPEN) ++ status = VCHIQ_ERROR; ++ else ++ { ++ service->instance = NULL; ++ vchiq_free_service_internal(service); ++ } ++ } + -+ if ((service->srvstate == VCHIQ_SRVSTATE_FREE) || -+ (service->srvstate == VCHIQ_SRVSTATE_OPEN)) -+ break; -+ -+ vchiq_log_warning(vchiq_core_log_level, -+ "%d: remove_service:%d - waiting in state %s", -+ service->state->id, service->localport, -+ srvstate_names[service->srvstate]); -+ } -+ -+ if ((status == VCHIQ_SUCCESS) && -+ (service->srvstate != VCHIQ_SRVSTATE_FREE)) -+ status = VCHIQ_ERROR; -+ -+ unlock_service(service); -+ -+ return status; ++ return status; +} + + -+/* This function may be called by kernel threads or user threads. -+ * User threads may receive VCHIQ_RETRY to indicate that a signal has been -+ * received and the call should be retried after being returned to user -+ * context. -+ * When called in blocking mode, the userdata field points to a bulk_waiter -+ * structure. -+ */ +VCHIQ_STATUS_T -+vchiq_bulk_transfer(VCHIQ_SERVICE_HANDLE_T handle, -+ VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata, -+ VCHIQ_BULK_MODE_T mode, VCHIQ_BULK_DIR_T dir) ++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_SERVICE_T *service = find_service_by_handle(handle); -+ VCHIQ_BULK_QUEUE_T *queue; -+ VCHIQ_BULK_T *bulk; -+ VCHIQ_STATE_T *state; -+ struct bulk_waiter *bulk_waiter = NULL; -+ 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; ++ 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 || -+ (service->srvstate != VCHIQ_SRVSTATE_OPEN) || -+ ((memhandle == VCHI_MEM_HANDLE_INVALID) && (offset == NULL)) || -+ (vchiq_check_service(service) != VCHIQ_SUCCESS)) -+ goto error_exit; ++ if (!is_valid_service(service) || ++ (service->srvstate != VCHIQ_SRVSTATE_OPEN) || ++ ((memhandle == VCHI_MEM_HANDLE_INVALID) && (offset == NULL)) || ++ (vchiq_check_service(service) != VCHIQ_SUCCESS)) ++ return VCHIQ_ERROR; + -+ switch (mode) { -+ case VCHIQ_BULK_MODE_NOCALLBACK: -+ case VCHIQ_BULK_MODE_CALLBACK: -+ break; -+ case VCHIQ_BULK_MODE_BLOCKING: -+ bulk_waiter = (struct bulk_waiter *)userdata; -+ sema_init(&bulk_waiter->event, 0); -+ bulk_waiter->actual = 0; -+ bulk_waiter->bulk = NULL; -+ break; -+ case VCHIQ_BULK_MODE_WAITING: -+ bulk_waiter = (struct bulk_waiter *)userdata; -+ bulk = bulk_waiter->bulk; -+ goto waiting; -+ default: -+ goto error_exit; -+ } ++ state = service->state; + -+ state = service->state; ++ if (vcos_mutex_lock(&service->bulk_mutex) != VCOS_SUCCESS) ++ return VCHIQ_RETRY; + -+ queue = (dir == VCHIQ_BULK_TRANSMIT) ? -+ &service->bulk_tx : &service->bulk_rx; ++ 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); ++ } + -+ if (mutex_lock_interruptible(&service->bulk_mutex) != 0) { -+ status = VCHIQ_RETRY; -+ goto error_exit; -+ } ++ bulk = &queue->bulks[BULK_INDEX(queue->local_insert)]; + -+ if (queue->local_insert == queue->remove + VCHIQ_NUM_SERVICE_BULKS) { -+ VCHIQ_SERVICE_STATS_INC(service, bulk_stalls); -+ do { -+ mutex_unlock(&service->bulk_mutex); -+ if (down_interruptible(&service->bulk_remove_event) -+ != 0) { -+ status = VCHIQ_RETRY; -+ goto error_exit; -+ } -+ if (mutex_lock_interruptible(&service->bulk_mutex) -+ != 0) { -+ status = VCHIQ_RETRY; -+ goto error_exit; -+ } -+ } while (queue->local_insert == queue->remove + -+ VCHIQ_NUM_SERVICE_BULKS); -+ } ++ if (mode == VCHIQ_BULK_MODE_BLOCKING) ++ { ++ vcos_event_create(&bulk_waiter.event, "bulk_waiter"); ++ bulk_waiter.actual = 0; ++ userdata = &bulk_waiter; ++ } + -+ bulk = &queue->bulks[BULK_INDEX(queue->local_insert)]; ++ bulk->mode = mode; ++ bulk->dir = dir; ++ bulk->userdata = userdata; ++ bulk->size = size; ++ bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED; + -+ 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; ++ } + -+ if (vchiq_prepare_bulk_data(bulk, memhandle, offset, size, dir) != -+ VCHIQ_SUCCESS) -+ goto unlock_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); + -+ wmb(); ++ 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) }; + -+ vchiq_log_info(vchiq_core_log_level, -+ "%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 (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++; ++ } + -+ 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) }; ++ 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 = queue_message(state, NULL, -+ VCHIQ_MAKE_MSG(dir_msgtype, -+ service->localport, service->remoteport), -+ &element, 1, sizeof(payload), 1); -+ if (status != VCHIQ_SUCCESS) { -+ vchiq_complete_bulk(bulk); -+ goto unlock_error_exit; -+ } -+ queue->local_insert++; -+ } ++ status = VCHIQ_SUCCESS; + -+ mutex_unlock(&service->bulk_mutex); ++ 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; + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%d: bt:%d %cx li=%x ri=%x p=%x", -+ state->id, -+ service->localport, dir_char, -+ queue->local_insert, queue->remote_insert, queue->process); ++ vcos_event_delete(&bulk_waiter.event); ++ } + -+waiting: -+ unlock_service(service); -+ -+ status = VCHIQ_SUCCESS; -+ -+ if (bulk_waiter) { -+ bulk_waiter->bulk = bulk; -+ if (down_interruptible(&bulk_waiter->event) != 0) -+ status = VCHIQ_RETRY; -+ else if (bulk_waiter->actual == VCHIQ_BULK_ACTUAL_ABORTED) -+ status = VCHIQ_ERROR; -+ } -+ -+ return status; -+ -+unlock_error_exit: -+ mutex_unlock(&service->bulk_mutex); ++ return status; + +error_exit: -+ if (service) -+ unlock_service(service); -+ return status; ++ 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) ++ const void *data, int size, void *userdata) +{ -+ return vchiq_bulk_transfer(handle, -+ VCHI_MEM_HANDLE_INVALID, (void *)data, size, userdata, -+ VCHIQ_BULK_MODE_CALLBACK, VCHIQ_BULK_TRANSMIT); ++ 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) ++ void *userdata) +{ -+ return vchiq_bulk_transfer(handle, -+ VCHI_MEM_HANDLE_INVALID, data, size, userdata, -+ VCHIQ_BULK_MODE_CALLBACK, VCHIQ_BULK_RECEIVE); ++ 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) ++ VCHI_MEM_HANDLE_T memhandle, const void *offset, int size, void *userdata) +{ -+ return vchiq_bulk_transfer(handle, -+ memhandle, (void *)offset, size, userdata, -+ VCHIQ_BULK_MODE_CALLBACK, VCHIQ_BULK_TRANSMIT); ++ 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) ++ VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata) +{ -+ return vchiq_bulk_transfer(handle, -+ memhandle, offset, size, userdata, -+ VCHIQ_BULK_MODE_CALLBACK, VCHIQ_BULK_RECEIVE); ++ 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) ++ void *userdata, VCHIQ_BULK_MODE_T mode) +{ -+ struct bulk_waiter bulk_waiter; -+ VCHIQ_STATUS_T status; -+ -+ switch (mode) { -+ case VCHIQ_BULK_MODE_NOCALLBACK: -+ case VCHIQ_BULK_MODE_CALLBACK: -+ break; -+ case VCHIQ_BULK_MODE_BLOCKING: -+ userdata = &bulk_waiter; -+ break; -+ default: -+ return VCHIQ_ERROR; -+ } -+ -+ status = vchiq_bulk_transfer(handle, -+ VCHI_MEM_HANDLE_INVALID, (void *)data, size, userdata, -+ mode, VCHIQ_BULK_TRANSMIT); -+ -+ /* This call is for kernel thread use and should not be interrupted */ -+ // dc4: remove as it does happen: BUG_ON(status == VCHIQ_RETRY); -+ return status; ++ 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) ++ void *userdata, VCHIQ_BULK_MODE_T mode) +{ -+ struct bulk_waiter bulk_waiter; -+ VCHIQ_STATUS_T status; -+ -+ switch (mode) { -+ case VCHIQ_BULK_MODE_NOCALLBACK: -+ case VCHIQ_BULK_MODE_CALLBACK: -+ break; -+ case VCHIQ_BULK_MODE_BLOCKING: -+ userdata = &bulk_waiter; -+ break; -+ default: -+ return VCHIQ_ERROR; -+ } -+ -+ status = vchiq_bulk_transfer(handle, -+ VCHI_MEM_HANDLE_INVALID, data, size, userdata, -+ mode, VCHIQ_BULK_RECEIVE); -+ -+ /* This call is for kernel thread use and should not be interrupted */ -+ BUG_ON(status == VCHIQ_RETRY); -+ return status; ++ 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) ++ VCHI_MEM_HANDLE_T memhandle, const void *offset, int size, void *userdata, ++ VCHIQ_BULK_MODE_T mode) +{ -+ struct bulk_waiter bulk_waiter; -+ VCHIQ_STATUS_T status; -+ -+ switch (mode) { -+ case VCHIQ_BULK_MODE_NOCALLBACK: -+ case VCHIQ_BULK_MODE_CALLBACK: -+ break; -+ case VCHIQ_BULK_MODE_BLOCKING: -+ userdata = &bulk_waiter; -+ break; -+ default: -+ return VCHIQ_ERROR; -+ } -+ -+ status = vchiq_bulk_transfer(handle, -+ memhandle, (void *)offset, size, userdata, -+ mode, VCHIQ_BULK_TRANSMIT); -+ -+ /* This call is for kernel thread use and should not be interrupted */ -+ BUG_ON(status == VCHIQ_RETRY); -+ return status; ++ 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) ++ VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata, ++ VCHIQ_BULK_MODE_T mode) +{ -+ struct bulk_waiter bulk_waiter; -+ VCHIQ_STATUS_T status; -+ -+ switch (mode) { -+ case VCHIQ_BULK_MODE_NOCALLBACK: -+ case VCHIQ_BULK_MODE_CALLBACK: -+ break; -+ case VCHIQ_BULK_MODE_BLOCKING: -+ userdata = &bulk_waiter; -+ break; -+ default: -+ return VCHIQ_ERROR; -+ } -+ -+ status = vchiq_bulk_transfer(handle, -+ memhandle, offset, size, userdata, -+ mode, VCHIQ_BULK_RECEIVE); -+ -+ /* This call is for kernel thread use and should not be interrupted */ -+ BUG_ON(status == VCHIQ_RETRY); -+ return status; ++ 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) ++ const VCHIQ_ELEMENT_T *elements, int count) +{ -+ VCHIQ_SERVICE_T *service = find_service_by_handle(handle); -+ VCHIQ_STATUS_T status = VCHIQ_ERROR; ++ VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *) handle; + -+ unsigned int size = 0; -+ unsigned int i; ++ unsigned int size = 0; ++ unsigned int i; + -+ if (!service || -+ (vchiq_check_service(service) != VCHIQ_SUCCESS)) -+ goto error_exit; ++ if (!is_valid_service(service) || ++ (service->srvstate != VCHIQ_SRVSTATE_OPEN) || ++ (vchiq_check_service(service) != VCHIQ_SUCCESS)) ++ 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); -+ goto error_exit; -+ } -+ size += elements[i].size; -+ } -+ } ++ 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); -+ goto error_exit; -+ } ++ if (size > VCHIQ_MAX_MSG_SIZE) ++ { ++ VCHIQ_SERVICE_STATS_INC(service, error_count); ++ return VCHIQ_ERROR; ++ } + -+ switch (service->srvstate) { -+ case VCHIQ_SRVSTATE_OPEN: -+ status = queue_message(service->state, service, -+ VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA, -+ service->localport, -+ service->remoteport), -+ elements, count, size, 1); -+ break; -+ case VCHIQ_SRVSTATE_OPENSYNC: -+ status = queue_message_sync(service->state, service, -+ VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA, -+ service->localport, -+ service->remoteport), -+ elements, count, size, 1); -+ break; -+ default: -+ status = VCHIQ_ERROR; -+ break; -+ } -+ -+error_exit: -+ if (service) -+ unlock_service(service); -+ -+ return status; ++ 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 = find_service_by_handle(handle); -+ VCHIQ_SHARED_STATE_T *remote; -+ VCHIQ_STATE_T *state; -+ int slot_index; ++ VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle; ++ VCHIQ_STATE_T *state; ++ int slot_index; ++ int msgid; + -+ if (!service) -+ return; ++ if (!is_valid_service(service)) ++ return; + -+ state = service->state; -+ remote = state->remote; ++ state = service->state; + -+ slot_index = SLOT_INDEX_FROM_DATA(state, (void *)header); ++ slot_index = SLOT_INDEX_FROM_DATA(state, (void *)header); + -+ if ((slot_index >= remote->slot_first) && -+ (slot_index <= remote->slot_last)) { -+ int msgid = header->msgid; -+ if (msgid & VCHIQ_MSGID_CLAIMED) { -+ VCHIQ_SLOT_INFO_T *slot_info = -+ SLOT_INFO_FROM_INDEX(state, slot_index); ++ 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); + -+ release_slot(state, slot_info, header, service); -+ } -+ } else if (slot_index == remote->slot_sync) -+ release_message_sync(state, header); ++ /* Rewrite the message header to prevent a double release */ ++ header->msgid = msgid & ~VCHIQ_MSGID_CLAIMED; + -+ unlock_service(service); ++ release_slot(state, slot_info); ++ } +} + -+static void -+release_message_sync(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header) ++int ++vchiq_get_client_id(VCHIQ_SERVICE_HANDLE_T handle) +{ -+ header->msgid = VCHIQ_MSGID_PADDING; -+ wmb(); -+ remote_event_signal(&state->remote->sync_release); ++ 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) ++ int config_size, VCHIQ_CONFIG_T *pconfig) +{ -+ VCHIQ_CONFIG_T config; ++ VCHIQ_CONFIG_T config; + -+ (void)instance; ++ 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; ++ 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; ++ if (config_size > sizeof(VCHIQ_CONFIG_T)) ++ return VCHIQ_ERROR; + -+ memcpy(pconfig, &config, -+ min(config_size, (int)(sizeof(VCHIQ_CONFIG_T)))); ++ memcpy(pconfig, &config, vcos_min(config_size, sizeof(VCHIQ_CONFIG_T))); + -+ return VCHIQ_SUCCESS; ++ return VCHIQ_SUCCESS; +} + +VCHIQ_STATUS_T +vchiq_set_service_option(VCHIQ_SERVICE_HANDLE_T handle, -+ VCHIQ_SERVICE_OPTION_T option, int value) ++ VCHIQ_SERVICE_OPTION_T option, int value) +{ -+ VCHIQ_SERVICE_T *service = find_service_by_handle(handle); -+ VCHIQ_STATUS_T status = VCHIQ_ERROR; ++ 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; ++ if (is_valid_service(service)) ++ { ++ switch (option) ++ { ++ case VCHIQ_SERVICE_OPTION_AUTOCLOSE: ++ service->auto_close = value; ++ status = VCHIQ_SUCCESS; ++ break; + -+ case VCHIQ_SERVICE_OPTION_SLOT_QUOTA: { -+ VCHIQ_SERVICE_QUOTA_T *service_quota = -+ &service->state->service_quotas[ -+ service->localport]; -+ if (value == 0) -+ value = service->state->default_slot_quota; -+ if ((value >= service_quota->slot_use_count) && -+ (value < (unsigned short)~0)) { -+ service_quota->slot_quota = value; -+ if ((value >= service_quota->slot_use_count) && -+ (service_quota->message_quota >= -+ service_quota->message_use_count)) { -+ /* Signal the service that it may have -+ ** dropped below its quota */ -+ up(&service_quota->quota_event); -+ } -+ status = VCHIQ_SUCCESS; -+ } -+ } break; ++ case VCHIQ_SERVICE_OPTION_SLOT_QUOTA: ++ { ++ VCHIQ_SERVICE_QUOTA_T *service_quota = ++ &service->state->service_quotas[service->localport]; ++ if (value == 0) ++ value = service->state->default_slot_quota; ++ if ((value >= service_quota->slot_use_count) && ++ (value < (unsigned short)~0)) ++ { ++ service_quota->slot_quota = value; ++ if ((value >= service_quota->slot_use_count) && ++ (service_quota->message_quota >= service_quota->message_use_count)) ++ { ++ /* Signal the service that it may have dropped below its quota */ ++ vcos_event_signal(&service_quota->quota_event); ++ } ++ status = VCHIQ_SUCCESS; ++ } ++ } ++ break; + -+ case VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA: { -+ VCHIQ_SERVICE_QUOTA_T *service_quota = -+ &service->state->service_quotas[ -+ service->localport]; -+ if (value == 0) -+ value = service->state->default_message_quota; -+ if ((value >= service_quota->message_use_count) && -+ (value < (unsigned short)~0)) { -+ service_quota->message_quota = value; -+ if ((value >= -+ service_quota->message_use_count) && -+ (service_quota->slot_quota >= -+ service_quota->slot_use_count)) -+ /* Signal the service that it may have -+ ** dropped below its quota */ -+ up(&service_quota->quota_event); -+ status = VCHIQ_SUCCESS; -+ } -+ } break; ++ case VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA: ++ { ++ VCHIQ_SERVICE_QUOTA_T *service_quota = ++ &service->state->service_quotas[service->localport]; ++ if (value == 0) ++ value = service->state->default_message_quota; ++ if ((value >= service_quota->message_use_count) && ++ (value < (unsigned short)~0)) ++ { ++ service_quota->message_quota = value; ++ if ((value >= service_quota->message_use_count) && ++ (service_quota->slot_quota >= service_quota->slot_use_count)) ++ { ++ /* Signal the service that it may have dropped below its quota */ ++ vcos_event_signal(&service_quota->quota_event); ++ } ++ status = VCHIQ_SUCCESS; ++ } ++ } ++ break; + -+ case VCHIQ_SERVICE_OPTION_SYNCHRONOUS: -+ if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) || -+ (service->srvstate == -+ VCHIQ_SRVSTATE_LISTENING)) { -+ service->sync = value; -+ status = VCHIQ_SUCCESS; -+ } -+ break; ++ default: ++ break; ++ } ++ } + -+ default: -+ break; -+ } -+ unlock_service(service); -+ } -+ -+ return status; ++ return status; +} + +void +vchiq_dump_shared_state(void *dump_context, VCHIQ_STATE_T *state, -+ VCHIQ_SHARED_STATE_T *shared, const char *label) ++ VCHIQ_SHARED_STATE_T *shared, const char *label) +{ -+ static const char *const debug_names[] = { -+ "", -+ "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; ++ static const char *const debug_names[] = ++ { ++ "", ++ "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 = 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); ++ 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 = snprintf(buf, sizeof(buf), -+ " Slots claimed:"); -+ 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 = 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 = 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 = snprintf(buf, sizeof(buf), " DEBUG: %s = %d(%x)", -+ debug_names[i], shared->debug[i], shared->debug[i]); -+ 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; ++ char buf[80]; ++ int len; ++ int i; + -+ len = 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), "State %d: %s", state->id, ++ conn_state_names[state->conn_state]); ++ vchiq_dump(dump_context, buf, len + 1); + -+ len = snprintf(buf, sizeof(buf), -+ " tx_pos=%x(@%x), rx_pos=%x(@%x)", -+ 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), ++ " 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 = snprintf(buf, sizeof(buf), -+ " Version: %d (min %d)", -+ VCHIQ_VERSION, VCHIQ_VERSION_MIN); -+ 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 = 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.error_count); -+ 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 = snprintf(buf, sizeof(buf), -+ " Slots: %d available (%d data), %d recyclable, %d stalls (%d data)", -+ state->slot_queue_available - -+ SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos), -+ state->data_quota - state->data_use_count, -+ state->local->slot_queue_recycle - state->slot_queue_available, -+ state->stats.slot_stalls, state->stats.data_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_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_shared_state(dump_context, state, state->local, "Local"); ++ vchiq_dump_shared_state(dump_context, state, state->remote, "Remote"); + -+ vchiq_dump_platform_instances(dump_context); ++ vchiq_dump_platform_instances(dump_context); + -+ for (i = 0; i < state->unused_service; i++) { -+ VCHIQ_SERVICE_T *service = find_service_by_port(state, i); ++ for (i = 0; i < state->unused_service; i++) { ++ VCHIQ_SERVICE_T *service = state->services[i]; + -+ if (service) { -+ vchiq_dump_service_state(dump_context, service); -+ unlock_service(service); -+ } -+ } ++ 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; ++ char buf[80]; ++ int len; + -+ len = snprintf(buf, sizeof(buf), "Service %d: %s (ref %u)", -+ service->localport, srvstate_names[service->srvstate], -+ service->ref_count - 1); /*Don't include the lock just taken*/ ++ 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; -+ int tx_pending, rx_pending; -+ if (service->remoteport != VCHIQ_PORT_FREE) { -+ int len2 = snprintf(remoteport, sizeof(remoteport), -+ "%d", service->remoteport); -+ if (service->public_fourcc != VCHIQ_FOURCC_INVALID) -+ snprintf(remoteport + len2, -+ sizeof(remoteport) - len2, -+ " (client %x)", service->client_id); -+ } else -+ strcpy(remoteport, "n/a"); ++ 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 += snprintf(buf + len, sizeof(buf) - len, -+ " '%c%c%c%c' remote %s (msg use %d/%d, slot use %d/%d)", -+ VCHIQ_FOURCC_AS_4CHARS(fourcc), -+ remoteport, -+ service_quota->message_use_count, -+ service_quota->message_quota, -+ service_quota->slot_use_count, -+ service_quota->slot_quota); ++ len += vcos_snprintf(buf + len, sizeof(buf) - len, ++ " '%c%c%c%c' remote %s (msg use %d/%d, slot use %d/%d)", ++ VCHIQ_FOURCC_AS_4CHARS(fourcc), ++ remoteport, ++ service_quota->message_use_count, ++ service_quota->message_quota, ++ service_quota->slot_use_count, ++ service_quota->slot_quota); + -+ vchiq_dump(dump_context, buf, len + 1); ++ if (VCHIQ_ENABLE_STATS) ++ { ++ vchiq_dump(dump_context, buf, len + 1); + -+ tx_pending = service->bulk_tx.local_insert - -+ service->bulk_tx.remote_insert; ++ 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); + -+ rx_pending = service->bulk_rx.local_insert - -+ service->bulk_rx.remote_insert; ++ 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 = snprintf(buf, sizeof(buf), -+ " Bulk: tx_pending=%d (size %d)," -+ " rx_pending=%d (size %d)", -+ tx_pending, -+ tx_pending ? service->bulk_tx.bulks[ -+ BULK_INDEX(service->bulk_tx.remove)].size : 0, -+ rx_pending, -+ rx_pending ? service->bulk_rx.bulks[ -+ BULK_INDEX(service->bulk_rx.remove)].size : 0); ++ 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); ++ } ++ } + -+ if (VCHIQ_ENABLE_STATS) { -+ vchiq_dump(dump_context, buf, len + 1); ++ vchiq_dump(dump_context, buf, len + 1); + -+ len = snprintf(buf, sizeof(buf), -+ " Ctrl: tx_count=%d, tx_bytes=%llu, " -+ "rx_count=%d, rx_bytes=%llu", -+ 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 = snprintf(buf, sizeof(buf), -+ " Bulk: tx_count=%d, tx_bytes=%llu, " -+ "rx_count=%d, rx_bytes=%llu", -+ 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 = 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); -+ -+ if (service->srvstate != VCHIQ_SRVSTATE_FREE) -+ vchiq_dump_platform_service_state(dump_context, service); ++ vchiq_dump_platform_service_state(dump_context, service); +} + + -+void -+vchiq_loud_error_header(void) ++VCHIQ_STATUS_T vchiq_send_remote_use(VCHIQ_STATE_T * state) +{ -+ vchiq_log_error(vchiq_core_log_level, -+ "============================================================" -+ "================"); -+ vchiq_log_error(vchiq_core_log_level, -+ "============================================================" -+ "================"); -+ vchiq_log_error(vchiq_core_log_level, "====="); ++ VCHIQ_STATUS_T status = VCHIQ_RETRY; ++ if(state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED) ++ { ++ status = queue_message(state, NULL, VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE, 0, 0), NULL, 0, 0, 0); ++ } ++ return status; +} + -+void -+vchiq_loud_error_footer(void) ++VCHIQ_STATUS_T vchiq_send_remote_release(VCHIQ_STATE_T * state) +{ -+ vchiq_log_error(vchiq_core_log_level, "====="); -+ vchiq_log_error(vchiq_core_log_level, -+ "============================================================" -+ "================"); -+ vchiq_log_error(vchiq_core_log_level, -+ "============================================================" -+ "================"); ++ VCHIQ_STATUS_T status = VCHIQ_RETRY; ++ if(state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED) ++ { ++ status = queue_message(state, NULL, VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_RELEASE, 0, 0), NULL, 0, 0, 0); ++ } ++ return status; +} + -+ -+VCHIQ_STATUS_T vchiq_send_remote_use(VCHIQ_STATE_T *state) ++VCHIQ_STATUS_T vchiq_send_remote_use_active(VCHIQ_STATE_T * state) +{ -+ VCHIQ_STATUS_T status = VCHIQ_RETRY; -+ if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED) -+ status = queue_message(state, NULL, -+ VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE, 0, 0), -+ NULL, 0, 0, 0); -+ return status; -+} -+ -+VCHIQ_STATUS_T vchiq_send_remote_release(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_STATUS_T status = VCHIQ_RETRY; -+ if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED) -+ status = queue_message(state, NULL, -+ VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_RELEASE, 0, 0), -+ NULL, 0, 0, 0); -+ return status; -+} -+ -+VCHIQ_STATUS_T vchiq_send_remote_use_active(VCHIQ_STATE_T *state) -+{ -+ VCHIQ_STATUS_T status = VCHIQ_RETRY; -+ if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED) -+ status = queue_message(state, NULL, -+ VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE_ACTIVE, 0, 0), -+ NULL, 0, 0, 0); -+ return status; -+} -+ -+void vchiq_log_dump_mem(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 += snprintf(s, 4, "%02x ", mem[offset]); -+ else -+ s += 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')) -+ vchiq_log_trace(VCHIQ_LOG_TRACE, -+ "%s: %08x: %s", label, addr, lineBuf); -+ else -+ vchiq_log_trace(VCHIQ_LOG_TRACE, -+ "%08x: %s", addr, lineBuf); -+ -+ addr += 16; -+ mem += 16; -+ if (numBytes > 16) -+ numBytes -= 16; -+ else -+ numBytes = 0; -+ } ++ VCHIQ_STATUS_T status = VCHIQ_RETRY; ++ if(state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED) ++ { ++ status = queue_message(state, NULL, VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE_ACTIVE, 0, 0), NULL, 0, 0, 0); ++ } ++ return status; +} 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..bcdff3a +index 0000000..37884bd --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_core.h -@@ -0,0 +1,673 @@ +@@ -0,0 +1,506 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -17265,105 +15256,58 @@ index 0000000..bcdff3a +#ifndef VCHIQ_CORE_H +#define VCHIQ_CORE_H + -+#include -+#include -+#include -+ +#include "vchiq_cfg.h" + +#include "vchiq.h" + -+/* Run time control of log level, based on KERN_XXX level. */ -+#define VCHIQ_LOG_DEFAULT 4 -+#define VCHIQ_LOG_ERROR 3 -+#define VCHIQ_LOG_WARNING 4 -+#define VCHIQ_LOG_INFO 6 -+#define VCHIQ_LOG_TRACE 7 -+ -+#define VCHIQ_LOG_PREFIX KERN_INFO "vchiq: " -+ -+#ifndef vchiq_log_error -+#define vchiq_log_error(cat, fmt, ...) \ -+ do { if (cat >= VCHIQ_LOG_ERROR) \ -+ printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0) -+#endif -+#ifndef vchiq_log_warning -+#define vchiq_log_warning(cat, fmt, ...) \ -+ do { if (cat >= VCHIQ_LOG_WARNING) \ -+ printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0) -+#endif -+#ifndef vchiq_log_info -+#define vchiq_log_info(cat, fmt, ...) \ -+ do { if (cat >= VCHIQ_LOG_INFO) \ -+ printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0) -+#endif -+#ifndef vchiq_log_trace -+#define vchiq_log_trace(cat, fmt, ...) \ -+ do { if (cat >= VCHIQ_LOG_TRACE) \ -+ printk(VCHIQ_LOG_PREFIX fmt "\n", ##__VA_ARGS__); } while (0) -+#endif -+ -+#define vchiq_loud_error(...) \ -+ vchiq_log_error(vchiq_core_log_level, "===== " __VA_ARGS__) -+ -+#ifndef vchiq_static_assert -+#define vchiq_static_assert(cond) __attribute__((unused)) \ -+ extern int vchiq_static_assert[(cond) ? 1 : -1] -+#endif -+ +#define IS_POW2(x) (x && ((x & (x - 1)) == 0)) + +/* Ensure that the slot size and maximum number of slots are powers of 2 */ -+vchiq_static_assert(IS_POW2(VCHIQ_SLOT_SIZE)); -+vchiq_static_assert(IS_POW2(VCHIQ_MAX_SLOTS)); -+vchiq_static_assert(IS_POW2(VCHIQ_MAX_SLOTS_PER_SIDE)); ++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) ++ 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_MSG_REMOTE_USE 12 /* - */ -+#define VCHIQ_MSG_REMOTE_RELEASE 13 /* - */ -+#define VCHIQ_MSG_REMOTE_USE_ACTIVE 14 /* - */ ++#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_MSG_REMOTE_USE 12 // - ++#define VCHIQ_MSG_REMOTE_RELEASE 13 // - ++#define VCHIQ_MSG_REMOTE_USE_ACTIVE 14 // - + +#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_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_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 ++ ((fourcc) >> 24) & 0xff, \ ++ ((fourcc) >> 16) & 0xff, \ ++ ((fourcc) >> 8) & 0xff, \ ++ ((fourcc) ) & 0xff + +/* Ensure the fields are wide enough */ -+vchiq_static_assert(VCHIQ_MSG_SRCPORT(VCHIQ_MAKE_MSG(0, 0, VCHIQ_PORT_MAX)) -+ == 0); -+vchiq_static_assert(VCHIQ_MSG_TYPE(VCHIQ_MAKE_MSG(0, VCHIQ_PORT_MAX, 0)) == 0); -+vchiq_static_assert((unsigned int)VCHIQ_PORT_MAX < -+ (unsigned int)VCHIQ_PORT_FREE); ++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_PADDING VCHIQ_MAKE_MSG(VCHIQ_MSG_PADDING,0,0) +#define VCHIQ_MSGID_CLAIMED 0x40000000 + +#define VCHIQ_FOURCC_INVALID 0x00000000 @@ -17373,7 +15317,7 @@ index 0000000..bcdff3a + +typedef uint32_t BITSET_T; + -+vchiq_static_assert((sizeof(BITSET_T) * 8) == 32); ++vcos_static_assert((sizeof(BITSET_T) * 8) == 32); + +#define BITSET_SIZE(b) ((b + 31) >> 5) +#define BITSET_WORD(b) (b >> 5) @@ -17384,380 +15328,314 @@ index 0000000..bcdff3a +#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) ++#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, ++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, ++ 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 ++ DEBUG_MAX +}; + +#if VCHIQ_ENABLE_DEBUG + -+#define DEBUG_INITIALISE(local) int *debug_ptr = (local)->debug; -+#define DEBUG_TRACE(d) \ -+ do { debug_ptr[DEBUG_ ## d] = __LINE__; dsb(); } while (0) -+#define DEBUG_VALUE(d, v) \ -+ do { debug_ptr[DEBUG_ ## d] = (v); dsb(); } while (0) -+#define DEBUG_COUNT(d) \ -+ do { debug_ptr[DEBUG_ ## d]++; dsb(); } while (0) ++#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_VALUE(d,v) +#define DEBUG_COUNT(d) + +#endif /* VCHIQ_ENABLE_DEBUG */ + -+typedef enum { -+ VCHIQ_CONNSTATE_DISCONNECTED, -+ VCHIQ_CONNSTATE_CONNECTING, -+ VCHIQ_CONNSTATE_CONNECTED, -+ VCHIQ_CONNSTATE_PAUSING, -+ VCHIQ_CONNSTATE_PAUSE_SENT, -+ VCHIQ_CONNSTATE_PAUSED, -+ VCHIQ_CONNSTATE_RESUMING ++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_OPENSYNC, -+ VCHIQ_SRVSTATE_CLOSESENT, -+ VCHIQ_SRVSTATE_CLOSERECVD, -+ VCHIQ_SRVSTATE_CLOSEWAIT, -+ VCHIQ_SRVSTATE_CLOSED ++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 ++enum ++{ ++ VCHIQ_POLL_TERMINATE, ++ VCHIQ_POLL_TXNOTIFY, ++ VCHIQ_POLL_RXNOTIFY, ++ VCHIQ_POLL_COUNT +}; + -+typedef enum { -+ VCHIQ_BULK_TRANSMIT, -+ VCHIQ_BULK_RECEIVE ++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; ++ 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 (mstr) */ -+ int remove; /* Bulk to notify the local client of, and remove, -+ ** next */ -+ VCHIQ_BULK_T bulks[VCHIQ_NUM_SERVICE_BULKS]; ++ 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 { -+ int armed; -+ int fired; -+ struct semaphore *event; ++ volatile int armed; ++ volatile int fired; ++ VCOS_EVENT_T * event; +} REMOTE_EVENT_T; + -+typedef struct opaque_platform_state_t *VCHIQ_PLATFORM_STATE_T; ++typedef struct opaque_platform_state_t* VCHIQ_PLATFORM_STATE_T; + +typedef struct vchiq_state_struct VCHIQ_STATE_T; + +typedef struct vchiq_slot_struct { -+ char data[VCHIQ_SLOT_SIZE]; ++ 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. */ -+ short use_count; -+ short release_count; ++ /* 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; -+ VCHIQ_SERVICE_HANDLE_T handle; -+ unsigned int ref_count; -+ int srvstate; -+ unsigned int localport; -+ unsigned int remoteport; -+ int public_fourcc; -+ int client_id; -+ char auto_close; -+ char sync; -+ char closing; -+ atomic_t poll_flags; -+ short version; -+ short version_min; ++ 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; ++ VCHIQ_STATE_T *state; ++ VCHIQ_INSTANCE_T instance; + -+ int service_use_count; ++ int service_use_count; + -+ VCHIQ_BULK_QUEUE_T bulk_tx; -+ VCHIQ_BULK_QUEUE_T bulk_rx; ++ VCHIQ_BULK_QUEUE_T bulk_tx; ++ VCHIQ_BULK_QUEUE_T bulk_rx; + -+ struct semaphore remove_event; -+ struct semaphore bulk_remove_event; -+ struct mutex bulk_mutex; ++ 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; ++ 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. ++ 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 { -+ unsigned short slot_quota; -+ unsigned short slot_use_count; -+ unsigned short message_quota; -+ unsigned short message_use_count; -+ struct semaphore quota_event; -+ int previous_tx_index; ++ unsigned short slot_quota; ++ unsigned short slot_use_count; ++ unsigned short message_quota; ++ unsigned short message_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; ++ /* 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; ++ /* The first and last (inclusive) slots allocated to the owner. */ ++ int slot_first; ++ int slot_last; + -+ /* The slot allocated to synchronous messages from the owner. */ -+ int slot_sync; ++ /* Signalling this event indicates that owner's slot handler thread should ++ run. */ ++ REMOTE_EVENT_T trigger; + -+ /* 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; + -+ /* 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. */ -+ int tx_pos; ++ /* This event should be signalled when a slot is recycled. */ ++ REMOTE_EVENT_T recycle; + -+ /* 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; + -+ /* The slot_queue index where the next recycled slot will be written. */ -+ int slot_queue_recycle; ++ /* A circular buffer of slot indexes. */ ++ int slot_queue[VCHIQ_MAX_SLOTS_PER_SIDE]; + -+ /* This event should be signalled when a synchronous message is sent. */ -+ REMOTE_EVENT_T sync_trigger; -+ -+ /* This event should be signalled when a synchronous message has been -+ ** released. */ -+ REMOTE_EVENT_T sync_release; -+ -+ /* A circular buffer of slot indexes. */ -+ int slot_queue[VCHIQ_MAX_SLOTS_PER_SIDE]; -+ -+ /* Debugging state */ -+ int debug[DEBUG_MAX]; ++ /* 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]; ++ 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; ++ 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; ++ VCHIQ_SHARED_STATE_T *local; ++ VCHIQ_SHARED_STATE_T *remote; ++ VCHIQ_SLOT_T *slot_data; + -+ unsigned short default_slot_quota; -+ unsigned short default_message_quota; ++ unsigned short default_slot_quota; ++ unsigned short default_message_quota; + -+ /* Event indicating connect message received */ -+ struct semaphore connect; ++ VCOS_EVENT_T connect; // event indicating connect message received ++ VCOS_MUTEX_T mutex; // mutex protecting services ++ VCHIQ_INSTANCE_T *instance; + -+ /* Mutex protecting services */ -+ struct mutex mutex; -+ VCHIQ_INSTANCE_T *instance; ++ VCOS_THREAD_T slot_handler_thread; // processes incoming messages ++ VCOS_THREAD_T recycle_thread; // processes recycled slots + -+ /* Processes incoming messages */ -+ struct task_struct *slot_handler_thread; ++ /* Local implementation of the trigger remote event */ ++ VCOS_EVENT_T trigger_event; + -+ /* Processes recycled slots */ -+ struct task_struct *recycle_thread; ++ /* Local implementation of the recycle remote event */ ++ VCOS_EVENT_T recycle_event; + -+ /* Processes synchronous messages */ -+ struct task_struct *sync_thread; ++ char *tx_data; ++ char *rx_data; ++ VCHIQ_SLOT_INFO_T *rx_info; + -+ /* Local implementation of the trigger remote event */ -+ struct semaphore trigger_event; ++ VCOS_MUTEX_T slot_mutex; + -+ /* Local implementation of the recycle remote event */ -+ struct semaphore recycle_event; ++ VCOS_MUTEX_T recycle_mutex; + -+ /* Local implementation of the sync trigger remote event */ -+ struct semaphore sync_trigger_event; ++ /* 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; + -+ /* Local implementation of the sync release remote event */ -+ struct semaphore sync_release_event; ++ /* A cached copy of local->tx_pos. Only write to local->tx_pos, and read ++ from remote->tx_pos. */ ++ int local_tx_pos; + -+ char *tx_data; -+ char *rx_data; -+ VCHIQ_SLOT_INFO_T *rx_info; ++ /* The slot_queue index of the slot to become available next. */ ++ int slot_queue_available; + -+ struct mutex slot_mutex; ++ /* A flag to indicate if any poll has been requested */ ++ int poll_needed; + -+ struct mutex recycle_mutex; ++ /* An array of bit sets indicating which services must be polled. */ ++ VCOS_ATOMIC_FLAGS_T poll_services[BITSET_SIZE(VCHIQ_MAX_SERVICES)]; + -+ struct mutex sync_mutex; ++ /* The number of the first unused service */ ++ int unused_service; + -+ struct mutex bulk_transfer_mutex; ++ /* Signalled when a free slot becomes available. */ ++ VCOS_EVENT_T slot_available_event; + -+ /* 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; ++ VCOS_EVENT_T slot_remove_event; + -+ /* A cached copy of local->tx_pos. Only write to local->tx_pos, and read -+ from remote->tx_pos. */ -+ int local_tx_pos; ++ struct state_stats_struct ++ { ++ int slot_stalls; ++ int ctrl_tx_count; ++ int ctrl_rx_count; ++ int error_count; ++ } stats; + -+ /* The slot_queue index of the slot to become available next. */ -+ int slot_queue_available; ++ 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]; + -+ /* A flag to indicate if any poll has been requested */ -+ int poll_needed; -+ -+ /* Ths index of the previous slot used for data messages. */ -+ int previous_data_index; -+ -+ /* The number of slots occupied by data messages. */ -+ unsigned short data_use_count; -+ -+ /* The maximum number of slots to be occupied by data messages. */ -+ unsigned short data_quota; -+ -+ /* An array of bit sets indicating which services must be polled. */ -+ atomic_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. */ -+ struct semaphore slot_available_event; -+ -+ struct semaphore slot_remove_event; -+ -+ /* Signalled when a free data slot becomes available. */ -+ struct semaphore data_quota_event; -+ -+ struct state_stats_struct { -+ int slot_stalls; -+ int data_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]; -+ -+ VCHIQ_PLATFORM_STATE_T platform_state; ++ VCHIQ_PLATFORM_STATE_T platform_state; +}; + -+struct bulk_waiter { -+ VCHIQ_BULK_T *bulk; -+ struct semaphore event; -+ int actual; -+}; -+ -+extern spinlock_t bulk_waiter_spinlock; -+ -+extern int vchiq_core_log_level; -+extern int vchiq_core_msg_log_level; -+extern int vchiq_sync_log_level; -+ -+extern VCHIQ_STATE_T *vchiq_states[VCHIQ_MAX_STATES]; -+ -+extern const char * -+get_conn_state_name(VCHIQ_CONNSTATE_T conn_state); -+ +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); ++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); ++ 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); @@ -17784,9 +15662,9 @@ index 0000000..bcdff3a +remote_event_pollall(VCHIQ_STATE_T *state); + +extern VCHIQ_STATUS_T -+vchiq_bulk_transfer(VCHIQ_SERVICE_HANDLE_T handle, -+ VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata, -+ VCHIQ_BULK_MODE_T mode, VCHIQ_BULK_DIR_T dir); ++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); @@ -17794,52 +15672,12 @@ index 0000000..bcdff3a +extern void +vchiq_dump_service_state(void *dump_context, VCHIQ_SERVICE_T *service); + -+extern void -+vchiq_loud_error_header(void); -+ -+extern void -+vchiq_loud_error_footer(void); -+ -+extern void -+request_poll(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service, int poll_type); -+ -+static inline VCHIQ_SERVICE_T * -+handle_to_service(VCHIQ_SERVICE_HANDLE_T handle) -+{ -+ VCHIQ_STATE_T *state = vchiq_states[(handle / VCHIQ_MAX_SERVICES) & -+ (VCHIQ_MAX_STATES - 1)]; -+ if (!state) -+ return NULL; -+ -+ return state->services[handle & (VCHIQ_MAX_SERVICES - 1)]; -+} -+ -+extern VCHIQ_SERVICE_T * -+find_service_by_handle(VCHIQ_SERVICE_HANDLE_T handle); -+ -+extern VCHIQ_SERVICE_T * -+find_service_by_port(VCHIQ_STATE_T *state, int localport); -+ -+extern VCHIQ_SERVICE_T * -+find_service_for_instance(VCHIQ_INSTANCE_T instance, -+ VCHIQ_SERVICE_HANDLE_T handle); -+ -+extern VCHIQ_SERVICE_T * -+next_service_by_instance(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance, -+ int *pidx); -+ -+extern void -+lock_service(VCHIQ_SERVICE_T *service); -+ -+extern void -+unlock_service(VCHIQ_SERVICE_T *service); -+ +/* The following functions are called from vchiq_core, and external -+** implementations must be provided. */ ++ 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); ++ VCHI_MEM_HANDLE_T memhandle, void *offset, int size, int dir); + +extern void +vchiq_transfer_bulk(VCHIQ_BULK_T *bulk); @@ -17853,15 +15691,9 @@ index 0000000..bcdff3a +extern void +remote_event_signal(REMOTE_EVENT_T *event); + -+void -+vchiq_platform_check_suspend(VCHIQ_STATE_T *state); -+ +extern void +vchiq_platform_paused(VCHIQ_STATE_T *state); + -+extern VCHIQ_STATUS_T -+vchiq_platform_resume(VCHIQ_STATE_T *state); -+ +extern void +vchiq_platform_resumed(VCHIQ_STATE_T *state); + @@ -17876,7 +15708,7 @@ index 0000000..bcdff3a + +extern void +vchiq_dump_platform_service_state(void *dump_context, -+ VCHIQ_SERVICE_T *service); ++ VCHIQ_SERVICE_T *service); + +extern VCHIQ_STATUS_T +vchiq_use_service_internal(VCHIQ_SERVICE_T *service); @@ -17884,138 +15716,37 @@ index 0000000..bcdff3a +extern VCHIQ_STATUS_T +vchiq_release_service_internal(VCHIQ_SERVICE_T *service); + -+extern void ++extern VCHIQ_STATUS_T +vchiq_on_remote_use(VCHIQ_STATE_T *state); + -+extern void ++extern VCHIQ_STATUS_T +vchiq_on_remote_release(VCHIQ_STATE_T *state); + +extern VCHIQ_STATUS_T +vchiq_platform_init_state(VCHIQ_STATE_T *state); + -+extern VCHIQ_STATUS_T -+vchiq_check_service(VCHIQ_SERVICE_T *service); -+ +extern void +vchiq_on_remote_use_active(VCHIQ_STATE_T *state); + +extern VCHIQ_STATUS_T -+vchiq_send_remote_use(VCHIQ_STATE_T *state); ++vchiq_send_remote_use(VCHIQ_STATE_T * state); + +extern VCHIQ_STATUS_T -+vchiq_send_remote_release(VCHIQ_STATE_T *state); ++vchiq_send_remote_release(VCHIQ_STATE_T * state); + +extern VCHIQ_STATUS_T -+vchiq_send_remote_use_active(VCHIQ_STATE_T *state); ++vchiq_send_remote_use_active(VCHIQ_STATE_T * state); + +extern void -+vchiq_platform_conn_state_changed(VCHIQ_STATE_T *state, -+ VCHIQ_CONNSTATE_T oldstate, VCHIQ_CONNSTATE_T newstate); -+ -+extern void -+vchiq_log_dump_mem(const char *label, uint32_t addr, const void *voidMem, -+ size_t numBytes); ++vchiq_platform_conn_state_changed(VCHIQ_STATE_T* state, VCHIQ_CONNSTATE_T oldstate, VCHIQ_CONNSTATE_T newstate); + +#endif -diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_genversion b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_genversion -new file mode 100644 -index 0000000..9f5b634 ---- /dev/null -+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_genversion -@@ -0,0 +1,87 @@ -+#!/usr/bin/perl -w -+ -+use strict; -+ -+# -+# Generate a version from available information -+# -+ -+my $prefix = shift @ARGV; -+my $root = shift @ARGV; -+ -+ -+if ( not defined $root ) { -+ die "usage: $0 prefix root-dir\n"; -+} -+ -+if ( ! -d $root ) { -+ die "root directory $root not found\n"; -+} -+ -+my $version = "unknown"; -+my $tainted = ""; -+ -+if ( -d "$root/.git" ) { -+ # attempt to work out git version. only do so -+ # on a linux build host, as cygwin builds are -+ # already slow enough -+ -+ if ( -f "/usr/bin/git" || -f "/usr/local/bin/git" ) { -+ if (not open(F, "git --git-dir $root/.git rev-parse --verify HEAD|")) { -+ $version = "no git version"; -+ } -+ else { -+ $version = ; -+ $version =~ s/[ \r\n]*$//; # chomp may not be enough (cygwin). -+ $version =~ s/^[ \r\n]*//; # chomp may not be enough (cygwin). -+ } -+ -+ if (open(G, "git --git-dir $root/.git status --porcelain|")) { -+ $tainted = ; -+ $tainted =~ s/[ \r\n]*$//; # chomp may not be enough (cygwin). -+ $tainted =~ s/^[ \r\n]*//; # chomp may not be enough (cygwin). -+ if (length $tainted) { -+ $version = join ' ', $version, "(tainted)"; -+ } -+ else { -+ $version = join ' ', $version, "(clean)"; -+ } -+ } -+ } -+} -+ -+my $hostname = `hostname`; -+$hostname =~ s/[ \r\n]*$//; # chomp may not be enough (cygwin). -+$hostname =~ s/^[ \r\n]*//; # chomp may not be enough (cygwin). -+ -+ -+print STDERR "Version $version\n"; -+print < -+ -+VC_DEBUG_DECLARE_STRING_VAR( ${prefix}_build_hostname, "$hostname" ); -+VC_DEBUG_DECLARE_STRING_VAR( ${prefix}_build_version, "$version" ); -+VC_DEBUG_DECLARE_STRING_VAR( ${prefix}_build_time, __TIME__ ); -+VC_DEBUG_DECLARE_STRING_VAR( ${prefix}_build_date, __DATE__ ); -+ -+const char *vchiq_get_build_hostname( void ) -+{ -+ return vchiq_build_hostname; -+} -+ -+const char *vchiq_get_build_version( void ) -+{ -+ return vchiq_build_version; -+} -+ -+const char *vchiq_get_build_date( void ) -+{ -+ return vchiq_build_date; -+} -+ -+const char *vchiq_get_build_time( void ) -+{ -+ return vchiq_build_time; -+} -+EOF 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..d89f89e +index 0000000..42d471b --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_if.h -@@ -0,0 +1,165 @@ +@@ -0,0 +1,155 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -18039,154 +15770,144 @@ index 0000000..d89f89e + +#include "interface/vchi/vchi_mh.h" + -+#define VCHIQ_SERVICE_HANDLE_INVALID 0 ++#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_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) vchiq_get_service_userdata(service) -+#define VCHIQ_GET_SERVICE_FOURCC(service) vchiq_get_service_fourcc(service) ++#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_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 ++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_WAITING /* Reserved for internal use */ ++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_SLOT_QUOTA, -+ VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA, -+ VCHIQ_SERVICE_OPTION_SYNCHRONOUS ++typedef enum ++{ ++ VCHIQ_SERVICE_OPTION_AUTOCLOSE, ++ VCHIQ_SERVICE_OPTION_SLOT_QUOTA, ++ VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA +} 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; ++ /* The message identifier - opaque to applications. */ ++ int msgid; + -+ /* Size of message data. */ -+ unsigned int size; ++ /* Size of message data. */ ++ unsigned int size; + -+ char data[0]; /* message */ ++ char data[0]; /* message */ +} VCHIQ_HEADER_T; + ++#ifdef __HIGHC__ ++#pragma warning (pop) ++#endif ++ +typedef struct { -+ const void *data; -+ int size; ++ const void *data; ++ int size; +} VCHIQ_ELEMENT_T; + -+typedef unsigned int VCHIQ_SERVICE_HANDLE_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 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; ++ 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 */ ++ 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 */ ++ 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; -+typedef void (*VCHIQ_REMOTE_USE_CALLBACK_T)(void *cb_arg); ++typedef void (*VCHIQ_REMOTE_USE_CALLBACK_T)(void* cb_arg); ++ + +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, -+ const VCHIQ_SERVICE_PARAMS_T *params, -+ VCHIQ_SERVICE_HANDLE_T *pservice); -+extern VCHIQ_STATUS_T vchiq_open_service(VCHIQ_INSTANCE_T instance, -+ const VCHIQ_SERVICE_PARAMS_T *params, -+ VCHIQ_SERVICE_HANDLE_T *pservice); ++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 void *vchiq_get_service_userdata(VCHIQ_SERVICE_HANDLE_T service); -+extern int vchiq_get_service_fourcc(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_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_remote_use(VCHIQ_INSTANCE_T instance, -+ VCHIQ_REMOTE_USE_CALLBACK_T callback, void *cb_arg); ++extern VCHIQ_STATUS_T vchiq_remote_use(VCHIQ_INSTANCE_T instance, VCHIQ_REMOTE_USE_CALLBACK_T callback, void* cb_arg); +extern VCHIQ_STATUS_T vchiq_remote_release(VCHIQ_INSTANCE_T instance); + -+extern VCHIQ_STATUS_T vchiq_dump_phys_mem(VCHIQ_SERVICE_HANDLE_T service, -+ void *ptr, size_t num_bytes); ++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..9bcacd2 +index 0000000..06298bf --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_ioctl.h -@@ -0,0 +1,114 @@ +@@ -0,0 +1,105 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -18215,98 +15936,89 @@ index 0000000..9bcacd2 +#define VCHIQ_INVALID_HANDLE -1 + +typedef struct { -+ VCHIQ_SERVICE_PARAMS_T params; -+ int is_open; -+ int is_vchi; -+ int handle; /* OUT */ ++ 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; ++ 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; ++ 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_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; ++ 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; ++ int handle; ++ int blocking; ++ int bufsize; ++ void *buf; +} VCHIQ_DEQUEUE_MESSAGE_T; + +typedef struct { -+ int config_size; -+ VCHIQ_CONFIG_T *pconfig; ++ int config_size; ++ VCHIQ_CONFIG_T *pconfig; +} VCHIQ_GET_CONFIG_T; + +typedef struct { -+ int handle; -+ VCHIQ_SERVICE_OPTION_T option; -+ int value; ++ int handle; ++ VCHIQ_SERVICE_OPTION_T option; ++ int value; +} VCHIQ_SET_SERVICE_OPTION_T; + +typedef struct { -+ void *virt_addr; -+ size_t num_bytes; ++ 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_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 \ -+ _IOWR(VCHIQ_IOC_MAGIC, 5, VCHIQ_QUEUE_BULK_TRANSFER_T) -+#define VCHIQ_IOC_QUEUE_BULK_RECEIVE \ -+ _IOWR(VCHIQ_IOC_MAGIC, 6, VCHIQ_QUEUE_BULK_TRANSFER_T) -+#define VCHIQ_IOC_AWAIT_COMPLETION \ -+ _IOWR(VCHIQ_IOC_MAGIC, 7, VCHIQ_AWAIT_COMPLETION_T) -+#define VCHIQ_IOC_DEQUEUE_MESSAGE \ -+ _IOWR(VCHIQ_IOC_MAGIC, 8, VCHIQ_DEQUEUE_MESSAGE_T) ++#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 _IOWR(VCHIQ_IOC_MAGIC, 7, VCHIQ_AWAIT_COMPLETION_T) ++#define VCHIQ_IOC_DEQUEUE_MESSAGE _IOWR(VCHIQ_IOC_MAGIC, 8, VCHIQ_DEQUEUE_MESSAGE_T) +#define VCHIQ_IOC_GET_CLIENT_ID _IO(VCHIQ_IOC_MAGIC, 9) -+#define VCHIQ_IOC_GET_CONFIG \ -+ _IOWR(VCHIQ_IOC_MAGIC, 10, VCHIQ_GET_CONFIG_T) ++#define VCHIQ_IOC_GET_CONFIG _IOWR(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_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..2855873 +index 0000000..e1768e4 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c -@@ -0,0 +1,241 @@ +@@ -0,0 +1,297 @@ +/***************************************************************************** +* Copyright 2001 - 2011 Broadcom Corporation. All rights reserved. +* @@ -18325,19 +16037,22 @@ index 0000000..2855873 + +#include +#include -+#include + +#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; ++ VCHIQ_STATE_T *state; + -+ int connected; ++ int connected; +}; + +/**************************************************************************** @@ -18346,42 +16061,40 @@ index 0000000..2855873 +* +***************************************************************************/ + -+VCHIQ_STATUS_T vchiq_initialise(VCHIQ_INSTANCE_T *instanceOut) ++VCHIQ_STATUS_T vchiq_initialise( VCHIQ_INSTANCE_T *instanceOut ) +{ -+ VCHIQ_STATUS_T status = VCHIQ_ERROR; -+ VCHIQ_STATE_T *state; -+ VCHIQ_INSTANCE_T instance = NULL; ++ VCHIQ_STATUS_T status = VCHIQ_ERROR; ++ VCHIQ_STATE_T *state; ++ VCHIQ_INSTANCE_T instance = NULL; + -+ vchiq_log_trace(vchiq_core_log_level, "%s called", __func__); ++ vcos_log_trace( "%s called", __func__ ); + -+ state = vchiq_get_state(); -+ if (!state) { -+ vchiq_log_error(vchiq_core_log_level, -+ "%s: videocore not initialized\n", __func__); -+ goto failed; -+ } ++ 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) { -+ vchiq_log_error(vchiq_core_log_level, -+ "%s: error allocating vchiq instance\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; ++ instance->connected = 0; ++ instance->state = state; + -+ *instanceOut = instance; -+ -+ status = VCHIQ_SUCCESS; ++ *instanceOut = instance; ++ ++ status = VCHIQ_SUCCESS; + +failed: -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p): returning %d", __func__, instance, status); ++ vcos_log_trace( "%s(%p): returning %d", __func__, instance, status ); + -+ return status; ++ return status; +} -+EXPORT_SYMBOL(vchiq_initialise); + +/**************************************************************************** +* @@ -18389,31 +16102,27 @@ index 0000000..2855873 +* +***************************************************************************/ + -+VCHIQ_STATUS_T vchiq_shutdown(VCHIQ_INSTANCE_T instance) ++VCHIQ_STATUS_T vchiq_shutdown( VCHIQ_INSTANCE_T instance ) +{ -+ VCHIQ_STATUS_T status; -+ VCHIQ_STATE_T *state = instance->state; ++ VCHIQ_STATUS_T status; ++ VCHIQ_STATE_T *state = instance->state; + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p) called", __func__, instance); ++ vcos_log_trace( "%s(%p) called", __func__, instance ); + -+ if (mutex_lock_interruptible(&state->mutex) != 0) -+ return VCHIQ_RETRY; ++ vcos_mutex_lock(&state->mutex); + -+ /* Remove all services */ -+ status = vchiq_shutdown_internal(state, instance); ++ /* Remove all services */ ++ status = vchiq_shutdown_internal(state, instance); + -+ mutex_unlock(&state->mutex); ++ vcos_mutex_unlock(&state->mutex); + -+ if (status == VCHIQ_SUCCESS) -+ kfree(instance); ++ if (status == VCHIQ_SUCCESS) ++ kfree(instance); + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p): returning %d", __func__, instance, status); ++ vcos_log_trace( "%s(%p): returning %d", __func__, instance, status ); + -+ return status; ++ return status; +} -+EXPORT_SYMBOL(vchiq_shutdown); + +/**************************************************************************** +* @@ -18423,7 +16132,7 @@ index 0000000..2855873 + +int vchiq_is_connected(VCHIQ_INSTANCE_T instance) +{ -+ return instance->connected; ++ return instance->connected; +} + +/**************************************************************************** @@ -18434,32 +16143,28 @@ index 0000000..2855873 + +VCHIQ_STATUS_T vchiq_connect(VCHIQ_INSTANCE_T instance) +{ -+ VCHIQ_STATUS_T status; -+ VCHIQ_STATE_T *state = instance->state; ++ VCHIQ_STATUS_T status; ++ VCHIQ_STATE_T *state = instance->state; + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p) called", __func__, instance); ++ vcos_log_trace( "%s(%p) called", __func__, instance ); + -+ if (mutex_lock_interruptible(&state->mutex) != 0) { -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s: call to mutex_lock failed", __func__); -+ status = VCHIQ_RETRY; -+ goto failed; -+ } -+ status = vchiq_connect_internal(state, 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; ++ if (status == VCHIQ_SUCCESS) ++ instance->connected = 1; + -+ mutex_unlock(&state->mutex); ++ vcos_mutex_unlock(&state->mutex); + +failed: -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p): returning %d", __func__, instance, status); ++ vcos_log_trace( "%s(%p): returning %d", __func__, instance, status ); + -+ return status; ++ return status; +} -+EXPORT_SYMBOL(vchiq_connect); + +/**************************************************************************** +* @@ -18468,42 +16173,22 @@ index 0000000..2855873 +***************************************************************************/ + +VCHIQ_STATUS_T vchiq_add_service( -+ VCHIQ_INSTANCE_T instance, -+ const VCHIQ_SERVICE_PARAMS_T *params, -+ VCHIQ_SERVICE_HANDLE_T *phandle) ++ VCHIQ_INSTANCE_T instance, ++ int fourcc, ++ VCHIQ_CALLBACK_T callback, ++ void *userdata, ++ VCHIQ_SERVICE_HANDLE_T *pservice) +{ -+ VCHIQ_STATUS_T status; -+ VCHIQ_STATE_T *state = instance->state; -+ VCHIQ_SERVICE_T *service = NULL; -+ int srvstate; ++ VCHIQ_SERVICE_PARAMS_T params; + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p) called", __func__, instance); ++ params.fourcc = fourcc; ++ params.callback = callback; ++ params.userdata = userdata; ++ params.version = 0; ++ params.version_min = 0; + -+ *phandle = VCHIQ_SERVICE_HANDLE_INVALID; -+ -+ srvstate = vchiq_is_connected(instance) -+ ? VCHIQ_SRVSTATE_LISTENING -+ : VCHIQ_SRVSTATE_HIDDEN; -+ -+ service = vchiq_add_service_internal( -+ state, -+ params, -+ srvstate, -+ instance); -+ -+ if (service) { -+ *phandle = service->handle; -+ status = VCHIQ_SUCCESS; -+ } else -+ status = VCHIQ_ERROR; -+ -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p): returning %d", __func__, instance, status); -+ -+ return status; ++ return vchiq_add_service_params(instance, ¶ms, pservice); +} -+EXPORT_SYMBOL(vchiq_add_service); + +/**************************************************************************** +* @@ -18512,48 +16197,1765 @@ index 0000000..2855873 +***************************************************************************/ + +VCHIQ_STATUS_T vchiq_open_service( -+ VCHIQ_INSTANCE_T instance, -+ const VCHIQ_SERVICE_PARAMS_T *params, -+ VCHIQ_SERVICE_HANDLE_T *phandle) ++ VCHIQ_INSTANCE_T instance, ++ int fourcc, ++ VCHIQ_CALLBACK_T callback, ++ void *userdata, ++ VCHIQ_SERVICE_HANDLE_T *pservice) +{ -+ VCHIQ_STATUS_T status = VCHIQ_ERROR; -+ VCHIQ_STATE_T *state = instance->state; -+ VCHIQ_SERVICE_T *service = NULL; ++ VCHIQ_SERVICE_PARAMS_T params; + -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p) called", __func__, instance); ++ params.fourcc = fourcc; ++ params.callback = callback; ++ params.userdata = userdata; ++ params.version = 0; ++ params.version_min = 0; + -+ *phandle = VCHIQ_SERVICE_HANDLE_INVALID; ++ return vchiq_open_service_params(instance, ¶ms, pservice); ++} + -+ if (!vchiq_is_connected(instance)) -+ goto failed; ++/**************************************************************************** ++* ++* vchiq_add_service_params ++* ++***************************************************************************/ + -+ service = vchiq_add_service_internal(state, -+ params, -+ VCHIQ_SRVSTATE_OPENING, -+ instance); ++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; + -+ if (service) { -+ status = vchiq_open_service_internal(service, current->pid); -+ if (status == VCHIQ_SUCCESS) -+ *phandle = service->handle; -+ else -+ vchiq_remove_service(service->handle); -+ } ++ 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: -+ vchiq_log_trace(vchiq_core_log_level, -+ "%s(%p): returning %d", __func__, instance, status); ++ vcos_log_trace( "%s(%p): returning %d", __func__, instance, status ); + -+ return 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..f1a8f42 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_lib.c +@@ -0,0 +1,1628 @@ ++/* ++ * 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 ++#include ++#include ++#include ++ ++#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); ++} ++ ++static __inline int ++is_valid_service(VCHIQ_SERVICE_T *service) ++{ ++ return ((service != NULL) && (service->fd != VCHIQ_INVALID_HANDLE)); ++} ++ ++/* ++ * 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, ¶ms, 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, ¶ms, 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 ); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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 ); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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 ); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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 ); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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 ); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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 ); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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 ); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ /* 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); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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 (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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); ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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(¶ms, 0, sizeof(params)); ++ params.fourcc = setup->service_id; ++ params.userdata = setup->callback_param; ++ ++ status = create_service((VCHIQ_INSTANCE_T)instance_handle, ++ ¶ms, ++ 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(¶ms, 0, sizeof(params)); ++ params.fourcc = setup->service_id; ++ params.userdata = setup->callback_param; ++ ++ status = create_service((VCHIQ_INSTANCE_T)instance_handle, ++ ¶ms, ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ 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; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ RETRY(ret,ioctl(service->fd, VCHIQ_IOC_RELEASE_SERVICE, service->handle)); ++ return ret; ++} ++ ++/*********************************************************** ++ * Name: vchiq_dump_phys_mem ++ * ++ * Arguments: const VCHI_SERVICE_HANDLE_T handle ++ * void *buffer ++ * size_t num_bytes ++ * ++ * Description: Dumps the physical memory associated with ++ * a buffer. ++ * ++ * Returns: void ++ * ++ ***********************************************************/ ++VCHIQ_STATUS_T vchiq_dump_phys_mem( VCHIQ_SERVICE_HANDLE_T handle, ++ void *ptr, ++ size_t num_bytes ) ++{ ++ VCHIQ_SERVICE_T *service = (VCHIQ_SERVICE_T *)handle; ++ VCHIQ_DUMP_MEM_T dump_mem; ++ int ret; ++ ++ if (!is_valid_service(service)) ++ return VCHIQ_ERROR; ++ ++ dump_mem.virt_addr = ptr; ++ dump_mem.num_bytes = num_bytes; ++ ++ RETRY(ret,ioctl(service->fd, VCHIQ_IOC_DUMP_PHYS_MEM, &dump_mem)); ++ return (ret >= 0) ? VCHIQ_SUCCESS : VCHIQ_ERROR; ++} ++ ++ ++ ++/* ++ * 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..e53ee0e +index 0000000..a465b91 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_memdrv.h -@@ -0,0 +1,52 @@ +@@ -0,0 +1,45 @@ +/***************************************************************************** +* Copyright 2001 - 2010 Broadcom Corporation. All rights reserved. +* @@ -18578,33 +17980,26 @@ index 0000000..e53ee0e + +/* ---- Constants and Types ---------------------------------------------- */ + -+typedef struct { -+ void *armSharedMemVirt; -+ dma_addr_t armSharedMemPhys; -+ size_t armSharedMemSize; ++typedef struct ++{ ++ void *armSharedMemVirt; ++ dma_addr_t armSharedMemPhys; ++ size_t armSharedMemSize; ++ ++ void *vcSharedMemVirt; ++ dma_addr_t vcSharedMemPhys; ++ size_t vcSharedMemSize; + -+ 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); ++void vchiq_get_shared_mem_info( VCHIQ_SHARED_MEM_INFO_T *info ); + +VCHIQ_STATUS_T vchiq_memdrv_initialise(void); + -+VCHIQ_STATUS_T vchiq_userdrv_create_instance( -+ const VCHIQ_PLATFORM_DATA_T * platform_data); -+ -+VCHIQ_STATUS_T vchiq_userdrv_suspend( -+ const VCHIQ_PLATFORM_DATA_T * platform_data); -+ -+VCHIQ_STATUS_T vchiq_userdrv_resume( -+ const VCHIQ_PLATFORM_DATA_T * platform_data); -+ +#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 @@ -18655,241 +18050,12 @@ index 0000000..e8d4e30 +} FRAGMENTS_T; + +#endif /* VCHIQ_PAGELIST_H */ -diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_proc.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_proc.c -new file mode 100644 -index 0000000..6e06c3b ---- /dev/null -+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_proc.c -@@ -0,0 +1,223 @@ -+/* -+ * Copyright (c) 2012 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 -+#include "vchiq_core.h" -+#include "vchiq_arm.h" -+ -+struct vchiq_proc_info { -+ /* Global 'vc' proc entry used by all instances */ -+ struct proc_dir_entry *vc_cfg_dir; -+ -+ /* one entry per client process */ -+ struct proc_dir_entry *clients; -+ -+ /* log categories */ -+ struct proc_dir_entry *log_categories; -+}; -+ -+static struct vchiq_proc_info proc_info; -+ -+struct proc_dir_entry *vchiq_proc_top(void) -+{ -+ BUG_ON(proc_info.vc_cfg_dir == NULL); -+ return proc_info.vc_cfg_dir; -+} -+ -+/**************************************************************************** -+* -+* log category entries -+* -+***************************************************************************/ -+#define PROC_WRITE_BUF_SIZE 256 -+ -+#define VCHIQ_LOG_ERROR_STR "error" -+#define VCHIQ_LOG_WARNING_STR "warning" -+#define VCHIQ_LOG_INFO_STR "info" -+#define VCHIQ_LOG_TRACE_STR "trace" -+ -+static int log_cfg_read(char *buffer, -+ char **start, -+ off_t off, -+ int count, -+ int *eof, -+ void *data) -+{ -+ int len = 0; -+ char *log_value = NULL; -+ -+ switch (*((int *)data)) { -+ case VCHIQ_LOG_ERROR: -+ log_value = VCHIQ_LOG_ERROR_STR; -+ break; -+ case VCHIQ_LOG_WARNING: -+ log_value = VCHIQ_LOG_WARNING_STR; -+ break; -+ case VCHIQ_LOG_INFO: -+ log_value = VCHIQ_LOG_INFO_STR; -+ break; -+ case VCHIQ_LOG_TRACE: -+ log_value = VCHIQ_LOG_TRACE_STR; -+ break; -+ default: -+ break; -+ } -+ -+ len += sprintf(buffer + len, -+ "%s\n", -+ log_value ? log_value : "(null)"); -+ -+ return len; -+} -+ -+ -+static int log_cfg_write(struct file *file, -+ const char __user *buffer, -+ unsigned long count, -+ void *data) -+{ -+ int *log_module = data; -+ char kbuf[PROC_WRITE_BUF_SIZE + 1]; -+ -+ (void)file; -+ -+ memset(kbuf, 0, PROC_WRITE_BUF_SIZE + 1); -+ if (count >= PROC_WRITE_BUF_SIZE) -+ count = PROC_WRITE_BUF_SIZE; -+ -+ if (copy_from_user(kbuf, -+ buffer, -+ count) != 0) -+ return -EFAULT; -+ kbuf[count - 1] = 0; -+ -+ if (strncmp("error", kbuf, strlen("error")) == 0) -+ *log_module = VCHIQ_LOG_ERROR; -+ else if (strncmp("warning", kbuf, strlen("warning")) == 0) -+ *log_module = VCHIQ_LOG_WARNING; -+ else if (strncmp("info", kbuf, strlen("info")) == 0) -+ *log_module = VCHIQ_LOG_INFO; -+ else if (strncmp("trace", kbuf, strlen("trace")) == 0) -+ *log_module = VCHIQ_LOG_TRACE; -+ else -+ *log_module = VCHIQ_LOG_DEFAULT; -+ -+ return count; -+} -+ -+/* Log category proc entries */ -+struct vchiq_proc_log_entry { -+ const char *name; -+ int *plevel; -+ struct proc_dir_entry *dir; -+}; -+ -+static struct vchiq_proc_log_entry vchiq_proc_log_entries[] = { -+ { "core", &vchiq_core_log_level }, -+ { "msg", &vchiq_core_msg_log_level }, -+ { "sync", &vchiq_sync_log_level }, -+ { "arm", &vchiq_arm_log_level }, -+}; -+static int n_log_entries = -+ sizeof(vchiq_proc_log_entries)/sizeof(vchiq_proc_log_entries[0]); -+ -+/* create an entry under /proc/vc/log for each log category */ -+static int vchiq_proc_create_log_entries(struct proc_dir_entry *top) -+{ -+ struct proc_dir_entry *dir; -+ size_t i; -+ int ret = 0; -+ -+ dir = proc_mkdir("log", proc_info.vc_cfg_dir); -+ if (!dir) -+ return -ENOMEM; -+ proc_info.log_categories = dir; -+ -+ for (i = 0; i < n_log_entries; i++) { -+ dir = create_proc_entry(vchiq_proc_log_entries[i].name, -+ 0644, -+ proc_info.log_categories); -+ if (!dir) { -+ ret = -ENOMEM; -+ break; -+ } -+ -+ dir->read_proc = &log_cfg_read; -+ dir->write_proc = &log_cfg_write; -+ dir->data = (void *)vchiq_proc_log_entries[i].plevel; -+ -+ vchiq_proc_log_entries[i].dir = dir; -+ } -+ return ret; -+} -+ -+ -+int vchiq_proc_init(void) -+{ -+ BUG_ON(proc_info.vc_cfg_dir != NULL); -+ -+ proc_info.vc_cfg_dir = proc_mkdir("vc", NULL); -+ if (proc_info.vc_cfg_dir == NULL) -+ goto fail; -+ -+ proc_info.clients = proc_mkdir("clients", -+ proc_info.vc_cfg_dir); -+ if (!proc_info.clients) -+ goto fail; -+ -+ if (vchiq_proc_create_log_entries(proc_info.vc_cfg_dir) != 0) -+ goto fail; -+ -+ return 0; -+ -+fail: -+ vchiq_proc_deinit(); -+ vchiq_log_error(vchiq_arm_log_level, -+ "%s: failed to create proc directory", -+ __func__); -+ -+ return -ENOMEM; -+} -+ -+/* remove all the proc entries */ -+void vchiq_proc_deinit(void) -+{ -+ /* log category entries */ -+ if (proc_info.log_categories) { -+ size_t i; -+ for (i = 0; i < n_log_entries; i++) -+ if (vchiq_proc_log_entries[i].dir) -+ remove_proc_entry( -+ vchiq_proc_log_entries[i].name, -+ proc_info.log_categories); -+ -+ remove_proc_entry(proc_info.log_categories->name, -+ proc_info.vc_cfg_dir); -+ } -+ if (proc_info.clients) -+ remove_proc_entry(proc_info.clients->name, -+ proc_info.vc_cfg_dir); -+ if (proc_info.vc_cfg_dir) -+ remove_proc_entry(proc_info.vc_cfg_dir->name, NULL); -+} -+ -+struct proc_dir_entry *vchiq_clients_top(void) -+{ -+ return proc_info.clients; -+} 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..0ed884e +index 0000000..88cd86f --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_shim.c -@@ -0,0 +1,787 @@ +@@ -0,0 +1,1001 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -18907,8 +18073,6 @@ index 0000000..0ed884e + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ -+#include -+#include + +#include "interface/vchi/vchi.h" +#include "vchiq.h" @@ -18918,42 +18082,56 @@ index 0000000..0ed884e + +#include + ++#if defined(__KERNEL__) ++#include ++#endif ++ +#define vchiq_status_to_vchi(status) ((int32_t)status) + +typedef struct { -+ VCHIQ_SERVICE_HANDLE_T handle; ++ VCHIQ_SERVICE_HANDLE_T handle; + -+ VCHIU_QUEUE_T queue; ++ VCHIU_QUEUE_T queue; + -+ VCHI_CALLBACK_T callback; -+ void *callback_param; ++ 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 * -+vchi_mphi_message_driver_func_table(void) ++mphi_get_func_table( void ) +{ -+ return NULL; ++ 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) ++single_get_func_table( void ) +{ -+ return NULL; ++ return NULL; +} + -+VCHI_CONNECTION_T *vchi_create_connection( -+ const VCHI_CONNECTION_API_T *function_table, -+ const VCHI_MESSAGE_DRIVER_T *low_level) ++VCHI_CONNECTION_T * vchi_create_connection( const VCHI_CONNECTION_API_T * function_table, ++ const VCHI_MESSAGE_DRIVER_T * low_level) +{ -+ (void)function_table; -+ (void)low_level; -+ return NULL; ++ vcos_unused(function_table); ++ vcos_unused(low_level); ++ return NULL; +} + +/*********************************************************** @@ -18962,64 +18140,57 @@ index 0000000..0ed884e + * 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 ++ * 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) ++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; ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; ++ VCHIQ_HEADER_T *header; + -+ WARN_ON((flags != VCHI_FLAGS_NONE) && -+ (flags != VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE)); ++ 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; ++ if (flags == VCHI_FLAGS_NONE) ++ if (vchiu_queue_is_empty(&service->queue)) ++ return -1; + -+ header = vchiu_queue_peek(&service->queue); ++ header = vchiu_queue_peek(&service->queue); + -+ *data = header->data; -+ *msg_size = header->size; ++ *data = header->data; ++ *msg_size = header->size; + -+ return 0; ++ return 0; +} -+EXPORT_SYMBOL(vchi_msg_peek); + +/*********************************************************** + * 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) ++ * 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) ++int32_t vchi_msg_remove( VCHI_SERVICE_HANDLE_T handle ) +{ -+ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; -+ VCHIQ_HEADER_T *header; ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; ++ VCHIQ_HEADER_T *header; + -+ header = vchiu_queue_pop(&service->queue); ++ header = vchiu_queue_pop(&service->queue); + -+ vchiq_release_message(service->handle, header); ++ vchiq_release_message(service->handle, header); + -+ return 0; ++ return 0; +} -+EXPORT_SYMBOL(vchi_msg_remove); + +/*********************************************************** + * Name: vchi_msg_queue @@ -19035,34 +18206,33 @@ index 0000000..0ed884e + * 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) ++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; ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; ++ VCHIQ_ELEMENT_T element = {data, data_size}; ++ VCHIQ_STATUS_T status; + -+ (void)msg_handle; ++ vcos_unused(msg_handle); + -+ WARN_ON(flags != VCHI_FLAGS_BLOCK_UNTIL_QUEUED); ++ vcos_assert(flags == VCHI_FLAGS_BLOCK_UNTIL_QUEUED); + -+ status = vchiq_queue_message(service->handle, &element, 1); ++ status = vchiq_queue_message(service->handle, &element, 1); + -+ /* vchiq_queue_message() may return VCHIQ_RETRY, so we need to -+ ** implement a retry mechanism since this function is supposed -+ ** to block until queued -+ */ -+ while (status == VCHIQ_RETRY) { -+ msleep(1); -+ 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); ++ return vchiq_status_to_vchi(status); +} -+EXPORT_SYMBOL(vchi_msg_queue); + +/*********************************************************** + * Name: vchi_bulk_queue_receive @@ -19078,50 +18248,107 @@ index 0000000..0ed884e + * 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) ++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; ++ 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: -+ WARN_ON(!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: -+ WARN(1, "unsupported message\n"); -+ return vchiq_status_to_vchi(VCHIQ_ERROR); -+ } ++ 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); ++ status = vchiq_bulk_receive(service->handle, data_dst, data_size, ++ bulk_handle, mode); + -+ /* vchiq_bulk_receive() may return VCHIQ_RETRY, so we need to -+ ** implement a retry mechanism since this function is supposed -+ ** to block until queued -+ */ -+ while (status == VCHIQ_RETRY) { -+ msleep(1); -+ 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); ++ 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); +} -+EXPORT_SYMBOL(vchi_bulk_queue_receive); + +/*********************************************************** + * Name: vchi_bulk_queue_transmit @@ -19137,51 +18364,110 @@ index 0000000..0ed884e + * 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) ++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; ++ 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: -+ WARN_ON(!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: -+ WARN(1, "unsupported message\n"); -+ return vchiq_status_to_vchi(VCHIQ_ERROR); -+ } ++ 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); ++ status = vchiq_bulk_transmit(service->handle, data_src, data_size, ++ bulk_handle, mode); + -+ /* vchiq_bulk_transmit() may return VCHIQ_RETRY, so we need to -+ ** implement a retry mechanism since this function is supposed -+ ** to block until queued -+ */ -+ while (status == VCHIQ_RETRY) { -+ msleep(1); -+ 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); ++ 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); +} -+EXPORT_SYMBOL(vchi_bulk_queue_transmit); + +/*********************************************************** + * Name: vchi_msg_dequeue @@ -19197,41 +18483,38 @@ index 0000000..0ed884e + * 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) ++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; ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; ++ VCHIQ_HEADER_T *header; + -+ WARN_ON((flags != VCHI_FLAGS_NONE) && -+ (flags != VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE)); ++ 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; ++ if (flags == VCHI_FLAGS_NONE) ++ if (vchiu_queue_is_empty(&service->queue)) ++ return -1; + -+ header = vchiu_queue_pop(&service->queue); ++ header = vchiu_queue_pop(&service->queue); + -+ memcpy(data, header->data, header->size < max_data_size_to_read ? -+ header->size : max_data_size_to_read); ++ memcpy(data, header->data, header->size < max_data_size_to_read ? header->size : max_data_size_to_read); + -+ *actual_msg_size = header->size; ++ *actual_msg_size = header->size; + -+ vchiq_release_message(service->handle, header); ++ vchiq_release_message(service->handle, header); + -+ return 0; ++ return 0; +} -+EXPORT_SYMBOL(vchi_msg_dequeue); + +/*********************************************************** + * Name: vchi_msg_queuev + * + * Arguments: VCHI_SERVICE_HANDLE_T handle, -+ * VCHI_MSG_VECTOR_T *vector, -+ * uint32_t count, ++ * const void *data, ++ * uint32_t data_size, + * VCHI_FLAGS_T flags, + * void *msg_handle + * @@ -19241,46 +18524,151 @@ index 0000000..0ed884e + * + ***********************************************************/ + -+vchiq_static_assert(sizeof(VCHI_MSG_VECTOR_T) == sizeof(VCHIQ_ELEMENT_T)); -+vchiq_static_assert(offsetof(VCHI_MSG_VECTOR_T, vec_base) == -+ offsetof(VCHIQ_ELEMENT_T, data)); -+vchiq_static_assert(offsetof(VCHI_MSG_VECTOR_T, vec_len) == -+ offsetof(VCHIQ_ELEMENT_T, size)); ++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) ++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; ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; + -+ (void)msg_handle; ++ vcos_unused(msg_handle); + -+ WARN_ON(flags != VCHI_FLAGS_BLOCK_UNTIL_QUEUED); ++ vcos_assert(flags == VCHI_FLAGS_BLOCK_UNTIL_QUEUED); + -+ return vchiq_status_to_vchi(vchiq_queue_message(service->handle, -+ (const VCHIQ_ELEMENT_T *)vector, count)); ++ return vchiq_status_to_vchi(vchiq_queue_message(service->handle, (const VCHIQ_ELEMENT_T *)vector, count)); +} -+EXPORT_SYMBOL(vchi_msg_queuev); ++ ++#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; iu.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; iu.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) ++ * 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) ++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); ++ vchiq_release_message((VCHIQ_SERVICE_HANDLE_T)message->service, (VCHIQ_HEADER_T *)message->message); + -+ return 0; ++ return 0; +} + +/*********************************************************** @@ -19292,40 +18680,37 @@ index 0000000..0ed884e + * 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. ++ * 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) ++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; ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; ++ VCHIQ_HEADER_T *header; + -+ WARN_ON((flags != VCHI_FLAGS_NONE) && -+ (flags != VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE)); ++ 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; ++ if (flags == VCHI_FLAGS_NONE) ++ if (vchiu_queue_is_empty(&service->queue)) ++ return -1; + -+ header = vchiu_queue_pop(&service->queue); ++ header = vchiu_queue_pop(&service->queue); + -+ *data = header->data; -+ *msg_size = header->size; ++ *data = header->data; ++ *msg_size = header->size; + -+ message_handle->service = -+ (struct opaque_vchi_service_t *)service->handle; -+ message_handle->message = header; ++ message_handle->service = (struct opaque_vchi_service_t *)service->handle; ++ message_handle->message = header; + -+ return 0; ++ return 0; +} + +/*********************************************************** @@ -19343,25 +18728,24 @@ index 0000000..0ed884e + * + ***********************************************************/ + -+int32_t vchi_initialise(VCHI_INSTANCE_T *instance_handle) ++int32_t vchi_initialise( VCHI_INSTANCE_T *instance_handle ) +{ -+ VCHIQ_INSTANCE_T instance; -+ VCHIQ_STATUS_T status; ++ VCHIQ_INSTANCE_T instance; ++ VCHIQ_STATUS_T status; + -+ status = vchiq_initialise(&instance); ++ status = vchiq_initialise(&instance); + -+ *instance_handle = (VCHI_INSTANCE_T)instance; ++ *instance_handle = (VCHI_INSTANCE_T)instance; + -+ return vchiq_status_to_vchi(status); ++ return vchiq_status_to_vchi(status); +} -+EXPORT_SYMBOL(vchi_initialise); + +/*********************************************************** + * Name: vchi_connect + * + * Arguments: VCHI_CONNECTION_T **connections + * const uint32_t num_connections -+ * VCHI_INSTANCE_T instance_handle) ++ * VCHI_INSTANCE_T instance_handle ) + * + * Description: Starts the command service on each connection, + * causing INIT messages to be pinged back and forth @@ -19369,18 +18753,17 @@ index 0000000..0ed884e + * Returns: 0 if successful, failure otherwise + * + ***********************************************************/ -+int32_t vchi_connect(VCHI_CONNECTION_T **connections, -+ const uint32_t num_connections, -+ VCHI_INSTANCE_T instance_handle) ++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; ++ VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle; + -+ (void)connections; -+ (void)num_connections; ++ vcos_unused(connections); ++ vcos_unused(num_connections); + -+ return vchiq_connect(instance); ++ return vchiq_connect(instance); +} -+EXPORT_SYMBOL(vchi_connect); + + +/*********************************************************** @@ -19394,12 +18777,11 @@ index 0000000..0ed884e + * Returns: 0 if successful, failure otherwise + * + ***********************************************************/ -+int32_t vchi_disconnect(VCHI_INSTANCE_T instance_handle) ++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)); ++ VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle; ++ return vchiq_status_to_vchi(vchiq_shutdown(instance)); +} -+EXPORT_SYMBOL(vchi_disconnect); + + +/*********************************************************** @@ -19416,188 +18798,168 @@ index 0000000..0ed884e + * + ***********************************************************/ + -+static VCHIQ_STATUS_T shim_callback(VCHIQ_REASON_T reason, -+ VCHIQ_HEADER_T *header, VCHIQ_SERVICE_HANDLE_T handle, void *bulk_user) ++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); ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)VCHIQ_GET_SERVICE_USERDATA(handle); + -+ switch (reason) { -+ case VCHIQ_MESSAGE_AVAILABLE: -+ vchiu_queue_push(&service->queue, header); ++ 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: -+ WARN(1, "not supported\n"); -+ break; -+ } ++ 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; ++ return VCHIQ_SUCCESS; +} + +static SHIM_SERVICE_T *service_alloc(VCHIQ_INSTANCE_T instance, -+ SERVICE_CREATION_T *setup) ++ SERVICE_CREATION_T *setup) +{ -+ SHIM_SERVICE_T *service = kzalloc(sizeof(SHIM_SERVICE_T), GFP_KERNEL); ++ SHIM_SERVICE_T *service = vcos_calloc(1, sizeof(SHIM_SERVICE_T), "vchiq_shim"); + -+ (void)instance; ++ vcos_unused(instance); + -+ if (service) { -+ if (vchiu_queue_init(&service->queue, 64)) { -+ service->callback = setup->callback; -+ service->callback_param = setup->callback_param; -+ } else { -+ kfree(service); -+ service = NULL; -+ } -+ } ++ 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; ++ return service; +} + +static void service_free(SHIM_SERVICE_T *service) +{ -+ if (service) { -+ vchiu_queue_delete(&service->queue); -+ kfree(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) ++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_SERVICE_PARAMS_T params; -+ VCHIQ_STATUS_T status; ++ 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; ++ } ++ } + -+ memset(¶ms, 0, sizeof(params)); -+ params.fourcc = setup->service_id; -+ params.callback = shim_callback; -+ params.userdata = service; -+ params.version = setup->version.version; -+ params.version_min = setup->version.version_min; ++ *handle = (VCHI_SERVICE_HANDLE_T)service; + -+ status = vchiq_open_service(instance, ¶ms, -+ &service->handle); -+ if (status != VCHIQ_SUCCESS) { -+ service_free(service); -+ service = NULL; -+ } -+ } -+ -+ *handle = (VCHI_SERVICE_HANDLE_T)service; -+ -+ return (service != NULL) ? 0 : -1; ++ return (service != NULL) ? 0 : -1; +} -+EXPORT_SYMBOL(vchi_service_open); + -+int32_t vchi_service_create(VCHI_INSTANCE_T instance_handle, -+ SERVICE_CREATION_T *setup, -+ VCHI_SERVICE_HANDLE_T *handle) ++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_SERVICE_PARAMS_T params; -+ VCHIQ_STATUS_T status; ++ 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; ++ } ++ } + -+ memset(¶ms, 0, sizeof(params)); -+ params.fourcc = setup->service_id; -+ params.callback = shim_callback; -+ params.userdata = service; -+ params.version = setup->version.version; -+ params.version_min = setup->version.version_min; -+ status = vchiq_add_service(instance, ¶ms, &service->handle); ++ *handle = (VCHI_SERVICE_HANDLE_T)service; + -+ if (status != VCHIQ_SUCCESS) { -+ service_free(service); -+ service = NULL; -+ } -+ } -+ -+ *handle = (VCHI_SERVICE_HANDLE_T)service; -+ -+ return (service != NULL) ? 0 : -1; ++ return (service != NULL) ? 0 : -1; +} -+EXPORT_SYMBOL(vchi_service_create); + -+int32_t vchi_service_close(const VCHI_SERVICE_HANDLE_T handle) ++int32_t vchi_service_close( const VCHI_SERVICE_HANDLE_T handle ) +{ -+ int32_t ret = -1; -+ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; -+ if (service) { -+ VCHIQ_STATUS_T status = vchiq_close_service(service->handle); -+ if (status == VCHIQ_SUCCESS) { -+ service_free(service); -+ service = NULL; -+ } ++ int32_t ret = -1; ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; ++ if(service) ++ { ++ VCHIQ_STATUS_T status = vchiq_close_service(service->handle); ++ if (status == VCHIQ_SUCCESS) ++ { ++ service_free(service); ++ service = NULL; ++ } + -+ ret = vchiq_status_to_vchi(status); -+ } -+ return ret; ++ ret = vchiq_status_to_vchi( status ); ++ } ++ return ret; +} -+EXPORT_SYMBOL(vchi_service_close); + -+int32_t vchi_service_destroy(const VCHI_SERVICE_HANDLE_T handle) ++int32_t vchi_service_destroy( const VCHI_SERVICE_HANDLE_T handle ) +{ -+ int32_t ret = -1; -+ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; -+ if (service) { -+ VCHIQ_STATUS_T status = vchiq_remove_service(service->handle); -+ if (status == VCHIQ_SUCCESS) { -+ service_free(service); -+ service = NULL; -+ } ++ int32_t ret = -1; ++ SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle; ++ if(service) ++ { ++ VCHIQ_STATUS_T status = vchiq_remove_service(service->handle); ++ if (status == VCHIQ_SUCCESS) ++ { ++ service_free(service); ++ service = NULL; ++ } + -+ ret = vchiq_status_to_vchi(status); -+ } -+ return ret; ++ ret = vchiq_status_to_vchi( status ); ++ } ++ return ret; +} -+EXPORT_SYMBOL(vchi_service_destroy); + +/* ---------------------------------------------------------------------- + * read a uint32_t from buffer. + * network format is defined to be little endian + * -------------------------------------------------------------------- */ +uint32_t -+vchi_readbuf_uint32(const void *_ptr) ++vchi_readbuf_uint32( const void *_ptr ) +{ -+ const unsigned char *ptr = _ptr; -+ return ptr[0] | (ptr[1] << 8) | (ptr[2] << 16) | (ptr[3] << 24); ++ const unsigned char *ptr = _ptr; ++ return ptr[0] | (ptr[1] << 8) | (ptr[2] << 16) | (ptr[3] << 24); +} + +/* ---------------------------------------------------------------------- @@ -19605,13 +18967,13 @@ index 0000000..0ed884e + * network format is defined to be little endian + * -------------------------------------------------------------------- */ +void -+vchi_writebuf_uint32(void *_ptr, uint32_t value) ++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); ++ 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); +} + +/* ---------------------------------------------------------------------- @@ -19619,10 +18981,10 @@ index 0000000..0ed884e + * network format is defined to be little endian + * -------------------------------------------------------------------- */ +uint16_t -+vchi_readbuf_uint16(const void *_ptr) ++vchi_readbuf_uint16( const void *_ptr ) +{ -+ const unsigned char *ptr = _ptr; -+ return ptr[0] | (ptr[1] << 8); ++ const unsigned char *ptr = _ptr; ++ return ptr[0] | (ptr[1] << 8); +} + +/* ---------------------------------------------------------------------- @@ -19630,11 +18992,11 @@ index 0000000..0ed884e + * network format is defined to be little endian + * -------------------------------------------------------------------- */ +void -+vchi_writebuf_uint16(void *_ptr, uint16_t value) ++vchi_writebuf_uint16( void *_ptr, uint16_t value ) +{ -+ unsigned char *ptr = _ptr; -+ ptr[0] = (value >> 0) & 0xFF; -+ ptr[1] = (value >> 8) & 0xFF; ++ unsigned char *ptr = _ptr; ++ ptr[0] = (value >> 0) & 0xFF; ++ ptr[1] = (value >> 8) & 0xFF; +} + +/*********************************************************** @@ -19647,15 +19009,16 @@ index 0000000..0ed884e + * Returns: void + * + ***********************************************************/ -+int32_t vchi_service_use(const VCHI_SERVICE_HANDLE_T handle) ++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; ++ 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; +} -+EXPORT_SYMBOL(vchi_service_use); + +/*********************************************************** + * Name: vchi_service_release @@ -19667,22 +19030,39 @@ index 0000000..0ed884e + * Returns: void + * + ***********************************************************/ -+int32_t vchi_service_release(const VCHI_SERVICE_HANDLE_T handle) ++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; ++ 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_msg_peek); ++EXPORT_SYMBOL(vchi_msg_remove); ++EXPORT_SYMBOL(vchi_service_close); ++EXPORT_SYMBOL(vchi_service_open); ++EXPORT_SYMBOL(vchi_service_create); ++EXPORT_SYMBOL(vchi_service_destroy); ++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..8ea8bea +index 0000000..20b67c3 --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.c -@@ -0,0 +1,105 @@ +@@ -0,0 +1,97 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -19703,97 +19083,89 @@ index 0000000..8ea8bea + +#include "vchiq_util.h" + -+static inline int is_pow2(int i) ++#if !defined(__KERNEL__) ++#include ++#endif ++ ++static __inline int is_pow2(int i) +{ -+ return i && !(i & (i - 1)); ++ return i && !(i & (i - 1)); +} + +int vchiu_queue_init(VCHIU_QUEUE_T *queue, int size) +{ -+ WARN_ON(!is_pow2(size)); ++ vcos_assert(is_pow2(size)); + -+ queue->size = size; -+ queue->read = 0; -+ queue->write = 0; ++ queue->size = size; ++ queue->read = 0; ++ queue->write = 0; + -+ sema_init(&queue->pop, 0); -+ sema_init(&queue->push, 0); ++ vcos_event_create(&queue->pop, "vchiu"); ++ vcos_event_create(&queue->push, "vchiu"); + -+ queue->storage = kzalloc(size * sizeof(VCHIQ_HEADER_T *), GFP_KERNEL); -+ if (queue->storage == NULL) { -+ vchiu_queue_delete(queue); -+ return 0; -+ } -+ return 1; ++ 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) +{ -+ if (queue->storage != NULL) -+ kfree(queue->storage); ++ 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; -+} -+ -+int vchiu_queue_is_full(VCHIU_QUEUE_T *queue) -+{ -+ return queue->write == queue->read + queue->size; ++ return queue->read == queue->write; +} + +void vchiu_queue_push(VCHIU_QUEUE_T *queue, VCHIQ_HEADER_T *header) +{ -+ while (queue->write == queue->read + queue->size) { -+ if (down_interruptible(&queue->pop) != 0) { -+ flush_signals(current); -+ } -+ } ++ while (queue->write == queue->read + queue->size) ++ vcos_event_wait(&queue->pop); + -+ queue->storage[queue->write & (queue->size - 1)] = header; ++ queue->storage[queue->write & (queue->size - 1)] = header; + -+ queue->write++; ++ queue->write++; + -+ up(&queue->push); ++ vcos_event_signal(&queue->push); +} + +VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue) +{ -+ while (queue->write == queue->read) { -+ if (down_interruptible(&queue->push) != 0) { -+ flush_signals(current); -+ } -+ } ++ while (queue->write == queue->read) ++ vcos_event_wait(&queue->push); + -+ up(&queue->push); // We haven't removed anything from the queue. -+ return queue->storage[queue->read & (queue->size - 1)]; ++ return queue->storage[queue->read & (queue->size - 1)]; +} + +VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue) +{ -+ VCHIQ_HEADER_T *header; ++ VCHIQ_HEADER_T *header; + -+ while (queue->write == queue->read) { -+ if (down_interruptible(&queue->push) != 0) { -+ flush_signals(current); -+ } -+ } ++ while (queue->write == queue->read) ++ vcos_event_wait(&queue->push); + -+ header = queue->storage[queue->read & (queue->size - 1)]; ++ header = queue->storage[queue->read & (queue->size - 1)]; + -+ queue->read++; ++ queue->read++; + -+ up(&queue->pop); ++ vcos_event_signal(&queue->pop); + -+ return header; ++ 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..948503d +index 0000000..6e0c84b --- /dev/null +++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_util.h -@@ -0,0 +1,67 @@ +@@ -0,0 +1,47 @@ +/* + * Copyright (c) 2010-2011 Broadcom Corporation. All rights reserved. + * @@ -19815,10 +19187,4360 @@ index 0000000..948503d +#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; ¶m->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( ¶m, cmd_globals.cmd_entry ); ++ ++ if ( param.use_log ) ++ { ++ cmd_log_results( ¶m ); ++ 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( ¶m, "\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..ce7816957 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_common.h +@@ -0,0 +1,87 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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_MIN?(x)-1:VCOS_THREAD_PRI_MIN) ++#else ++/* bigger numbers are lower priority */ ++#define VCOS_THREAD_PRI_MORE(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..154b200 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_blockpool.h +@@ -0,0 +1,271 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..3948a57 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.c +@@ -0,0 +1,311 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++VideoCore OS Abstraction Layer - event flags implemented via mutexes ++=============================================================================*/ ++ ++#include "interface/vcos/vcos.h" ++#include "interface/vcos/generic/vcos_generic_event_flags.h" ++ ++#include ++ ++/** 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..8776ebe +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_event_flags.h +@@ -0,0 +1,118 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..370562d +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_named_sem.h +@@ -0,0 +1,92 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..bf7945c +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_quickslow_mutex.h +@@ -0,0 +1,86 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..27563ea +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_reentrant_mtx.h +@@ -0,0 +1,86 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..22c059a +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_generic_tls.h +@@ -0,0 +1,155 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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 (tlsslots[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 (tlsslots[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..fd0e198 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_joinable_thread_from_plain.h +@@ -0,0 +1,214 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..ec9e07b +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_latch_from_sem.h +@@ -0,0 +1,59 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..8b05179 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_logcat.c +@@ -0,0 +1,560 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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 ++ * (,)* ++ * ++ * 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..dd0574a +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.c +@@ -0,0 +1,84 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++VideoCore OS Abstraction Layer - memory alloc implementation ++=============================================================================*/ ++ ++#include "interface/vcos/vcos.h" ++ ++#ifndef _vcos_platform_malloc ++#include ++#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..8e2a18e +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mem_from_malloc.h +@@ -0,0 +1,65 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..a6a52b4 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_mutexes_are_reentrant.h +@@ -0,0 +1,79 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..655dc25 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/generic/vcos_thread_reaper.h +@@ -0,0 +1,46 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..1db1ecb +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/stdint.h +@@ -0,0 +1,31 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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 so we have to provide one of ++ our own. */ ++ ++#include /* 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..0385540 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel.c +@@ -0,0 +1,627 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++VideoCore OS Abstraction Layer - pthreads types ++=============================================================================*/ ++ ++#define VCOS_INLINE_BODIES ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#if defined( CONFIG_BCM_KNLLOG_SUPPORT ) ++#include ++#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 ++#include ++#include ++#include ++ ++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..4a9cedf +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_linuxkernel_misc.c +@@ -0,0 +1,111 @@ ++/***************************************************************************** ++* 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 ++#include ++#include ++#include ++#include ++ ++/***************************************************************************** ++* ++* 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 ++ ++/* ---- 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..381688e +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform.h +@@ -0,0 +1,507 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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 +#include +#include -+#include ++#include +#include +#include +#include @@ -19835,65 +23557,4000 @@ index 0000000..948503d +#include +#include + -+#include "vchiq_if.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 + -+typedef struct { -+ int size; -+ int read; -+ int write; ++/* 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 */ + -+ struct semaphore pop; -+ struct semaphore push; ++typedef struct semaphore VCOS_SEMAPHORE_T; ++typedef struct semaphore VCOS_EVENT_T; ++typedef struct mutex VCOS_MUTEX_T; ++typedef volatile int VCOS_ONCE_T; + -+ VCHIQ_HEADER_T **storage; -+} VCHIU_QUEUE_T; ++typedef unsigned int VCOS_UNSIGNED; ++typedef unsigned int VCOS_OPTION; ++typedef atomic_t VCOS_ATOMIC_FLAGS_T; + -+extern int vchiu_queue_init(VCHIU_QUEUE_T *queue, int size); -+extern void vchiu_queue_delete(VCHIU_QUEUE_T *queue); ++typedef struct ++{ ++ struct timer_list linux_timer; ++ void *context; ++ void (*expiration_routine)(void *context); + -+extern int vchiu_queue_is_empty(VCHIU_QUEUE_T *queue); -+extern int vchiu_queue_is_full(VCHIU_QUEUE_T *queue); ++} VCOS_TIMER_T; + -+extern void vchiu_queue_push(VCHIU_QUEUE_T *queue, VCHIQ_HEADER_T *header); ++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; + -+extern VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue); -+extern VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue); ++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..5f82aaf +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/linuxkernel/vcos_platform_types.h +@@ -0,0 +1,59 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++VideoCore OS Abstraction Layer - platform-specific types and defines ++=============================================================================*/ ++ ++#ifndef VCOS_PLATFORM_TYPES_H ++#define VCOS_PLATFORM_TYPES_H ++ ++#include ++#include ++#include ++#include ++ ++#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 ++#include ++#include ++#include ++ ++#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 ++ ++#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..e37d795 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos.h +@@ -0,0 +1,212 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..1e72dff +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_assert.h +@@ -0,0 +1,280 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..317abf3 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_atomic_flags.h +@@ -0,0 +1,83 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..f3817f6 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_build_info.h +@@ -0,0 +1,23 @@ ++/* ++ * 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 ++ */ ++ ++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..a270de8 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_ctype.h +@@ -0,0 +1,40 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++VideoCore OS Abstraction Layer - public header file ++=============================================================================*/ ++ ++#ifndef VCOS_CTYPE_H ++#define VCOS_CTYPE_H ++ ++/** ++ * \file ++ * ++ * ctype functions. ++ * ++ */ ++ ++#ifdef __KERNEL__ ++#include ++#else ++#include ++#endif + +#endif + -diff --git a/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_version.c b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_version.c +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..87bea8b +index 0000000..0a683c0b --- /dev/null -+++ b/drivers/misc/vc04_services/interface/vchiq_arm/vchiq_version.c -@@ -0,0 +1,27 @@ -+#include "vchiq_build_info.h" -+#include ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_dlfcn.h +@@ -0,0 +1,80 @@ ++/* ++ * 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 ++ */ + -+VC_DEBUG_DECLARE_STRING_VAR( vchiq_build_hostname, "dc4-arm-01" ); -+VC_DEBUG_DECLARE_STRING_VAR( vchiq_build_version, "9245b4c35b99b3870e1f7dc598c5692b3c66a6f0 (tainted)" ); -+VC_DEBUG_DECLARE_STRING_VAR( vchiq_build_time, __TIME__ ); -+VC_DEBUG_DECLARE_STRING_VAR( vchiq_build_date, __DATE__ ); ++/*============================================================================= ++VCOS - abstraction over dynamic library opening ++=============================================================================*/ + -+const char *vchiq_get_build_hostname( void ) ++#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..38612f9 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_event.h +@@ -0,0 +1,108 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..9eee410 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_event_flags.h +@@ -0,0 +1,109 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..9fc5eca +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_init.h +@@ -0,0 +1,54 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..9702822 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_logging.h +@@ -0,0 +1,290 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++VideoCore OS Abstraction Layer - logging support ++=============================================================================*/ ++ ++#ifndef VCOS_LOGGING_H ++#define VCOS_LOGGING_H ++ ++#ifdef __cplusplus ++extern "C" { ++#endif ++ ++#include ++ ++#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 ++ * ++ * VCOS_LOG_CAT_T my_category; ++ * ++ * .... ++ * ++ * vcos_log_trace("Stuff happened: %d", n_stuff); ++ * ++ */ ++ ++/** Logging levels */ ++typedef enum VCOS_LOG_LEVEL_T +{ -+ return vchiq_build_hostname; ++ 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; +} + -+const char *vchiq_get_build_version( void ) -+{ -+ return vchiq_build_version; -+} ++#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) + -+const char *vchiq_get_build_date( void ) -+{ -+ return vchiq_build_date; -+} ++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 char *vchiq_get_build_time( void ) -+{ -+ return vchiq_build_time; ++/* ++ * 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..9473cec +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_lowlevel_thread.h +@@ -0,0 +1,118 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..af16208 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_mem.h +@@ -0,0 +1,92 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..f0ef70b +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_msgqueue.h +@@ -0,0 +1,168 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..14387a8 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_mutex.h +@@ -0,0 +1,103 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..0533c10 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_once.h +@@ -0,0 +1,53 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..2760dd7 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_semaphore.h +@@ -0,0 +1,126 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..3c7669d +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_stdbool.h +@@ -0,0 +1,34 @@ ++/* ++ * 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 VCOS_STDBOOL_H ++#define VCOS_STDBOOL_H ++ ++#ifndef __cplusplus ++ ++#if defined(__STDC__) && (__STDC_VERSION__ >= 199901L) ++#include ++#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..c9a3e02 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_stdint.h +@@ -0,0 +1,203 @@ ++/* ++ * 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 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 ++ ++#elif defined( __GNUC__ ) ++ ++#include ++ ++#elif defined(_MSC_VER) /* Visual C define equivalent types */ ++ ++#include /* 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 ++ ++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 ++ ++#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..c3d875f +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_string.h +@@ -0,0 +1,84 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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 ++#else ++#include ++#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..ee34648 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_thread.h +@@ -0,0 +1,270 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..375dd01 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_thread_attr.h +@@ -0,0 +1,87 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..1612334 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_timer.h +@@ -0,0 +1,106 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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..25de671 +--- /dev/null ++++ b/drivers/misc/vc04_services/interface/vcos/vcos_types.h +@@ -0,0 +1,211 @@ ++/* ++ * 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 ++ */ ++ ++/*============================================================================= ++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_.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 "" ++# 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 diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c index 34416d4..13cde4a 100644 --- a/drivers/mmc/card/block.c @@ -83200,10 +90857,10 @@ index 0000000..2f63ac6 +} diff --git a/sound/arm/bcm2835-vchiq.c b/sound/arm/bcm2835-vchiq.c new file mode 100755 -index 0000000..14ca707 +index 0000000..3b7ed1e --- /dev/null +++ b/sound/arm/bcm2835-vchiq.c -@@ -0,0 +1,826 @@ +@@ -0,0 +1,825 @@ +/***************************************************************************** +* Copyright 2011 Broadcom Corporation. All rights reserved. +* @@ -83432,7 +91089,6 @@ index 0000000..14ca707 + /* Open the VCHI service connections */ + for (i = 0; i < num_connections; i++) { + SERVICE_CREATION_T params = { -+ VCHI_VERSION(VC_AUDIOSERV_VER), // version + VC_AUDIO_SERVER_NAME, // 4cc service code + vchi_connections[i], // passed in fn pointers + 0, // rx fifo size (unused) @@ -84612,10 +92268,10 @@ index 0000000..b966e28 +#endif /* __SOUND_ARM_BCM2835_H */ diff --git a/sound/arm/vc_vchi_audioserv_defs.h b/sound/arm/vc_vchi_audioserv_defs.h new file mode 100644 -index 0000000..2020ce4 +index 0000000..d610734 --- /dev/null +++ b/sound/arm/vc_vchi_audioserv_defs.h -@@ -0,0 +1,114 @@ +@@ -0,0 +1,112 @@ +/***************************************************************************** +* Copyright 2011 Broadcom Corporation. All rights reserved. +* @@ -84633,8 +92289,6 @@ index 0000000..2020ce4 +#ifndef _VC_AUDIO_DEFS_H_ +#define _VC_AUDIO_DEFS_H_ + -+#define VC_AUDIOSERV_VER 1 -+ +// FourCC code used for VCHI connection +#define VC_AUDIO_SERVER_NAME MAKE_FOURCC("AUDS") +