void ipcon_dispatch(IPConnection *ipcon) {
unsigned char buffer[RECV_BUFFER_SIZE] = { 0 };
int length = read(ipcon->fd, buffer, RECV_BUFFER_SIZE);
if(length < 0) {
unsigned char* buf = buffer;
int handled = 0;
do {
uint8_t function_id = ipcon_get_function_id_from_data(buf);
uint16_t part_length = ipcon_get_length_from_data(buf);
handled += part_length;
if (function_id == FUNCTION_ENUMERATE_CALLBACK) {
EnumerateReturn *er = (EnumerateReturn *)buf;
char str_uid[MAX_BASE58_STR_SIZE];
ipcon_base58encode(er->device_uid, str_uid);
if (ipcon->enumerate_callback != NULL) {
ipcon->enumerate_callback(str_uid,
er->device_name,
er->device_stack_id,
er->is_new);
}
} else {
uint8_t stack_id = ipcon_get_stack_id_from_data(buf);
Device *device = ipcon->devices[stack_id];
device->device_callbacks[function_id](device, buf);
}
buf += part_length;
} while(handled < length);
}
}
static void ipcon_callback_queue_enqueue(IPConnection *ipcon, const unsigned char *buffer)
{
uint16_t length = ipcon_get_length_from_data(buffer);
CallbackQueueNode *node = (CallbackQueueNode *)malloc(offsetof(CallbackQueueNode, buffer) + length);
node->next = NULL;
memcpy(node->buffer, buffer, length);
#ifdef _WIN32
EnterCriticalSection(&ipcon->callback_queue_mutex);
#else
pthread_mutex_lock(&ipcon->callback_queue_mutex);
#endif
if (ipcon->callback_queue_tail == NULL) {
ipcon->callback_queue_head = node;
ipcon->callback_queue_tail = node;
} else {
ipcon->callback_queue_tail->next = node;
ipcon->callback_queue_tail = node;
}
#ifdef _WIN32
LeaveCriticalSection(&ipcon->callback_queue_mutex);
ReleaseSemaphore(ipcon->callback_queue_semaphore, 1, NULL);
#else
pthread_mutex_unlock(&ipcon->callback_queue_mutex);
sem_post(ipcon->callback_queue_semaphore);
#endif
}
int ipcon_handle_enumerate(IPConnection *ipcon, const unsigned char *buffer) {
uint16_t length = ipcon_get_length_from_data(buffer);
if(ipcon->enumerate_callback == NULL) {
return length;
}
ipcon_callback_queue_enqueue(ipcon, buffer);
return length;
}
void ipcon_handle_message(IPConnection *ipcon, const unsigned char *buffer) {
uint8_t function_id = ipcon_get_function_id_from_data(buffer);
if(function_id == FUNCTION_GET_STACK_ID) {
ipcon_handle_add_device(ipcon, buffer);
return;
}
if(function_id == FUNCTION_ENUMERATE_CALLBACK) {
ipcon_handle_enumerate(ipcon, buffer);
return;
}
uint8_t stack_id = ipcon_get_stack_id_from_data(buffer);
uint16_t length = ipcon_get_length_from_data(buffer);
if(ipcon->devices[stack_id] == NULL) {
// Response from an unknown device, ignoring it
return;
}
Device *device = ipcon->devices[stack_id];
DeviceResponse *response = &device->response;
if(response->function_id == function_id) {
if(response->length != length) {
fprintf(stderr,
"Received malformed message from %d, ignoring it\n",
stack_id);
return;
}
memcpy(response->buffer, buffer, length);
response->length = length;
#ifdef _WIN32
ReleaseSemaphore(device->response_semaphore, 1, NULL);
#else
pthread_mutex_lock(&device->response_mutex);
device->response_flag = true;
pthread_cond_signal(&device->response_cond);
pthread_mutex_unlock(&device->response_mutex);
#endif
return;
}
if(device->registered_callbacks[function_id] != NULL) {
ipcon_callback_queue_enqueue(ipcon, buffer);
return;
}
// Message seems to be OK, but can't be handled, most likely
// a callback without registered function
}
static void ipcon_callback_queue_enqueue(IPConnection *ipcon, const unsigned char *buffer) {
uint16_t length = ipcon_get_length_from_data(buffer);
CallbackQueueNode *node = (CallbackQueueNode *)malloc(offsetof(CallbackQueueNode, buffer) + length);
node->next = NULL;
memcpy(node->buffer, buffer, length);
ipcon_mutex_lock(&ipcon->callback_queue_mutex);
if (ipcon->callback_queue_tail == NULL) {
ipcon->callback_queue_head = node;
ipcon->callback_queue_tail = node;
} else {
ipcon->callback_queue_tail->next = node;
ipcon->callback_queue_tail = node;
}
ipcon_mutex_unlock(&ipcon->callback_queue_mutex);
ipcon_semaphore_release(ipcon->callback_queue_semaphore);
}
static THREAD_RETURN_TYPE ipcon_receive_loop(void *param) {
IPConnection *ipcon = (IPConnection*)param;
unsigned char pending_data[RECV_BUFFER_SIZE] = { 0 };
int pending_length = 0;
while(ipcon->thread_receive_flag) {
#ifdef _WIN32
int length = recv(ipcon->s, (char *)(pending_data + pending_length),
RECV_BUFFER_SIZE - pending_length, 0);
if(!ipcon->thread_receive_flag) {
break;
}
if(length == SOCKET_ERROR) {
if (WSAGetLastError() == WSAEINTR) {
continue;
}
fprintf(stderr, "A socket error occurred, destroying IPConnection\n");
ipcon_destroy(ipcon);
THREAD_RETURN;
}
#else
int length = read(ipcon->fd, pending_data + pending_length,
RECV_BUFFER_SIZE - pending_length);
if(!ipcon->thread_receive_flag) {
break;
}
if(length < 0) {
if (errno == EINTR) {
continue;
}
fprintf(stderr, "A socket error occurred, destroying IPConnection\n");
ipcon_destroy(ipcon);
THREAD_RETURN;
}
#endif
if(length == 0) {
if(ipcon->thread_receive_flag) {
fprintf(stderr, "Socket disconnected by Server, destroying IPConnection\n");
ipcon_destroy(ipcon);
}
THREAD_RETURN;
}
pending_length += length;
while(true) {
if(pending_length < 4) {
// Wait for complete header
break;
}
length = ipcon_get_length_from_data(pending_data);
if(pending_length < length) {
// Wait for complete packet
break;
}
ipcon_handle_message(ipcon, pending_data);
memmove(pending_data, pending_data + length, pending_length - length);
pending_length -= length;
}
}
THREAD_RETURN;
}