static void _update_socket (ArvGvStreamThreadData *thread_data, ArvBuffer *buffer) { int buffer_size, fd; if (thread_data->socket_buffer_option == ARV_GV_STREAM_SOCKET_BUFFER_FIXED && thread_data->socket_buffer_size <= 0) return; fd = g_socket_get_fd (thread_data->socket); switch (thread_data->socket_buffer_option) { case ARV_GV_STREAM_SOCKET_BUFFER_FIXED: buffer_size = thread_data->socket_buffer_size; break; case ARV_GV_STREAM_SOCKET_BUFFER_AUTO: if (thread_data->socket_buffer_size <= 0) buffer_size = buffer->size; else buffer_size = MIN (buffer->size, thread_data->socket_buffer_size); break; } if (buffer_size != thread_data->current_socket_buffer_size) { setsockopt (fd, SOL_SOCKET, SO_RCVBUF, &buffer_size, sizeof (buffer_size)); thread_data->current_socket_buffer_size = buffer_size; arv_debug_stream_thread ("[GvStream::update_socket] Socket buffer size set to %d", buffer_size); } }
static void _process_data_block (ArvGvStreamThreadData *thread_data, ArvGvStreamFrameData *frame, ArvGvspPacket *packet, guint32 packet_id, size_t read_count) { size_t block_size; ptrdiff_t block_offset; ptrdiff_t block_end; if (frame->buffer->status != ARV_BUFFER_STATUS_FILLING) return; if (packet_id > frame->n_packets - 2 || packet_id < 1) { arv_gvsp_packet_debug (packet, read_count, ARV_DEBUG_LEVEL_DEBUG); frame->buffer->status = ARV_BUFFER_STATUS_WRONG_PACKET_ID; return; } block_size = arv_gvsp_packet_get_data_size (read_count); block_offset = (packet_id - 1) * thread_data->data_size; block_end = block_size + block_offset; if (block_end > frame->buffer->size) { arv_debug_stream_thread ("[GvStream::_process_data_block] %d unexpected bytes in packet %u " " for frame %u", block_end - frame->buffer->size, packet_id, frame->frame_id); thread_data->n_size_mismatch_errors++; block_end = frame->buffer->size; block_size = block_end - block_offset; } memcpy (((char *) frame->buffer->data) + block_offset, &packet->data, block_size); if (frame->packet_data[packet_id].time_us > 0) { thread_data->n_resent_packets++; arv_log_stream_thread ("[GvStream::_process_data_block] Received resent packet %u for frame %u", packet_id, frame->frame_id); } }
static void * arv_uv_stream_thread (void *data) { ArvUvStreamThreadData *thread_data = data; ArvUvspPacket *packet; ArvBuffer *buffer = NULL; void *incoming_buffer; guint64 offset; size_t transferred; arv_log_stream_thread ("Start USB3Vision stream thread"); incoming_buffer = g_malloc (MAXIMUM_TRANSFER_SIZE); if (thread_data->callback != NULL) thread_data->callback (thread_data->user_data, ARV_STREAM_CALLBACK_TYPE_INIT, NULL); offset = 0; while (!thread_data->cancel) { size_t size; transferred = 0; if (buffer == NULL) size = thread_data->leader_size; else { if (offset < buffer->priv->size) size = MIN (thread_data->payload_size, buffer->priv->size - offset); else size = thread_data->trailer_size; } /* Avoid unnecessary memory copy by transferring data directly to the image buffer */ if (buffer != NULL && buffer->priv->status == ARV_BUFFER_STATUS_FILLING && offset + size <= buffer->priv->size) packet = buffer->priv->data + offset; else packet = incoming_buffer; arv_uv_device_bulk_transfer (thread_data->uv_device, 0x81 | LIBUSB_ENDPOINT_IN, packet, size, &transferred, 0, NULL); if (transferred > 0) { ArvUvspPacketType packet_type; arv_log_sp ("Received %d bytes", transferred); arv_uvsp_packet_debug (packet, ARV_DEBUG_LEVEL_LOG); packet_type = arv_uvsp_packet_get_packet_type (packet); switch (packet_type) { case ARV_UVSP_PACKET_TYPE_LEADER: if (buffer != NULL) { arv_debug_stream_thread ("New leader received while a buffer is still open"); buffer->priv->status = ARV_BUFFER_STATUS_MISSING_PACKETS; arv_stream_push_output_buffer (thread_data->stream, buffer); thread_data->n_failures++; buffer = NULL; } buffer = arv_stream_pop_input_buffer (thread_data->stream); if (buffer != NULL) { buffer->priv->status = ARV_BUFFER_STATUS_FILLING; buffer->priv->gvsp_payload_type = ARV_GVSP_PAYLOAD_TYPE_IMAGE; arv_uvsp_packet_get_region (packet, &buffer->priv->width, &buffer->priv->height, &buffer->priv->x_offset, &buffer->priv->y_offset); buffer->priv->frame_id = arv_uvsp_packet_get_frame_id (packet); buffer->priv->timestamp_ns = arv_uvsp_packet_get_timestamp (packet); offset = 0; } else thread_data->n_underruns++; break; case ARV_UVSP_PACKET_TYPE_TRAILER: if (buffer != NULL) { arv_log_stream_thread ("Received %" G_GUINT64_FORMAT " bytes - expected %" G_GUINT64_FORMAT, offset, buffer->priv->size); buffer->priv->status = ARV_BUFFER_STATUS_SUCCESS; arv_stream_push_output_buffer (thread_data->stream, buffer); thread_data->n_completed_buffers++; buffer = NULL; } break; case ARV_UVSP_PACKET_TYPE_DATA: if (buffer != NULL && buffer->priv->status == ARV_BUFFER_STATUS_FILLING) { if (offset + transferred <= buffer->priv->size) { if (packet == incoming_buffer) memcpy (((char *) buffer->priv->data) + offset, packet, transferred); offset += transferred; } else buffer->priv->status = ARV_BUFFER_STATUS_SIZE_MISMATCH; } break; default: arv_debug_stream_thread ("Unkown packet type"); break; } } } if (thread_data->callback != NULL) thread_data->callback (thread_data->user_data, ARV_STREAM_CALLBACK_TYPE_EXIT, NULL); g_free (incoming_buffer); arv_log_stream_thread ("Stop USB3Vision stream thread"); return NULL; }
void * arv_fake_gv_camera_thread (void *user_data) { ArvFakeGvCamera *gv_camera = user_data; ArvBuffer *image_buffer = NULL; GError *error = NULL; GSocketAddress *stream_address = NULL; void *packet_buffer; size_t packet_size; size_t payload = 0; guint16 block_id; ptrdiff_t offset; guint32 gv_packet_size; packet_buffer = g_malloc (ARV_FAKE_GV_CAMERA_BUFFER_SIZE); do { 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; g_object_unref (image_buffer); image_buffer = NULL; arv_debug_stream_thread ("[FakeGvCamera::stream_thread] Stop stream"); } g_usleep (100000); } else { if (stream_address == NULL) { GInetAddress *inet_address; char *inet_address_string; stream_address = arv_fake_camera_get_stream_address (gv_camera->camera); inet_address = g_inet_socket_address_get_address (G_INET_SOCKET_ADDRESS (stream_address)); inet_address_string = g_inet_address_to_string (inet_address); arv_debug_stream_thread ("[FakeGvCamera::stream_thread] Start stream to %s (%d)", inet_address_string, g_inet_socket_address_get_port (G_INET_SOCKET_ADDRESS (stream_address))); g_free (inet_address_string); payload = arv_fake_camera_get_payload (gv_camera->camera); image_buffer = arv_buffer_new (payload, NULL); } arv_fake_camera_wait_for_next_frame (gv_camera->camera); arv_fake_camera_fill_buffer (gv_camera->camera, image_buffer, &gv_packet_size); block_id = 0; packet_size = ARV_FAKE_GV_CAMERA_BUFFER_SIZE; arv_gvsp_packet_new_data_leader (image_buffer->frame_id, block_id, image_buffer->timestamp_ns, image_buffer->pixel_format, image_buffer->width, image_buffer->height, image_buffer->x_offset, image_buffer->y_offset, packet_buffer, &packet_size); g_socket_send_to (gv_camera->gvsp_socket, stream_address, packet_buffer, packet_size, NULL, &error); if (error != NULL) { arv_warning_stream_thread ("[ArvFakeGvCamera::stream_thread] Socket send error [%s]", error->message); g_error_free (error); error = NULL; } block_id++; offset = 0; while (offset < payload) { size_t data_size; data_size = MIN (gv_packet_size - ARV_GVSP_PACKET_PROTOCOL_OVERHEAD, payload - offset); packet_size = ARV_FAKE_GV_CAMERA_BUFFER_SIZE; arv_gvsp_packet_new_data_block (image_buffer->frame_id, block_id, data_size, ((char *) image_buffer->data) + offset, packet_buffer, &packet_size); g_socket_send_to (gv_camera->gvsp_socket, stream_address, packet_buffer, packet_size, NULL, NULL); offset += data_size; block_id++; } packet_size = ARV_FAKE_GV_CAMERA_BUFFER_SIZE; arv_gvsp_packet_new_data_trailer (image_buffer->frame_id, block_id, packet_buffer, &packet_size); g_socket_send_to (gv_camera->gvsp_socket, stream_address, packet_buffer, packet_size, NULL, NULL); } } while (!cancel); if (stream_address != NULL) g_object_unref (stream_address); if (image_buffer != NULL) g_object_unref (image_buffer); g_free (packet_buffer); return NULL; }
static void * arv_gv_stream_thread (void *data) { ArvGvStreamThreadData *thread_data = data; ArvGvStreamFrameData *frame; ArvGvspPacket *packet; guint32 packet_id; guint32 frame_id; GTimeVal current_time; guint64 time_us; GPollFD poll_fd; size_t read_count; int timeout_ms; int n_events; int i; gboolean first_packet = TRUE; thread_data->frames = NULL; arv_debug_stream_thread ("[GvStream::stream_thread] Packet timeout = %g ms", thread_data->packet_timeout_us / 1000.0); arv_debug_stream_thread ("[GvStream::stream_thread] Frame retention = %g ms", thread_data->frame_retention_us / 1000.0); if (thread_data->callback != NULL) thread_data->callback (thread_data->user_data, ARV_STREAM_CALLBACK_TYPE_INIT, NULL); poll_fd.fd = g_socket_get_fd (thread_data->socket); poll_fd.events = G_IO_IN; poll_fd.revents = 0; packet = g_malloc0 (ARV_GV_STREAM_INCOMING_BUFFER_SIZE); do { if (thread_data->frames != NULL) timeout_ms = thread_data->packet_timeout_us / 1000; else timeout_ms = ARV_GV_STREAM_POLL_TIMEOUT_US / 1000; n_events = g_poll (&poll_fd, 1, timeout_ms); g_get_current_time (¤t_time); time_us = current_time.tv_sec * 1000000 + current_time.tv_usec; if (n_events > 0) { thread_data->n_received_packets++; read_count = g_socket_receive (thread_data->socket, (char *) packet, ARV_GV_STREAM_INCOMING_BUFFER_SIZE, NULL, NULL); frame_id = arv_gvsp_packet_get_frame_id (packet); packet_id = arv_gvsp_packet_get_packet_id (packet); if (first_packet) { thread_data->last_frame_id = frame_id - 1; first_packet = FALSE; } frame = _find_frame_data (thread_data, frame_id, packet, packet_id, read_count, time_us); if (frame != NULL) { ArvGvspPacketType packet_type = arv_gvsp_packet_get_packet_type (packet); if (packet_type != ARV_GVSP_PACKET_TYPE_OK && packet_type != ARV_GVSP_PACKET_TYPE_RESEND) { arv_debug_stream_thread ("[GvStream::stream_thread]" " Error packet at dt = %" G_GINT64_FORMAT ", packet id = %u" " frame id = %u", time_us - frame->first_packet_time_us, packet_id, frame->frame_id); arv_gvsp_packet_debug (packet, read_count, ARV_DEBUG_LEVEL_DEBUG); frame->error_packet_received = TRUE; thread_data->n_error_packets++; } else { /* Check for duplicated packets */ if (packet_id < frame->n_packets) { if (frame->packet_data[packet_id].received) thread_data->n_duplicated_packets++; else frame->packet_data[packet_id].received = TRUE; } /* Keep track of last packet of a continuous block starting from packet 0 */ for (i = frame->last_valid_packet + 1; i < frame->n_packets; i++) if (!frame->packet_data[i].received) break; frame->last_valid_packet = i - 1; switch (arv_gvsp_packet_get_content_type (packet)) { case ARV_GVSP_CONTENT_TYPE_DATA_LEADER: _process_data_leader (thread_data, frame, packet, packet_id); break; case ARV_GVSP_CONTENT_TYPE_DATA_BLOCK: _process_data_block (thread_data, frame, packet, packet_id, read_count); break; case ARV_GVSP_CONTENT_TYPE_DATA_TRAILER: _process_data_trailer (thread_data, frame, packet, packet_id); break; default: thread_data->n_ignored_packets++; break; } _missing_packet_check (thread_data, frame, packet_id, time_us); } } else thread_data->n_ignored_packets++; } else frame = NULL; _check_frame_completion (thread_data, time_us, frame); } while (!thread_data->cancel); _flush_frames (thread_data); if (thread_data->callback != NULL) thread_data->callback (thread_data->user_data, ARV_STREAM_CALLBACK_TYPE_EXIT, NULL); g_free (packet); return NULL; }
static void _check_frame_completion (ArvGvStreamThreadData *thread_data, guint64 time_us, ArvGvStreamFrameData *current_frame) { GSList *iter; ArvGvStreamFrameData *frame; gboolean can_close_frame = TRUE; for (iter = thread_data->frames; iter != NULL;) { frame = iter->data; if (can_close_frame && thread_data->packet_resend == ARV_GV_STREAM_PACKET_RESEND_NEVER && iter->next != NULL) { frame->buffer->status = ARV_BUFFER_STATUS_MISSING_PACKETS; arv_debug_stream_thread ("[GvStream::_check_frame_completion] Incomplete frame %u", frame->frame_id); _close_frame (thread_data, frame); thread_data->frames = iter->next; g_slist_free_1 (iter); iter = thread_data->frames; continue; } if (can_close_frame && frame->last_valid_packet == frame->n_packets - 1) { frame->buffer->status = ARV_BUFFER_STATUS_SUCCESS; arv_log_stream_thread ("[GvStream::_check_frame_completion] Completed frame %u", frame->frame_id); _close_frame (thread_data, frame); thread_data->frames = iter->next; g_slist_free_1 (iter); iter = thread_data->frames; continue; } if (can_close_frame && time_us - frame->last_packet_time_us >= thread_data->frame_retention_us) { frame->buffer->status = ARV_BUFFER_STATUS_TIMEOUT; arv_debug_stream_thread ("[GvStream::_check_frame_completion] Timeout for frame %u " "at dt = %Lu", frame->frame_id, time_us - frame->first_packet_time_us); #if 0 if (arv_debug_check (&arv_debug_category_stream_thread, ARV_DEBUG_LEVEL_LOG)) { int i; arv_log_stream_thread ("frame_id = %Lu", frame->frame_id); arv_log_stream_thread ("last_valid_packet = %d", frame->last_valid_packet); for (i = 0; i < frame->n_packets; i++) { arv_log_stream_thread ("%d - time = %Lu%s", i, frame->packet_data[i].time_us, frame->packet_data[i].received ? " - OK" : ""); } } #endif _close_frame (thread_data, frame); thread_data->frames = iter->next; g_slist_free_1 (iter); iter = thread_data->frames; continue; } can_close_frame = FALSE; if (frame != current_frame && time_us - frame->last_packet_time_us >= thread_data->packet_timeout_us) { _missing_packet_check (thread_data, frame, frame->n_packets - 1, time_us); iter = iter->next; continue; } iter = iter->next; } }
static ArvGvStreamFrameData * _find_frame_data (ArvGvStreamThreadData *thread_data, guint32 frame_id, ArvGvspPacket *packet, guint32 packet_id, size_t read_count, guint64 time_us) { ArvGvStreamFrameData *frame = NULL; ArvBuffer *buffer; GSList *iter; guint n_packets = 0; gint16 frame_id_inc; for (iter = thread_data->frames; iter != NULL; iter = iter->next) { frame = iter->data; if (frame->frame_id == frame_id) { frame->last_packet_time_us = time_us; return frame; } } frame_id_inc = (gint16) frame_id - (gint16) thread_data->last_frame_id; if (frame_id_inc < 1 && frame_id_inc > -ARV_GV_STREAM_DISCARD_LATE_FRAME_THRESHOLD) { arv_debug_stream_thread ("[GvStream::_find_frame_data] Discard late frame %u (last: %u)", frame_id, thread_data->last_frame_id); return NULL; } buffer = arv_stream_pop_input_buffer (thread_data->stream); if (buffer == NULL) { thread_data->n_underruns++; return NULL; } frame = g_new0 (ArvGvStreamFrameData, 1); frame->error_packet_received = FALSE; frame->frame_id = frame_id; frame->last_valid_packet = -1; frame->buffer = buffer; _update_socket (thread_data, frame->buffer); frame->buffer->status = ARV_BUFFER_STATUS_FILLING; n_packets = (frame->buffer->size + thread_data->data_size - 1) / thread_data->data_size + 2; frame->first_packet_time_us = time_us; frame->last_packet_time_us = time_us; frame->packet_data = g_new0 (ArvGvStreamPacketData, n_packets); frame->n_packets = n_packets; if (thread_data->callback != NULL && frame->buffer != NULL) thread_data->callback (thread_data->user_data, ARV_STREAM_CALLBACK_TYPE_START_BUFFER, NULL); thread_data->last_frame_id = frame_id; if (frame_id_inc > 1) { thread_data->n_missing_frames++; arv_log_stream_thread ("[GvStream::_find_frame_data] Missed %d frame(s) before %u", frame_id_inc - 1, frame_id); } thread_data->frames = g_slist_append (thread_data->frames, frame); arv_log_stream_thread ("[GvStream::_find_frame_data] Start frame %u", frame_id); return frame; }
void * _thread (void *user_data) { ArvGvFakeCamera *gv_fake_camera = user_data; ArvBuffer *image_buffer = NULL; GError *error = NULL; GSocketAddress *stream_address = NULL; void *packet_buffer; size_t packet_size; size_t payload = 0; guint16 block_id; ptrdiff_t offset; guint32 gv_packet_size; GInputVector input_vector; int n_events; gboolean is_streaming = FALSE; input_vector.buffer = g_malloc0 (ARV_GV_FAKE_CAMERA_BUFFER_SIZE); input_vector.size = ARV_GV_FAKE_CAMERA_BUFFER_SIZE; packet_buffer = g_malloc (ARV_GV_FAKE_CAMERA_BUFFER_SIZE); do { guint64 next_timestamp_us; guint64 sleep_time_us; if (is_streaming) { sleep_time_us = arv_fake_camera_get_sleep_time_for_next_frame (gv_fake_camera->priv->camera, &next_timestamp_us); } else { sleep_time_us = 100000; next_timestamp_us = g_get_real_time () + sleep_time_us; } do { gint timeout_ms; timeout_ms = MIN (100, (next_timestamp_us - g_get_real_time ()) / 1000LL); if (timeout_ms < 0) timeout_ms = 0; n_events = g_poll (gv_fake_camera->priv->gvcp_fds, 2, timeout_ms); if (n_events > 0) { GSocketAddress *remote_address = NULL; int count; count = g_socket_receive_message (gv_fake_camera->priv->gvcp_socket, &remote_address, &input_vector, 1, NULL, NULL, NULL, NULL, NULL); if (count > 0) { if (handle_control_packet (gv_fake_camera, gv_fake_camera->priv->gvcp_socket, remote_address, input_vector.buffer, count)) arv_debug_device ("[GvFakeCamera::thread] Control packet received"); } g_clear_object (&remote_address); if (gv_fake_camera->priv->discovery_socket != NULL) { count = g_socket_receive_message (gv_fake_camera->priv->discovery_socket, &remote_address, &input_vector, 1, NULL, NULL, NULL, NULL, NULL); if (count > 0) { if (handle_control_packet (gv_fake_camera, gv_fake_camera->priv->discovery_socket, remote_address, input_vector.buffer, count)) arv_debug_device ("[GvFakeCamera::thread]" " Control packet received on discovery socket\n"); } g_clear_object (&remote_address); } if (arv_fake_camera_get_control_channel_privilege (gv_fake_camera->priv->camera) == 0 || arv_fake_camera_get_acquisition_status (gv_fake_camera->priv->camera) == 0) { if (stream_address != NULL) { g_object_unref (stream_address); stream_address = NULL; g_object_unref (image_buffer); image_buffer = NULL; arv_debug_stream_thread ("[GvFakeCamera::thread] Stop stream"); } is_streaming = FALSE; } } } while (!g_atomic_int_get (&gv_fake_camera->priv->cancel) && g_get_real_time () < next_timestamp_us); if (arv_fake_camera_get_control_channel_privilege (gv_fake_camera->priv->camera) != 0 && arv_fake_camera_get_acquisition_status (gv_fake_camera->priv->camera) != 0) { if (stream_address == NULL) { GInetAddress *inet_address; char *inet_address_string; stream_address = arv_fake_camera_get_stream_address (gv_fake_camera->priv->camera); inet_address = g_inet_socket_address_get_address (G_INET_SOCKET_ADDRESS (stream_address)); inet_address_string = g_inet_address_to_string (inet_address); arv_debug_stream_thread ("[GvFakeCamera::thread] Start stream to %s (%d)", inet_address_string, g_inet_socket_address_get_port (G_INET_SOCKET_ADDRESS (stream_address))); g_free (inet_address_string); payload = arv_fake_camera_get_payload (gv_fake_camera->priv->camera); image_buffer = arv_buffer_new (payload, NULL); } arv_fake_camera_fill_buffer (gv_fake_camera->priv->camera, image_buffer, &gv_packet_size); arv_debug_stream_thread ("[GvFakeCamera::thread] Send frame %d", image_buffer->priv->frame_id); block_id = 0; packet_size = ARV_GV_FAKE_CAMERA_BUFFER_SIZE; arv_gvsp_packet_new_data_leader (image_buffer->priv->frame_id, block_id, image_buffer->priv->timestamp_ns, image_buffer->priv->pixel_format, image_buffer->priv->width, image_buffer->priv->height, image_buffer->priv->x_offset, image_buffer->priv->y_offset, packet_buffer, &packet_size); g_socket_send_to (gv_fake_camera->priv->gvsp_socket, stream_address, packet_buffer, packet_size, NULL, &error); if (error != NULL) { arv_warning_stream_thread ("[GvFakeCamera::thread] Failed to send leader for frame %d: %s", image_buffer->priv->frame_id, error->message); g_clear_error (&error); } block_id++; offset = 0; while (offset < payload) { size_t data_size; data_size = MIN (gv_packet_size - ARV_GVSP_PACKET_PROTOCOL_OVERHEAD, payload - offset); packet_size = ARV_GV_FAKE_CAMERA_BUFFER_SIZE; arv_gvsp_packet_new_data_block (image_buffer->priv->frame_id, block_id, data_size, ((char *) image_buffer->priv->data) + offset, packet_buffer, &packet_size); g_socket_send_to (gv_fake_camera->priv->gvsp_socket, stream_address, packet_buffer, packet_size, NULL, &error); if (error != NULL) { arv_debug_stream_thread ("[GvFakeCamera::thread] Failed to send frame block %d for frame: %s", block_id, image_buffer->priv->frame_id, error->message); g_clear_error (&error); } offset += data_size; block_id++; } packet_size = ARV_GV_FAKE_CAMERA_BUFFER_SIZE; arv_gvsp_packet_new_data_trailer (image_buffer->priv->frame_id, block_id, packet_buffer, &packet_size); g_socket_send_to (gv_fake_camera->priv->gvsp_socket, stream_address, packet_buffer, packet_size, NULL, &error); if (error != NULL) { arv_debug_stream_thread ("[GvFakeCamera::thread] Failed to send trailer for frame %d: %s", image_buffer->priv->frame_id, error->message); g_clear_error (&error); } is_streaming = TRUE; } } while (!g_atomic_int_get (&gv_fake_camera->priv->cancel)); if (stream_address != NULL) g_object_unref (stream_address); if (image_buffer != NULL) g_object_unref (image_buffer); g_free (packet_buffer); g_free (input_vector.buffer); return NULL; }