static bool
sol_mavlink_fd_handler(void *data, int fd, uint32_t cond)
{
struct sol_mavlink *mavlink = data;
mavlink_message_t msg = { 0 };
mavlink_status_t status;
uint8_t buf[MAVLINK_MAX_PACKET_LEN] = { 0 };
int i, res;
res = recv(mavlink->fd, buf, MAVLINK_MAX_PACKET_LEN, 0);
if (res == -1) {
if (errno == EINTR) {
SOL_INF("Could not read socket, retrying.");
return true;
} else {
SOL_WRN("Could not read socket. %s",
sol_util_strerrora(errno));
return false;
}
}
for (i = 0; i < res; ++i) {
if (!mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
continue;
switch (msg.msgid) {
case MAVLINK_MSG_ID_GPS_RAW_INT:
sol_mavlink_position_handler(mavlink, &msg);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
sol_mavlink_heartbeat_handler(mavlink, &msg);
break;
case MAVLINK_MSG_ID_STATUSTEXT:
sol_mavlink_statustext_handler(&msg);
break;
case MAVLINK_MSG_ID_HOME_POSITION:
sol_mavlink_home_position_handler(mavlink, &msg);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
sol_mavlink_mission_reached_handler(mavlink);
break;
default:
SOL_INF("Unhandled event, msgid: %d", msg.msgid);
}
}
if (mavlink->status == SOL_MAVLINK_STATUS_FULL_SETUP) {
mavlink->status = SOL_MAVLINK_STATUS_READY;
if (CHECK_HANDLER(mavlink, connect))
HANDLERS(mavlink)->connect((void *)mavlink->data, mavlink);
}
return true;
}
static bool
on_stdin(void *data, int fd, uint32_t flags)
{
uint16_t i;
struct sol_http_progressive_response *sse;
struct sol_buffer value = SOL_BUFFER_INIT_EMPTY;
if (flags & (SOL_FD_FLAGS_ERR | SOL_FD_FLAGS_HUP)) {
fprintf(stderr, "ERROR: Something wrong happened with file descriptor: %d\n", fd);
goto err;
}
if (flags & SOL_FD_FLAGS_IN) {
int err;
struct sol_blob *blob;
/* this will loop trying to read as much data as possible to buffer. */
err = sol_util_load_file_fd_buffer(fd, &value);
if (err < 0) {
fprintf(stderr, "ERROR: failed to read from stdin: %s\n",
sol_util_strerrora(-err));
goto err;
}
if (value.used == 0) {
/* no data usually means ^D on the terminal, quit the application */
printf("no data on stdin, quitting.\n");
should_quit = true;
SOL_PTR_VECTOR_FOREACH_IDX (&responses, sse, i)
sol_http_progressive_response_del(sse, true);
goto end;
}
blob = sol_buffer_to_blob(&value);
if (!blob) {
fprintf(stderr, "Could not alloc the blob data\n");
goto err;
}
SOL_PTR_VECTOR_FOREACH_IDX (&responses, sse, i)
sol_http_progressive_response_sse_feed(sse, blob);
sol_blob_unref(blob);
}
sol_buffer_fini(&value);
return true;
err:
sol_quit_with_code(EXIT_FAILURE);
end:
stdin_watch = NULL;
sol_buffer_fini(&value);
return false;
}
static void
on_fork(void *data)
{
const char *argv[] = {
"/usr/bin/dbus-daemon",
"--config-file=/etc/dbus-1/system.conf",
"--nofork",
NULL
};
if (mkdir("/run/dbus", 0755) < 0 && errno != EEXIST) {
SOL_WRN("could not create /run/dbus");
goto error;
}
if (access("/etc/dbus-1/system.conf", R_OK) < 0) {
FILE *fp;
SOL_INF("/etc/dbus-1/system.conf does not exist, create one as /run/dbus/system.conf");
fp = fopen("/run/dbus/system.conf", "we");
if (!fp) {
SOL_WRN("could not create /run/dbus/system.conf: %s", sol_util_strerrora(errno));
goto error;
}
fputs("<!DOCTYPE busconfig PUBLIC \"-//freedesktop//DTD D-Bus Bus Configuration 1.0//EN\" \"http://www.freedesktop.org/standards/dbus/1.0/busconfig.dtd\">\n"
"<busconfig>\n"
"<type>system</type>\n"
"<listen>unix:path=/run/dbus/system_bus_socket</listen>\n"
"<policy context=\"default\">\n"
"<allow user=\"*\"/>\n"
"<allow own=\"*\"/>\n"
"<allow send_type=\"method_call\"/>\n"
"<allow send_type=\"signal\"/>\n"
"<allow send_type=\"method_return\"/>\n"
"<allow send_type=\"error\"/>\n"
"<allow receive_type=\"method_call\"/>\n"
"<allow receive_type=\"signal\"/>\n"
"<allow receive_type=\"method_return\"/>\n"
"<allow receive_type=\"error\"/>\n"
"</policy>\n"
"</busconfig>\n",
fp);
fclose(fp);
argv[1] = "--config-file=/run/dbus/system.conf";
}
execv(argv[0], (char *const *)argv);
error:
sol_platform_linux_fork_run_exit(EXIT_FAILURE);
}
static void
mode_changed_cb(void *data, struct sol_mavlink *mavlink)
{
int err;
enum sol_mavlink_mode mode = sol_mavlink_get_mode(mavlink);
bool armed = sol_mavlink_check_armed(mavlink);
if (mode == SOL_MAVLINK_MODE_GUIDED && !armed) {
err = sol_mavlink_set_armed(mavlink, true);
if (err < 0) {
SOL_ERR("Could not arm vechicle: %s", sol_util_strerrora(-err));
}
}
}
static int
delta_process(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet)
{
struct timestamp_comparison_data *mdata = data;
struct timespec sub_result;
time_t result;
int r;
int32_t output;
if (!two_vars_get_value(mdata, port, packet))
return 0;
result = mdata->val[1].tv_sec - mdata->val[0].tv_sec;
if (result > INT32_MAX) {
sol_flow_send_error_packet(node, ERANGE,
"Delta is too big for seconds: %s", sol_util_strerrora(ERANGE));
return 0;
}
output = result;
r = sol_flow_send_irange_value_packet(node,
SOL_FLOW_NODE_TYPE_TIMESTAMP_DELTA__OUT__SECONDS, output);
SOL_INT_CHECK(r, < 0, r);
sol_util_timespec_sub(&mdata->val[0], &mdata->val[1], &sub_result);
result = sub_result.tv_sec * NSEC_PER_SEC + sub_result.tv_nsec;
if (result > INT32_MAX) {
SOL_DBG("Delta is too big for nanoseconds: %s",
sol_util_strerrora(ERANGE));
return 0;
}
output = result;
return sol_flow_send_irange_value_packet(node,
SOL_FLOW_NODE_TYPE_TIMESTAMP_DELTA__OUT__NANO_SECONDS, output);
}
/* Update libsoletta-gdb.py before changing the function and parameters below. */
int
sol_flow_node_init(struct sol_flow_node *node, struct sol_flow_node *parent, const char *name, const struct sol_flow_node_type *type, const struct sol_flow_node_options *options)
{
struct sol_flow_node_container_type *parent_type = NULL;
SOL_NULL_CHECK(type, -EINVAL);
SOL_NULL_CHECK(node, -EINVAL);
SOL_NULL_CHECK(type->get_ports_counts, -EINVAL);
SOL_FLOW_NODE_TYPE_API_CHECK(type, SOL_FLOW_NODE_TYPE_API_VERSION, -EINVAL);
SOL_FLOW_NODE_OPTIONS_API_CHECK(options, SOL_FLOW_NODE_OPTIONS_API_VERSION, -EINVAL);
if (type->init_type)
type->init_type();
node->type = type;
if (parent) {
SOL_FLOW_NODE_TYPE_IS_CONTAINER_CHECK(parent, -EINVAL);
node->parent = parent;
parent_type = (struct sol_flow_node_container_type *)parent->type;
if (parent_type->add)
parent_type->add(parent, node);
}
if (name)
node->id = strdup(name);
if (type->open) {
int r = type->open(node, node->data, options);
if (r < 0) {
if (parent_type) {
if (parent_type->remove)
parent_type->remove(parent, node);
}
SOL_WRN("failed to create node of type=%p: %s",
type, sol_util_strerrora(-r));
free(node->id);
node->id = NULL;
return r;
}
}
inspector_did_open_node(node, options);
return 0;
}
static void
install_cb(void *data, int status)
{
struct sol_flow_node *node = data;
struct update_data *mdata = sol_flow_node_get_private_data(node);
if (status < 0) {
sol_flow_send_error_packet(node, -status,
"Error while installing update: %s", sol_util_strerrora(-status));
}
sol_flow_send_bool_packet(node,
SOL_FLOW_NODE_TYPE_UPDATE_INSTALL__OUT__SUCCESS, status == 0);
mdata->handle = NULL;
}
void
send_direction_vector_output(struct gtk_common_data *mdata)
{
double values[3];
int r;
extract_value(mdata, values, 3, "X", "Y", "Z");
r = sol_flow_send_direction_vector_components_packet(mdata->node,
SOL_FLOW_NODE_TYPE_GTK_DIRECTION_VECTOR_EDITOR__OUT__OUT,
values[0], values[1], values[2]);
if (r < 0)
SOL_WRN("Could not send the direction vector packet. Reason: %s",
sol_util_strerrora(-r));
}
static int
time_process(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet)
{
struct timespec current_time;
int r;
r = sol_util_timespec_get_realtime(¤t_time);
if (r < 0) {
sol_flow_send_error_packet(node, r,
"Could not fetch current time: %s", sol_util_strerrora(r));
return 0;
}
return sol_flow_send_timestamp_packet(node,
SOL_FLOW_NODE_TYPE_TIMESTAMP_TIME__OUT__OUT, ¤t_time);
}
static void
position_changed_cb(void *data, struct sol_mavlink *mavlink)
{
int err;
struct sol_mavlink_position pos;
err = sol_mavlink_get_curr_position(mavlink, &pos);
if (err < 0) {
SOL_ERR("Could not get current position: %s", sol_util_strerrora(-err));
return;
}
if (sol_mavlink_check_armed(mavlink))
printf("lat: %f, long: %f, alt: %f\n", pos.latitude, pos.longitude,
pos.altitude);
}
static void
takeoff(struct sol_mavlink *mavlink)
{
int err;
struct sol_mavlink_position takeoff = { 0 };
takeoff.altitude = TAKEOFF_ALT;
err = sol_mavlink_takeoff(mavlink, &takeoff);
if (err < 0) {
SOL_ERR("Could not takeoff: %s", sol_util_strerrora(-err));
return;
}
printf(">>>> Taking off.\n");
}
SOL_API int
sol_util_iterate_dir(const char *path,
enum sol_util_iterate_dir_reason (*iterate_dir_cb)(void *data,
const char *dir_path,
struct dirent *ent),
const void *data)
{
DIR *dir;
struct dirent *ent, *res;
int r;
long name_max;
size_t len;
SOL_NULL_CHECK(path, -EINVAL);
SOL_NULL_CHECK(iterate_dir_cb, -EINVAL);
/* See readdir_r(3) */
name_max = pathconf(path, _PC_NAME_MAX);
if (name_max == -1)
name_max = 255;
len = offsetof(struct dirent, d_name) + name_max + 1;
ent = malloc(len);
SOL_NULL_CHECK(ent, -ENOMEM);
dir = opendir(path);
if (!dir) {
int aux_errno = errno;
SOL_INF("Could not open dir [%s] to iterate: %s", path,
sol_util_strerrora(aux_errno));
free(ent);
return -aux_errno;
}
r = readdir_r(dir, ent, &res);
SOL_INT_CHECK_GOTO(r, != 0, exit);
while (res) {
if (!streq(res->d_name, ".") && !streq(res->d_name, "..")) {
r = iterate_dir_cb((void *)data, path, res);
SOL_INT_CHECK_GOTO(r, < 0, exit);
if (r == SOL_UTIL_ITERATE_DIR_STOP)
break;
}
r = readdir_r(dir, ent, &res);
SOL_INT_CHECK_GOTO(r, != 0, exit);
}
/*
* do things getty would do to spawn a shell, basically become the
* session leader of the given tty, then make stdio/stdout/stderr use
* it.
*/
static void
do_shell(const char *tty)
{
char term_buf[128];
const char *envp[] = {
term_buf,
"HOME=/",
NULL,
};
char tty_path[PATH_MAX];
pid_t pid, tsid;
int r;
SOL_INF("no getty, exec shell: %s", shell);
r = snprintf(term_buf, sizeof(term_buf), "TERM=%s",
term ? term : get_term_for_tty(tty));
if (r < 0 || r >= (int)sizeof(term_buf))
envp[0] = "TERM=vt102";
else
envp[0] = term_buf;
pid = setsid();
if (pid < 0) {
int fd;
SOL_WRN("could not setsid(): %s", sol_util_strerrora(errno));
pid = getpid();
fd = open("/dev/tty", O_RDWR | O_NONBLOCK);
if (fd >= 0) {
sighandler_t oldsig;
/* man:tty(4)
* TIOCNOTTY:
* Detach the calling process from its controlling terminal.
*
* If the process is the session leader, then SIGHUP and
* SIGCONT signals are sent to the foreground process
* group and all processes in the current session lose
* their controlling tty.
*/
oldsig = signal(SIGHUP, SIG_IGN);
r = ioctl(fd, TIOCNOTTY);
close(fd);
signal(SIGHUP, oldsig);
SOL_INT_CHECK_GOTO(r, < 0, end);
}
void
send_float_output(struct gtk_common_data *mdata)
{
double value;
struct sol_drange drange = SOL_DRANGE_INIT();
int r;
extract_value(mdata, &value, 1, "Float");
drange.val = value;
r = sol_flow_send_drange_packet(mdata->node,
SOL_FLOW_NODE_TYPE_GTK_FLOAT_EDITOR__OUT__OUT, &drange);
if (r < 0)
SOL_WRN("Could not send the location packet. Reason: %s",
sol_util_strerrora(-r));
}
bool
sol_util_iterate_dir(const char *path, bool (*iterate_dir_cb)(void *data, const char *dir_path, struct dirent *ent), const void *data)
{
DIR *dir;
struct dirent *ent, *res;
int success;
long name_max;
size_t len;
bool result = false;
SOL_NULL_CHECK(path, false);
SOL_NULL_CHECK(iterate_dir_cb, false);
/* See readdir_r(3) */
name_max = pathconf(path, _PC_NAME_MAX);
if (name_max == -1)
name_max = 255;
len = offsetof(struct dirent, d_name) + name_max + 1;
ent = malloc(len);
SOL_NULL_CHECK(ent, false);
dir = opendir(path);
if (!dir) {
SOL_INF("Could not open dir [%s] to iterate: %s", path,
sol_util_strerrora(errno));
free(ent);
return false;
}
success = readdir_r(dir, ent, &res);
while (success == 0 && res) {
if (iterate_dir_cb((void *)data, path, res)) {
result = true;
break;
}
success = readdir_r(dir, ent, &res);
}
free(ent);
closedir(dir);
return result;
}
static void
spi_transfer(struct sol_spi *spi, const uint8_t *tx, uint8_t *rx, size_t size)
{
struct spi_ioc_transfer tr;
memset(&tr, 0, sizeof(struct spi_ioc_transfer));
tr.tx_buf = (uintptr_t)tx;
tr.rx_buf = (uintptr_t)rx;
tr.len = size;
tr.bits_per_word = spi->bits_per_word;
if (ioctl(spi->fd, SPI_IOC_MESSAGE(1), &tr) == -1) {
SOL_WRN("%u,%u: Unable to perform SPI transfer: %s", spi->bus,
spi->chip_select, sol_util_strerrora(errno));
return;
}
spi->transfer.status = size;
}
static int
hostname_start(const struct sol_platform_linux_micro_module *mod, const char *service)
{
struct sol_file_reader *reader;
struct sol_str_slice str;
const char *s, *p, *end;
int err = 0;
reader = sol_file_reader_open("/etc/hostname");
SOL_NULL_CHECK_MSG(reader, -errno, "could not read /etc/hostname");
str = sol_file_reader_get_all(reader);
s = p = str.data;
end = s + str.len;
for (; s < end; s++) {
if (!isblank(*s))
break;
}
for (p = end - 1; p > s; p--) {
if (!isblank(*p))
break;
}
if (s >= p) {
SOL_WRN("no hostname in /etc/hostname");
err = -ENOENT;
} else if (sethostname(s, p - s) < 0) {
SOL_WRN("could not set hostname: %s", sol_util_strerrora(errno));
err = -errno;
}
sol_file_reader_close(reader);
if (err == 0)
sol_platform_linux_micro_inform_service_state(service, SOL_PLATFORM_SERVICE_STATE_ACTIVE);
else
sol_platform_linux_micro_inform_service_state(service, SOL_PLATFORM_SERVICE_STATE_FAILED);
return err;
}
static bool
watchdog_keep_alive(void *data)
{
int reply, err;
SOL_DBG("keep watchdog alive");
err = ioctl(watchdog_fd, WDIOC_KEEPALIVE, &reply);
if (reply != WDIOF_KEEPALIVEPING)
SOL_WRN("unexpected watchdog keepalive reply=%#x, expected=%#x. Ignored.",
reply, WDIOF_KEEPALIVEPING);
if (err == 0)
return true;
SOL_WRN("failed to keep watchdog alive: %s", sol_util_strerrora(errno));
close(watchdog_fd);
watchdog_fd = -1;
watchdog_timeout = NULL;
sol_platform_linux_micro_inform_service_state(service_name, SOL_PLATFORM_SERVICE_STATE_FAILED);
return false;
}
void
send_location_output(struct gtk_common_data *mdata)
{
struct sol_location loc;
double values[3];
int r;
extract_value(mdata, values, 3, "Latitude", "Longitude", "Altitude");
loc.lat = values[0];
loc.lon = values[1];
loc.alt = values[2];
r = sol_flow_send_location_packet(mdata->node,
SOL_FLOW_NODE_TYPE_GTK_LOCATION_EDITOR__OUT__OUT,
&loc);
if (r < 0)
SOL_WRN("Could not send the location packet. Reason: %s",
sol_util_strerrora(-r));
}
//! [uart write completed]
static void
producer_data_written(void *data, struct sol_uart *uart, struct sol_blob *blob, int status)
{
struct sol_str_slice slice;
slice = sol_str_slice_from_blob(blob);
if (status < 0) {
fprintf(stderr, "Could not write the UUID %.*s - Reason: %s\n", SOL_STR_SLICE_PRINT(slice),
sol_util_strerrora(-status));
sol_quit();
} else {
printf("Producer: UUID %.*s written\n", SOL_STR_SLICE_PRINT(slice));
if (pending_blob) { //If we have a pending blob now it's the time to try to send it!
if (!send_blob(pending_blob)) {
fprintf(stderr, "Could not send the pending blob!\n");
sol_quit();
}
}
}
}
static int
read_file(void *data, const char *name, const char **buf, size_t *size)
{
struct runner *r = data;
struct sol_file_reader *fr = NULL;
struct sol_str_slice slice;
char *path;
int err;
err = asprintf(&path, "%s/%s", r->dirname, name);
if (err < 0) {
err = -ENOMEM;
goto error;
}
fr = sol_file_reader_open(path);
if (!fr) {
err = -errno;
SOL_ERR("Couldn't open input file '%s': %s", path, sol_util_strerrora(errno));
goto error;
}
err = sol_ptr_vector_append(&r->file_readers, fr);
if (err < 0)
goto error;
free(path);
slice = sol_file_reader_get_all(fr);
*buf = slice.data;
*size = slice.len;
return 0;
error:
free(path);
if (fr)
sol_file_reader_close(fr);
return err;
}
static int
file_reader_load(struct file_reader_data *mdata)
{
struct sol_file_reader *reader;
struct sol_str_slice slice;
if (!mdata->path)
return 0;
reader = sol_file_reader_open(mdata->path);
if (!reader) {
sol_flow_send_error_packet(mdata->node, errno,
"Could not load \"%s\": %s", mdata->path, sol_util_strerrora(errno));
return -errno;
}
slice = sol_file_reader_get_all(reader);
SOL_DBG("loaded path=\"%s\", data=%p, len=%zd", mdata->path, slice.data, slice.len);
mdata->reader_blob = sol_blob_new(&file_reader_blob_type, NULL,
reader, sizeof(reader));
SOL_NULL_CHECK_GOTO(mdata->reader_blob, err_reader);
mdata->content_blob = sol_blob_new(SOL_BLOB_TYPE_NOFREE,
mdata->reader_blob,
slice.data, slice.len);
SOL_NULL_CHECK_GOTO(mdata->content_blob, err_content);
return sol_flow_send_blob_packet(mdata->node,
SOL_FLOW_NODE_TYPE_FILE_READER__OUT__OUT,
mdata->content_blob);
err_content:
sol_blob_unref(mdata->reader_blob);
err_reader:
sol_file_reader_close(reader);
return -ENOMEM;
}
static int32_t
get_int32_packet_and_log(const struct sol_flow_node *n, uint16_t port, const struct sol_flow_packet *packet)
{
const struct sol_flow_node_type *type;
const struct sol_flow_port_description *port_desc;
int32_t value;
int err;
/* get the struct sol_irange::value member. This function also validates if the
* given packet is of requested type (irange), otherise will return an -errno.
*/
err = sol_flow_packet_get_irange_value(packet, &value);
if (err < 0) {
fprintf(stderr, "ERROR: could not get irange packet value: %p %s\n",
packet, sol_util_strerrora(-err));
return err;
}
/* log the value to stdout. First we get the node type from
* current node (minutes or seconds), then we find the port
* description from its index. with that we can get the port name.
*/
type = sol_flow_node_get_type(n);
port_desc = sol_flow_node_get_port_out_description(type, port);
if (!port_desc) {
fprintf(stderr, "ERROR: no output port description for index %" PRIu16
" of node %p\n",
port, n);
return -ENOENT;
}
printf("node type %s port #%" PRIu16 " '%s' (%s): %" PRId32 "\n",
type->description->name, port, port_desc->name,
port_desc->data_type, value);
return value;
}
static void
startup(void)
{
const char *algorithm = "sha256";
const char *key = NULL;
char **argv = sol_argv();
int i, argc = sol_argc();
size_t chunk_size = -1;
if (argc < 2) {
fprintf(stderr,
"Usage:\n\t%s [-a <algorithm>] [-c chunk_size] [-k key] <file1> .. <fileN>\n", argv[0]);
sol_quit_with_code(EXIT_FAILURE);
return;
}
for (i = 1; i < argc; i++) {
struct feed_ctx *ctx;
struct sol_message_digest_config cfg = {
SOL_SET_API_VERSION(.api_version = SOL_MESSAGE_DIGEST_CONFIG_API_VERSION, )
.algorithm = algorithm,
.on_feed_done = on_feed_done,
.on_digest_ready = on_digest_ready,
};
struct sol_file_reader *fr;
struct sol_blob *blob;
struct sol_message_digest *mdh;
int r;
if (argv[i][0] == '-') {
if (argv[i][1] == 'a') {
if (i + 1 < argc) {
algorithm = argv[i + 1];
i++;
continue;
} else
fputs("ERROR: argument -a missing value.\n", stderr);
} else if (argv[i][1] == 'k') {
if (i + 1 < argc) {
key = argv[i + 1];
i++;
continue;
} else
fputs("ERROR: argument -a missing value.\n", stderr);
} else if (argv[i][1] == 'c') {
if (i + 1 < argc) {
chunk_size = atoi(argv[i + 1]);
i++;
continue;
} else
fputs("ERROR: argument -c missing value.\n", stderr);
} else
fprintf(stderr, "ERROR: unknown option %s\n", argv[i]);
sol_quit_with_code(EXIT_FAILURE);
return;
}
fr = sol_file_reader_open(argv[i]);
if (!fr) {
fprintf(stderr, "ERROR: could not open file '%s': %s\n",
argv[i], sol_util_strerrora(errno));
continue;
}
blob = sol_file_reader_to_blob(fr);
if (!blob) {
fprintf(stderr, "ERROR: could not create blob for file '%s'\n",
argv[i]);
continue;
}
cfg.data = ctx = calloc(1, sizeof(struct feed_ctx));
if (!ctx) {
fprintf(stderr, "ERROR: could not allocate context memory "
"to process file '%s'\n", argv[i]);
sol_blob_unref(blob);
continue;
}
ctx->file = argv[i];
ctx->start = sol_util_timespec_get_current();
ctx->done = 0;
ctx->chunk_size = chunk_size;
if (key)
cfg.key = sol_str_slice_from_str(key);
mdh = sol_message_digest_new(&cfg);
if (!mdh) {
fprintf(stderr, "ERROR: could not create message digest for "
" algorithm \"%s\": %s\n",
algorithm, sol_util_strerrora(errno));
sol_blob_unref(blob);
free(ctx);
continue;
}
if (chunk_size <= 0) {
r = sol_message_digest_feed(mdh, blob, true);
if (r < 0) {
fprintf(stderr, "ERROR: could not feed message for "
" algorithm \"%s\": %s\n",
algorithm, sol_util_strerrora(-r));
sol_blob_unref(blob);
sol_message_digest_del(mdh);
free(ctx);
continue;
}
} else {
size_t offset = 0;
while (offset < blob->size) {
size_t remaining = blob->size - offset;
size_t clen = remaining > chunk_size ? chunk_size : remaining;
uint8_t *cmem = (uint8_t *)blob->mem + offset;
bool is_last = offset + clen == blob->size;
struct sol_blob *chunk = sol_blob_new(&SOL_BLOB_TYPE_NO_FREE_DATA,
blob, cmem, clen);
if (!chunk) {
fprintf(stderr, "ERROR: could not create chunk blob at "
"mem %p, size=%zd\n", cmem, clen);
sol_blob_unref(blob);
sol_message_digest_del(mdh);
free(ctx);
continue;
}
r = sol_message_digest_feed(mdh, chunk, is_last);
if (r < 0) {
fprintf(stderr, "ERROR: could not feed chunk for "
" algorithm \"%s\": %s\n",
algorithm, sol_util_strerrora(-r));
sol_blob_unref(blob);
sol_blob_unref(chunk);
sol_message_digest_del(mdh);
free(ctx);
continue;
}
sol_blob_unref(chunk);
offset += clen;
}
}
sol_blob_unref(blob);
pending++;
}
SOL_API int
sol_flow_node_named_options_init_from_strv(
struct sol_flow_node_named_options *named_opts,
const struct sol_flow_node_type *type,
const char *const *strv)
{
#ifndef SOL_FLOW_NODE_TYPE_DESCRIPTION_ENABLED
SOL_WRN("This function needs NODE_DESCRIPTION=y in the build config.");
return -ENOTSUP;
#else
const struct sol_flow_node_options_description *desc;
struct sol_flow_node_named_options_member *members = NULL, *m;
uint16_t members_count = 0;
const struct sol_flow_node_options_member_description *mdesc;
const char *const *entry;
int r;
SOL_NULL_CHECK(named_opts, -EINVAL);
if (type->init_type)
type->init_type();
SOL_NULL_CHECK(type->description, -EINVAL);
SOL_NULL_CHECK(type->description->options, -EINVAL);
SOL_NULL_CHECK(type->description->options->members, -EINVAL);
desc = type->description->options;
if (strv) {
for (entry = strv; *entry; entry++) {
members_count++;
}
members = calloc(members_count, sizeof(struct sol_flow_node_named_options_member));
}
for (entry = strv, m = members; entry && *entry != NULL; entry++, m++) {
const char *key, *value;
unsigned int key_len;
if (split_option(*entry, &key, &key_len, &value)) {
r = -EINVAL;
SOL_DBG("Invalid option #%u format: \"%s\"", (unsigned)(entry - strv), *entry);
goto end;
}
for (mdesc = desc->members; mdesc && mdesc->name; mdesc++) {
if (sol_str_slice_str_eq(SOL_STR_SLICE_STR(key, key_len), mdesc->name))
break;
}
if (!mdesc || !mdesc->name) {
r = -EINVAL;
SOL_DBG("Unknown option: \"%s\"", *entry);
goto end;
}
m->type = sol_flow_node_options_member_type_from_string(mdesc->data_type);
m->name = mdesc->name;
r = sol_flow_node_named_options_parse_member(m, value, mdesc);
if (r < 0) {
SOL_DBG("Could not parse member #%u "
"name=\"%s\", type=\"%s\", option=\"%s\": %s",
(unsigned)(mdesc - desc->members), mdesc->name,
mdesc->data_type, *entry, sol_util_strerrora(-r));
goto end;
}
SOL_DBG("Parsed option \"%s\" member #%u "
"name=\"%s\", type=\"%s\", offset=%hu, size=%hu",
*entry, (unsigned)(mdesc - desc->members), mdesc->name,
mdesc->data_type, mdesc->offset, mdesc->size);
}
named_opts->members = members;
members = NULL;
named_opts->count = members_count;
r = 0;
end:
free(members);
return r;
#endif
}
static int32_t
_i2c_smbus_ioctl(int dev, uint8_t rw, uint8_t command, size_t size, union i2c_smbus_data *data)
{
struct i2c_smbus_ioctl_data ioctldata = {
.read_write = rw,
.command = command,
.size = 0,
.data = data
};
switch (size) {
case 1:
ioctldata.size = I2C_SMBUS_BYTE_DATA;
break;
case 2:
ioctldata.size = I2C_SMBUS_WORD_DATA;
break;
default:
ioctldata.size = I2C_SMBUS_BLOCK_DATA;
}
if (ioctl(dev, I2C_SMBUS, &ioctldata) == -1) {
return -errno;
}
return 0;
}
SOL_API bool
sol_i2c_write_quick(const struct sol_i2c *i2c, bool rw)
{
struct i2c_smbus_ioctl_data ioctldata = {
.read_write = rw,
.command = 0,
.size = I2C_SMBUS_QUICK,
.data = NULL
};
SOL_NULL_CHECK(i2c, false);
if (ioctl(i2c->dev, I2C_SMBUS, &ioctldata) == -1) {
SOL_WRN("Unable to perform I2C-SMBus write quick (bus = %u,"
" device address = %u): %s", i2c->bus, i2c->addr,
sol_util_strerrora(errno));
return false;
}
return true;
}
static bool
write_byte(const struct sol_i2c *i2c, uint8_t byte)
{
struct i2c_smbus_ioctl_data ioctldata = {
.read_write = I2C_SMBUS_WRITE,
.command = byte,
.size = I2C_SMBUS_BYTE,
.data = NULL
};
if (ioctl(i2c->dev, I2C_SMBUS, &ioctldata) == -1) {
SOL_WRN("Unable to perform I2C-SMBus write byte (bus = %u,"
" device address = %u): %s",
i2c->bus, i2c->addr, sol_util_strerrora(errno));
return false;
}
return true;
}
static bool
read_byte(const struct sol_i2c *i2c, uint8_t *byte)
{
union i2c_smbus_data data;
struct i2c_smbus_ioctl_data ioctldata = {
.read_write = I2C_SMBUS_READ,
.command = 0,
.size = I2C_SMBUS_BYTE,
.data = &data,
};
if (ioctl(i2c->dev, I2C_SMBUS, &ioctldata) == -1) {
SOL_WRN("Unable to perform I2C-SMBus read byte (bus = %u,"
" device address = %u): %s",
i2c->bus, i2c->addr, sol_util_strerrora(errno));
return false;
}
*byte = data.byte;
return true;
}
SOL_API ssize_t
sol_i2c_read(const struct sol_i2c *i2c, uint8_t *values, size_t count)
{
size_t i;
SOL_NULL_CHECK(i2c, -EINVAL);
SOL_NULL_CHECK(values, -EINVAL);
SOL_INT_CHECK(count, == 0, -EINVAL);
for (i = 0; i < count; i++) {
uint8_t byte;
if (!read_byte(i2c, &byte))
return -errno;
*values = byte;
values++;
}
return i;
}
SOL_API bool
sol_i2c_write(const struct sol_i2c *i2c, uint8_t *values, size_t count)
{
size_t i;
SOL_NULL_CHECK(i2c, false);
SOL_NULL_CHECK(values, false);
SOL_INT_CHECK(count, == 0, false);
for (i = 0; i < count; i++) {
if (!write_byte(i2c, *values))
return false;
values++;
}
return true;
}
static int
sol_i2c_plain_read_register(const struct sol_i2c *i2c,
uint8_t command,
uint8_t *values,
size_t count)
{
struct i2c_msg msgs[] = {
{
.addr = i2c->addr,
.flags = 0,
.len = 1,
.buf = &command
},
{
.addr = i2c->addr,
.flags = I2C_M_RD,
.len = count,
.buf = values,
}
};
static void
test_strtodn(void)
{
#ifdef HAVE_LOCALE
char *oldloc;
const char *comma_locales[] = {
"pt", "pt_BR", "de", "it", "ru", NULL
};
const char *comma_locale;
#endif
char dbl_max_str[256], neg_dbl_max_str[256];
char dbl_max_str_overflow[256], neg_dbl_max_str_overflow[256];
const struct test {
const char *str;
double reference;
int expected_errno;
bool use_locale;
int endptr_offset;
} *itr, tests[] = {
{ "0", 0.0, 0, false, -1 },
{ "123", 123.0, 0, false, -1 },
{ "1.0", 1.0, 0, false, -1 },
{ "123.456", 123.456, 0, false, -1 },
{ "345e+12", 345e12, 0, false, -1 },
{ "345e-12", 345e-12, 0, false, -1 },
{ "345E+12", 345e12, 0, false, -1 },
{ "345E-12", 345e-12, 0, false, -1 },
{ "-1.0", -1.0, 0, false, -1 },
{ "-123.456", -123.456, 0, false, -1 },
{ "-345e+12", -345e12, 0, false, -1 },
{ "-345e-12", -345e-12, 0, false, -1 },
{ "-345E+12", -345e12, 0, false, -1 },
{ "-345E-12", -345e-12, 0, false, -1 },
{ "-345.678e+12", -345.678e12, 0, false, -1 },
{ "-345.678e-12", -345.678e-12, 0, false, -1 },
{ "-345.678E+12", -345.678e12, 0, false, -1 },
{ "-345.678E-12", -345.678e-12, 0, false, -1 },
{ dbl_max_str, DBL_MAX, 0, false, -1 },
{ neg_dbl_max_str, -DBL_MAX, 0, false, -1 },
{ dbl_max_str_overflow, DBL_MAX, ERANGE, false, -1 },
{ neg_dbl_max_str_overflow, -DBL_MAX, ERANGE, false, -1 },
{ "x", 0, 0, false, 0 },
{ "1x", 1.0, 0, false, 1 },
{ "12,3", 12.0, 0, false, 2 },
{ "", 0, 0, false, 0 },
#ifdef HAVE_LOCALE
/* commas as decimal separators */
{ "1,0", 1.0, 0, true, -1 },
{ "123,456", 123.456, 0, true, -1 },
{ "345e+12", 345e12, 0, true, -1 },
{ "345e-12", 345e-12, 0, true, -1 },
{ "345E+12", 345e12, 0, true, -1 },
{ "345E-12", 345e-12, 0, true, -1 },
{ "-1,0", -1.0, 0, true, -1 },
{ "-123,456", -123.456, 0, true, -1 },
{ "-345e+12", -345e12, 0, true, -1 },
{ "-345e-12", -345e-12, 0, true, -1 },
{ "-345E+12", -345e12, 0, true, -1 },
{ "-345E-12", -345e-12, 0, true, -1 },
{ "-345,678e+12", -345.678e12, 0, true, -1 },
{ "-345,678e-12", -345.678e-12, 0, true, -1 },
{ "-345,678E+12", -345.678e12, 0, true, -1 },
{ "-345,678E-12", -345.678e-12, 0, true, -1 },
{ "12.3", 12.0, 0, true, 2 },
#endif
{}
};
#ifdef HAVE_LOCALE
oldloc = setlocale(LC_ALL, NULL);
if (oldloc)
oldloc = strdupa(oldloc);
setlocale(LC_ALL, "C");
#endif
snprintf(dbl_max_str, sizeof(dbl_max_str), "%.64g", DBL_MAX);
snprintf(neg_dbl_max_str, sizeof(neg_dbl_max_str), "%.64g", -DBL_MAX);
snprintf(dbl_max_str_overflow, sizeof(dbl_max_str_overflow), "%.64g0", DBL_MAX);
snprintf(neg_dbl_max_str_overflow, sizeof(neg_dbl_max_str_overflow), "%.64g0", -DBL_MAX);
#ifdef HAVE_LOCALE
{
const char **loc;
comma_locale = NULL;
for (loc = comma_locales; *loc != NULL; loc++) {
if (setlocale(LC_ALL, *loc)) {
setlocale(LC_ALL, oldloc);
SOL_DBG("Using locale '%s' to produce commas as "
"decimal separator. Ex: %0.2f", *loc, 1.23);
comma_locale = *loc;
break;
}
}
if (!comma_locale) {
setlocale(LC_ALL, oldloc);
SOL_WRN("Couldn't find a locale with decimal commas");
}
}
#endif
for (itr = tests; itr->str != NULL; itr++) {
double value;
char buf[512];
char *endptr;
size_t slen = strlen(itr->str);
int endptr_offset, wanted_endptr_offset;
int reterr;
snprintf(buf, sizeof(buf), "%s123garbage", itr->str);
if (comma_locale)
setlocale(LC_ALL, comma_locale);
else if (itr->use_locale) {
SOL_DBG("SKIP (no comma locale): '%s'", itr->str);
continue;
}
value = sol_util_strtodn(buf, &endptr, slen, itr->use_locale);
reterr = errno;
endptr_offset = endptr - buf;
if (comma_locale)
setlocale(LC_ALL, oldloc);
wanted_endptr_offset = itr->endptr_offset;
if (wanted_endptr_offset < 0)
wanted_endptr_offset = slen;
if (itr->expected_errno == 0 && reterr == 0) {
if (sol_drange_val_equal(itr->reference, value)) {
SOL_DBG("OK: parsed '%s' as %g (locale:%u)", itr->str, value,
itr->use_locale);
} else {
SOL_WRN("FAILED: parsed '%s' as %.64g where %.64g was expected"
" (difference = %g) (locale:%u)",
itr->str, value, itr->reference, itr->reference - value,
itr->use_locale);
FAIL();
}
} else if (itr->expected_errno == 0 && reterr < 0) {
SOL_WRN("FAILED: parsing '%s' failed with errno = %d (%s) (locale:%u)",
itr->str, reterr, sol_util_strerrora(reterr), itr->use_locale);
FAIL();
} else if (itr->expected_errno != 0 && reterr == 0) {
SOL_WRN("FAILED: parsing '%s' should fail with errno = %d (%s)"
", but got success with errno = %d (%s), value = %g (locale:%u)",
itr->str,
itr->expected_errno, sol_util_strerrora(itr->expected_errno),
reterr, sol_util_strerrora(reterr), value, itr->use_locale);
FAIL();
} else if (itr->expected_errno != 0 && reterr < 0) {
if (itr->expected_errno != reterr) {
SOL_WRN("FAILED: parsing '%s' should fail with errno = %d (%s)"
", but got errno = %d (%s), value = %g (locale:%u)",
itr->str,
itr->expected_errno, sol_util_strerrora(itr->expected_errno),
reterr, sol_util_strerrora(reterr), value, itr->use_locale);
FAIL();
} else if (!sol_drange_val_equal(itr->reference, value)) {
SOL_WRN("FAILED: parsing '%s' should result in %.64g"
", but got %.64g (difference = %g) (locale:%u)",
itr->str, itr->reference, value, itr->reference - value,
itr->use_locale);
FAIL();
} else {
SOL_DBG("OK: parsed '%s' as %g, setting errno = %d (%s) (locale:%u)",
itr->str, value, reterr, sol_util_strerrora(reterr), itr->use_locale);
}
}
if (wanted_endptr_offset != endptr_offset) {
SOL_WRN("FAILED: parsing '%s' should stop at offset %d, but got %d (locale:%u)",
itr->str, wanted_endptr_offset, endptr_offset, itr->use_locale);
FAIL();
}
}
}
static int
sol_mavlink_init_serial(struct sol_mavlink *mavlink)
{
struct termios tty = { 0 };
char *portname;
int baud_rate;
baud_rate = mavlink->config->baud_rate;
if (!baud_rate) {
baud_rate = 115200;
SOL_INF("No baud_rate config provided, setting default: 115200");
}
portname = sol_str_slice_to_str(*mavlink->address);
if (!portname) {
SOL_ERR("Could not format portname string - %s",
sol_util_strerrora(errno));
return -errno;
}
mavlink->fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC | O_CLOEXEC);
if (mavlink->fd == -1) {
SOL_ERR("Could not open serial port: %s - %s", portname,
sol_util_strerrora(errno));
goto err;
}
if (tcgetattr(mavlink->fd, &tty) != 0) {
SOL_ERR("Could not read serial attr: %s", sol_util_strerrora(errno));
goto attr_err;
}
if (cfsetospeed(&tty, baud_rate) == -1) {
SOL_ERR("Could not set serial output speed - %s",
sol_util_strerrora(errno));
goto attr_err;
}
if (cfsetispeed(&tty, baud_rate) == -1) {
SOL_ERR("Could not set serial input speed - %s",
sol_util_strerrora(errno));
goto attr_err;
}
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;
tty.c_iflag &= ~IGNBRK;
tty.c_lflag = 0;
tty.c_oflag = 0;
tty.c_cc[VMIN] = 0;
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
tty.c_cflag |= (CLOCAL | CREAD);
tty.c_cflag &= ~(PARENB | PARODD);
tty.c_cflag |= 0;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CRTSCTS;
if (tcsetattr(mavlink->fd, TCSANOW, &tty) != 0) {
SOL_ERR("Could not set serial attr: %s", sol_util_strerrora(errno));
goto attr_err;
}
free(portname);
return 0;
attr_err:
close(mavlink->fd);
err:
free(portname);
return -errno;
}
static int32_t
_i2c_smbus_ioctl(int dev, uint8_t rw, uint8_t command, size_t size, union i2c_smbus_data *data)
{
struct i2c_smbus_ioctl_data ioctldata = {
.read_write = rw,
.command = command,
.size = 0,
.data = data
};
switch (size) {
case 1:
ioctldata.size = I2C_SMBUS_BYTE_DATA;
break;
case 2:
ioctldata.size = I2C_SMBUS_WORD_DATA;
break;
default:
ioctldata.size = I2C_SMBUS_BLOCK_DATA;
}
if (ioctl(dev, I2C_SMBUS, &ioctldata) == -1) {
return -errno;
}
return 0;
}
static void
_i2c_write_quick(struct sol_i2c *i2c, bool rw)
{
struct i2c_smbus_ioctl_data ioctldata = {
.read_write = rw,
.command = 0,
.size = I2C_SMBUS_QUICK,
.data = NULL
};
if (ioctl(i2c->dev, I2C_SMBUS, &ioctldata) == -1) {
SOL_WRN("Unable to perform I2C-SMBus write quick (bus = %u,"
" device address = %u): %s", i2c->bus, i2c->addr,
sol_util_strerrora(errno));
return;
}
i2c->async.status = 1;
}
static void
_i2c_write_quick_dispatch(struct sol_i2c *i2c)
{
if (!i2c->async.write_quick_cb.cb) return;
i2c->async.write_quick_cb.cb((void *)i2c->async.cb_data, i2c,
i2c->async.status);
}
#ifdef WORKER_THREAD
static bool
i2c_write_quick_worker_thread_iterate(void *data)
{
struct sol_i2c *i2c = data;
_i2c_write_quick(i2c, (bool)(intptr_t)i2c->async.data);
return false;
}
static void
i2c_worker_thread_finished(void *data)
{
struct sol_i2c *i2c = data;
i2c->async.worker = NULL;
i2c->async.dispatch(i2c);
}
#else
static bool
i2c_write_quick_timeout_cb(void *data)
{
struct sol_i2c *i2c = data;
_i2c_write_quick(i2c, (bool)(intptr_t)i2c->async.data);
i2c->async.timeout = NULL;
i2c->async.dispatch(i2c);
return false;
}
#endif
SOL_API struct sol_i2c_pending *
sol_i2c_write_quick(struct sol_i2c *i2c, bool rw, void (*write_quick_cb)(void *cb_data, struct sol_i2c *i2c, ssize_t status), const void *cb_data)
{
#ifdef WORKER_THREAD
struct sol_worker_thread_spec spec = {
.api_version = SOL_WORKER_THREAD_SPEC_API_VERSION,
.setup = NULL,
.cleanup = NULL,
.iterate = i2c_write_quick_worker_thread_iterate,
.finished = i2c_worker_thread_finished,
.feedback = NULL,
.data = i2c
};
#endif
SOL_NULL_CHECK(i2c, NULL);
SOL_INT_CHECK(i2c->dev, == 0, NULL);
BUSY_CHECK(i2c, NULL);
i2c->async.data = (uint8_t *)(long)rw;
i2c->async.status = -1;
i2c->async.write_quick_cb.cb = write_quick_cb;
i2c->async.dispatch = _i2c_write_quick_dispatch;
i2c->async.cb_data = cb_data;
#ifdef WORKER_THREAD
i2c->async.worker = sol_worker_thread_new(&spec);
SOL_NULL_CHECK(i2c->async.worker, NULL);
return (struct sol_i2c_pending *)i2c->async.worker;
#else
i2c->async.timeout = sol_timeout_add(0, i2c_write_quick_timeout_cb, i2c);
SOL_NULL_CHECK(i2c->async.timeout, NULL);
return (struct sol_i2c_pending *)i2c->async.timeout;
#endif
}
static bool
write_byte(const struct sol_i2c *i2c, uint8_t byte)
{
struct i2c_smbus_ioctl_data ioctldata = {
.read_write = I2C_SMBUS_WRITE,
.command = byte,
.size = I2C_SMBUS_BYTE,
.data = NULL
};
if (ioctl(i2c->dev, I2C_SMBUS, &ioctldata) == -1) {
SOL_WRN("Unable to perform I2C-SMBus write byte (bus = %u,"
" device address = %u): %s",
i2c->bus, i2c->addr, sol_util_strerrora(errno));
return false;
}
return true;
}
static bool
read_byte(const struct sol_i2c *i2c, uint8_t *byte)
{
union i2c_smbus_data data;
struct i2c_smbus_ioctl_data ioctldata = {
.read_write = I2C_SMBUS_READ,
.command = 0,
.size = I2C_SMBUS_BYTE,
.data = &data,
};
if (ioctl(i2c->dev, I2C_SMBUS, &ioctldata) == -1) {
SOL_WRN("Unable to perform I2C-SMBus read byte (bus = %u,"
" device address = %u): %s",
i2c->bus, i2c->addr, sol_util_strerrora(errno));
return false;
}
*byte = data.byte;
return true;
}
static void
_i2c_read(struct sol_i2c *i2c, uint8_t *values)
{
size_t i;
for (i = 0; i < i2c->async.count; i++) {
uint8_t byte;
if (!read_byte(i2c, &byte))
return;
*values = byte;
values++;
}
i2c->async.status = i2c->async.count;
}
static void
_i2c_read_write_dispatch(struct sol_i2c *i2c)
{
if (!i2c->async.read_write_cb.cb) return;
i2c->async.read_write_cb.cb((void *)i2c->async.cb_data, i2c,
i2c->async.data, i2c->async.status);
}
#ifdef WORKER_THREAD
static bool
i2c_read_worker_thread_iterate(void *data)
{
struct sol_i2c *i2c = data;
_i2c_read(i2c, i2c->async.data);
return false;
}
#else
static bool
i2c_read_timeout_cb(void *data)
{
struct sol_i2c *i2c = data;
_i2c_read(i2c, i2c->async.data);
i2c->async.timeout = NULL;
i2c->async.dispatch(i2c);
return false;
}
#endif
SOL_API struct sol_i2c_pending *
sol_i2c_read(struct sol_i2c *i2c, uint8_t *values, size_t count, void (*read_cb)(void *cb_data, struct sol_i2c *i2c, uint8_t *data, ssize_t status), const void *cb_data)
{
#ifdef WORKER_THREAD
struct sol_worker_thread_spec spec = {
.api_version = SOL_WORKER_THREAD_SPEC_API_VERSION,
.setup = NULL,
.cleanup = NULL,
.iterate = i2c_read_worker_thread_iterate,
.finished = i2c_worker_thread_finished,
.feedback = NULL,
.data = i2c
};
#endif
SOL_NULL_CHECK(i2c, NULL);
SOL_NULL_CHECK(values, NULL);
SOL_INT_CHECK(count, == 0, NULL);
SOL_INT_CHECK(i2c->dev, == 0, NULL);
BUSY_CHECK(i2c, NULL);
i2c->async.data = values;
i2c->async.count = count;
i2c->async.status = -1;
i2c->async.read_write_cb.cb = read_cb;
i2c->async.dispatch = _i2c_read_write_dispatch;
i2c->async.cb_data = cb_data;
#ifdef WORKER_THREAD
i2c->async.worker = sol_worker_thread_new(&spec);
SOL_NULL_CHECK(i2c->async.worker, NULL);
return (struct sol_i2c_pending *)i2c->async.worker;
#else
i2c->async.timeout = sol_timeout_add(0, i2c_read_timeout_cb, i2c);
SOL_NULL_CHECK(i2c->async.timeout, NULL);
return (struct sol_i2c_pending *)i2c->async.timeout;
#endif
}
static void
_i2c_write(struct sol_i2c *i2c, uint8_t *values)
{
size_t i;
for (i = 0; i < i2c->async.count; i++) {
if (!write_byte(i2c, *values))
return;
values++;
}
i2c->async.status = i2c->async.count;
}
#ifdef WORKER_THREAD
static bool
i2c_write_worker_thread_iterate(void *data)
{
struct sol_i2c *i2c = data;
_i2c_write(i2c, i2c->async.data);
return false;
}
#else
static bool
i2c_write_timeout_cb(void *data)
{
struct sol_i2c *i2c = data;
_i2c_write(i2c, i2c->async.data);
i2c->async.timeout = NULL;
i2c->async.dispatch(i2c);
return false;
}
#endif
SOL_API struct sol_i2c_pending *
sol_i2c_write(struct sol_i2c *i2c, uint8_t *values, size_t count, void (*write_cb)(void *cb_data, struct sol_i2c *i2c, uint8_t *data, ssize_t status), const void *cb_data)
{
#ifdef WORKER_THREAD
struct sol_worker_thread_spec spec = {
.api_version = SOL_WORKER_THREAD_SPEC_API_VERSION,
.setup = NULL,
.cleanup = NULL,
.iterate = i2c_write_worker_thread_iterate,
.finished = i2c_worker_thread_finished,
.feedback = NULL,
.data = i2c
};
#endif
SOL_NULL_CHECK(i2c, NULL);
SOL_NULL_CHECK(values, NULL);
SOL_INT_CHECK(count, == 0, NULL);
SOL_INT_CHECK(i2c->dev, == 0, NULL);
BUSY_CHECK(i2c, NULL);
i2c->async.data = values;
i2c->async.count = count;
i2c->async.status = -1;
i2c->async.read_write_cb.cb = write_cb;
i2c->async.dispatch = _i2c_read_write_dispatch;
i2c->async.cb_data = cb_data;
#ifdef WORKER_THREAD
i2c->async.worker = sol_worker_thread_new(&spec);
SOL_NULL_CHECK(i2c->async.worker, NULL);
return (struct sol_i2c_pending *)i2c->async.worker;
#else
i2c->async.timeout = sol_timeout_add(0, i2c_write_timeout_cb, i2c);
SOL_NULL_CHECK(i2c->async.timeout, NULL);
return (struct sol_i2c_pending *)i2c->async.timeout;
#endif
}
static int
sol_i2c_plain_read_register(const struct sol_i2c *i2c,
uint8_t command,
uint8_t *values,
size_t count)
{
struct i2c_msg msgs[] = {
{
.addr = i2c->addr,
.flags = 0,
.len = 1,
.buf = &command
},
{
.addr = i2c->addr,
.flags = I2C_M_RD,
.len = count,
.buf = values,
}
};
static int
flow_node_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options)
{
struct flow_static_type *type;
struct flow_static_data *fsd;
const struct sol_flow_static_node_spec *spec;
char *node_storage_it;
int r, i;
type = (struct flow_static_type *)node->type;
fsd = data;
fsd->nodes = calloc(type->node_count, sizeof(struct sol_flow_node *));
fsd->node_storage = calloc(1, type->node_storage_size);
if (!fsd->nodes || !fsd->node_storage) {
r = -ENOMEM;
goto error_alloc;
}
/* Assure flow_send_idle()'s timeout is the first registered, so
* that timeouts coming from nodes' init/open functions and that
* may produce packets will always have them delivered */
r = flow_delay_send(node, fsd);
SOL_INT_CHECK(r, < 0, r);
sol_list_init(&fsd->delayed_packets);
/* Set all pointers before calling nodes methods */
node_storage_it = fsd->node_storage;
for (spec = type->node_specs, i = 0; spec->type != NULL; spec++, i++) {
struct sol_flow_node *child_node = (struct sol_flow_node *)node_storage_it;
fsd->nodes[i] = child_node;
child_node->parent_data = INT_TO_PTR(i);
node_storage_it += calc_node_size(spec);
}
for (spec = type->node_specs, i = 0; spec->type != NULL; spec++, i++) {
struct sol_flow_node *child_node = fsd->nodes[i];
struct sol_flow_node_options *child_opts;
child_opts = sol_flow_node_get_options(spec->type, spec->opts);
if (!child_opts) {
SOL_WRN("failed to get options for node #%u, type=%p: %s",
(unsigned)(spec - type->node_specs), spec->type,
sol_util_strerrora(errno));
}
if (type->child_opts_set)
type->child_opts_set(i, options, child_opts);
r = sol_flow_node_init(child_node, node, spec->name, spec->type,
child_opts);
sol_flow_node_free_options(spec->type, child_opts);
if (r < 0) {
SOL_WRN("failed to init node #%u, type=%p, opts=%p: %s",
(unsigned)(spec - type->node_specs), spec->type, spec->opts,
sol_util_strerrora(-r));
goto error_nodes;
}
}
r = connect_nodes(type, fsd);
if (r < 0)
goto error_conns;
return 0;
error_conns:
error_nodes:
/* Skip the failed index, since it doesn't need fini. */
for (i--; i >= 0; i--)
sol_flow_node_fini(fsd->nodes[i]);
error_alloc:
free(fsd->node_storage);
free(fsd->nodes);
return r;
}
