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 THREAD_RETURN_TYPE ipcon_callback_loop(void *param) {
IPConnection *ipcon = (IPConnection*)param;
while(ipcon->thread_callback_flag) {
if(!ipcon_semaphore_request(ipcon->callback_queue_semaphore)) {
continue;
}
if(!ipcon->thread_callback_flag) {
break;
}
if(ipcon->callback_queue_head == NULL) {
continue;
}
ipcon_mutex_lock(&ipcon->callback_queue_mutex);
CallbackQueueNode *node = ipcon->callback_queue_head;
ipcon->callback_queue_head = node->next;
node->next = NULL;
if(ipcon->callback_queue_tail == node) {
ipcon->callback_queue_head = NULL;
ipcon->callback_queue_tail = NULL;
}
ipcon_mutex_unlock(&ipcon->callback_queue_mutex);
uint8_t function_id = ipcon_get_function_id_from_data(node->buffer);
if(function_id == FUNCTION_ENUMERATE_CALLBACK) {
EnumerateReturn *er = (EnumerateReturn *)node->buffer;
er->length = ipcon_leconvert_uint16_from(er->length);
er->device_uid = ipcon_leconvert_uint64_from(er->device_uid);
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(node->buffer);
Device *device = ipcon->devices[stack_id];
device->callback_wrappers[function_id](device, node->buffer);
}
free(node);
}
THREAD_RETURN;
}
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
}
void ipcon_callback_loop(void *param) {
#else
void *ipcon_callback_loop(void *param) {
#endif
IPConnection *ipcon = (IPConnection*)param;
while(ipcon->recv_loop_flag) {
#ifdef _WIN32
if (WaitForSingleObject(ipcon->callback_queue_semaphore, INFINITE) != 0) {
continue;
}
#else
if (sem_wait(ipcon->callback_queue_semaphore) != 0) {
continue;
}
#endif
if (!ipcon->recv_loop_flag) {
break;
}
if (ipcon->callback_queue_head == NULL) {
continue;
}
#ifdef _WIN32
EnterCriticalSection(&ipcon->callback_queue_mutex);
#else
pthread_mutex_lock(&ipcon->callback_queue_mutex);
#endif
CallbackQueueNode *node = ipcon->callback_queue_head;
ipcon->callback_queue_head = node->next;
node->next = NULL;
if (ipcon->callback_queue_tail == node) {
ipcon->callback_queue_head = NULL;
ipcon->callback_queue_tail = NULL;
}
#ifdef _WIN32
LeaveCriticalSection(&ipcon->callback_queue_mutex);
#else
pthread_mutex_unlock(&ipcon->callback_queue_mutex);
#endif
uint8_t function_id = ipcon_get_function_id_from_data(node->buffer);
if (function_id == FUNCTION_ENUMERATE_CALLBACK) {
EnumerateReturn *er = (EnumerateReturn *)node->buffer;
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(node->buffer);
Device *device = ipcon->devices[stack_id];
device->device_callbacks[function_id](device, node->buffer);
}
free(node);
}
#ifndef _WIN32
return NULL;
#endif
}
