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); }