Commit d29b2b1b authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Added batch polling for serial_2002

parent 88f97efd
......@@ -29,6 +29,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <errno.h>
#include <moberg.h>
#include <moberg_config.h>
......@@ -48,7 +49,7 @@ struct moberg_device_context {
char *name;
int baud;
long timeout;
int fd;
struct serial2002_io io;
} port;
struct remap_analog {
int count;
......@@ -65,8 +66,13 @@ struct moberg_device_context {
struct digital_map {
unsigned char index;
} map[32];
} digital_in, digital_out,
encoder_in;
} digital_in, digital_out, encoder_in;
struct batch {
int active;
struct timespec expires;
struct serial2002_data digital[32];
struct serial2002_data channel[32];
} batch;
};
struct moberg_channel_context {
......@@ -101,6 +107,125 @@ struct moberg_channel_encoder_in {
struct moberg_channel_context channel_context;
};
static struct timespec timespec_sub(struct timespec t1, struct timespec t2)
{
struct timespec result;
result.tv_sec = t1.tv_sec - t2.tv_sec;
result.tv_nsec = t1.tv_nsec - t2.tv_nsec;
if (result.tv_nsec < 0) {
result.tv_sec--;
result.tv_nsec += 1000000000L;
}
return result;
}
static struct timespec timespec_add(struct timespec t1, struct timespec t2)
{
struct timespec result;
result.tv_sec = t1.tv_sec + t2.tv_sec;
result.tv_nsec = t1.tv_nsec + t2.tv_nsec;
if (result.tv_nsec > 1000000000L) {
result.tv_sec++;
result.tv_nsec -= 1000000000L;
}
return result;
}
static struct moberg_status batch_sampling(
struct moberg_device_context *device,
struct analog_map *analog,
struct digital_map *digital,
struct digital_map *encoder,
struct serial2002_data *data)
{
struct moberg_status result = MOBERG_OK;
if (! data) {
result = MOBERG_ERRNO(EINVAL);
goto return_result;
}
struct timespec start, diff;
if (clock_gettime(CLOCK_REALTIME, &start) < 0) {
result = MOBERG_ERRNO(errno);
goto return_result;
}
diff = timespec_sub(device->batch.expires, start);
if (diff.tv_sec < 0 ||
(digital && device->batch.digital[digital->index].kind != is_digital) ||
(analog && device->batch.channel[analog->index].kind != is_channel) ||
(encoder && device->batch.channel[encoder->index].kind != is_channel)) {
/* Batch has expired */
int expected = 0;
for (int i = 0 ; i < device->digital_in.count ; i++) {
struct digital_map *map = &device->digital_in.map[i];
device->batch.digital[map->index].kind = is_invalid;
expected++;
result = serial2002_poll_digital(&device->port.io, map->index, 0);
if (! OK(result)) {
goto return_result;
}
}
for (int i = 0 ; i < device->analog_in.count ; i++) {
struct analog_map *map = &device->analog_in.map[i];
device->batch.channel[map->index].kind = is_invalid;
expected++;
result = serial2002_poll_channel(&device->port.io, map->index, 0);
if (! OK(result)) {
goto return_result;
}
}
for (int i = 0 ; i < device->encoder_in.count ; i++) {
struct digital_map *map = &device->encoder_in.map[i];
device->batch.channel[map->index].kind = is_invalid;
expected++;
result = serial2002_poll_channel(&device->port.io, map->index, 0);
if (! OK(result)) {
goto return_result;
}
}
serial2002_flush(&device->port.io);
while (expected > 0) {
struct serial2002_data data;
result = serial2002_read(&device->port.io, device->port.timeout, &data);
if (! OK(result)) {
goto return_result;
}
if (data.kind == is_digital) {
device->batch.digital[data.index] = data;
expected--;
} else if (data.kind == is_channel) {
device->batch.channel[data.index] = data;
expected--;
} else {
result = MOBERG_ERRNO(EINVAL);
goto return_result;
}
}
struct timespec now;
if (clock_gettime(CLOCK_REALTIME, &now) < 0) {
result = MOBERG_ERRNO(errno);
goto return_result;
}
diff = timespec_sub(now, start);
device->batch.expires = timespec_add(now, diff);
}
if (digital &&
device->batch.digital[digital->index].kind == is_digital) {
*data = device->batch.digital[digital->index];
device->batch.digital[digital->index].kind = is_invalid;
} else if (analog &&
device->batch.channel[analog->index].kind == is_channel) {
*data = device->batch.channel[analog->index];
device->batch.channel[analog->index].kind = is_invalid;
} else if (encoder &&
device->batch.channel[encoder->index].kind == is_channel) {
*data = device->batch.channel[encoder->index];
device->batch.channel[encoder->index].kind = is_invalid;
}
return_result:
return result;
}
static struct moberg_status analog_in_read(
struct moberg_channel_analog_in *analog_in,
double *value)
......@@ -111,19 +236,22 @@ static struct moberg_status analog_in_read(
struct moberg_device_context *device = channel->device;
struct serial2002_data data;
struct analog_map map = device->analog_in.map[channel->index];
struct moberg_status result = serial2002_poll_channel(
device->port.fd, map.index);
if (! OK(result)) {
goto return_result;
}
result = serial2002_read(device->port.fd, device->port.timeout, &data);
if (OK(result)) {
struct moberg_status result;
if (device->batch.active) {
result = batch_sampling(device, &map, NULL, NULL, &data);
if (! OK(result)) { goto return_result; }
} else {
result = serial2002_poll_channel(&device->port.io, map.index, 0);
if (! OK(result)) { goto return_result; }
result = serial2002_read(&device->port.io, device->port.timeout, &data);
if (! OK(result)) { goto return_result; }
if ((data.kind != is_channel) || (data.index != map.index)) {
result = MOBERG_ERRNO(ECHRNG);
goto return_result;
}
*value = (data.value * map.delta + map.min);
}
*value = (data.value * map.delta + map.min);
return_result:
return result;
err_einval:
......@@ -153,7 +281,7 @@ static struct moberg_status analog_out_write(
}
struct serial2002_data data = { is_channel, map.index, as_long };
struct moberg_status result = serial2002_write(device->port.fd, data);
struct moberg_status result = serial2002_write(&device->port.io, data, 1);
if (OK(result) && actual_value) {
*actual_value = data.value * map.delta + map.min;
}
......@@ -170,25 +298,26 @@ static struct moberg_status digital_in_read(
struct moberg_device_context *device = channel->device;
struct serial2002_data data = { 0, 0 };
struct digital_map map = device->digital_in.map[channel->index];
struct moberg_status result = serial2002_poll_digital(
device->port.fd, map.index);
if (! OK(result)) {
goto return_result;
}
result = serial2002_read(device->port.fd, device->port.timeout, &data);
if (OK(result)) {
struct moberg_status result;
if (device->batch.active) {
result = batch_sampling(device, NULL, &map, NULL, &data);
if (! OK(result)) { goto return_result; }
} else {
result = serial2002_poll_digital(&device->port.io, map.index, 1);
if (! OK(result)) { goto return_result; }
result = serial2002_read(&device->port.io, device->port.timeout, &data);
if (! OK(result)) { goto return_result; }
if ((data.kind != is_digital) || (data.index != map.index)) {
result = MOBERG_ERRNO(ECHRNG);
goto return_result;
}
*value = data.value != 0;
}
*value = data.value != 0;
return_result:
return result;
err_einval:
return MOBERG_ERRNO(EINVAL);
*value = 1;
return MOBERG_OK;
}
static struct moberg_status digital_out_write(
......@@ -200,7 +329,7 @@ static struct moberg_status digital_out_write(
struct moberg_device_context *device = channel->device;
struct digital_map map = device->digital_out.map[channel->index];
struct serial2002_data data = { is_digital, map.index, desired_value != 0 };
struct moberg_status result = serial2002_write(device->port.fd, data);
struct moberg_status result = serial2002_write(&device->port.io, data, 0);
if (OK(result) && actual_value) {
*actual_value = data.value;
}
......@@ -217,19 +346,21 @@ static struct moberg_status encoder_in_read(
struct moberg_device_context *device = channel->device;
struct serial2002_data data;
struct digital_map map = device->encoder_in.map[channel->index];
struct moberg_status result = serial2002_poll_channel(
device->port.fd, map.index);
if (! OK(result)) {
goto return_result;
}
result = serial2002_read(device->port.fd, device->port.timeout, &data);
if (OK(result)) {
struct moberg_status result;
if (device->batch.active) {
result = batch_sampling(device, NULL, NULL, &map, &data);
if (! OK(result)) { goto return_result; }
} else {
result = serial2002_poll_channel(&device->port.io, map.index, 0);
if (! OK(result)) { goto return_result; }
result = serial2002_read(&device->port.io, device->port.timeout, &data);
if (! OK(result)) { goto return_result; }
if ((data.kind != is_channel) || (data.index != map.index)) {
result = MOBERG_ERRNO(ECHRNG);
goto return_result;
}
*value = (data.value);
}
*value = (data.value);
return_result:
return result;
err_einval:
......@@ -337,8 +468,12 @@ static struct moberg_status device_open(struct moberg_device_context *device)
/* It's expected for this to fail for at least some USB serial adapters */
ioctl(fd, TIOCSSERIAL, &settings);
}
device->port.io.fd = fd;
device->port.io.read.pos = 0;
device->port.io.write.pos = 0;
struct serial2002_config config;
result = serial2002_read_config(fd, device->port.timeout, &config);
result = serial2002_read_config(&device->port.io,
device->port.timeout, &config);
if (! OK(result)) { goto err_result; }
remap_analog(&device->analog_in, SERIAL2002_ANALOG_IN,
config.channel_in, 31);
......@@ -350,7 +485,6 @@ static struct moberg_status device_open(struct moberg_device_context *device)
config.digital_out, 32);
remap_digital(&device->encoder_in, SERIAL2002_COUNTER_IN,
config.channel_in, 31);
device->port.fd = fd;
}
device->port.count++;
return MOBERG_OK;
......@@ -368,7 +502,7 @@ static struct moberg_status device_close(struct moberg_device_context *device)
if (device->port.count < 0) { errno = ENODEV; goto err_errno; }
device->port.count--;
if (device->port.count == 0) {
if (close(device->port.fd) < 0) { goto err_errno; }
if (close(device->port.io.fd) < 0) { goto err_errno; }
}
return MOBERG_OK;
err_errno:
......@@ -486,6 +620,9 @@ static struct moberg_status parse_config(
}
if (! acceptsym(c, tok_SEMICOLON, NULL)) { goto syntax_err; }
device->port.timeout = timeout.u.integer.value * multiplier;
} else if (acceptkeyword(c, "batch_sampling")) {
device->batch.active = 1;
if (! acceptsym(c, tok_SEMICOLON, NULL)) { goto syntax_err; }
} else {
goto syntax_err;
}
......
......@@ -24,14 +24,15 @@
#include <poll.h>
#include <errno.h>
#include <string.h>
#include <sys/ioctl.h>
#include <moberg_inline.h>
#include <serial2002_lib.h>
static struct moberg_status tty_write(int fd, unsigned char *buf, int count)
struct moberg_status serial2002_flush(struct serial2002_io *io)
{
int n = 0;
while (n < count) {
int written = write(fd, &buf[n], count - n);
while (n < io->write.pos) {
int written = write(io->fd, &io->write.data[n], io->write.pos - n);
if (written == 0) {
return MOBERG_ERRNO(ENODATA);
} else if (written < 0) {
......@@ -39,50 +40,105 @@ static struct moberg_status tty_write(int fd, unsigned char *buf, int count)
}
n += written;
}
io->write.pos = 0;
return MOBERG_OK;
}
static struct moberg_status tty_read(int fd, long timeout, unsigned char *value)
static struct moberg_status tty_write(struct serial2002_io *io,
unsigned char *buf,
int count,
int flush)
{
struct pollfd pollfd;
struct moberg_status result = MOBERG_OK;
while (1) {
pollfd.fd = fd;
for (int i = 0 ; i < count ; i++) {
io->write.data[io->write.pos] = buf[i];
io->write.pos++;
if (io->write.pos >= sizeof(io->write.data)) {
result = serial2002_flush(io);
if (! OK(result)) {
goto return_result;
}
}
}
if (flush) {
result = serial2002_flush(io);
if (! OK(result)) {
goto return_result;
}
}
return_result:
return result;
}
static struct moberg_status tty_read(struct serial2002_io *io,
long timeout,
unsigned char *value)
{
struct moberg_status result = MOBERG_OK;
if (io->read.pos >= io->read.count) {
struct pollfd pollfd;
pollfd.fd = io->fd;
pollfd.events = POLLRDNORM | POLLRDBAND | POLLIN | POLLHUP | POLLERR;
int err = poll(&pollfd, 1, timeout / 1000);
if (err >= 1) {
break;
if (err == 0) {
result = MOBERG_ERRNO(ETIMEDOUT);
goto return_result;
} else if (err < 0) {
result = MOBERG_ERRNO(errno);
goto return_result;
}
int available;
err = ioctl(io->fd, FIONREAD, &available);
if (err < 0) {
result = MOBERG_ERRNO(errno);
goto return_result;
}
if (available > sizeof(io->read.data)) {
available = sizeof(io->read.data);
}
err = read(io->fd, &io->read.data[0], available);
if (err > 0) {
io->read.pos = 0;
io->read.count = err;
} else if (err == 0) {
return MOBERG_ERRNO(ETIMEDOUT);
result = MOBERG_ERRNO(ENODATA);
goto return_result;
} else if (err < 0) {
return MOBERG_ERRNO(errno);
result = MOBERG_ERRNO(errno);
goto return_result;
}
}
int err = read(fd, value, 1);
if (err == 1) {
return MOBERG_OK;
} else {
return MOBERG_ERRNO(errno);
}
return_result:
*value = io->read.data[io->read.pos];
io->read.pos++;
return result;
}
struct moberg_status serial2002_poll_digital(int fd, int channel)
struct moberg_status serial2002_poll_digital(struct serial2002_io *io,
int channel,
int flush)
{
unsigned char cmd;
cmd = 0x40 | (channel & 0x1f);
return tty_write(fd, &cmd, 1);
return tty_write(io, &cmd, 1, flush);
}
struct moberg_status serial2002_poll_channel(int fd, int channel)
struct moberg_status serial2002_poll_channel(struct serial2002_io *io,
int channel,
int flush)
{
unsigned char cmd;
cmd = 0x60 | (channel & 0x1f);
return tty_write(fd, &cmd, 1);
return tty_write(io, &cmd, 1, flush);
}
struct moberg_status serial2002_read(int f, long timeout,
struct moberg_status serial2002_read(struct serial2002_io *io,
long timeout,
struct serial2002_data *value)
{
int length;
......@@ -93,7 +149,7 @@ struct moberg_status serial2002_read(int f, long timeout,
length = 0;
while (value->kind == is_invalid) {
unsigned char data;
struct moberg_status result = tty_read(f, timeout, &data);
struct moberg_status result = tty_read(io, timeout, &data);
if (! OK(result)) {
return result;
}
......@@ -141,11 +197,13 @@ struct moberg_status serial2002_read(int f, long timeout,
return MOBERG_OK;
}
struct moberg_status serial2002_write(int f, struct serial2002_data data)
struct moberg_status serial2002_write(struct serial2002_io *io,
struct serial2002_data data,
int flush)
{
if (data.kind == is_digital) {
unsigned char ch = ((data.value << 5) & 0x20) | (data.index & 0x1f);
return tty_write(f, &ch, 1);
return tty_write(io, &ch, 1, 1);
} else {
unsigned char ch[6];
int i = 0;
......@@ -169,7 +227,7 @@ struct moberg_status serial2002_write(int f, struct serial2002_data data)
i++;
ch[i] = ((data.value << 5) & 0x60) | (data.index & 0x1f);
i++;
return tty_write(f, ch, i);
return tty_write(io, ch, i, 1);
}
}
......@@ -217,35 +275,35 @@ static struct moberg_status update_channel(struct serial2002_channel *channel,
return MOBERG_OK;
}
static void discard_pending(int fd)
static void discard_pending(struct serial2002_io *io)
{
struct pollfd pollfd;
while (1) {
pollfd.fd = fd;
pollfd.fd = io->fd;
pollfd.events = POLLRDNORM | POLLRDBAND | POLLIN | POLLHUP | POLLERR;
int err = poll(&pollfd, 1, 0);
if (err <= 0) {
break;
} else {
char discard;
read(fd, &discard, 1);
read(io->fd, &discard, 1);
}
}
}
static struct moberg_status do_read_config(
int fd,
struct serial2002_io *io,
long timeout,
struct serial2002_config *config)
{
struct serial2002_data data = { 0, 0 };
discard_pending(fd);
discard_pending(io);
memset(config, 0, sizeof(*config));
serial2002_poll_channel(fd, 31);
serial2002_poll_channel(io, 31, 1);
while (1) {
struct moberg_status result = serial2002_read(fd, timeout, &data);
struct moberg_status result = serial2002_read(io, timeout, &data);
if (! OK(result)) { return result; }
unsigned int channel = (data.value & 0x0001f);
unsigned int kind = (data.value & 0x00e0) >> 5;
......@@ -311,11 +369,11 @@ static struct moberg_status check_config(struct serial2002_config *c)
}
struct moberg_status serial2002_read_config(
int fd,
struct serial2002_io *io,
long timeout,
struct serial2002_config *config)
{
struct moberg_status result = do_read_config(fd, timeout, config);
struct moberg_status result = do_read_config(io, timeout, config);
if (OK(result)) {
result = check_config(config);
}
......
......@@ -49,17 +49,35 @@ struct serial2002_config {
digital_in[32], digital_out[32];
};
struct moberg_status serial2002_poll_digital(int fd, int channel);
struct serial2002_io {
int fd;
struct buffer {
unsigned char data[128];
int pos;
int count;
} read, write;
};
struct moberg_status serial2002_poll_digital(struct serial2002_io *io,
int channel,
int flush);
struct moberg_status serial2002_poll_channel(int fd, int channel);
struct moberg_status serial2002_poll_channel(struct serial2002_io *io,
int channel,
int flush);
struct moberg_status serial2002_read(int fd, long timeout,
struct moberg_status serial2002_read(struct serial2002_io *io,
long timeout,
struct serial2002_data *data);
struct moberg_status serial2002_write(int fd, struct serial2002_data data);
struct moberg_status serial2002_write(struct serial2002_io *io,
struct serial2002_data data,
int flush);
struct moberg_status serial2002_read_config(int fd,
struct moberg_status serial2002_read_config(struct serial2002_io *io,
long timeout,
struct serial2002_config *config);
struct moberg_status serial2002_flush(struct serial2002_io *io);
#endif
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment