static void *
arv_fake_stream_thread (void *data)
{
ArvFakeStreamThreadData *thread_data = data;
ArvBuffer *buffer;
arv_log_stream_thread ("[FakeStream::thread] Start");
if (thread_data->callback != NULL)
thread_data->callback (thread_data->user_data, ARV_STREAM_CALLBACK_TYPE_INIT, NULL);
while (!thread_data->cancel) {
arv_fake_camera_wait_for_next_frame (thread_data->camera);
buffer = arv_stream_pop_input_buffer (thread_data->stream);
if (buffer != NULL) {
arv_fake_camera_fill_buffer (thread_data->camera, buffer, NULL);
if (buffer->status == ARV_BUFFER_STATUS_SUCCESS)
thread_data->n_completed_buffers++;
else
thread_data->n_failures++;
if (thread_data->callback != NULL)
thread_data->callback (thread_data->user_data, ARV_STREAM_CALLBACK_TYPE_BUFFER_DONE,
buffer);
arv_stream_push_output_buffer (thread_data->stream, buffer);
} else
thread_data->n_underruns++;
}
if (thread_data->callback != NULL)
thread_data->callback (thread_data->user_data, ARV_STREAM_CALLBACK_TYPE_EXIT, NULL);
arv_log_stream_thread ("[FakeStream::thread] Stop");
return NULL;
}
static void
_missing_packet_check (ArvGvStreamThreadData *thread_data,
ArvGvStreamFrameData *frame,
guint32 packet_id,
guint64 time_us)
{
int i;
if (thread_data->packet_resend == ARV_GV_STREAM_PACKET_RESEND_NEVER ||
frame->error_packet_received)
return;
if (packet_id < frame->n_packets) {
int first_missing = -1;
for (i = frame->last_valid_packet + 1; i <= packet_id; i++) {
if (!frame->packet_data[i].received &&
(frame->packet_data[i].time_us == 0 ||
(time_us - frame->packet_data[i].time_us > thread_data->packet_timeout_us))) {
if (first_missing < 0)
first_missing = i;
} else
if (first_missing >= 0) {
int j;
arv_log_stream_thread ("[GvStream::_missing_packet_check]"
" Resend request at dt = %" G_GINT64_FORMAT ", packet id = %u/%u",
time_us - frame->first_packet_time_us,
packet_id, frame->n_packets);
_send_packet_request (thread_data, frame->frame_id,
first_missing, i - 1);
for (j = first_missing; j < i; j++)
frame->packet_data[j].time_us = time_us;
thread_data->n_resend_requests += (i - first_missing);
first_missing = -1;
}
}
if (first_missing >= 0) {
int j;
arv_log_stream_thread ("[GvStream::_missing_packet_check]"
" Resend request at dt = %" G_GINT64_FORMAT", packet id = %u/%u",
time_us - frame->first_packet_time_us,
packet_id, frame->n_packets);
_send_packet_request (thread_data, frame->frame_id,
first_missing, i - 1);
for (j = first_missing; j < i; j++)
frame->packet_data[j].time_us = time_us;
thread_data->n_resend_requests += (i - first_missing);
}
}
}
static void
_close_frame (ArvGvStreamThreadData *thread_data, ArvGvStreamFrameData *frame)
{
GTimeVal current_time;
gint64 current_time_us;
if (frame->buffer->status == ARV_BUFFER_STATUS_SUCCESS)
thread_data->n_completed_buffers++;
else
if (frame->buffer->status != ARV_BUFFER_STATUS_ABORTED)
thread_data->n_failures++;
if (frame->buffer->status == ARV_BUFFER_STATUS_TIMEOUT)
thread_data->n_timeouts++;
if (frame->buffer->status == ARV_BUFFER_STATUS_ABORTED)
thread_data->n_aborteds++;
if (frame->buffer->status != ARV_BUFFER_STATUS_SUCCESS &&
frame->buffer->status != ARV_BUFFER_STATUS_ABORTED)
thread_data->n_missing_packets += (int) frame->n_packets - (frame->last_valid_packet + 1);
if (thread_data->callback != NULL)
thread_data->callback (thread_data->user_data,
ARV_STREAM_CALLBACK_TYPE_BUFFER_DONE,
frame->buffer);
g_get_current_time (¤t_time);
current_time_us = current_time.tv_sec * 1000000 + current_time.tv_usec;
if (thread_data->statistic_count > 5) {
arv_statistic_fill (thread_data->statistic, 0,
current_time_us - frame->first_packet_time_us,
frame->buffer->frame_id);
} else
thread_data->statistic_count++;
arv_stream_push_output_buffer (thread_data->stream, frame->buffer);
arv_log_stream_thread ("[GvStream::_close_frame] Close frame %u", frame->frame_id);
frame->buffer = NULL;
frame->frame_id = 0;
g_free (frame->packet_data);
g_free (frame);
}
static void
_process_data_leader (ArvGvStreamThreadData *thread_data,
ArvGvStreamFrameData *frame,
ArvGvspPacket *packet,
guint32 packet_id)
{
if (frame->buffer->priv->status != ARV_BUFFER_STATUS_FILLING)
return;
if (packet_id != 0) {
frame->buffer->priv->status = ARV_BUFFER_STATUS_WRONG_PACKET_ID;
return;
}
frame->buffer->priv->gvsp_payload_type = arv_gvsp_packet_get_payload_type (packet);
frame->buffer->priv->frame_id = arv_gvsp_packet_get_frame_id (packet);
if (frame->buffer->priv->gvsp_payload_type != ARV_GVSP_PAYLOAD_TYPE_H264) {
if (G_LIKELY (thread_data->timestamp_tick_frequency != 0))
frame->buffer->priv->timestamp_ns = arv_gvsp_packet_get_timestamp (packet,
thread_data->timestamp_tick_frequency);
else {
GTimeVal time;
g_get_current_time (&time);
frame->buffer->priv->timestamp_ns = ((guint64) time.tv_sec * 1000000000LL) + ((guint64) time.tv_usec * 1000) ;
}
} else
frame->buffer->priv->timestamp_ns = g_get_real_time () * 1000LL;
if (frame->buffer->priv->gvsp_payload_type == ARV_GVSP_PAYLOAD_TYPE_IMAGE) {
frame->buffer->priv->x_offset = arv_gvsp_packet_get_x_offset (packet);
frame->buffer->priv->y_offset = arv_gvsp_packet_get_y_offset (packet);
frame->buffer->priv->width = arv_gvsp_packet_get_width (packet);
frame->buffer->priv->height = arv_gvsp_packet_get_height (packet);
frame->buffer->priv->pixel_format = arv_gvsp_packet_get_pixel_format (packet);
}
if (frame->packet_data[packet_id].time_us > 0) {
thread_data->n_resent_packets++;
arv_log_stream_thread ("[GvStream::_process_data_leader] Received resent packet %u for frame %u",
packet_id, frame->frame_id);
}
}
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
_process_data_trailer (ArvGvStreamThreadData *thread_data,
ArvGvStreamFrameData *frame,
ArvGvspPacket *packet,
guint32 packet_id)
{
if (frame->buffer->status != ARV_BUFFER_STATUS_FILLING)
return;
if (packet_id != frame->n_packets - 1) {
frame->buffer->status = ARV_BUFFER_STATUS_WRONG_PACKET_ID;
return;
}
if (frame->packet_data[packet_id].time_us > 0) {
thread_data->n_resent_packets++;
arv_log_stream_thread ("[GvStream::_process_data_trailer] Received resent packet %u for frame %u",
packet_id, frame->frame_id);
}
}
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_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;
}
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;
}