int red_stack_init(void) {
int i = 0;
int phase = 0;
log_debug("Initializing RED Brick SPI Stack subsystem");
_red_stack_spi_poll_delay = config_get_option_value("poll_delay.spi")->integer;
if (gpio_sysfs_export(RED_STACK_RESET_PIN_GPIO_NUM) < 0) {
// Just issue a warning, RED Brick will work without reset interrupt
log_warn("Could not export GPIO %d in sysfs, disabling reset interrupt",
RED_STACK_RESET_PIN_GPIO_NUM);
} else {
if ((_red_stack_reset_fd = gpio_sysfs_get_value_fd(RED_STACK_RESET_PIN_GPIO_NAME)) < 0) {
// Just issue a warning, RED Brick will work without reset interrupt
log_warn("Could not retrieve fd for GPIO %s in sysfs, disabling reset interrupt",
RED_STACK_RESET_PIN_GPIO_NAME);
} else {
// If everything worked we can set the interrupt to falling.
// We ignore the return value here, it may work despite error.
gpio_sysfs_set_edge(RED_STACK_RESET_PIN_GPIO_NAME, "falling");
}
}
// create base stack
if (stack_create(&_red_stack.base, "red_stack", red_stack_dispatch_to_spi) < 0) {
log_error("Could not create base stack for RED Brick SPI Stack: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
phase = 1;
// add to stacks array
if (hardware_add_stack(&_red_stack.base) < 0) {
goto cleanup;
}
phase = 2;
if ((_red_stack_notification_event = eventfd(0, 0)) < 0) {
log_error("Could not create red stack notification event: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
phase = 3;
// Add notification pipe as event source.
// Event is used to dispatch packets.
if (event_add_source(_red_stack_notification_event, EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ, red_stack_dispatch_from_spi, NULL) < 0) {
log_error("Could not add red stack notification pipe as event source");
goto cleanup;
}
phase = 4;
// Initialize SPI packet queues
for (i = 0; i < RED_STACK_SPI_MAX_SLAVES; i++) {
if (queue_create(&_red_stack.slaves[i].packet_to_spi_queue, sizeof(REDStackPacket)) < 0) {
log_error("Could not create SPI queue %d: %s (%d)",
i, get_errno_name(errno), errno);
goto cleanup;
}
}
if (semaphore_create(&_red_stack_dispatch_packet_from_spi_semaphore) < 0) {
log_error("Could not create SPI request semaphore: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
for (i = 0; i < RED_STACK_SPI_MAX_SLAVES; i++) {
mutex_create(&_red_stack.slaves[i].packet_queue_mutex);
}
phase = 5;
if (red_stack_init_spi() < 0) {
goto cleanup;
}
// Add reset interrupt as event source
if (_red_stack_reset_fd > 0) {
char buf[2];
lseek(_red_stack_reset_fd, 0, SEEK_SET);
if (read(_red_stack_reset_fd, buf, 2) < 0) {} // ignore return value
if (event_add_source(_red_stack_reset_fd, EVENT_SOURCE_TYPE_GENERIC,
EVENT_PRIO | EVENT_ERROR, red_stack_reset_handler, NULL) < 0) {
log_error("Could not add reset fd event");
goto cleanup;
}
}
phase = 6;
cleanup:
switch (phase) { // no breaks, all cases fall through intentionally
case 5:
for (i = 0; i < RED_STACK_SPI_MAX_SLAVES; i++) {
mutex_destroy(&_red_stack.slaves[i].packet_queue_mutex);
}
semaphore_destroy(&_red_stack_dispatch_packet_from_spi_semaphore);
case 4:
for (i--; i >= 0; i--) {
queue_destroy(&_red_stack.slaves[i].packet_to_spi_queue, NULL);
}
event_remove_source(_red_stack_notification_event, EVENT_SOURCE_TYPE_GENERIC);
case 3:
close(_red_stack_notification_event);
case 2:
hardware_remove_stack(&_red_stack.base);
case 1:
stack_destroy(&_red_stack.base);
default:
break;
}
return phase == 6 ? 0 : -1;
}
static void bricklet_stack_transceive(BrickletStack *bricklet_stack) {
// If we have not seen any data from the Bricklet we increase a counter.
// If the counter reaches BRICKLET_STACK_FIRST_MESSAGE_TRIES we assume that
// there is no Bricklet and we stop trying to send to initial message (if a
// Bricklet is hotplugged it will send a enumerate itself).
if(!bricklet_stack->data_seen) {
if(bricklet_stack->first_message_tries < BRICKLET_STACK_FIRST_MESSAGE_TRIES) {
bricklet_stack->first_message_tries++;
} else {
bricklet_stack->buffer_send_length = 0;
}
}
const uint16_t length_read = bricklet_stack_check_missing_length(bricklet_stack);
if(bricklet_stack->buffer_send_length == 0) {
// If buffer is empty we try to send request from the queue.
bricklet_stack_check_request_queue(bricklet_stack);
if((bricklet_stack->buffer_send_length == 0) && (bricklet_stack->ack_to_send)) {
// If there is no request in the queue (buffer still empty)
// and we have to send an ACK still, we send the ACK.
bricklet_stack_send_ack(bricklet_stack);
}
}
uint16_t length_write = bricklet_stack->wait_for_ack ? 0 : bricklet_stack->buffer_send_length;
uint16_t length = MAX(MAX(length_read, length_write), 1);
uint8_t rx[SPITFP_MAX_TFP_MESSAGE_LENGTH] = {0};
uint8_t tx[SPITFP_MAX_TFP_MESSAGE_LENGTH] = {0};
if((length == 1) || (!bricklet_stack->data_seen)) {
// If there is nothing to read or to write, we give the Bricklet some breathing
// room before we start polling again.
// If we have nothing to send and we are currently not awaiting data from the Bricklet, we will
// poll every 200 us.
uint32_t sleep_us = 200;
if(!bricklet_stack->data_seen) {
// If we have never seen any data, we will first poll every 1ms with the StackEnumerate message
// and switch to polling every 500ms after we tried BRICKLET_STACK_FIRST_MESSAGE_TRIES times.
// In this case there is likely no Bricklet connected. If a Bricklet is hotpluged "data_seen"
// will be true and we will switch to polling every 200us immediately.
if(bricklet_stack->first_message_tries < BRICKLET_STACK_FIRST_MESSAGE_TRIES) {
sleep_us = 1*1000;
} else {
sleep_us = 500*1000;
}
}
struct timespec t;
t.tv_sec = 0;
t.tv_nsec = 1000*sleep_us;
clock_nanosleep(CLOCK_MONOTONIC, 0, &t, NULL);
}
memcpy(tx, bricklet_stack->buffer_send, length_write);
struct spi_ioc_transfer spi_transfer = {
.tx_buf = (unsigned long)tx,
.rx_buf = (unsigned long)rx,
.len = length,
};
// Make sure that we only access SPI once at a time
mutex_lock(bricklet_stack->config.mutex);
// Do chip select by hand if necessary
if(bricklet_stack->config.chip_select_driver == CHIP_SELECT_GPIO) {
if(gpio_sysfs_set_output(&bricklet_stack->config.chip_select_gpio_sysfs, GPIO_SYSFS_VALUE_LOW) < 0) {
log_error("Could not enable chip select");
return;
}
}
int rc = ioctl(bricklet_stack->spi_fd, SPI_IOC_MESSAGE(1), &spi_transfer);
// If the length is 1 (i.e. we wanted to see if the SPI slave has data for us)
// and he does have data for us, we will immediately retrieve the data without
// giving back the mutex.
if((length == 1) && (rx[0] != 0) && (rc == length) && (length_write == 0)) {
// First add the one byte of already received data to the ringbuffer
ringbuffer_add(&bricklet_stack->ringbuffer_recv, rx[0]);
// Set rc to 0, so if there is no more data to read, we don't get the
// "unexpected result" error
rc = 0;
// Get length for rest of message
length = bricklet_stack_check_missing_length(bricklet_stack);
if(length != 0) {
// Set first byte back to 0 and the new length, the rest was not touched
// and we don't need to reinizialize it.
rx[0] = 0;
spi_transfer.len = length;
rc = ioctl(bricklet_stack->spi_fd, SPI_IOC_MESSAGE(1), &spi_transfer);
}
}
// Do chip deselect by hand if necessary
if(bricklet_stack->config.chip_select_driver == CHIP_SELECT_GPIO) {
if(gpio_sysfs_set_output(&bricklet_stack->config.chip_select_gpio_sysfs, GPIO_SYSFS_VALUE_HIGH) < 0) {
log_error("Could not disable chip select");
return;
}
}
mutex_unlock(bricklet_stack->config.mutex);
if (rc < 0) {
log_error("ioctl failed: %s (%d)", get_errno_name(errno), errno);
return;
}
if (rc != length) {
log_error("ioctl has unexpected result (actual: %d != expected: %d)", rc, length);
return;
}
// We don't expect an ACK to be acked, so we can set the length to 0 here
if(bricklet_stack->buffer_send_length == SPITFP_PROTOCOL_OVERHEAD) {
bricklet_stack->buffer_send_length = 0;
}
if(bricklet_stack->buffer_send_length >= SPITFP_MIN_TFP_MESSAGE_LENGTH) {
bricklet_stack->wait_for_ack = true;
}
for(uint16_t i = 0; i < length; i++) {
ringbuffer_add(&bricklet_stack->ringbuffer_recv, rx[i]);
}
}
static void bricklet_stack_spi_thread(void *opaque) {
BrickletStack *bricklet_stack = (BrickletStack*)opaque;
bricklet_stack->spi_thread_running = true;
// Depending on the configuration we wait on startup for
// other Bricklets to identify themself first.
struct timespec t = {
.tv_sec = bricklet_stack->config.startup_wait_time,
};
clock_nanosleep(CLOCK_MONOTONIC, 0, &t, NULL);
// Pre-fill the send buffer with the "StackEnumerate"-Packet.
// This packet will trigger an initial enumeration in the Bricklet.
// If the Brick Daemon is restarted, we need to
// trigger the initial enumeration, since the Bricklet does not know
// that it has to enumerate itself again.
PacketHeader header = {
.uid = 0,
.length = sizeof(PacketHeader),
.function_id = FUNCTION_STACK_ENUMERATE,
.sequence_number_and_options = 0x08, // return expected
.error_code_and_future_use = 0
};
bricklet_stack_send_ack_and_message(bricklet_stack, (uint8_t*)&header, sizeof(PacketHeader));
while (bricklet_stack->spi_thread_running) {
bricklet_stack_transceive(bricklet_stack);
bricklet_stack_check_message(bricklet_stack);
}
}
static int bricklet_stack_init_spi(BrickletStack *bricklet_stack) {
// Use hw chip select if it is done by SPI hardware unit, otherwise set SPI_NO_CS flag.
const int mode = BRICKLET_STACK_SPI_CONFIG_MODE | (bricklet_stack->config.chip_select_driver == CHIP_SELECT_HARDWARE ? 0 : SPI_NO_CS);
const int lsb_first = BRICKLET_STACK_SPI_CONFIG_LSB_FIRST;
const int bits_per_word = BRICKLET_STACK_SPI_CONFIG_BITS_PER_WORD;
const int max_speed_hz = BRICKLET_STACK_SPI_CONFIG_MAX_SPEED_HZ;
// Open spidev
bricklet_stack->spi_fd = open(bricklet_stack->config.spi_device, O_RDWR);
if (bricklet_stack->spi_fd < 0) {
log_error("Could not open %s: : %s (%d)",
bricklet_stack->config.spi_device, get_errno_name(errno), errno);
return -1;
}
if (ioctl(bricklet_stack->spi_fd, SPI_IOC_WR_MODE, &mode) < 0) {
log_error("Could not configure SPI mode: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
if (ioctl(bricklet_stack->spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &max_speed_hz) < 0) {
log_error("Could not configure SPI max speed: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
if (ioctl(bricklet_stack->spi_fd, SPI_IOC_WR_BITS_PER_WORD, &bits_per_word) < 0) {
log_error("Could not configure SPI bits per word: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
if (ioctl(bricklet_stack->spi_fd, SPI_IOC_WR_LSB_FIRST, &lsb_first) < 0) {
log_error("Could not configure SPI lsb first: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
thread_create(&bricklet_stack->spi_thread, bricklet_stack_spi_thread, bricklet_stack);
return 0;
}
BrickletStack* bricklet_stack_init(BrickletStackConfig *config) {
int phase = 0;
char bricklet_stack_name[129] = {'\0'};
char notification_name[129] = {'\0'};
log_debug("Initializing BrickletStack subsystem for '%s' (num %d)", config->spi_device, config->chip_select_gpio_sysfs.num);
if(config->chip_select_driver == CHIP_SELECT_GPIO) {
if(gpio_sysfs_export(&config->chip_select_gpio_sysfs) < 0) {
goto cleanup;
}
if(gpio_sysfs_set_direction(&config->chip_select_gpio_sysfs, GPIO_SYSFS_DIRECTION_OUTPUT) < 0) {
goto cleanup;
}
if(gpio_sysfs_set_output(&config->chip_select_gpio_sysfs, GPIO_SYSFS_VALUE_HIGH) < 0) {
goto cleanup;
}
}
// create bricklet_stack struct
BrickletStack *bricklet_stack = (BrickletStack*)malloc(sizeof(BrickletStack));
if(bricklet_stack == NULL) {
goto cleanup;
}
memset(bricklet_stack, 0, sizeof(BrickletStack));
bricklet_stack->spi_fd = -1;
bricklet_stack->spi_thread_running = false;
memcpy(&bricklet_stack->config, config, sizeof(BrickletStackConfig));
ringbuffer_init(&bricklet_stack->ringbuffer_recv,
BRICKLET_STACK_SPI_RECEIVE_BUFFER_LENGTH,
bricklet_stack->buffer_recv);
// create base stack
if (snprintf(bricklet_stack_name, 128, "bricklet-stack-%s", bricklet_stack->config.spi_device) < 0) {
goto cleanup;
}
if (stack_create(&bricklet_stack->base, bricklet_stack_name, bricklet_stack_dispatch_to_spi) < 0) {
log_error("Could not create base stack for BrickletStack: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
phase = 1;
// add to stacks array
if (hardware_add_stack(&bricklet_stack->base) < 0) {
goto cleanup;
}
phase = 2;
if ((bricklet_stack->notification_event = eventfd(0, EFD_NONBLOCK | EFD_SEMAPHORE)) < 0) {
log_error("Could not create bricklet notification event: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
phase = 3;
// Add notification pipe as event source.
// Event is used to dispatch packets.
if (snprintf(notification_name, 128, "bricklet-stack-notification-%s", bricklet_stack->config.spi_device) < 0) {
goto cleanup;
}
if (event_add_source(bricklet_stack->notification_event, EVENT_SOURCE_TYPE_GENERIC,
notification_name, EVENT_READ,
bricklet_stack_dispatch_from_spi, bricklet_stack) < 0) {
log_error("Could not add bricklet notification pipe as event source");
goto cleanup;
}
phase = 4;
// Initialize SPI packet queues
if (queue_create(&bricklet_stack->request_queue, sizeof(Packet)) < 0) {
log_error("Could not create SPI request queue: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
mutex_create(&bricklet_stack->request_queue_mutex);
phase = 5;
if (queue_create(&bricklet_stack->response_queue, sizeof(Packet)) < 0) {
log_error("Could not create SPI response queue: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
mutex_create(&bricklet_stack->response_queue_mutex);
phase = 6;
if (bricklet_stack_init_spi(bricklet_stack) < 0) {
goto cleanup;
}
phase = 7;
cleanup:
switch (phase) { // no breaks, all cases fall through intentionally
case 6:
mutex_destroy(&bricklet_stack->response_queue_mutex);
queue_destroy(&bricklet_stack->response_queue, NULL);
// fall through
case 5:
mutex_destroy(&bricklet_stack->request_queue_mutex);
queue_destroy(&bricklet_stack->request_queue, NULL);
// fall through
case 4:
event_remove_source(bricklet_stack->notification_event, EVENT_SOURCE_TYPE_GENERIC);
// fall through
case 3:
robust_close(bricklet_stack->notification_event);
// fall through
case 2:
hardware_remove_stack(&bricklet_stack->base);
// fall through
case 1:
stack_destroy(&bricklet_stack->base);
// fall through
default:
break;
}
return phase == 7 ? bricklet_stack : NULL;
}
void bricklet_stack_exit(BrickletStack *bricklet_stack) {
// Remove event as possible poll source
event_remove_source(bricklet_stack->notification_event, EVENT_SOURCE_TYPE_GENERIC);
// Make sure that Thread shuts down properly
if (bricklet_stack->spi_thread_running) {
bricklet_stack->spi_thread_running = false;
thread_join(&bricklet_stack->spi_thread);
thread_destroy(&bricklet_stack->spi_thread);
}
hardware_remove_stack(&bricklet_stack->base);
stack_destroy(&bricklet_stack->base);
queue_destroy(&bricklet_stack->request_queue, NULL);
mutex_destroy(&bricklet_stack->request_queue_mutex);
queue_destroy(&bricklet_stack->response_queue, NULL);
mutex_destroy(&bricklet_stack->response_queue_mutex);
// Close file descriptors
robust_close(bricklet_stack->notification_event);
robust_close(bricklet_stack->spi_fd);
// Everything is closed and the threads are destroyed. We can
// now free the Bricklet Stack memory. It will not be accessed anymore.
free(bricklet_stack);
}
