int mesh_init(void) {
uint16_t mesh_listen_port = \
(uint16_t)config_get_option_value("listen.mesh_port")->integer;
if (mesh_listen_port == 0) {
log_info("Mesh support is disabled");
return 0;
}
log_info("Initializing mesh subsystem");
if (mesh_start_listening(&mesh_listen_socket,
mesh_listen_port,
socket_create_allocated) >= 0) {
is_mesh_listen_socket_open = true;
}
if (!is_mesh_listen_socket_open) {
log_error("Could not open mesh listen socket");
return -1;
}
// Create mesh stack array.
if (array_create(&mesh_stacks, MAX_MESH_STACKS, sizeof(MeshStack), false) < 0) {
log_error("Could not create mesh stack array: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
return 0;
}
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);
}
}
static const char *network_get_address_family_name(int family, bool report_dual_stack) {
switch (family) {
case AF_INET:
return "IPv4";
case AF_INET6:
if (report_dual_stack && config_get_option_value("listen.dual_stack")->boolean) {
return "IPv6 dual-stack";
} else {
return "IPv6";
}
default:
return "<unknown>";
}
}
int client_create(Client *client, const char *name, IO *io,
uint32_t authentication_nonce,
ClientDestroyDoneFunction destroy_done) {
log_debug("Creating client from %s (handle: %d)", io->type, io->handle);
string_copy(client->name, sizeof(client->name), name);
client->io = io;
client->disconnected = false;
client->request_used = 0;
client->request_header_checked = false;
client->pending_request_count = 0;
client->authentication_state = CLIENT_AUTHENTICATION_STATE_DISABLED;
client->authentication_nonce = authentication_nonce;
client->destroy_done = destroy_done;
if (config_get_option_value("authentication.secret")->string != NULL) {
client->authentication_state = CLIENT_AUTHENTICATION_STATE_ENABLED;
}
node_reset(&client->pending_request_sentinel);
// create response writer
if (writer_create(&client->response_writer, client->io,
"response", packet_get_response_signature,
"client", client_get_recipient_signature,
client_recipient_disconnect, client) < 0) {
log_error("Could not create response writer: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
// add I/O object as event source
return event_add_source(client->io->handle, EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ, client_handle_read, client);
}
int mesh_start_listening(Socket *mesh_listen_socket,
uint16_t mesh_listen_socket_port,
SocketCreateAllocatedFunction create_allocated) {
int phase = 0;
struct addrinfo *resolved_address = NULL;
const char *address = config_get_option_value("listen.address")->string;
log_info("Opening mesh listen socket (P: %u)", mesh_listen_socket_port);
resolved_address = socket_hostname_to_address(address, mesh_listen_socket_port);
// Currently no IPv6 support for mesh.
if(resolved_address->ai_family == AF_INET6) {
log_error("Mesh gateway does not support IPv6");
goto CLEANUP;
}
if (resolved_address == NULL) {
log_error("Could not resolve mesh listen address '%s' (P: %u): %s (%d)",
address,
mesh_listen_socket_port,
get_errno_name(errno),
errno);
goto CLEANUP;
}
phase = 1;
// Create socket.
if (socket_create(mesh_listen_socket) < 0) {
log_error("Could not create mesh listen socket: %s (%d)",
get_errno_name(errno),
errno);
goto CLEANUP;
}
phase = 2;
if (socket_open(mesh_listen_socket,
resolved_address->ai_family,
resolved_address->ai_socktype,
resolved_address->ai_protocol) < 0) {
log_error("Could not open %s mesh listen socket: %s (%d)",
network_get_address_family_name(resolved_address->ai_family, false),
get_errno_name(errno),
errno);
goto CLEANUP;
}
#ifndef _WIN32
/*
* On Unix the SO_REUSEADDR socket option allows to rebind sockets in
* CLOSE-WAIT state. this is a desired effect. On Windows SO_REUSEADDR
* allows to rebind sockets in any state. This is dangerous. Therefore,
* don't set SO_REUSEADDR on Windows. Sockets can be rebound in CLOSE-WAIT
* state on Windows by default.
*/
if (socket_set_address_reuse(mesh_listen_socket, true) < 0) {
log_error("Could not enable address-reuse mode for mesh listen socket: %s (%d)",
get_errno_name(errno),
errno);
goto CLEANUP;
}
#endif
// Bind socket and start to listen.
if (socket_bind(mesh_listen_socket,
resolved_address->ai_addr,
resolved_address->ai_addrlen) < 0) {
log_error("Could not bind %s mesh listen socket to (A: %s, P: %u): %s (%d)",
network_get_address_family_name(resolved_address->ai_family, true),
address,
mesh_listen_socket_port,
get_errno_name(errno),
errno);
goto CLEANUP;
}
if (socket_listen(mesh_listen_socket, 10, create_allocated) < 0) {
log_error("Could not listen to %s mesh socket (A: %s, P: %u): %s (%d)",
network_get_address_family_name(resolved_address->ai_family, true),
address,
mesh_listen_socket_port,
get_errno_name(errno),
errno);
goto CLEANUP;
}
log_info("Mesh gateway started listening on (A: %s, P: %u, F: %s)",
address,
mesh_listen_socket_port,
network_get_address_family_name(resolved_address->ai_family, true));
if(event_add_source(mesh_listen_socket->base.handle,
EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ,
mesh_handle_accept,
mesh_listen_socket) < 0) {
goto CLEANUP;
}
phase = 3;
freeaddrinfo(resolved_address);
CLEANUP:
switch(phase) { // No breaks, all cases fall through intentionally.
case 2:
socket_destroy(mesh_listen_socket);
case 1:
freeaddrinfo(resolved_address);
default:
break;
}
return phase == 3 ? 0 : -1;
}
int main(int argc, char **argv) {
int exit_code = EXIT_FAILURE;
int i;
bool help = false;
bool version = false;
bool check_config = false;
bool daemon = false;
const char *debug_filter = NULL;
int pid_fd = -1;
#ifdef BRICKD_WITH_LIBUDEV
bool initialized_udev = false;
#endif
for (i = 1; i < argc; ++i) {
if (strcmp(argv[i], "--help") == 0) {
help = true;
} else if (strcmp(argv[i], "--version") == 0) {
version = true;
} else if (strcmp(argv[i], "--check-config") == 0) {
check_config = true;
} else if (strcmp(argv[i], "--daemon") == 0) {
daemon = true;
} else if (strcmp(argv[i], "--debug") == 0) {
if (i + 1 < argc && strncmp(argv[i + 1], "--", 2) != 0) {
debug_filter = argv[++i];
} else {
debug_filter = "";
}
} else {
fprintf(stderr, "Unknown option '%s'\n\n", argv[i]);
print_usage();
return EXIT_FAILURE;
}
}
if (help) {
print_usage();
return EXIT_SUCCESS;
}
if (version) {
printf("%s\n", VERSION_STRING);
return EXIT_SUCCESS;
}
if (prepare_paths() < 0) {
return EXIT_FAILURE;
}
if (check_config) {
return config_check(_config_filename) < 0 ? EXIT_FAILURE : EXIT_SUCCESS;
}
config_init(_config_filename);
if (config_has_error()) {
fprintf(stderr, "Error(s) occurred while reading config file '%s'\n",
_config_filename);
goto error_config;
}
log_init();
if (daemon) {
pid_fd = daemon_start(_log_filename, &_log_file, _pid_filename, true);
} else {
pid_fd = pid_file_acquire(_pid_filename, getpid());
if (pid_fd == PID_FILE_ALREADY_ACQUIRED) {
fprintf(stderr, "Already running according to '%s'\n", _pid_filename);
}
}
if (pid_fd < 0) {
goto error_pid_file;
}
log_info("Brick Daemon %s started (pid: %u, daemonized: %d)",
VERSION_STRING, getpid(), daemon ? 1 : 0);
if (debug_filter != NULL) {
log_enable_debug_override(debug_filter);
}
if (config_has_warning()) {
log_warn("Warning(s) in config file '%s', run with --check-config option for details",
_config_filename);
}
if (event_init() < 0) {
goto error_event;
}
if (signal_init(handle_sighup, handle_sigusr1) < 0) {
goto error_signal;
}
if (hardware_init() < 0) {
goto error_hardware;
}
if (usb_init() < 0) {
goto error_usb;
}
#ifdef BRICKD_WITH_LIBUDEV
if (!usb_has_hotplug()) {
if (udev_init() < 0) {
goto error_udev;
}
initialized_udev = true;
}
#endif
if (network_init() < 0) {
goto error_network;
}
if (mesh_init() < 0) {
goto error_mesh;
}
#ifdef BRICKD_WITH_RED_BRICK
if (gpio_init() < 0) {
goto error_gpio;
}
if (redapid_init() < 0) {
goto error_redapid;
}
if (red_stack_init() < 0) {
goto error_red_stack;
}
if (red_extension_init() < 0) {
goto error_red_extension;
}
if (red_usb_gadget_init() < 0) {
goto error_red_usb_gadget;
}
red_led_set_trigger(RED_LED_GREEN, config_get_option_value("led_trigger.green")->symbol);
red_led_set_trigger(RED_LED_RED, config_get_option_value("led_trigger.red")->symbol);
#endif
if (event_run(network_cleanup_clients_and_zombies) < 0) {
goto error_run;
}
#ifdef BRICKD_WITH_RED_BRICK
hardware_announce_disconnect();
network_announce_red_brick_disconnect();
red_usb_gadget_announce_red_brick_disconnect();
#endif
exit_code = EXIT_SUCCESS;
error_run:
#ifdef BRICKD_WITH_RED_BRICK
red_usb_gadget_exit();
error_red_usb_gadget:
red_extension_exit();
error_red_extension:
red_stack_exit();
error_red_stack:
redapid_exit();
error_redapid:
//gpio_exit();
error_gpio:
#endif
network_exit();
error_mesh:
mesh_exit();
error_network:
#ifdef BRICKD_WITH_LIBUDEV
if (initialized_udev) {
udev_exit();
}
error_udev:
#endif
usb_exit();
error_usb:
hardware_exit();
error_hardware:
signal_exit();
error_signal:
event_exit();
error_event:
log_info("Brick Daemon %s stopped", VERSION_STRING);
error_pid_file:
if (pid_fd >= 0) {
pid_file_release(_pid_filename, pid_fd);
}
log_exit();
error_config:
config_exit();
return exit_code;
}
int network_init(void) {
uint16_t plain_port = (uint16_t)config_get_option_value("listen.plain_port")->integer;
uint16_t websocket_port = (uint16_t)config_get_option_value("listen.websocket_port")->integer;
log_debug("Initializing network subsystem");
node_reset(&_pending_request_sentinel);
if (config_get_option_value("authentication.secret")->string != NULL) {
log_info("Authentication is enabled");
_next_authentication_nonce = get_random_uint32();
}
// create client array. the Client struct is not relocatable, because a
// pointer to it is passed as opaque parameter to the event subsystem
if (array_create(&_clients, 32, sizeof(Client), false) < 0) {
log_error("Could not create client array: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
// create zombie array. the Zombie struct is not relocatable, because a
// pointer to it is passed as opaque parameter to its timer object
if (array_create(&_zombies, 32, sizeof(Zombie), false) < 0) {
log_error("Could not create zombie array: %s (%d)",
get_errno_name(errno), errno);
array_destroy(&_clients, (ItemDestroyFunction)client_destroy);
return -1;
}
if (network_open_server_socket(&_plain_server_socket, plain_port,
socket_create_allocated) >= 0) {
_plain_server_socket_open = true;
}
if (websocket_port != 0) {
if (config_get_option_value("authentication.secret")->string == NULL) {
log_warn("WebSocket support is enabled without authentication");
}
if (network_open_server_socket(&_websocket_server_socket, websocket_port,
websocket_create_allocated) >= 0) {
_websocket_server_socket_open = true;
}
}
if (!_plain_server_socket_open && !_websocket_server_socket_open) {
log_error("Could not open any socket to listen to");
array_destroy(&_zombies, (ItemDestroyFunction)zombie_destroy);
array_destroy(&_clients, (ItemDestroyFunction)client_destroy);
return -1;
}
return 0;
}
static int network_open_server_socket(Socket *server_socket, uint16_t port,
SocketCreateAllocatedFunction create_allocated) {
int phase = 0;
const char *address = config_get_option_value("listen.address")->string;
struct addrinfo *resolved_address = NULL;
bool dual_stack;
log_debug("Opening server socket on port %u", port);
// resolve listen address
// FIXME: bind to all returned addresses, instead of just the first one.
// requires special handling if IPv4 and IPv6 addresses are returned
// and dual-stack mode is enabled
resolved_address = socket_hostname_to_address(address, port);
if (resolved_address == NULL) {
log_error("Could not resolve listen address '%s' (port: %u): %s (%d)",
address, port, get_errno_name(errno), errno);
goto cleanup;
}
phase = 1;
// create socket
if (socket_create(server_socket) < 0) {
log_error("Could not create socket: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
phase = 2;
if (socket_open(server_socket, resolved_address->ai_family,
resolved_address->ai_socktype, resolved_address->ai_protocol) < 0) {
log_error("Could not open %s server socket: %s (%d)",
network_get_address_family_name(resolved_address->ai_family, false),
get_errno_name(errno), errno);
goto cleanup;
}
if (resolved_address->ai_family == AF_INET6) {
dual_stack = config_get_option_value("listen.dual_stack")->boolean;
if (socket_set_dual_stack(server_socket, dual_stack) < 0) {
log_error("Could not %s dual-stack mode for IPv6 server socket: %s (%d)",
dual_stack ? "enable" : "disable",
get_errno_name(errno), errno);
goto cleanup;
}
}
#ifndef _WIN32
// on Unix the SO_REUSEADDR socket option allows to rebind sockets in
// CLOSE-WAIT state. this is a desired effect. on Windows SO_REUSEADDR
// allows to rebind sockets in any state. this is dangerous. therefore,
// don't set SO_REUSEADDR on Windows. sockets can be rebound in CLOSE-WAIT
// state on Windows by default.
if (socket_set_address_reuse(server_socket, true) < 0) {
log_error("Could not enable address-reuse mode for server socket: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
#endif
// bind socket and start to listen
if (socket_bind(server_socket, resolved_address->ai_addr,
resolved_address->ai_addrlen) < 0) {
log_error("Could not bind %s server socket to '%s' on port %u: %s (%d)",
network_get_address_family_name(resolved_address->ai_family, true),
address, port, get_errno_name(errno), errno);
goto cleanup;
}
if (socket_listen(server_socket, 10, create_allocated) < 0) {
log_error("Could not listen to %s server socket bound to '%s' on port %u: %s (%d)",
network_get_address_family_name(resolved_address->ai_family, true),
address, port, get_errno_name(errno), errno);
goto cleanup;
}
log_debug("Started listening to '%s' (%s) on port %u",
address,
network_get_address_family_name(resolved_address->ai_family, true),
port);
if (event_add_source(server_socket->base.handle, EVENT_SOURCE_TYPE_GENERIC,
EVENT_READ, network_handle_accept, server_socket) < 0) {
goto cleanup;
}
phase = 3;
freeaddrinfo(resolved_address);
cleanup:
switch (phase) { // no breaks, all cases fall through intentionally
case 2:
socket_destroy(server_socket);
case 1:
freeaddrinfo(resolved_address);
default:
break;
}
return phase == 3 ? 0 : -1;
}
int main(int argc, char **argv) {
int exit_code = EXIT_FAILURE;
int i;
bool help = false;
bool version = false;
bool check_config = false;
bool daemon = false;
bool debug = false;
bool libusb_debug = false;
int pid_fd = -1;
#ifdef BRICKD_WITH_LIBUDEV
bool initialized_udev = false;
#endif
for (i = 1; i < argc; ++i) {
if (strcmp(argv[i], "--help") == 0) {
help = true;
} else if (strcmp(argv[i], "--version") == 0) {
version = true;
} else if (strcmp(argv[i], "--check-config") == 0) {
check_config = true;
} else if (strcmp(argv[i], "--daemon") == 0) {
daemon = true;
} else if (strcmp(argv[i], "--debug") == 0) {
debug = true;
} else if (strcmp(argv[i], "--libusb-debug") == 0) {
libusb_debug = true;
} else {
fprintf(stderr, "Unknown option '%s'\n\n", argv[i]);
print_usage();
return EXIT_FAILURE;
}
}
if (help) {
print_usage();
return EXIT_SUCCESS;
}
if (version) {
printf("%s\n", VERSION_STRING);
return EXIT_SUCCESS;
}
if (prepare_paths() < 0) {
return EXIT_FAILURE;
}
if (check_config) {
return config_check(_config_filename) < 0 ? EXIT_FAILURE : EXIT_SUCCESS;
}
config_init(_config_filename);
log_init();
if (daemon) {
pid_fd = daemon_start(_log_filename, _pid_filename, true);
} else {
pid_fd = pid_file_acquire(_pid_filename, getpid());
if (pid_fd == PID_FILE_ALREADY_ACQUIRED) {
fprintf(stderr, "Already running according to '%s'\n", _pid_filename);
}
}
if (pid_fd < 0) {
goto error_log;
}
log_set_debug_override(debug);
log_set_level(LOG_CATEGORY_EVENT, config_get_option_value("log_level.event")->log_level);
log_set_level(LOG_CATEGORY_USB, config_get_option_value("log_level.usb")->log_level);
log_set_level(LOG_CATEGORY_NETWORK, config_get_option_value("log_level.network")->log_level);
log_set_level(LOG_CATEGORY_HOTPLUG, config_get_option_value("log_level.hotplug")->log_level);
log_set_level(LOG_CATEGORY_HARDWARE, config_get_option_value("log_level.hardware")->log_level);
log_set_level(LOG_CATEGORY_WEBSOCKET, config_get_option_value("log_level.websocket")->log_level);
#ifdef BRICKD_WITH_RED_BRICK
log_set_level(LOG_CATEGORY_RED_BRICK, config_get_option_value("log_level.red_brick")->log_level);
log_set_level(LOG_CATEGORY_SPI, config_get_option_value("log_level.spi")->log_level);
log_set_level(LOG_CATEGORY_RS485, config_get_option_value("log_level.rs485")->log_level);
#endif
log_set_level(LOG_CATEGORY_OTHER, config_get_option_value("log_level.other")->log_level);
if (config_has_error()) {
log_error("Error(s) occurred while reading config file '%s'",
_config_filename);
goto error_config;
}
if (daemon) {
log_info("Brick Daemon %s started (daemonized)", VERSION_STRING);
} else {
log_info("Brick Daemon %s started", VERSION_STRING);
}
if (config_has_warning()) {
log_error("Warning(s) in config file '%s', run with --check-config option for details",
_config_filename);
}
if (event_init() < 0) {
goto error_event;
}
if (signal_init(handle_sigusr1) < 0) {
goto error_signal;
}
if (hardware_init() < 0) {
goto error_hardware;
}
if (usb_init(libusb_debug) < 0) {
goto error_usb;
}
#ifdef BRICKD_WITH_LIBUDEV
if (!usb_has_hotplug()) {
if (udev_init() < 0) {
goto error_udev;
}
initialized_udev = true;
}
#endif
if (network_init() < 0) {
goto error_network;
}
#ifdef BRICKD_WITH_RED_BRICK
if (gpio_init() < 0) {
goto error_gpio;
}
if (redapid_init() < 0) {
goto error_redapid;
}
if (red_stack_init() < 0) {
goto error_red_stack;
}
if (red_extension_init() < 0) {
goto error_red_extension;
}
if (red_usb_gadget_init() < 0) {
goto error_red_usb_gadget;
}
red_led_set_trigger(RED_LED_GREEN, config_get_option_value("led_trigger.green")->red_led_trigger);
red_led_set_trigger(RED_LED_RED, config_get_option_value("led_trigger.red")->red_led_trigger);
#endif
if (event_run(network_cleanup_clients_and_zombies) < 0) {
goto error_run;
}
#ifdef BRICKD_WITH_RED_BRICK
hardware_announce_disconnect();
network_announce_red_brick_disconnect();
red_usb_gadget_announce_red_brick_disconnect();
#endif
exit_code = EXIT_SUCCESS;
error_run:
#ifdef BRICKD_WITH_RED_BRICK
red_usb_gadget_exit();
error_red_usb_gadget:
red_extension_exit();
error_red_extension:
red_stack_exit();
error_red_stack:
redapid_exit();
error_redapid:
//gpio_exit();
error_gpio:
#endif
network_exit();
error_network:
#ifdef BRICKD_WITH_LIBUDEV
if (initialized_udev) {
udev_exit();
}
error_udev:
#endif
usb_exit();
error_usb:
hardware_exit();
error_hardware:
signal_exit();
error_signal:
event_exit();
error_event:
log_info("Brick Daemon %s stopped", VERSION_STRING);
error_config:
error_log:
log_exit();
if (pid_fd >= 0) {
pid_file_release(_pid_filename, pid_fd);
}
config_exit();
return exit_code;
}
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;
}
