SOL_API int
sol_util_replace_str_from_slice_if_changed(char **str,
const struct sol_str_slice slice)
{
SOL_NULL_CHECK(str, -EINVAL);
if (!slice.len) {
free(*str);
*str = NULL;
return 0;
}
if (*str) {
char *tmp = *str;
size_t str_len = strlen(*str);
if (str_len == slice.len && memcmp(*str, slice.data, str_len) == 0) {
return 0;
} else if (str_len < slice.len) {
tmp = realloc(*str, slice.len + 1);
SOL_NULL_CHECK(tmp, -ENOMEM);
}
memcpy(tmp, slice.data, slice.len);
tmp[slice.len] = '\0';
*str = tmp;
} else {
*str = sol_str_slice_to_string(slice);
SOL_NULL_CHECK(*str, -ENOMEM);
}
return 0;
}
static int
sol_mavlink_init_tcp(struct sol_mavlink *mavlink)
{
struct hostent *server;
struct sockaddr_in serveraddr = { 0 };
char *hostname;
int err, tcp_flag = 1;
mavlink->fd = socket(AF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0);
if (mavlink->fd == -1) {
SOL_ERR("Could not create a socket to specified address - %s",
sol_util_strerrora(errno));
return -errno;
}
hostname = sol_str_slice_to_string(*mavlink->address);
if (!hostname) {
SOL_ERR("Could not format hostname string - %s",
sol_util_strerrora(errno));
goto err;
}
server = gethostbyname(hostname);
if (!server) {
SOL_ERR("No such host: %s - (%s)", hostname,
sol_util_strerrora(h_errno));
errno = h_errno;
goto err;
}
serveraddr.sin_family = AF_INET;
memcpy(server->h_addr, &serveraddr.sin_addr.s_addr, server->h_length);
serveraddr.sin_port = htons(mavlink->port);
err = setsockopt
(mavlink->fd, IPPROTO_TCP, TCP_NODELAY, &tcp_flag, sizeof(int));
if (err < 0) {
SOL_ERR("Could not set NODELAY option to tcp socket - (%s)",
sol_util_strerrora(errno));
goto sock_err;
}
err = connect(mavlink->fd, &serveraddr, sizeof(serveraddr));
if (err < 0) {
SOL_ERR("Could not stablish connection to: %s:%d - (%s)", hostname,
mavlink->port, sol_util_strerrora(errno));
goto sock_err;
}
free(hostname);
return 0;
sock_err:
free(hostname);
err:
close(mavlink->fd);
return -errno;
}
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_string(*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;
}
