static void
_send_packet_request (ArvGvStreamThreadData *thread_data,
guint32 frame_id,
guint32 first_block,
guint32 last_block)
{
ArvGvcpPacket *packet;
size_t packet_size;
thread_data->packet_id = arv_gvcp_next_packet_id (thread_data->packet_id);
packet = arv_gvcp_packet_new_packet_resend_cmd (frame_id, first_block, last_block,
thread_data->packet_id, &packet_size);
arv_log_stream_thread ("[GvStream::send_packet_request] frame_id = %u (%d - %d)",
frame_id, first_block, last_block);
arv_gvcp_packet_debug (packet, ARV_DEBUG_LEVEL_LOG);
g_socket_send_to (thread_data->socket, thread_data->device_address, (const char *) packet, packet_size,
NULL, NULL);
arv_gvcp_packet_free (packet);
}
static void
arv_gv_interface_receive_hello_packet (ArvGvInterface *gv_interface)
{
GPollFD *poll_fd;
GSList *iter;
char buffer[ARV_GV_INTERFACE_SOCKET_BUFFER_SIZE] __attribute__((may_alias));
int count;
int i;
if (gv_interface->priv->n_discover_infos ==0)
return;
poll_fd = g_new (GPollFD, gv_interface->priv->n_discover_infos);
for (i = 0, iter = gv_interface->priv->discover_infos_list; iter != NULL; i++, iter = iter->next) {
ArvGvInterfaceDiscoverInfos *infos = iter->data;
poll_fd[i].fd = g_socket_get_fd (infos->socket);
poll_fd[i].events = G_IO_IN;
poll_fd[i].revents = 0;
}
do {
if (g_poll (poll_fd, gv_interface->priv->n_discover_infos,
ARV_GV_INTERFACE_DISCOVERY_TIMEOUT_MS) == 0) {
g_free (poll_fd);
return;
}
for (i = 0, iter = gv_interface->priv->discover_infos_list; iter != NULL; i++, iter = iter->next) {
ArvGvInterfaceDiscoverInfos *infos = iter->data;
do {
g_socket_set_blocking (infos->socket, FALSE);
count = g_socket_receive (infos->socket, buffer, ARV_GV_INTERFACE_SOCKET_BUFFER_SIZE,
NULL, NULL);
g_socket_set_blocking (infos->socket, TRUE);
if (count > 0) {
ArvGvcpPacket *packet = (ArvGvcpPacket *) buffer;
if (g_ntohs (packet->header.command) == ARV_GVCP_COMMAND_DISCOVERY_ACK &&
g_ntohs (packet->header.id) == 0xffff) {
ArvGvInterfaceDeviceInfos *device_infos;
GInetAddress *interface_address;
char *address_string;
char *data = buffer + sizeof (ArvGvcpHeader);
char *serial_number;
char *manufacturer;
char *key;
arv_gvcp_packet_debug (packet, ARV_DEBUG_LEVEL_LOG);
manufacturer = g_strndup (&data[ARV_GVBS_MANUFACTURER_NAME_OFFSET],
ARV_GVBS_MANUFACTURER_NAME_SIZE);
serial_number = g_strndup (&data[ARV_GVBS_SERIAL_NUMBER_OFFSET],
ARV_GVBS_SERIAL_NUMBER_SIZE);
key = g_strdup_printf ("%s-%s", manufacturer, serial_number);
g_free (manufacturer);
g_free (serial_number);
interface_address = g_inet_socket_address_get_address
(G_INET_SOCKET_ADDRESS (infos->interface_address));
device_infos = arv_gv_interface_device_infos_new (interface_address,
data);
address_string = g_inet_address_to_string (interface_address);
arv_debug_interface ("[GvInterface::discovery] Device '%s' found "
"(interface %s)",
key, address_string);
g_free (address_string);
g_hash_table_insert (gv_interface->priv->devices,
key, device_infos);
g_hash_table_insert (gv_interface->priv->devices_by_mac,
device_infos->mac_string, device_infos);
}
}
} while (count > 0);
}
} while (1);
}
void
handle_control_packet (ArvFakeGvCamera *gv_camera, GSocket *socket,
GSocketAddress *remote_address,
ArvGvcpPacket *packet, size_t size)
{
ArvGvcpPacket *ack_packet = NULL;
size_t ack_packet_size;
guint32 block_address;
guint32 block_size;
guint16 packet_id;
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_warning_device ("[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, ARV_DEBUG_LEVEL_LOG);
packet_id = arv_gvcp_packet_get_packet_id (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_device ("[FakeGvCamera::handle_control_packet] Discovery command");
arv_fake_camera_read_memory (gv_camera->camera, 0, ARV_GVBS_DISCOVERY_DATA_SIZE,
&ack_packet->data);
break;
case ARV_GVCP_COMMAND_READ_MEMORY_CMD:
arv_gvcp_packet_get_read_memory_cmd_infos (packet, &block_address, &block_size);
arv_debug_device ("[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,
packet_id, &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));
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_device ("[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_id,
&ack_packet_size);
break;
case ARV_GVCP_COMMAND_READ_REGISTER_CMD:
arv_gvcp_packet_get_read_register_cmd_infos (packet, ®ister_address);
arv_fake_camera_read_register (gv_camera->camera, register_address, ®ister_value);
arv_debug_device ("[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_id,
&ack_packet_size);
if (register_address == ARV_GVBS_CONTROL_CHANNEL_PRIVILEGE_OFFSET)
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, ®ister_address, ®ister_value);
arv_fake_camera_write_register (gv_camera->camera, register_address, register_value);
arv_debug_device ("[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_id,
&ack_packet_size);
break;
default:
arv_warning_device ("[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, ARV_DEBUG_LEVEL_LOG);
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 void
arv_gv_interface_discover (ArvGvInterface *gv_interface)
{
ArvGvDiscoverSocketList *socket_list;
GSList *iter;
char buffer[ARV_GV_INTERFACE_SOCKET_BUFFER_SIZE];
int count;
int i;
socket_list = arv_gv_discover_socket_list_new ();
if (socket_list->n_sockets < 1) {
arv_gv_discover_socket_list_free (socket_list);
return;
}
arv_gv_discover_socket_list_send_discover_packet (socket_list);
do {
if (g_poll (socket_list->poll_fds, socket_list->n_sockets, ARV_GV_INTERFACE_DISCOVERY_TIMEOUT_MS) == 0) {
arv_gv_discover_socket_list_free (socket_list);
return;
}
for (i = 0, iter = socket_list->sockets; iter != NULL; i++, iter = iter->next) {
ArvGvDiscoverSocket *discover_socket = iter->data;
do {
g_socket_set_blocking (discover_socket->socket, FALSE);
count = g_socket_receive (discover_socket->socket, buffer, ARV_GV_INTERFACE_SOCKET_BUFFER_SIZE,
NULL, NULL);
g_socket_set_blocking (discover_socket->socket, TRUE);
if (count > 0) {
ArvGvcpPacket *packet = (ArvGvcpPacket *) buffer;
if (g_ntohs (packet->header.command) == ARV_GVCP_COMMAND_DISCOVERY_ACK &&
g_ntohs (packet->header.id) == 0xffff) {
ArvGvInterfaceDeviceInfos *device_infos;
GInetAddress *interface_address;
char *address_string;
char *data = buffer + sizeof (ArvGvcpHeader);
arv_gvcp_packet_debug (packet, ARV_DEBUG_LEVEL_LOG);
interface_address = g_inet_socket_address_get_address
(G_INET_SOCKET_ADDRESS (discover_socket->interface_address));
device_infos = arv_gv_interface_device_infos_new (interface_address,
data);
address_string = g_inet_address_to_string (interface_address);
arv_debug_interface ("[GvInterface::discovery] Device '%s' found "
"(interface %s) user_name '%s' - MAC_name '%s'",
device_infos->name, address_string,
device_infos->user_name,
device_infos->mac_string);
g_free (address_string);
if (device_infos->name != NULL && device_infos->name[0] != '\0') {
arv_gv_interface_device_infos_ref (device_infos);
g_hash_table_replace (gv_interface->priv->devices,
device_infos->name, device_infos);
}
if (device_infos->user_name != NULL && device_infos->user_name[0] != '\0') {
arv_gv_interface_device_infos_ref (device_infos);
g_hash_table_replace (gv_interface->priv->devices,
device_infos->user_name, device_infos);
}
arv_gv_interface_device_infos_ref (device_infos);
g_hash_table_replace (gv_interface->priv->devices, device_infos->mac_string, device_infos);
arv_gv_interface_device_infos_unref (device_infos);
}
}
} while (count > 0);
}
} while (1);
}
gboolean
handle_control_packet (ArvGvFakeCamera *gv_fake_camera, GSocket *socket,
GSocketAddress *remote_address,
ArvGvcpPacket *packet, size_t size)
{
ArvGvcpPacket *ack_packet = NULL;
size_t ack_packet_size;
guint32 block_address;
guint32 block_size;
guint16 packet_id;
guint16 packet_type;
guint32 register_address;
guint32 register_value;
gboolean write_access;
gboolean success = FALSE;
if (gv_fake_camera->priv->controller_address != NULL) {
gint64 time;
guint64 elapsed_ms;
time = g_get_real_time ();
elapsed_ms = (time - gv_fake_camera->priv->controller_time) / 1000;
if (elapsed_ms > arv_fake_camera_get_heartbeat_timeout (gv_fake_camera->priv->camera)) {
g_object_unref (gv_fake_camera->priv->controller_address);
gv_fake_camera->priv->controller_address = NULL;
write_access = TRUE;
arv_warning_device ("[GvFakeCamera::handle_control_packet] Heartbeat timeout");
arv_fake_camera_set_control_channel_privilege (gv_fake_camera->priv->camera, 0);
} else
write_access = _g_inet_socket_address_is_equal
(G_INET_SOCKET_ADDRESS (remote_address),
G_INET_SOCKET_ADDRESS (gv_fake_camera->priv->controller_address));
} else
write_access = TRUE;
arv_gvcp_packet_debug (packet, ARV_DEBUG_LEVEL_LOG);
packet_id = arv_gvcp_packet_get_packet_id (packet);
packet_type = arv_gvcp_packet_get_packet_type (packet);
if (packet_type != ARV_GVCP_PACKET_TYPE_CMD) {
arv_warning_device ("[GvFakeCamera::handle_control_packet] Unknown packet type");
return FALSE;
}
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_device ("[GvFakeCamera::handle_control_packet] Discovery command");
arv_fake_camera_read_memory (gv_fake_camera->priv->camera, 0, ARV_GVBS_DISCOVERY_DATA_SIZE,
&ack_packet->data);
break;
case ARV_GVCP_COMMAND_READ_MEMORY_CMD:
arv_gvcp_packet_get_read_memory_cmd_infos (packet, &block_address, &block_size);
arv_debug_device ("[GvFakeCamera::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,
packet_id, &ack_packet_size);
arv_fake_camera_read_memory (gv_fake_camera->priv->camera, block_address, block_size,
arv_gvcp_packet_get_read_memory_ack_data (ack_packet));
break;
case ARV_GVCP_COMMAND_WRITE_MEMORY_CMD:
arv_gvcp_packet_get_write_memory_cmd_infos (packet, &block_address, &block_size);
if (!write_access) {
arv_warning_device("[GvFakeCamera::handle_control_packet] Ignore Write memory command %d (%d) not controller",
block_address, block_size);
break;
}
arv_debug_device ("[GvFakeCamera::handle_control_packet] Write memory command %d (%d)",
block_address, block_size);
arv_fake_camera_write_memory (gv_fake_camera->priv->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_id,
&ack_packet_size);
break;
case ARV_GVCP_COMMAND_READ_REGISTER_CMD:
arv_gvcp_packet_get_read_register_cmd_infos (packet, ®ister_address);
arv_fake_camera_read_register (gv_fake_camera->priv->camera, register_address, ®ister_value);
arv_debug_device ("[GvFakeCamera::handle_control_packet] Read register command %d -> %d",
register_address, register_value);
ack_packet = arv_gvcp_packet_new_read_register_ack (register_value, packet_id,
&ack_packet_size);
if (register_address == ARV_GVBS_CONTROL_CHANNEL_PRIVILEGE_OFFSET)
gv_fake_camera->priv->controller_time = g_get_real_time ();
break;
case ARV_GVCP_COMMAND_WRITE_REGISTER_CMD:
arv_gvcp_packet_get_write_register_cmd_infos (packet, ®ister_address, ®ister_value);
if (!write_access) {
arv_warning_device("[GvFakeCamera::handle_control_packet] Ignore Write register command %d (%d) not controller",
register_address, register_value);
break;
}
arv_fake_camera_write_register (gv_fake_camera->priv->camera, register_address, register_value);
arv_debug_device ("[GvFakeCamera::handle_control_packet] Write register command %d -> %d",
register_address, register_value);
ack_packet = arv_gvcp_packet_new_write_register_ack (1, packet_id,
&ack_packet_size);
break;
default:
arv_warning_device ("[GvFakeCamera::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, ARV_DEBUG_LEVEL_LOG);
g_free (ack_packet);
success = TRUE;
}
if (gv_fake_camera->priv->controller_address == NULL &&
arv_fake_camera_get_control_channel_privilege (gv_fake_camera->priv->camera) != 0) {
g_object_ref (remote_address);
arv_debug_device("[GvFakeCamera::handle_control_packet] New controller");
gv_fake_camera->priv->controller_address = remote_address;
gv_fake_camera->priv->controller_time = g_get_real_time ();
}
else if (gv_fake_camera->priv->controller_address != NULL &&
arv_fake_camera_get_control_channel_privilege (gv_fake_camera->priv->camera) == 0) {
g_object_unref (gv_fake_camera->priv->controller_address);
arv_debug_device("[GvFakeCamera::handle_control_packet] Controller releases");
gv_fake_camera->priv->controller_address = NULL;
gv_fake_camera->priv->controller_time = g_get_real_time ();
}
return success;
}
static gboolean
_read_memory (ArvGvDeviceIOData *io_data, guint32 address, guint32 size, void *buffer, GError **error)
{
ArvGvcpPacket *packet;
size_t packet_size;
size_t answer_size;
int count;
unsigned int n_retries = 0;
gboolean success = FALSE;
answer_size = arv_gvcp_packet_get_read_memory_ack_size (size);
g_return_val_if_fail (answer_size <= ARV_GV_DEVICE_BUFFER_SIZE, FALSE);
g_mutex_lock (io_data->mutex);
packet = arv_gvcp_packet_new_read_memory_cmd (address,
((size + sizeof (guint32) - 1)
/ sizeof (guint32)) * sizeof (guint32),
0, &packet_size);
do {
io_data->packet_id = arv_gvcp_next_packet_id (io_data->packet_id);
arv_gvcp_packet_set_packet_id (packet, io_data->packet_id);
arv_gvcp_packet_debug (packet, ARV_DEBUG_LEVEL_LOG);
g_socket_send_to (io_data->socket, io_data->device_address,
(const char *) packet, packet_size,
NULL, NULL);
if (g_poll (&io_data->poll_in_event, 1, io_data->gvcp_timeout_ms) > 0) {
count = g_socket_receive (io_data->socket, io_data->buffer,
ARV_GV_DEVICE_BUFFER_SIZE, NULL, NULL);
if (count >= answer_size) {
ArvGvcpPacket *ack_packet = io_data->buffer;
ArvGvcpPacketType packet_type;
ArvGvcpCommand command;
guint16 packet_id;
arv_gvcp_packet_debug (ack_packet, ARV_DEBUG_LEVEL_LOG);
packet_type = arv_gvcp_packet_get_packet_type (ack_packet);
command = arv_gvcp_packet_get_command (ack_packet);
packet_id = arv_gvcp_packet_get_packet_id (ack_packet);
if (packet_type == ARV_GVCP_PACKET_TYPE_ACK &&
command == ARV_GVCP_COMMAND_READ_MEMORY_ACK &&
packet_id == io_data->packet_id) {
memcpy (buffer, arv_gvcp_packet_get_read_memory_ack_data (ack_packet), size);
success = TRUE;
} else
arv_gvcp_packet_debug (ack_packet, ARV_DEBUG_LEVEL_WARNING);
}
}
n_retries++;
} while (!success && n_retries < io_data->gvcp_n_retries);
arv_gvcp_packet_free (packet);
g_mutex_unlock (io_data->mutex);
if (!success) {
if (error != NULL && *error == NULL)
*error = g_error_new (ARV_DEVICE_ERROR, ARV_DEVICE_STATUS_TIMEOUT,
"[ArvDevice::read_memory] Timeout");
}
return success;
}
gboolean
_write_register (ArvGvDeviceIOData *io_data, guint32 address, guint32 value, GError **error)
{
ArvGvcpPacket *packet;
size_t packet_size;
int count;
unsigned int n_retries = 0;
gboolean success = FALSE;
g_mutex_lock (io_data->mutex);
packet = arv_gvcp_packet_new_write_register_cmd (address, value, io_data->packet_id, &packet_size);
do {
io_data->packet_id = arv_gvcp_next_packet_id (io_data->packet_id);
arv_gvcp_packet_set_packet_id (packet, io_data->packet_id);
arv_gvcp_packet_debug (packet, ARV_DEBUG_LEVEL_LOG);
g_socket_send_to (io_data->socket, io_data->device_address, (const char *) packet, packet_size,
NULL, NULL);
if (g_poll (&io_data->poll_in_event, 1, io_data->gvcp_timeout_ms) > 0) {
count = g_socket_receive (io_data->socket, io_data->buffer,
ARV_GV_DEVICE_BUFFER_SIZE, NULL, NULL);
if (count > 0) {
ArvGvcpPacket *ack_packet = io_data->buffer;
ArvGvcpPacketType packet_type;
ArvGvcpCommand command;
guint16 packet_id;
arv_gvcp_packet_debug (ack_packet, ARV_DEBUG_LEVEL_LOG);
packet_type = arv_gvcp_packet_get_packet_type (ack_packet);
command = arv_gvcp_packet_get_command (ack_packet);
packet_id = arv_gvcp_packet_get_packet_id (ack_packet);
arv_log_gvcp ("%d, %d, %d", packet_type, command, packet_id);
if (packet_type == ARV_GVCP_PACKET_TYPE_ACK &&
command == ARV_GVCP_COMMAND_WRITE_REGISTER_ACK &&
packet_id == io_data->packet_id)
success = TRUE;
else
arv_gvcp_packet_debug (ack_packet, ARV_DEBUG_LEVEL_WARNING);
}
}
n_retries++;
} while (!success && n_retries < io_data->gvcp_n_retries);
arv_gvcp_packet_free (packet);
g_mutex_unlock (io_data->mutex);
if (!success) {
if (error != NULL && *error == NULL)
*error = g_error_new (ARV_DEVICE_ERROR, ARV_DEVICE_STATUS_TIMEOUT,
"[ArvDevice::write_register] Timeout");
}
return success;
}