[PATCH 3/5] staging: vchiq_arm: Add vchiq_remove_connected_callback

[Michael Zoran]

The connected callback mechanism of vchiq is missing the ability
to remove a connected callback, such as when a module unloads.

So add the vchiq_remove_connected_callback to support unloading
modules that hook into vchiq.

Signed-off-by: Michael Zoran <mzoran at crowfest.net>
---
 .../interface/vchiq_arm/vchiq_connected.c          | 23 ++++++++++++++++++++--
 .../interface/vchiq_arm/vchiq_connected.h          |  1 +
 2 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c
index 7ea29665bd0c..6de7e1237467 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c
@@ -94,6 +94,21 @@ void vchiq_add_connected_callback(VCHIQ_CONNECTED_CALLBACK_T callback)
 	mutex_unlock(&g_connected_mutex);
 }
 
+void vchiq_remove_connected_callback(VCHIQ_CONNECTED_CALLBACK_T callback)
+{
+	int i;
+
+	connected_init();
+
+	mutex_lock(&g_connected_mutex);
+
+	for (i = 0; i <  g_num_deferred_callbacks; i++)
+		if (g_deferred_callback[i] == callback)
+			g_deferred_callback[i] = NULL;
+
+	mutex_unlock(&g_connected_mutex);
+}
+
 /****************************************************************************
 *
 * This function is called by the vchiq stack once it has been connected to
@@ -110,8 +125,12 @@ void vchiq_call_connected_callbacks(void)
 	if (mutex_lock_killable(&g_connected_mutex) != 0)
 		return;
 
-	for (i = 0; i <  g_num_deferred_callbacks; i++)
-		g_deferred_callback[i]();
+	for (i = 0; i <  g_num_deferred_callbacks; i++) {
+		if (g_deferred_callback[i])
+			g_deferred_callback[i]();
+
+		g_deferred_callback[i] = NULL;
+	}
 
 	g_num_deferred_callbacks = 0;
 	g_connected = 1;
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h
index 863b3e335c1a..b2e9b045677a 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.h
@@ -45,6 +45,7 @@ typedef void (*VCHIQ_CONNECTED_CALLBACK_T)(void);
 /* ---- Function Prototypes ---------------------------------------------- */
 
 void vchiq_add_connected_callback(VCHIQ_CONNECTED_CALLBACK_T callback);
+void vchiq_remove_connected_callback(VCHIQ_CONNECTED_CALLBACK_T callback);
 void vchiq_call_connected_callbacks(void);
 
 #endif /* VCHIQ_CONNECTED_H */
-- 
2.11.0