SOL_API int
sol_buffer_remove_data(struct sol_buffer *buf, size_t size, size_t offset)
{
int r;
size_t total;
SOL_NULL_CHECK(buf, -EINVAL);
SOL_EXP_CHECK(buf->flags & SOL_BUFFER_FLAGS_MEMORY_NOT_OWNED, -EPERM);
if ((buf->used < offset))
return -EINVAL;
r = sol_util_size_add(size, offset, &total);
SOL_INT_CHECK(r, < 0, r);
size = total <= buf->used ? size : buf->used - offset;
if (buf->used != total) {
memmove((char *)buf->data + offset,
(char *)buf->data + total,
buf->used - size - offset);
}
buf->used -= size;
return 0;
}
SOL_API struct sol_i2c_pending *
sol_i2c_write_register(struct sol_i2c *i2c, uint8_t reg, const uint8_t *data,
size_t count, void (*write_reg_cb)(void *cb_data, struct sol_i2c *i2c,
uint8_t reg, uint8_t *data, ssize_t status), const void *cb_data)
{
qm_rc_t ret;
errno = EINVAL;
SOL_NULL_CHECK(i2c, NULL);
SOL_NULL_CHECK(data, NULL);
SOL_INT_CHECK(count, == 0, NULL);
if (qm_i2c_get_status(i2c->bus) != QM_I2C_IDLE) {
errno = EBUSY;
return NULL;
}
i2c->xfer.type = WRITE_REG;
i2c->xfer.rw_reg = write_reg_cb;
i2c->xfer.user_data = cb_data;
i2c->xfer.data = data;
i2c->xfer.length = count;
i2c->xfer.multiple_count = 1;
i2c->xfer.multiple_done = 0;
i2c->xfer.reg = reg;
i2c->xfer.status = 0;
ret = begin_transfer(i2c->bus, i2c->slave_addr, i2c->bus, &i2c->xfer.reg, 1,
NULL, 0, false);
errno = EINVAL;
SOL_EXP_CHECK(ret != QM_RC_OK, NULL);
errno = 0;
return (struct sol_i2c_pending *)i2c;
}
SOL_API struct sol_i2c_pending *
sol_i2c_read(struct sol_i2c *i2c, uint8_t *data, size_t count,
void (*read_cb)(void *cb_data, struct sol_i2c *i2c, uint8_t *data,
ssize_t status), const void *cb_data)
{
qm_rc_t ret;
errno = EINVAL;
SOL_NULL_CHECK(i2c, NULL);
SOL_NULL_CHECK(data, NULL);
SOL_INT_CHECK(count, == 0, NULL);
if (qm_i2c_get_status(i2c->bus) != QM_I2C_IDLE) {
errno = EBUSY;
return NULL;
}
i2c->xfer.type = READ;
i2c->xfer.rw = read_cb;
i2c->xfer.user_data = cb_data;
i2c->xfer.data = data;
i2c->xfer.length = count;
i2c->xfer.status = 0;
ret = begin_transfer(i2c->bus, i2c->slave_addr, i2c->bus, NULL, 0,
data, i2c->xfer.length, true);
errno = EINVAL;
SOL_EXP_CHECK(ret != QM_RC_OK, NULL);
errno = 0;
return (struct sol_i2c_pending *)i2c;
}
SOL_API int
sol_socket_set_write_monitor(struct sol_socket *s, bool on)
{
SOL_EXP_CHECK(!(s && s->type), -EINVAL);
SOL_SOCKET_TYPE_CHECK_API_VERSION(s->type, -EINVAL);
SOL_NULL_CHECK(s->type->set_write_monitor, -ENOSYS);
return s->type->set_write_monitor(s, on);
}
SOL_API void
sol_socket_del(struct sol_socket *s)
{
SOL_EXP_CHECK(!(s && s->type));
SOL_SOCKET_TYPE_CHECK_API_VERSION(s->type);
SOL_NULL_CHECK(s->type->del);
s->type->del(s);
}
SOL_API int
sol_socket_bind(struct sol_socket *s, const struct sol_network_link_addr *addr)
{
SOL_EXP_CHECK(!(s && s->type), -EINVAL);
SOL_SOCKET_TYPE_CHECK_API_VERSION(s->type, -EINVAL);
SOL_NULL_CHECK(s->type->bind, -ENOSYS);
return s->type->bind(s, addr);
}
SOL_API int
sol_socket_join_group(struct sol_socket *s, int ifindex, const struct sol_network_link_addr *group)
{
SOL_EXP_CHECK(!(s && s->type), -EINVAL);
SOL_SOCKET_TYPE_CHECK_API_VERSION(s->type, -EINVAL);
SOL_NULL_CHECK(s->type->join_group, -ENOSYS);
return s->type->join_group(s, ifindex, group);
}
SOL_API ssize_t
sol_socket_recvmsg(struct sol_socket *s, struct sol_buffer *buffer,
struct sol_network_link_addr *cliaddr)
{
SOL_EXP_CHECK(!(s && s->type), -EINVAL);
SOL_SOCKET_TYPE_CHECK_API_VERSION(s->type, -EINVAL);
SOL_NULL_CHECK(s->type->recvmsg, -ENOSYS);
SOL_NULL_CHECK(buffer, -EINVAL);
return s->type->recvmsg(s, buffer, cliaddr);
}
SOL_API int
sol_buffer_resize(struct sol_buffer *buf, size_t new_size)
{
char *new_data;
SOL_NULL_CHECK(buf, -EINVAL);
SOL_EXP_CHECK(buf->flags & SOL_BUFFER_FLAGS_FIXED_CAPACITY, -EPERM);
SOL_EXP_CHECK(buf->flags & SOL_BUFFER_FLAGS_NO_FREE, -EPERM);
if (buf->capacity == new_size)
return 0;
new_data = realloc(buf->data, new_size);
if (!new_data && new_size)
return -errno;
buf->data = new_data;
buf->capacity = new_size;
return 0;
}
SOL_API int
sol_spi_transfer(struct sol_spi *spi, const uint8_t *tx, uint8_t *rx,
size_t count, void (*transfer_cb)(void *cb_data, struct sol_spi *spi,
const uint8_t *tx, uint8_t *rx, ssize_t status), const void *cb_data)
{
qm_rc_t ret;
SOL_NULL_CHECK(spi, -EINVAL);
SOL_INT_CHECK(count, == 0, -EINVAL);
if (qm_spi_get_status(spi->bus) == QM_SPI_BUSY)
return -EBUSY;
spi->xfer.xfer.tx = (uint8_t *)tx;
spi->xfer.xfer.tx_len = count;
spi->xfer.xfer.rx = (uint8_t *)rx;
spi->xfer.xfer.rx_len = count;
spi->xfer.xfer.tx_callback = tx_callback;
spi->xfer.xfer.rx_callback = rx_callback;
spi->xfer.xfer.err_callback = err_callback;
spi->xfer.xfer.id = spi->bus;
spi->xfer.cb = transfer_cb;
spi->xfer.data = cb_data;
ret = qm_spi_set_config(spi->bus, &spi->config);
SOL_EXP_CHECK(ret != QM_RC_OK, -EINVAL);
ret = qm_spi_slave_select(spi->bus, spi->slave);
SOL_EXP_CHECK(ret != QM_RC_OK, -EINVAL);
qm_gpio_clear_pin(spi->slave_select.port, spi->slave_select.pin);
ret = qm_spi_irq_transfer(spi->bus, &spi->xfer.xfer);
SOL_EXP_CHECK(ret != QM_RC_OK, -EINVAL);
in_transfer[spi->xfer.xfer.id] = spi;
return 0;
}
SOL_API bool
sol_spi_transfer(struct sol_spi *spi, const uint8_t *tx, uint8_t *rx, size_t size, void (*transfer_cb)(void *cb_data, struct sol_spi *spi, const uint8_t *tx, uint8_t *rx, ssize_t status), const void *cb_data)
{
#ifdef PTHREAD
struct sol_worker_thread_spec spec = {
.api_version = SOL_WORKER_THREAD_SPEC_API_VERSION,
.setup = NULL,
.cleanup = NULL,
.iterate = spi_worker_thread_iterate,
.finished = spi_worker_thread_finished,
.feedback = NULL,
.data = spi
};
#endif
SOL_NULL_CHECK(spi, false);
SOL_INT_CHECK(size, == 0, false);
#ifdef PTHREAD
SOL_EXP_CHECK(spi->transfer.worker, false);
#else
SOL_EXP_CHECK(spi->transfer.timeout, false);
#endif
spi->transfer.tx = tx;
spi->transfer.rx = rx;
spi->transfer.count = size;
spi->transfer.cb = transfer_cb;
spi->transfer.cb_data = cb_data;
spi->transfer.status = -1;
#ifdef PTHREAD
spi->transfer.worker = sol_worker_thread_new(&spec);
SOL_NULL_CHECK(spi->transfer.worker, false);
#else
spi->transfer.timeout = sol_timeout_add(0, spi_timeout_cb, spi);
SOL_NULL_CHECK(spi->transfer.timeout, false);
#endif
return true;
}
SOL_API int
sol_buffer_resize(struct sol_buffer *buf, size_t new_size)
{
char *new_data;
SOL_NULL_CHECK(buf, -EINVAL);
SOL_EXP_CHECK(buf->flags & SOL_BUFFER_FLAGS_MEMORY_NOT_OWNED, -EPERM);
if (buf->capacity == new_size)
return 0;
new_data = realloc(buf->data, new_size);
if (!new_data && new_size)
return -errno;
buf->data = new_data;
buf->capacity = new_size;
if (buf->used > new_size)
buf->used = new_size;
return 0;
}
SOL_API bool
sol_spi_transfer(struct sol_spi *spi, const uint8_t *tx_user, uint8_t *rx, size_t count, void (*transfer_cb)(void *cb_data, struct sol_spi *spi, const uint8_t *tx, uint8_t *rx, ssize_t status), const void *cb_data)
{
uint8_t *tx = (uint8_t *)tx_user;
SOL_NULL_CHECK(spi, false);
SOL_EXP_CHECK(spi->transfer.timeout, false);
spi->transfer.intern_allocated_buffer_flags = 0;
if (tx == NULL) {
tx = calloc(count, sizeof(uint8_t));
SOL_NULL_CHECK(tx, false);
spi->transfer.intern_allocated_buffer_flags = INTERN_ALLOCATED_TX_BUFFER;
}
if (rx == NULL) {
rx = calloc(count, sizeof(uint8_t));
SOL_NULL_CHECK_GOTO(rx, rx_alloc_fail);
spi->transfer.intern_allocated_buffer_flags = INTERN_ALLOCATED_RX_BUFFER;
}
spi->transfer.tx = tx;
spi->transfer.rx = rx;
spi->transfer.count = count;
spi->transfer.status = -1;
spi->transfer.cb = transfer_cb;
spi->transfer.cb_data = cb_data;
spi->transfer.timeout = sol_timeout_add(0, spi_timeout_cb, spi);
SOL_NULL_CHECK_GOTO(spi->transfer.timeout, timeout_fail);
return true;
timeout_fail:
if (spi->transfer.intern_allocated_buffer_flags & INTERN_ALLOCATED_RX_BUFFER)
free(rx);
rx_alloc_fail:
if (spi->transfer.intern_allocated_buffer_flags & INTERN_ALLOCATED_TX_BUFFER)
free(tx);
return false;
}
SOL_API struct sol_spi *
sol_spi_open(unsigned int bus, const struct sol_spi_config *config)
{
struct sol_spi *spi;
SOL_LOG_INTERNAL_INIT_ONCE;
if (unlikely(config->api_version != SOL_SPI_CONFIG_API_VERSION)) {
SOL_WRN("Couldn't open SPI that has unsupported version '%u', "
"expected version is '%u'",
config->api_version, SOL_SPI_CONFIG_API_VERSION);
return NULL;
}
SOL_EXP_CHECK(config->bits_per_word != 8, NULL);
spi = malloc(sizeof(struct sol_spi));
SOL_NULL_CHECK(spi, NULL);
spi_poweron(bus);
spi_acquire(bus);
spi_conf_pins(bus);
if (spi_init_master(bus, config->mode, uint32_to_spi_speed_enum(config->frequency)) != 0) {
SOL_WRN("%u,%u: Unable to setup SPI", bus, config->chip_select);
spi_release(bus);
free(spi);
return NULL;
}
spi_release(spi->bus);
spi->bus = bus;
spi->cs_pin = config->chip_select;
spi->transfer.timeout = NULL;
gpio_init(spi->cs_pin, GPIO_DIR_OUT, GPIO_NOPULL);
gpio_set(spi->cs_pin);
return spi;
}
SOL_API int
sol_mavlink_set_armed(struct sol_mavlink *mavlink, bool armed)
{
mavlink_message_t msg = { 0 };
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len, res;
bool curr;
SOL_NULL_CHECK(mavlink, -EINVAL);
curr = sol_mavlink_check_armed(mavlink);
SOL_EXP_CHECK(curr == !!armed, -EINVAL);
mavlink_msg_command_long_pack(mavlink->sysid, mavlink->compid,
&msg, 0, 0, MAV_CMD_COMPONENT_ARM_DISARM, 0,
!!armed, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
res = write(mavlink->fd, buf, len);
SOL_INT_CHECK(res, < len, -errno);
return 0;
}
SOL_API struct sol_spi *
sol_spi_open(unsigned int bus, const struct sol_spi_config *config)
{
struct sol_spi *spi;
qm_spi_t max_bus_available;
int ret;
SOL_LOG_INTERNAL_INIT_ONCE;
/* QM_SPI_NUM is always considering that both master and the slave
* exist, so we can't use it to check the valid buses to use */
#if QUARK_SE
max_bus_available = QM_SPI_MST_1;
#else
max_bus_available = QM_SPI_MST_0;
#endif
SOL_EXP_CHECK(bus >= max_bus_available, NULL);
SOL_NULL_CHECK(config, NULL);
#ifndef SOL_NO_API_VERSION
if (unlikely(config->api_version != SOL_SPI_CONFIG_API_VERSION)) {
SOL_WRN("Couldn't open SPI that has unsupported version '%u', "
"expected version is '%u'",
config->api_version, SOL_SPI_CONFIG_API_VERSION);
return NULL;
}
#endif
if (config->chip_select > 3) {
SOL_WRN("Invalid chip_select value '%u'. Value must be between 0 and 3.",
config->chip_select);
return NULL;
}
if ((config->bits_per_word < 4) || (config->bits_per_word > 32)) {
SOL_WRN("Invalid bits_per_word value '%" PRIu8 "'. Value must be "
"between 4 and 32.", config->bits_per_word);
return NULL;
}
spi = calloc(1, sizeof(*spi));
SOL_NULL_CHECK(spi, NULL);
if (!spi_irq_event) {
bool r;
spi_irq_event = process_alloc_event();
r = sol_mainloop_contiki_event_handler_add(&spi_irq_event, NULL,
spi_cb_dispatch, NULL);
SOL_EXP_CHECK_GOTO(!r, error);
}
spi->bus = bus;
spi->slave = BIT(config->chip_select);
spi->config.frame_size = config->bits_per_word - 1;
spi->config.transfer_mode = QM_SPI_TMOD_TX_RX;
spi->config.bus_mode = config->mode;
spi->config.clk_divider = 32000000 / config->frequency;
switch (spi->bus) {
case QM_SPI_MST_0:
clk_periph_enable(CLK_PERIPH_CLK | CLK_PERIPH_SPI_M0_REGISTER);
qm_irq_request(QM_IRQ_SPI_MASTER_0, qm_spi_master_0_isr);
break;
#if QUARK_SE
case QM_SPI_MST_1:
qm_irq_request(QM_IRQ_SPI_MASTER_1, qm_spi_master_1_isr);
break;
#endif
case QM_SPI_SLV_0:
case QM_SPI_NUM:
/* We checked if we were passed the limit before, so we should never
* hit this point. Using all the enum values and no default, however,
* allows us to rely on the compiler to know if there are values
* we are not considering (depending on warning levels) */
break;
}
ret = spi_set_gpio_ss(spi);
SOL_INT_CHECK_GOTO(ret, < 0, error);
return spi;
error:
free(spi);
return NULL;
}
static int
spi_set_gpio_ss(struct sol_spi *spi)
{
qm_gpio_port_config_t cfg;
uint32_t mask;
qm_rc_t ret;
#if QUARK_SE
spi->slave_select.port = QM_GPIO_0;
switch (spi->bus) {
case QM_SPI_MST_0:
switch (spi->slave) {
case QM_SPI_SS_0:
spi->slave_select.pin = 24;
break;
case QM_SPI_SS_1:
spi->slave_select.pin = 25;
break;
case QM_SPI_SS_2:
spi->slave_select.pin = 26;
break;
case QM_SPI_SS_3:
spi->slave_select.pin = 27;
break;
default:
return -EINVAL;
}
break;
case QM_SPI_MST_1:
switch (spi->slave) {
case QM_SPI_SS_0:
spi->slave_select.pin = 11;
break;
case QM_SPI_SS_1:
spi->slave_select.pin = 12;
break;
case QM_SPI_SS_2:
spi->slave_select.pin = 13;
break;
case QM_SPI_SS_3:
spi->slave_select.pin = 14;
break;
default:
return -EINVAL;
}
break;
default:
return -EINVAL;
}
#elif QUARK_D2000
spi->slave_select.port = QM_GPIO_0;
switch (spi->slave) {
case QM_SPI_SS_0:
spi->slave_select.pin = 0;
break;
case QM_SPI_SS_1:
spi->slave_select.pin = 1;
break;
case QM_SPI_SS_2:
spi->slave_select.pin = 2;
break;
case QM_SPI_SS_3:
spi->slave_select.pin = 3;
break;
default:
return -EINVAL;
}
#endif
mask = BIT(spi->slave_select.pin);
ret = qm_gpio_get_config(spi->slave_select.port, &cfg);
SOL_EXP_CHECK(ret != QM_RC_OK, -EIO);
cfg.direction |= mask;
cfg.int_en &= mask;
ret = qm_gpio_set_config(spi->slave_select.port, &cfg);
SOL_EXP_CHECK(ret != QM_RC_OK, -EIO);
qm_gpio_set_pin(spi->slave_select.port, spi->slave_select.pin);
return 0;
}
