void arv_fake_camera_fill_buffer (ArvFakeCamera *camera, ArvBuffer *buffer, guint32 *packet_size) { struct timespec time; guint32 width; guint32 height; guint32 exposure_time_us; guint32 gain; guint32 pixel_format; size_t payload; if (camera == NULL || buffer == NULL) return; clock_gettime (CLOCK_MONOTONIC, &time); width = _get_register (camera, ARV_FAKE_CAMERA_REGISTER_WIDTH); height = _get_register (camera, ARV_FAKE_CAMERA_REGISTER_HEIGHT); payload = width * height; if (buffer->priv->size < payload) { buffer->priv->status = ARV_BUFFER_STATUS_SIZE_MISMATCH; return; } buffer->priv->gvsp_payload_type = ARV_GVSP_PAYLOAD_TYPE_IMAGE; buffer->priv->width = width; buffer->priv->height = height; buffer->priv->status = ARV_BUFFER_STATUS_SUCCESS; buffer->priv->timestamp_ns = ((guint64) time.tv_sec) * 1000000000LL + time.tv_nsec; buffer->priv->frame_id = camera->priv->frame_id++; buffer->priv->pixel_format = _get_register (camera, ARV_FAKE_CAMERA_REGISTER_PIXEL_FORMAT); #if GLIB_CHECK_VERSION(2,32,0) g_mutex_lock (&camera->priv->fill_pattern_mutex); #else g_mutex_lock (camera->priv->fill_pattern_mutex); #endif arv_fake_camera_read_register (camera, ARV_FAKE_CAMERA_REGISTER_EXPOSURE_TIME_US, &exposure_time_us); arv_fake_camera_read_register (camera, ARV_FAKE_CAMERA_REGISTER_GAIN_RAW, &gain); arv_fake_camera_read_register (camera, ARV_FAKE_CAMERA_REGISTER_PIXEL_FORMAT, &pixel_format); camera->priv->fill_pattern_callback (buffer, camera->priv->fill_pattern_data, exposure_time_us, gain, pixel_format); #if GLIB_CHECK_VERSION(2,32,0) g_mutex_unlock (&camera->priv->fill_pattern_mutex); #else g_mutex_unlock (camera->priv->fill_pattern_mutex); #endif if (packet_size != NULL) *packet_size = _get_register (camera, ARV_GVBS_STREAM_CHANNEL_0_PACKET_SIZE_OFFSET); }
guint32 arv_fake_camera_get_heartbeat_timeout (ArvFakeCamera *camera) { guint32 value; arv_fake_camera_read_register (camera, ARV_GVBS_HEARTBEAT_TIMEOUT_OFFSET, &value); return value; }
guint32 arv_fake_camera_get_control_channel_privilege (ArvFakeCamera *camera) { guint32 value; arv_fake_camera_read_register (camera, ARV_GVBS_CONTROL_CHANNEL_PRIVILEGE_OFFSET, &value); return value; }
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 gboolean arv_fake_device_read_register (ArvDevice *device, guint32 address, guint32 *value) { return arv_fake_camera_read_register (ARV_FAKE_DEVICE (device)->priv->camera, address, value); }
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; }