[aravis] fake_gv_camera: implement heartbeat monitoring.

[Emmanuel Pacaud <emmanuel src gnome org>]

commit da32aea223f157a6053f672b6ea4b93dd5f1c089
Author: Emmanuel Pacaud <emmanuel gnome org>
Date:   Mon Jun 7 07:27:54 2010 +0200

    fake_gv_camera: implement heartbeat monitoring.

 src/arvfakecamera.c   |   28 +++++++++++++++++
 src/arvfakecamera.h   |    4 ++
 src/arvfakegvcamera.c |   80 ++++++++++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 111 insertions(+), 1 deletions(-)
---
diff --git a/src/arvfakecamera.c b/src/arvfakecamera.c
index 32e89fc..d7f3206 100644
--- a/src/arvfakecamera.c
+++ b/src/arvfakecamera.c
@@ -244,6 +244,32 @@ arv_fake_camera_set_trigger_frequency (ArvFakeCamera *camera, double frequency)
 	camera->priv->frame_id = 0;
 }
 
+guint32
+arv_fake_camera_get_control_channel_privilege (ArvFakeCamera *camera)
+{
+	guint32 value;
+
+	arv_fake_camera_read_register (camera, ARV_GVBS_CONTROL_CHANNEL_PRIVILEGE, &value);
+
+	return value;
+}
+
+void
+arv_fake_camera_set_control_channel_privilege (ArvFakeCamera *camera, guint32 privilege)
+{
+	arv_fake_camera_write_register (camera, ARV_GVBS_CONTROL_CHANNEL_PRIVILEGE, privilege);
+}
+
+guint32
+arv_fake_camera_get_heartbeat_timeout (ArvFakeCamera *camera)
+{
+	guint32 value;
+
+	arv_fake_camera_read_register (camera, ARV_GVBS_HEARTBEAT_TIMEOUT, &value);
+
+	return value;
+}
+
 void
 arv_set_fake_camera_genicam_filename (const char *filename)
 {
@@ -346,8 +372,10 @@ arv_fake_camera_new (const char *serial_number)
 	arv_fake_camera_write_register (fake_camera, ARV_FAKE_CAMERA_REGISTER_GAIN_RAW, 0);
 	arv_fake_camera_write_register (fake_camera, ARV_FAKE_CAMERA_REGISTER_GAIN_MODE, 0);
 
+	arv_fake_camera_write_register (fake_camera, ARV_GVBS_HEARTBEAT_TIMEOUT, 3000);
 	arv_fake_camera_write_register (fake_camera, ARV_GVBS_TIMESTAMP_TICK_FREQUENCY_HIGH, 0);
 	arv_fake_camera_write_register (fake_camera, ARV_GVBS_TIMESTAMP_TICK_FREQUENCY_LOW, 1000000000);
+	arv_fake_camera_write_register (fake_camera, ARV_GVBS_CONTROL_CHANNEL_PRIVILEGE, 0);
 
 	return fake_camera;
 }
diff --git a/src/arvfakecamera.h b/src/arvfakecamera.h
index 2ecff02..e458c56 100644
--- a/src/arvfakecamera.h
+++ b/src/arvfakecamera.h
@@ -113,6 +113,10 @@ guint32 	arv_fake_camera_get_acquisition_status 	(ArvFakeCamera *camera);
 GSocketAddress *arv_fake_camera_get_stream_address 	(ArvFakeCamera *camera);
 void		arv_fake_camera_set_inet_address	(ArvFakeCamera *camera, GInetAddress *address);
 
+guint32		arv_fake_camera_get_control_channel_privilege	(ArvFakeCamera *camera);
+void		arv_fake_camera_set_control_channel_privilege	(ArvFakeCamera *camera, guint32 privilege);
+guint32		arv_fake_camera_get_heartbeat_timeout		(ArvFakeCamera *camera);
+
 void 		arv_set_fake_camera_genicam_filename 	(const char *filename);
 const char *	arv_get_fake_camera_genicam_data	(size_t *size);
 
diff --git a/src/arvfakegvcamera.c b/src/arvfakegvcamera.c
index 1bc8941..6e1c9d7 100644
--- a/src/arvfakegvcamera.c
+++ b/src/arvfakegvcamera.c
@@ -18,6 +18,10 @@ typedef struct {
 	ArvFakeCamera *camera;
 	GPollFD gvcp_fds[2];
 	guint n_gvcp_fds;
+
+	GSocketAddress *controller_address;
+	struct timespec controller_time;
+
 	GSocket *gvcp_socket;
 	GSocket *gvsp_socket;
 	GSocket *discovery_socket;
@@ -26,6 +30,36 @@ typedef struct {
 	gboolean cancel;
 } ArvFakeGvCamera;
 
+gboolean
+_g_inet_socket_address_is_equal (GInetSocketAddress *a, GInetSocketAddress *b)
+{
+	GInetAddress *a_addr;
+	GInetAddress *b_addr;
+	char *a_str;
+	char *b_str;
+	gboolean result;
+
+	if (!G_IS_INET_SOCKET_ADDRESS (a) ||
+	    !G_IS_INET_SOCKET_ADDRESS (b))
+		return FALSE;
+
+	if (g_inet_socket_address_get_port (a) != g_inet_socket_address_get_port (b))
+		return FALSE;
+
+	a_addr = g_inet_socket_address_get_address (a);
+	b_addr = g_inet_socket_address_get_address (b);
+
+	a_str = g_inet_address_to_string (a_addr);
+	b_str = g_inet_address_to_string (b_addr);
+
+	result = g_strcmp0 (a_str, b_str) == 0;
+
+	g_free (a_str);
+	g_free (b_str);
+
+	return result;
+}
+
 void *
 arv_fake_gv_camera_thread (void *user_data)
 {
@@ -43,7 +77,8 @@ arv_fake_gv_camera_thread (void *user_data)
 	packet_buffer = g_malloc (ARV_FAKE_GV_CAMERA_BUFFER_SIZE);
 
 	do {
-		if (arv_fake_camera_get_acquisition_status (gv_camera->camera) == 0) {
+		if (arv_fake_camera_get_control_channel_privilege (gv_camera->camera) == 0 ||
+		    arv_fake_camera_get_acquisition_status (gv_camera->camera) == 0) {
 			if (stream_address != NULL) {
 				g_object_unref (stream_address);
 				stream_address = NULL;
@@ -253,6 +288,8 @@ arv_fake_gv_camera_free (ArvFakeGvCamera *gv_camera)
 	if (gv_camera->discovery_socket != NULL)
 		g_object_unref (gv_camera->discovery_socket);
 	g_object_unref (gv_camera->camera);
+	if (gv_camera->controller_address != NULL)
+		g_object_unref (gv_camera->controller_address);
 	g_free (gv_camera);
 }
 
@@ -268,6 +305,30 @@ handle_control_packet (ArvFakeGvCamera *gv_camera, GSocket *socket,
 	guint32 packet_count;
 	guint32 register_address;
 	guint32 register_value;
+	gboolean write_access;
+
+	if (gv_camera->controller_address != NULL) {
+		struct timespec time;
+		guint64 elapsed_ms;
+
+		clock_gettime (CLOCK_MONOTONIC, &time);
+
+		elapsed_ms = 1000 * (time.tv_sec - gv_camera->controller_time.tv_sec) +
+			(time.tv_nsec - gv_camera->controller_time.tv_nsec) / 1000000;
+
+		if (elapsed_ms > arv_fake_camera_get_heartbeat_timeout (gv_camera->camera)) {
+			g_object_ref (gv_camera->controller_address);
+			gv_camera->controller_address = NULL;
+			write_access = TRUE;
+			arv_debug ("camera", "[FakeGvCamera::handle_control_packet] Heartbeat timeout");
+			arv_fake_camera_set_control_channel_privilege (gv_camera->camera, 0);
+		} else
+			write_access = _g_inet_socket_address_is_equal
+				(G_INET_SOCKET_ADDRESS (remote_address),
+				 G_INET_SOCKET_ADDRESS (gv_camera->controller_address));
+	} else
+		write_access = TRUE;
+
 
 	arv_gvcp_packet_debug (packet);
 
@@ -290,6 +351,9 @@ handle_control_packet (ArvFakeGvCamera *gv_camera, GSocket *socket,
 						     arv_gvcp_packet_get_read_memory_ack_data (ack_packet));
 			break;
 		case ARV_GVCP_COMMAND_WRITE_MEMORY_CMD:
+			if (!write_access)
+				break;
+
 			arv_gvcp_packet_get_write_memory_cmd_infos (packet, &block_address, &block_size);
 			arv_debug ("camera", "[FakeGvCamera::handle_control_packet] Write memory command %d (%d)",
 				   block_address, block_size);
@@ -305,8 +369,15 @@ handle_control_packet (ArvFakeGvCamera *gv_camera, GSocket *socket,
 				   register_address, register_value);
 			ack_packet = arv_gvcp_packet_new_read_register_ack (register_value, packet_count,
 									    &ack_packet_size);
+
+			if (register_address == ARV_GVBS_CONTROL_CHANNEL_PRIVILEGE)
+				clock_gettime (CLOCK_MONOTONIC, &gv_camera->controller_time);
+
 			break;
 		case ARV_GVCP_COMMAND_WRITE_REGISTER_CMD:
+			if (!write_access)
+				break;
+
 			arv_gvcp_packet_get_write_register_cmd_infos (packet, &register_address, &register_value);
 			arv_fake_camera_write_register (gv_camera->camera, register_address, register_value);
 			arv_debug ("camera", "[FakeGvCamera::handle_control_packet] Write register command %d -> %d",
@@ -323,6 +394,13 @@ handle_control_packet (ArvFakeGvCamera *gv_camera, GSocket *socket,
 		arv_gvcp_packet_debug (ack_packet);
 		g_free (ack_packet);
 	}
+
+	if (gv_camera->controller_address == NULL &&
+	    arv_fake_camera_get_control_channel_privilege (gv_camera->camera) != 0) {
+		g_object_ref (remote_address);
+		gv_camera->controller_address = remote_address;
+		clock_gettime (CLOCK_MONOTONIC, &gv_camera->controller_time);
+	}
 }
 
 static char *arv_option_interface_name = "lo";