[aravis] fake: implement read/write memory/register packets.

[Emmanuel Pacaud <emmanuel src gnome org>]

commit 355260e46055697444c26b115eb0e3758bcf37c1
Author: Emmanuel Pacaud <emmanuel gnome org>
Date:   Mon May 31 22:13:03 2010 +0200

    fake: implement read/write memory/register packets.

 src/arvfakegvcamera.c |   58 ++++++++++++++++++++++++++++++----------
 src/arvgvcp.c         |   69 +++++++++++++++++++++++++++++++++++++++++++++++++
 src/arvgvcp.h         |   51 ++++++++++++++++++++++++++++++++++++
 3 files changed, 163 insertions(+), 15 deletions(-)
---
diff --git a/src/arvfakegvcamera.c b/src/arvfakegvcamera.c
index eb5f433..a79f934 100644
--- a/src/arvfakegvcamera.c
+++ b/src/arvfakegvcamera.c
@@ -27,39 +27,67 @@ handle_control_packet (ArvFakeGvCamera *gv_camera, GSocket *socket,
 		       GSocketAddress *remote_address,
 		       ArvGvcpPacket *packet, size_t size)
 {
-	ArvGvcpPacket *ack_packet;
+	ArvGvcpPacket *ack_packet = NULL;
 	size_t ack_packet_size;
 	guint32 block_address;
 	guint32 block_size;
+	guint32 packet_count;
+	guint32 register_address;
+	guint32 register_value;
 
 	arv_gvcp_packet_debug (packet);
 
+	packet_count = arv_gvcp_packet_get_packet_count (packet);
+
 	switch (g_ntohs (packet->header.command)) {
 		case ARV_GVCP_COMMAND_DISCOVERY_CMD:
 			ack_packet = arv_gvcp_packet_new_discovery_ack (&ack_packet_size);
+			arv_debug ("camera", "[FakeGvCamera::handle_control_packet] Discovery command");
 			arv_fake_camera_read_memory (gv_camera->camera, 0, ARV_GVBS_DISCOVERY_DATA_SIZE,
 						     &ack_packet->data);
-			g_socket_send_to (socket, remote_address, (char *) ack_packet, ack_packet_size, NULL, NULL);
-			arv_gvcp_packet_debug (ack_packet);
-			g_free (ack_packet);
 			break;
 		case ARV_GVCP_COMMAND_READ_MEMORY_CMD:
-			memcpy (&block_address, &packet->data[0], sizeof (guint32));
-			memcpy (&block_size, &packet->data[sizeof(guint32)], sizeof (guint32));
-			block_address = g_ntohl (block_address);
-			block_size = g_ntohl (block_size);
+			arv_gvcp_packet_get_read_memory_cmd_infos (packet, &block_address, &block_size);
+			arv_debug ("camera", "[FakeGvCamera::handle_control_packet] Read memory command %d (%d)",
+				   block_address, block_size);
 			ack_packet = arv_gvcp_packet_new_read_memory_ack (block_address, block_size,
-									  arv_gvcp_packet_get_packet_count (packet),
-									  &ack_packet_size);
+									  packet_count, &ack_packet_size);
 			arv_fake_camera_read_memory (gv_camera->camera, block_address, block_size,
 						     arv_gvcp_packet_get_read_memory_ack_data (ack_packet));
-			g_socket_send_to (socket, remote_address, (char *) ack_packet, ack_packet_size, NULL, NULL);
-			arv_gvcp_packet_debug (ack_packet);
-			g_free (ack_packet);
-			g_message ("Read command %d (%d)", block_address, block_size);
+			break;
+		case ARV_GVCP_COMMAND_WRITE_MEMORY_CMD:
+			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);
+			arv_fake_camera_write_memory (gv_camera->camera, block_address, block_size,
+						      arv_gvcp_packet_get_write_memory_cmd_data (packet));
+			ack_packet = arv_gvcp_packet_new_write_memory_ack (block_address, packet_count,
+									   &ack_packet_size);
+			break;
+		case ARV_GVCP_COMMAND_READ_REGISTER_CMD:
+			arv_gvcp_packet_get_read_register_cmd_infos (packet, &register_address);
+			arv_fake_camera_read_register (gv_camera->camera, register_address, &register_value);
+			arv_debug ("camera", "[FakeGvCamera::handle_control_packet] Read register command %d -> %d",
+				   register_address, register_value);
+			ack_packet = arv_gvcp_packet_new_read_register_ack (register_value, packet_count,
+									    &ack_packet_size);
+			break;
+		case ARV_GVCP_COMMAND_WRITE_REGISTER_CMD:
+			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",
+				   register_address, register_value);
+			ack_packet = arv_gvcp_packet_new_write_register_ack (register_value, packet_count,
+									     &ack_packet_size);
 			break;
 		default:
-			g_message ("Unknown command");
+			arv_debug ("camera", "[FakeGvCamera::handle_control_packet] Unknown command");
+	}
+
+	if (ack_packet != NULL) {
+		g_socket_send_to (socket, remote_address, (char *) ack_packet, ack_packet_size, NULL, NULL);
+		arv_gvcp_packet_debug (ack_packet);
+		g_free (ack_packet);
 	}
 }
 
diff --git a/src/arvgvcp.c b/src/arvgvcp.c
index 6a38d47..c1fc94d 100644
--- a/src/arvgvcp.c
+++ b/src/arvgvcp.c
@@ -105,6 +105,29 @@ arv_gvcp_packet_new_write_memory_cmd (guint32 address, guint32 size, guint32 pac
 }
 
 ArvGvcpPacket *
+arv_gvcp_packet_new_write_memory_ack (guint32 address,
+				      guint32 packet_count, size_t *packet_size)
+{
+	ArvGvcpPacket *packet;
+	guint32 n_address = g_htonl (address);
+
+	g_return_val_if_fail (packet_size != NULL, NULL);
+
+	*packet_size = sizeof (ArvGvcpHeader) + sizeof (guint32);
+
+	packet = g_malloc (*packet_size);
+
+	packet->header.packet_type = g_htons (ARV_GVCP_PACKET_TYPE_ACK);
+	packet->header.command = g_htons (ARV_GVCP_COMMAND_WRITE_MEMORY_ACK);
+	packet->header.size = g_htons (sizeof (guint32));
+	packet->header.count = g_htons (packet_count);
+
+	memcpy (&packet->data, &n_address, sizeof (guint32));
+
+	return packet;
+}
+
+ArvGvcpPacket *
 arv_gvcp_packet_new_read_register_cmd (guint32 address, guint32 packet_count, size_t *packet_size)
 {
 	ArvGvcpPacket *packet;
@@ -127,6 +150,29 @@ arv_gvcp_packet_new_read_register_cmd (guint32 address, guint32 packet_count, si
 }
 
 ArvGvcpPacket *
+arv_gvcp_packet_new_read_register_ack (guint32 value,
+				       guint32 packet_count, size_t *packet_size)
+{
+	ArvGvcpPacket *packet;
+	guint32 n_value = g_htonl (value);
+
+	g_return_val_if_fail (packet_size != NULL, NULL);
+
+	*packet_size = sizeof (ArvGvcpHeader) + sizeof (guint32);
+
+	packet = g_malloc (*packet_size);
+
+	packet->header.packet_type = g_htons (ARV_GVCP_PACKET_TYPE_ACK);
+	packet->header.command = g_htons (ARV_GVCP_COMMAND_READ_REGISTER_ACK);
+	packet->header.size = g_htons (sizeof (guint32));
+	packet->header.count = g_htons (packet_count);
+
+	memcpy (&packet->data, &n_value, sizeof (guint32));
+
+	return packet;
+}
+
+ArvGvcpPacket *
 arv_gvcp_packet_new_write_register_cmd (guint32 address, guint32 value,
 					guint32 packet_count, size_t *packet_size)
 {
@@ -152,6 +198,29 @@ arv_gvcp_packet_new_write_register_cmd (guint32 address, guint32 value,
 }
 
 ArvGvcpPacket *
+arv_gvcp_packet_new_write_register_ack 	(guint32 address,
+					 guint32 packet_count, size_t *packet_size)
+{
+	ArvGvcpPacket *packet;
+	guint32 n_address = g_htonl (address);
+
+	g_return_val_if_fail (packet_size != NULL, NULL);
+
+	*packet_size = sizeof (ArvGvcpHeader) + sizeof (guint32);
+
+	packet = g_malloc (*packet_size);
+
+	packet->header.packet_type = g_htons (ARV_GVCP_PACKET_TYPE_ACK);
+	packet->header.command = g_htons (ARV_GVCP_COMMAND_WRITE_REGISTER_ACK);
+	packet->header.size = g_htons (sizeof (guint32));
+	packet->header.count = g_htons (packet_count);
+
+	memcpy (&packet->data, &n_address, sizeof (guint32));
+
+	return packet;
+}
+
+ArvGvcpPacket *
 arv_gvcp_packet_new_discovery_cmd (size_t *packet_size)
 {
 	ArvGvcpPacket *packet;
diff --git a/src/arvgvcp.h b/src/arvgvcp.h
index 11fc840..918d27e 100644
--- a/src/arvgvcp.h
+++ b/src/arvgvcp.h
@@ -101,10 +101,16 @@ ArvGvcpPacket * 	arv_gvcp_packet_new_read_memory_ack 	(guint32 address, guint32
 								 size_t *packet_size);
 ArvGvcpPacket * 	arv_gvcp_packet_new_write_memory_cmd	(guint32 address, guint32 size,
 								 guint32 packet_count, size_t *packet_size);
+ArvGvcpPacket * 	arv_gvcp_packet_new_write_memory_ack	(guint32 address,
+								 guint32 packet_count, size_t *packet_size);
 ArvGvcpPacket * 	arv_gvcp_packet_new_read_register_cmd 	(guint32 address,
 								 guint32 packet_count, size_t *packet_size);
+ArvGvcpPacket * 	arv_gvcp_packet_new_read_register_ack 	(guint32 value,
+								 guint32 packet_count, size_t *packet_size);
 ArvGvcpPacket * 	arv_gvcp_packet_new_write_register_cmd 	(guint32 address, guint32 value,
 								 guint32 packet_count, size_t *packet_size);
+ArvGvcpPacket * 	arv_gvcp_packet_new_write_register_ack 	(guint32 address,
+								 guint32 packet_count, size_t *packet_size);
 ArvGvcpPacket * 	arv_gvcp_packet_new_discovery_cmd 	(size_t *size);
 ArvGvcpPacket * 	arv_gvcp_packet_new_discovery_ack 	(size_t *packet_size);
 ArvGvcpPacket * 	arv_gvcp_packet_new_packet_resend_cmd 	(guint32 frame_id,
@@ -129,6 +135,17 @@ arv_gvcp_packet_get_packet_count (ArvGvcpPacket *packet)
 	return g_ntohs (packet->header.count);
 }
 
+static inline void
+arv_gvcp_packet_get_read_memory_cmd_infos (const ArvGvcpPacket *packet, guint32 *address, guint32 *size)
+{
+	if (packet == NULL)
+		return;
+	if (address != NULL)
+		*address = g_ntohl (*((guint32 *) ((void *) packet + sizeof (ArvGvcpPacket))));
+	if (size != NULL)
+		*size = g_ntohl (*((guint32 *) ((void *) packet + sizeof (ArvGvcpPacket) + sizeof (guint32))));
+}
+
 static inline size_t
 arv_gvcp_packet_get_read_memory_ack_size (guint32 data_size)
 {
@@ -141,6 +158,18 @@ arv_gvcp_packet_get_read_memory_ack_data (const ArvGvcpPacket *packet)
 	return (void *) packet + sizeof (ArvGvcpHeader) + sizeof (guint32);
 }
 
+static inline void
+arv_gvcp_packet_get_write_memory_cmd_infos (const ArvGvcpPacket *packet, guint32 *address, guint32 *size)
+{
+	if (packet == NULL)
+		return;
+
+	if (address != NULL)
+		*address = g_ntohl (*((guint32 *) ((void *) packet + sizeof (ArvGvcpPacket))));
+	if (size != NULL)
+		*size = g_ntohs (packet->header.size) - sizeof (guint32);
+}
+
 static inline void *
 arv_gvcp_packet_get_write_memory_cmd_data (const ArvGvcpPacket *packet)
 {
@@ -153,12 +182,34 @@ arv_gvcp_packet_get_write_memory_ack_size (void)
 	return sizeof (ArvGvcpPacket) + sizeof (guint32);
 }
 
+static inline void
+arv_gvcp_packet_get_read_register_cmd_infos (const ArvGvcpPacket *packet, guint32 *address)
+{
+	if (packet == NULL)
+		return;
+	if (address != NULL)
+		*address = g_ntohl (*((guint32 *) ((void *) packet + sizeof (ArvGvcpPacket))));
+}
+
 static inline guint32
 arv_gvcp_packet_get_read_register_ack_value (const ArvGvcpPacket *packet)
 {
+	if (packet == NULL)
+		return 0;
 	return g_ntohl (*((guint32 *) ((void *) packet + sizeof (ArvGvcpPacket))));
 }
 
+static inline void
+arv_gvcp_packet_get_write_register_cmd_infos (const ArvGvcpPacket *packet, guint32 *address, guint32 *value)
+{
+	if (packet == NULL)
+		return;
+	if (address != NULL)
+		*address = g_ntohl (*((guint32 *) ((void *) packet + sizeof (ArvGvcpPacket))));
+	if (value != NULL)
+		*value = g_ntohl (*((guint32 *) ((void *) packet + sizeof (ArvGvcpPacket) + sizeof (guint32))));
+}
+
 G_END_DECLS
 
 #endif