int navilink_read_device(NavilinkDevice* device, NavilinkPacket* packet)
{
assert(device != NULL);
uint8_t* buff = (void*)(&device->buffer[0]);
memset(buff, 0, NAVILINK_MAX_PACKET_SIZE);
enum sp_return result = sp_blocking_read(device->serial_port, buff, 4, 1000);
if (result < 0) {
goto manage_serial_error;
}
if (buff[0] != NAVILINK_PACK_START1 && buff[1] != NAVILINK_PACK_START2) {
navilink_set_current_error(device, NAVILINK_ERROR_INVALID_START_BYTE, NULL);
return -1;
}
uint16_t packet_length = 0;
memcpy(&packet_length, &buff[2], 2);
packet_length = _le16_to_host(packet_length);
result = sp_blocking_read(device->serial_port, &buff[4], packet_length + 4, 1000);
if (result < 0) {
goto manage_serial_error;
}
#if (__DEBUG_CMD)
printf("READ:");
DumpHex(buff, packet_length + 8);
#endif
return navilink_read_packet(packet, device->buffer);
manage_serial_error:
CATCH_LIBSERIAL_ERROR(device);
return -1;
}
u32 piksi_port_read(u8 *buff, u32 n, void *context)
{
(void)context;
u32 result;
result = sp_blocking_read(piksi_port, buff, n, 0);
return result;
}
/**
* Read a line from the specified serial port.
*
* @param serial Previously initialized serial port structure.
* @param buf Buffer where to store the bytes that are read.
* @param buflen Size of the buffer.
* @param[in] timeout_ms How long to wait for a line to come in.
*
* Reading stops when CR of LR is found, which is stripped from the buffer.
*
* @retval SR_OK Success.
* @retval SR_ERR Failure.
*
* @private
*/
SR_PRIV int serial_readline(struct sr_serial_dev_inst *serial, char **buf,
int *buflen, gint64 timeout_ms)
{
gint64 start, remaining;
int maxlen, len;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
if (!serial->data) {
sr_dbg("Cannot use unopened serial port %s.", serial->port);
return -1;
}
start = g_get_monotonic_time();
remaining = timeout_ms;
maxlen = *buflen;
*buflen = len = 0;
while (1) {
len = maxlen - *buflen - 1;
if (len < 1)
break;
len = sp_blocking_read(serial->data, *buf + *buflen, 1, remaining);
if (len > 0) {
*buflen += len;
*(*buf + *buflen) = '\0';
if (*buflen > 0 && (*(*buf + *buflen - 1) == '\r'
|| *(*buf + *buflen - 1) == '\n')) {
/* Strip CR/LF and terminate. */
*(*buf + --*buflen) = '\0';
break;
}
}
/* Reduce timeout by time elapsed. */
remaining = timeout_ms - ((g_get_monotonic_time() - start) / 1000);
if (remaining <= 0)
/* Timeout */
break;
if (len < 1)
g_usleep(2000);
}
if (*buflen)
sr_dbg("Received %d: '%s'.", *buflen, *buf);
return SR_OK;
}
std::string sp_readline(struct sp_port* port) {
std::string buffer = "";
char c;
/*
Commands are sent from the host in hoofing great chunks one line
at a time, so the chances of us hitting a timeout before getting our '\n'
are slim.
*/
while (sp_blocking_read(port, &c, 1, 1000) > 0) {
if (c == '\n') break;
if (c != '\r') buffer += c;
}
return buffer;
}
/**
* Read a number of bytes from the specified serial port.
*
* @param serial Previously initialized serial port structure.
* @param buf Buffer where to store the bytes that are read.
* @param count The number of bytes to read.
*
* @return The number of bytes read, or a negative error code upon failure.
*/
SR_PRIV int serial_read(struct sr_serial_dev_inst *serial, void *buf,
size_t count)
{
ssize_t ret;
char *error;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
if (serial->fd == -1) {
sr_dbg("Cannot use unopened serial port %s (fd %d).",
serial->port, serial->fd);
return SR_ERR;
}
if (serial->nonblocking)
ret = sp_nonblocking_read(serial->data, buf, count);
else
ret = sp_blocking_read(serial->data, buf, count, 0);
switch (ret) {
case SP_ERR_ARG:
sr_err("Attempted serial port read with invalid arguments.");
return SR_ERR_ARG;
case SP_ERR_FAIL:
error = sp_last_error_message();
sr_err("Read error: %s.", error);
sp_free_error_message(error);
return SR_ERR;
}
if (ret > 0)
sr_spew("Read %d/%d bytes (fd %d).", ret, count, serial->fd);
return ret;
}
static int _serial_read(struct sr_serial_dev_inst *serial, void *buf,
size_t count, int nonblocking, unsigned int timeout_ms)
{
ssize_t ret;
char *error;
if (!serial) {
sr_dbg("Invalid serial port.");
return SR_ERR;
}
if (!serial->data) {
sr_dbg("Cannot use unopened serial port %s.", serial->port);
return SR_ERR;
}
if (nonblocking)
ret = sp_nonblocking_read(serial->data, buf, count);
else
ret = sp_blocking_read(serial->data, buf, count, timeout_ms);
switch (ret) {
case SP_ERR_ARG:
sr_err("Attempted serial port read with invalid arguments.");
return SR_ERR_ARG;
case SP_ERR_FAIL:
error = sp_last_error_message();
sr_err("Read error (%d): %s.", sp_last_error_code(), error);
sp_free_error_message(error);
return SR_ERR;
}
if (ret > 0)
sr_spew("Read %zd/%zu bytes.", ret, count);
return ret;
}
int serial_send(struct sp_port *my_port, const void *command, size_t write_bytes_count, void *response) {
int ret, read_count=6;
if (DO_BLOCKING_READ_WRITE) {
unsigned int timeout = READ_WRITE_TIMEOUT_SEC*1000;
ret = sp_blocking_write(my_port, command, write_bytes_count, timeout);
if (ret <= 0) {
printf("Failed to write serial bytes.\n");
return 0;
}
ret = sp_blocking_read(my_port, response, (size_t)read_count, timeout);
} else {
int fd;
ret = sp_get_port_handle(my_port,&fd);
if (ret != SP_OK) {
printf("Failed to get port handle for nonblocking read/write.\n");
}
fd_set fds;
FD_ZERO(&fds);
FD_SET(fd,&fds);
struct timespec ts;
ts.tv_sec = READ_WRITE_TIMEOUT_SEC;
ret = pselect(fd+1, NULL, &fds, NULL, &ts, NULL);
if (ret < 0 && errno != EINTR) {
printf("Error in pselect for write.\n");
return 0;
} else if (ret == 0) {
printf("Timeout while waiting to write.\n");
return 0;
}
ret = sp_nonblocking_write(my_port, command, write_bytes_count);
FD_ZERO(&fds);
FD_SET(fd,&fds);
ts.tv_sec = READ_WRITE_TIMEOUT_SEC;
ret = pselect(fd+1, &fds, NULL, NULL, &ts, NULL);
if (ret < 0 && errno != EINTR) {
printf("Error in pselect for write.\n");
return 0;
} else if (ret == 0) {
printf("Timeout while waiting to write.\n");
return 0;
}
ret = sp_nonblocking_read(my_port, response, (size_t)read_count);
}
if (ret < read_count) {
printf("Successful write to serial, but failed on serial read.\n");
return 0;
} else {
return ret;
}
}
