// New packet from brickd event loop is queued to be sent via Modbus
void rs485_extension_dispatch_to_modbus(Stack *stack, Packet *request, Recipient *recipient) {
RS485ExtensionPacket *queued_request;
(void)stack;
if(request->header.uid == 0 || recipient == NULL) {
int is;
log_debug("RS485: Broadcasting to all available Modbus slaves");
for(is = 0; is < _rs485_extension.slave_num; is++) {
queued_request = queue_push(&_rs485_extension.packet_to_modbus_queue);
queued_request->slave_address = _rs485_extension.slaves[is];
memcpy(&queued_request->packet, request, request->header.length);
log_debug("RS485: Packet is queued to be sent to slave %d over Modbus. Function signature = (%s)",
_rs485_extension.slaves[is],
packet_get_request_signature(packet_signature, request));
}
}
else if (recipient != NULL) {
queued_request = queue_push(&_rs485_extension.packet_to_modbus_queue);
queued_request->slave_address = recipient->opaque;
memcpy(&queued_request->packet, request, request->header.length);
log_debug("RS485: Packet is queued to be send to slave %d over Modbus. Function signature = (%s)",
recipient->opaque,
packet_get_request_signature(packet_signature, request));
}
}
static void client_handle_request(Client *client, Packet *request) {
char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];
EmptyResponse response;
// handle requests meant for brickd
if (uint32_from_le(request->header.uid) == UID_BRICK_DAEMON) {
// add as pending request if response is expected
if (packet_header_get_response_expected(&request->header)) {
network_client_expects_response(client, request);
}
if (request->header.function_id == FUNCTION_GET_AUTHENTICATION_NONCE) {
if (request->header.length != sizeof(GetAuthenticationNonceRequest)) {
log_error("Received authentication-nonce request (%s) from client ("CLIENT_SIGNATURE_FORMAT") with wrong length, disconnecting client",
packet_get_request_signature(packet_signature, request),
client_expand_signature(client));
client->disconnected = true;
return;
}
client_handle_get_authentication_nonce_request(client, (GetAuthenticationNonceRequest *)request);
} else if (request->header.function_id == FUNCTION_AUTHENTICATE) {
if (request->header.length != sizeof(AuthenticateRequest)) {
log_error("Received authenticate request (%s) from client ("CLIENT_SIGNATURE_FORMAT") with wrong length, disconnecting client",
packet_get_request_signature(packet_signature, request),
client_expand_signature(client));
client->disconnected = true;
return;
}
client_handle_authenticate_request(client, (AuthenticateRequest *)request);
} else {
response.header = request->header;
response.header.length = sizeof(response);
packet_header_set_error_code(&response.header,
PACKET_E_FUNCTION_NOT_SUPPORTED);
client_dispatch_response(client, NULL, (Packet *)&response, false, false);
}
} else if (client->authentication_state == CLIENT_AUTHENTICATION_STATE_DISABLED ||
client->authentication_state == CLIENT_AUTHENTICATION_STATE_DONE) {
// add as pending request if response is expected...
if (packet_header_get_response_expected(&request->header)) {
network_client_expects_response(client, request);
}
// ...then dispatch it to the hardware
hardware_dispatch_request(request);
} else {
log_packet_debug("Client ("CLIENT_SIGNATURE_FORMAT") is not authenticated, dropping request (%s)",
client_expand_signature(client),
packet_get_request_signature(packet_signature, request));
}
}
void hardware_dispatch_request(Packet *request) {
char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];
int i;
Stack *stack;
int rc;
bool dispatched = false;
if (_stacks.count == 0) {
log_packet_debug("No stacks connected, dropping request (%s)",
packet_get_request_signature(packet_signature, request));
return;
}
if (request->header.uid == 0) {
log_packet_debug("Broadcasting request (%s) to %d stack(s)",
packet_get_request_signature(packet_signature, request),
_stacks.count);
// broadcast to all stacks
for (i = 0; i < _stacks.count; ++i) {
stack = *(Stack **)array_get(&_stacks, i);
stack_dispatch_request(stack, request, true);
}
} else {
log_packet_debug("Dispatching request (%s) to %d stack(s)",
packet_get_request_signature(packet_signature, request),
_stacks.count);
// dispatch to all stacks, not only the first one that might claim to
// know the UID
for (i = 0; i < _stacks.count; ++i) {
stack = *(Stack **)array_get(&_stacks, i);
rc = stack_dispatch_request(stack, request, false);
if (rc < 0) {
continue;
} else if (rc > 0) {
dispatched = true;
}
}
if (dispatched) {
return;
}
log_packet_debug("Broadcasting request because UID is currently unknown");
// broadcast to all stacks, as no stack claimed to know the UID
for (i = 0; i < _stacks.count; ++i) {
stack = *(Stack **)array_get(&_stacks, i);
stack_dispatch_request(stack, request, true);
}
}
}
static void client_handle_authenticate_request(Client *client,
AuthenticateRequest *request) {
uint32_t nonces[2];
uint8_t digest[SHA1_DIGEST_LENGTH];
const char *secret;
char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];
AuthenticateResponse response;
if (client->authentication_state == CLIENT_AUTHENTICATION_STATE_DISABLED) {
log_error("Client ("CLIENT_SIGNATURE_FORMAT") tries to authenticate, but authentication is disabled, disconnecting client",
client_expand_signature(client));
client->disconnected = true;
return;
}
if (client->authentication_state != CLIENT_AUTHENTICATION_STATE_NONCE_SEND) {
log_error("Client ("CLIENT_SIGNATURE_FORMAT") performed invalid authentication sequence (%s -> %s), disconnecting client",
client_expand_signature(client),
client_get_authentication_state_name(client->authentication_state),
client_get_authentication_state_name(CLIENT_AUTHENTICATION_STATE_DONE));
client->disconnected = true;
return;
}
memcpy(&nonces[0], &client->authentication_nonce, sizeof(client->authentication_nonce));
memcpy(&nonces[1], request->client_nonce, sizeof(request->client_nonce));
secret = config_get_option_value("authentication.secret")->string;
hmac_sha1((uint8_t *)secret, strlen(secret),
(uint8_t *)nonces, sizeof(nonces), digest);
if (memcmp(request->digest, digest, SHA1_DIGEST_LENGTH) != 0) {
log_error("Authenticate request (%s) from client ("CLIENT_SIGNATURE_FORMAT") did not contain the expected data, disconnecting client",
packet_get_request_signature(packet_signature, (Packet *)request),
client_expand_signature(client));
client->disconnected = true;
return;
}
client->authentication_state = CLIENT_AUTHENTICATION_STATE_DONE;
log_info("Client ("CLIENT_SIGNATURE_FORMAT") successfully finished authentication",
client_expand_signature(client));
if (packet_header_get_response_expected(&request->header)) {
response.header = request->header;
response.header.length = sizeof(response);
packet_header_set_error_code(&response.header, PACKET_E_SUCCESS);
client_dispatch_response(client, NULL, (Packet *)&response, false, false);
}
}
// New packet from brickd event loop is queued to be written to stack via SPI
static int red_stack_dispatch_to_spi(Stack *stack, Packet *request, Recipient *recipient) {
REDStackPacket *queued_request;
(void)stack;
if (request->header.uid == 0) {
// UID = 0 -> Broadcast to all UIDs
uint8_t is;
for (is = 0; is < _red_stack.slave_num; is++) {
mutex_lock(&_red_stack.slaves[is].packet_queue_mutex);
queued_request = queue_push(&_red_stack.slaves[is].packet_to_spi_queue);
queued_request->status = RED_STACK_PACKET_STATUS_ADDED;
queued_request->slave = &_red_stack.slaves[is];
memcpy(&queued_request->packet, request, request->header.length);
mutex_unlock(&_red_stack.slaves[is].packet_queue_mutex);
log_packet_debug("Request is queued to be broadcast to slave %d (%s)",
is, packet_get_request_signature(packet_signature, request));
}
} else if (recipient != NULL) {
// Get slave for recipient opaque (== stack_address)
REDStackSlave *slave = &_red_stack.slaves[recipient->opaque];
mutex_lock(&(slave->packet_queue_mutex));
queued_request = queue_push(&(slave->packet_to_spi_queue));
queued_request->status = RED_STACK_PACKET_STATUS_ADDED;
queued_request->slave = slave;
memcpy(&queued_request->packet, request, request->header.length);
mutex_unlock(&(slave->packet_queue_mutex));
log_packet_debug("Packet is queued to be send to slave %d over SPI (%s)",
slave->stack_address,
packet_get_request_signature(packet_signature, request));
}
return 0;
}
void network_client_expects_response(Client *client, Packet *request) {
PendingRequest *pending_request;
char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];
if (client->pending_request_count >= CLIENT_MAX_PENDING_REQUESTS) {
log_warn("Pending requests list for client ("CLIENT_SIGNATURE_FORMAT") is full, dropping %d pending request(s)",
client_expand_signature(client),
client->pending_request_count - CLIENT_MAX_PENDING_REQUESTS + 1);
while (client->pending_request_count >= CLIENT_MAX_PENDING_REQUESTS) {
pending_request = containerof(client->pending_request_sentinel.next, PendingRequest, client_node);
pending_request_remove_and_free(pending_request);
}
}
pending_request = calloc(1, sizeof(PendingRequest));
if (pending_request == NULL) {
log_error("Could not allocate pending request: %s (%d)",
get_errno_name(ENOMEM), ENOMEM);
return;
}
node_reset(&pending_request->global_node);
node_insert_before(&_pending_request_sentinel, &pending_request->global_node);
node_reset(&pending_request->client_node);
node_insert_before(&client->pending_request_sentinel, &pending_request->client_node);
++client->pending_request_count;
pending_request->client = client;
pending_request->zombie = NULL;
memcpy(&pending_request->header, &request->header, sizeof(PacketHeader));
#ifdef BRICKD_WITH_PROFILING
pending_request->arrival_time = microseconds();
#endif
log_packet_debug("Added pending request (%s) for client ("CLIENT_SIGNATURE_FORMAT")",
packet_get_request_signature(packet_signature, request),
client_expand_signature(client));
}
// New packet from brickd event loop is queued to be written to BrickletStack via SPI
static int bricklet_stack_dispatch_to_spi(Stack *stack, Packet *request, Recipient *recipient) {
BrickletStack *bricklet_stack = (BrickletStack*)stack;
Packet *queued_request;
if((request->header.uid != 0) && (recipient == NULL)) {
return 0;
}
if (!bricklet_stack->data_seen) {
return 0;
}
mutex_lock(&bricklet_stack->request_queue_mutex);
queued_request = queue_push(&bricklet_stack->request_queue);
memcpy(queued_request, request, request->header.length);
mutex_unlock(&bricklet_stack->request_queue_mutex);
log_packet_debug("Packet is queued to be send over SPI (%s)",
packet_get_request_signature(packet_signature, request));
return 0;
}
static void client_handle_read(void *opaque) {
Client *client = opaque;
int length;
const char *message = NULL;
char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];
length = io_read(client->io, (uint8_t *)&client->request + client->request_used,
sizeof(Packet) - client->request_used);
if (length == 0) {
log_info("Client ("CLIENT_SIGNATURE_FORMAT") disconnected by peer",
client_expand_signature(client));
client->disconnected = true;
return;
}
if (length < 0) {
if (length == IO_CONTINUE) {
// no actual data received
} else if (errno_interrupted()) {
log_debug("Receiving from client ("CLIENT_SIGNATURE_FORMAT") was interrupted, retrying",
client_expand_signature(client));
} else if (errno_would_block()) {
log_debug("Receiving from client ("CLIENT_SIGNATURE_FORMAT") would block, retrying",
client_expand_signature(client));
} else {
log_error("Could not receive from client ("CLIENT_SIGNATURE_FORMAT"), disconnecting client: %s (%d)",
client_expand_signature(client), get_errno_name(errno), errno);
client->disconnected = true;
}
return;
}
client->request_used += length;
while (!client->disconnected && client->request_used > 0) {
if (client->request_used < (int)sizeof(PacketHeader)) {
// wait for complete header
break;
}
if (!client->request_header_checked) {
if (!packet_header_is_valid_request(&client->request.header, &message)) {
// FIXME: include packet_get_content_dump output in the error message
log_error("Received invalid request (%s) from client ("CLIENT_SIGNATURE_FORMAT"), disconnecting client: %s",
packet_get_request_signature(packet_signature, &client->request),
client_expand_signature(client), message);
client->disconnected = true;
return;
}
client->request_header_checked = true;
}
length = client->request.header.length;
if (client->request_used < length) {
// wait for complete packet
break;
}
if (client->request.header.function_id == FUNCTION_DISCONNECT_PROBE) {
log_packet_debug("Received disconnect probe from client ("CLIENT_SIGNATURE_FORMAT"), dropping request",
client_expand_signature(client));
} else {
log_packet_debug("Received request (%s) from client ("CLIENT_SIGNATURE_FORMAT")",
packet_get_request_signature(packet_signature, &client->request),
client_expand_signature(client));
client_handle_request(client, &client->request);
}
memmove(&client->request, (uint8_t *)&client->request + length,
client->request_used - length);
client->request_used -= length;
client->request_header_checked = false;
}
}
// Main SPI loop. This runs independently from the brickd event thread.
// Data between RED Brick and SPI slave is exchanged every 500us.
// If there is no data to be send, we cycle through the slaves and request
// data. If there is data to be send the slave that ought to receive
// the data gets priority. This can greatly reduce latency in a big stack.
static void red_stack_spi_thread(void *opaque) {
REDStackPacket *packet_to_spi = NULL;
uint8_t stack_address_cycle;
int ret;
(void)opaque;
do {
stack_address_cycle = 0;
_red_stack_reset_detected = 0;
_red_stack.slave_num = 0;
red_stack_spi_create_routing_table();
_red_stack_spi_thread_running = false;
if (_red_stack.slave_num > 0) {
_red_stack_spi_thread_running = true;
}
// Ignore resets that we received in the meantime to prevent race conditions.
_red_stack_reset_detected = 0;
while (_red_stack_spi_thread_running) {
REDStackSlave *slave = &_red_stack.slaves[stack_address_cycle];
REDStackPacket *request = NULL;
memset(&_red_stack.packet_from_spi, 0, sizeof(Packet));
// Get packet from queue. The queue contains that are to be
// send over SPI. It is filled through from the main brickd
// event thread, so we have to make sure that there is not race
// condition.
if(slave->next_packet_empty) {
slave->next_packet_empty = false;
packet_to_spi = NULL;
} else {
mutex_lock(&(slave->packet_queue_mutex));
packet_to_spi = queue_peek(&slave->packet_to_spi_queue);
mutex_unlock(&(slave->packet_queue_mutex));
}
stack_address_cycle++;
if (stack_address_cycle >= _red_stack.slave_num) {
stack_address_cycle = 0;
}
// Set request if we have a packet to send
if (packet_to_spi != NULL) {
log_packet_debug("Packet will now be send over SPI (%s)",
packet_get_request_signature(packet_signature, &packet_to_spi->packet));
request = packet_to_spi;
}
ret = red_stack_spi_transceive_message(request, &_red_stack.packet_from_spi, slave);
if ((ret & RED_STACK_TRANSCEIVE_RESULT_MASK_SEND) == RED_STACK_TRANSCEIVE_RESULT_SEND_OK) {
if ((!((ret & RED_STACK_TRANSCEIVE_RESULT_MASK_READ) == RED_STACK_TRANSCEIVE_RESULT_READ_ERROR))) {
// If we send a packet it must have come from the queue, so we can
// pop it from the queue now.
// If the sending didn't work (for whatever reason), we don't pop it
// and therefore we will automatically try to send it again in the next cycle.
mutex_lock(&(slave->packet_queue_mutex));
queue_pop(&slave->packet_to_spi_queue, NULL);
mutex_unlock(&(slave->packet_queue_mutex));
}
}
// If we received a packet, we will dispatch it immediately.
// We have some time until we try the next SPI communication anyway.
if ((ret & RED_STACK_TRANSCEIVE_RESULT_MASK_READ) == RED_STACK_TRANSCEIVE_RESULT_READ_OK) {
// TODO: Check again if packet is valid?
// We did already check the hash.
// Before the dispatching we insert the stack position into an enumerate message
red_stack_spi_insert_position(slave);
red_stack_spi_request_dispatch_response_event();
// Wait until message is dispatched, so we don't overwrite it
// accidentally.
semaphore_acquire(&_red_stack_dispatch_packet_from_spi_semaphore);
}
SLEEP_NS(0, 1000*_red_stack_spi_poll_delay);
}
if (_red_stack.slave_num == 0) {
pthread_mutex_lock(&_red_stack_wait_for_reset_mutex);
// Use helper to be save against spurious wakeups
_red_stack_wait_for_reset_helper = 0;
while (_red_stack_wait_for_reset_helper == 0) {
pthread_cond_wait(&_red_stack_wait_for_reset_cond, &_red_stack_wait_for_reset_mutex);
}
pthread_mutex_unlock(&_red_stack_wait_for_reset_mutex);
}
if (_red_stack_reset_detected > 0) {
red_stack_spi_handle_reset();
}
} while (_red_stack_reset_detected > 0);
}
