void red_stack_exit(void) {
int i;
int slave;
// Remove reset interrupt as event source
if (_red_stack_reset_fd > 0) {
event_remove_source(_red_stack_reset_fd, EVENT_SOURCE_TYPE_GENERIC);
}
// Remove event as possible poll source
event_remove_source(_red_stack_notification_event, EVENT_SOURCE_TYPE_GENERIC);
// Make sure that Thread shuts down properly
if (_red_stack_spi_thread_running) {
_red_stack_spi_thread_running = false;
// Write in eventfd to make sure that we are not blocking the Thread
eventfd_t ev = 1;
eventfd_write(_red_stack_notification_event, ev);
thread_join(&_red_stack_spi_thread);
thread_destroy(&_red_stack_spi_thread);
}
// Thread is not running anymore, we make sure that all slaves are deselected
for (slave = 0; slave < RED_STACK_SPI_MAX_SLAVES; slave++) {
red_stack_spi_deselect(&_red_stack.slaves[slave]);
}
// We can also free the queue and stack now, nobody will use them anymore
for (i = 0; i < RED_STACK_SPI_MAX_SLAVES; i++) {
queue_destroy(&_red_stack.slaves[i].packet_to_spi_queue, NULL);
}
hardware_remove_stack(&_red_stack.base);
stack_destroy(&_red_stack.base);
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);
// Close file descriptors
close(_red_stack_notification_event);
close(_red_stack_spi_fd);
}
// Exit function called from central brickd code
void rs485_extension_exit(void) {
if (!_initialized) {
return;
}
// Remove event as possible poll source
event_remove_source(_send_verify_event, EVENT_SOURCE_TYPE_GENERIC);
event_remove_source(_master_poll_slave_event, EVENT_SOURCE_TYPE_GENERIC);
event_remove_source(_rs485_serial_fd, EVENT_SOURCE_TYPE_GENERIC);
event_remove_source(_master_retry_event, EVENT_SOURCE_TYPE_GENERIC);
event_remove_source(_partial_receive_timeout_event, EVENT_SOURCE_TYPE_GENERIC);
// We can also free the queue and stack now, nobody will use them anymore
queue_destroy(&_rs485_extension.packet_to_modbus_queue, NULL);
hardware_remove_stack(&_rs485_extension.base);
stack_destroy(&_rs485_extension.base);
// Close file descriptors
close(_send_verify_event);
close(_master_poll_slave_event);
close(_partial_receive_timeout_event);
close(_master_retry_event);
close(_rs485_serial_fd);
}
// Init function called from central brickd code
int rs485_extension_init(void) {
uint8_t _tmp_eeprom_read_buf[4];
int _eeprom_read_status;
int phase = 0;
log_info("RS485: Checking presence of extension");
// Modbus config: TYPE
_eeprom_read_status =
i2c_eeprom_read((uint16_t)RS485_EXTENSION_MODBUS_CONFIG_LOCATION_TYPE,
_tmp_eeprom_read_buf, 4);
if (_eeprom_read_status <= 0) {
log_error("RS485: EEPROM read error. Most probably no RS485 extension present");
return 0;
}
_modbus_serial_config_type = (uint32_t)((_tmp_eeprom_read_buf[0] << 0) |
(_tmp_eeprom_read_buf[1] << 8) |
(_tmp_eeprom_read_buf[2] << 16) |
(_tmp_eeprom_read_buf[3] << 24));
if (_modbus_serial_config_type == RS485_EXTENSION_TYPE) {
log_info("RS485: Initializing extension subsystem");
// Create base stack
if(stack_create(&_rs485_extension.base, "rs485_extension",
(StackDispatchRequestFunction)rs485_extension_dispatch_to_modbus) < 0) {
log_error("RS485: Could not create base stack for extension: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
phase = 1;
// Add to stacks array
if(hardware_add_stack(&_rs485_extension.base) < 0) {
goto cleanup;
}
phase = 2;
// Initialize modbus packet queue
if(queue_create(&_rs485_extension.packet_to_modbus_queue, sizeof(RS485ExtensionPacket)) < 0) {
log_error("RS485: Could not create Modbus queue: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
// Reading and storing eeprom config
// Modbus config: ADDRESS
_eeprom_read_status =
i2c_eeprom_read((uint16_t)RS485_EXTENSION_MODBUS_CONFIG_LOCATION_ADDRESS,
_tmp_eeprom_read_buf, 4);
if (_eeprom_read_status <= 0) {
log_error("RS485: Could not read config ADDRESS from EEPROM");
goto cleanup;
}
_modbus_serial_config_address = (uint32_t)((_tmp_eeprom_read_buf[0] << 0) |
(_tmp_eeprom_read_buf[1] << 8) |
(_tmp_eeprom_read_buf[2] << 16) |
(_tmp_eeprom_read_buf[3] << 24));
// Modbus config: BAUDRATE
_eeprom_read_status = i2c_eeprom_read((uint16_t)RS485_EXTENSION_MODBUS_CONFIG_LOCATION_BAUDRATE,
_tmp_eeprom_read_buf, 4);
if (_eeprom_read_status <= 0) {
log_error("RS485: Could not read config BAUDRATE from EEPROM");
goto cleanup;
}
_modbus_serial_config_baudrate = (uint32_t)((_tmp_eeprom_read_buf[0] << 0) |
(_tmp_eeprom_read_buf[1] << 8) |
(_tmp_eeprom_read_buf[2] << 16) |
(_tmp_eeprom_read_buf[3] << 24));
// Modbus config: PARITY
_eeprom_read_status = i2c_eeprom_read((uint16_t)RS485_EXTENSION_MODBUS_CONFIG_LOCATION_PARTIY,
_tmp_eeprom_read_buf, 1);
if (_eeprom_read_status <= 0) {
log_error("RS485: Could not read config PARITY from EEPROM");
goto cleanup;
}
if(_tmp_eeprom_read_buf[0] == RS485_EXTENSION_SERIAL_PARITY_NONE) {
_modbus_serial_config_parity = RS485_EXTENSION_SERIAL_PARITY_NONE;
}
else if (_tmp_eeprom_read_buf[0] == RS485_EXTENSION_SERIAL_PARITY_EVEN){
_modbus_serial_config_parity = RS485_EXTENSION_SERIAL_PARITY_EVEN;
}
else {
_modbus_serial_config_parity = RS485_EXTENSION_SERIAL_PARITY_ODD;
}
// Modbus config: STOPBITS
_eeprom_read_status =
i2c_eeprom_read((uint16_t)RS485_EXTENSION_MODBUS_CONFIG_LOCATION_STOPBITS,
_tmp_eeprom_read_buf, 1);
if (_eeprom_read_status <= 0) {
log_error("RS485: Could not read config STOPBITS from EEPROM");
goto cleanup;
}
_modbus_serial_config_stopbits = _tmp_eeprom_read_buf[0];
// Modbus config (if master): SLAVE ADDRESSES
if(_modbus_serial_config_address == 0) {
_rs485_extension.slave_num = 0;
uint16_t _current_eeprom_location =
RS485_EXTENSION_MODBUS_CONFIG_LOCATION_SLAVE_ADDRESSES_START;
uint32_t _current_slave_address;
_rs485_extension.slave_num = 0;
do {
_eeprom_read_status = i2c_eeprom_read(_current_eeprom_location, _tmp_eeprom_read_buf, 4);
if (_eeprom_read_status <= 0) {
log_error("RS485: Could not read config SLAVE ADDRESSES from EEPROM");
goto cleanup;
}
_current_slave_address = (uint32_t)((_tmp_eeprom_read_buf[0] << 0) |
(_tmp_eeprom_read_buf[1] << 8) |
(_tmp_eeprom_read_buf[2] << 16) |
(_tmp_eeprom_read_buf[3] << 24));
if(_current_slave_address != 0) {
_rs485_extension.slaves[_rs485_extension.slave_num] = _current_slave_address;
_rs485_extension.slave_num ++;
}
_current_eeprom_location = _current_eeprom_location + 4;
}
while(_current_slave_address != 0
&&
_rs485_extension.slave_num < RS485_EXTENSION_MODBUS_MAX_SLAVES);
}
// Configuring serial interface from the configs
if(rs485_extension_serial_init(RS485_EXTENSION_SERIAL_DEVICE) < 0) {
goto cleanup;
}
// Initial RS485 TX/RX state
init_tx_rx_state();
phase = 3;
// Setup partial data receive timer
setup_timer(&partial_receive_timer, TIME_UNIT_NSEC, PARTIAL_RECEIVE_TIMEOUT);
_partial_receive_timeout_event = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
if(!(_partial_receive_timeout_event < 0)) {
if(event_add_source(_partial_receive_timeout_event, EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ, partial_receive_timeout_handler, NULL) < 0) {
log_error("RS485: Could not add partial receive timeout notification pipe as event source");
goto cleanup;
}
}
else {
log_error("RS485: Could not create partial receive timer");
goto cleanup;
}
phase = 4;
// Adding serial data available event
if(event_add_source(_rs485_serial_fd, EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ, rs485_serial_data_available_handler, NULL) < 0) {
log_error("RS485: Could not add new serial data event");
goto cleanup;
}
phase = 5;
// Setup master retry timer
setup_timer(&master_retry_timer, TIME_UNIT_NSEC, MASTER_RETRY_TIMEOUT);
_master_retry_event = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
if(!(_master_retry_event < 0)) {
if(event_add_source(_master_retry_event, EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ, master_retry_timeout_handler, NULL) < 0) {
log_error("RS485: Could not add Modbus master retry notification pipe as event source");
goto cleanup;
}
}
else {
log_error("RS485: Could not create Modbus master retry timer");
goto cleanup;
}
phase = 6;
// Setup send verify timer
setup_timer(&send_verify_timer, TIME_UNIT_NSEC, SEND_VERIFY_TIMEOUT);
_send_verify_event = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
if(!(_send_verify_event < 0)) {
if(event_add_source(_send_verify_event, EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ, send_verify_timeout_handler, NULL) < 0) {
log_error("RS485: Could not add Modbus send verify notification pipe as event source");
goto cleanup;
}
}
else {
log_error("RS485: Could not create Modbus send verify timer");
goto cleanup;
}
phase = 7;
// Get things going in case of a master
if(_modbus_serial_config_address == 0 && _rs485_extension.slave_num > 0) {
// Setup master poll slave timer
setup_timer(&master_poll_slave_timer, TIME_UNIT_NSEC, MASTER_POLL_SLAVE_TIMEOUT);
_master_poll_slave_event = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
if(!(_master_poll_slave_event < 0) ) {
if(event_add_source(_master_poll_slave_event, EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ, master_poll_slave_timeout_handler, NULL) < 0) {
log_error("RS485: Could not add Modbus master poll slave notification pipe as event source");
goto cleanup;
}
}
else {
log_error("RS485: Could not create Modbus master poll slave timer");
goto cleanup;
}
master_poll_slave_timeout_handler(NULL);
}
phase = 8;
_initialized = true;
}
else {
log_info("RS485: Extension not present");
goto cleanup;
}
cleanup:
switch (phase) { // no breaks, all cases fall through intentionally
case 7:
close(_send_verify_event);
event_remove_source(_send_verify_event, EVENT_SOURCE_TYPE_GENERIC);
case 6:
close(_master_retry_event);
event_remove_source(_master_retry_event, EVENT_SOURCE_TYPE_GENERIC);
case 5:
close(_rs485_serial_fd);
event_remove_source(_rs485_serial_fd, EVENT_SOURCE_TYPE_GENERIC);
case 4:
close(_partial_receive_timeout_event);
event_remove_source(_partial_receive_timeout_event, EVENT_SOURCE_TYPE_GENERIC);
case 3:
queue_destroy(&_rs485_extension.packet_to_modbus_queue, NULL);
case 2:
hardware_remove_stack(&_rs485_extension.base);
case 1:
stack_destroy(&_rs485_extension.base);
default:
break;
}
return phase == 8 ? 0 : -1;
}
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);
}
