Code:
#include <stdlib.h>
#include <string.h>
#include <limits.h>
#include <stdio.h>
#include <errno.h>
/* Since the device ID is one char long, we can have up to 2**CHAR_BIT devices. */
#define MAX_DEVICES (UCHAR_MAX + 1U)
/* Device state map */
typedef struct {
unsigned char state[MAX_DEVICES];
unsigned short var1[MAX_DEVICES];
unsigned short var2[MAX_DEVICES];
} device_map_t;
#define STATE_UNUSED 0U
#define STATE_DEVICE 1U /* Only used by example_add()! */
void map_init(device_map_t *const map)
{
if (map) {
size_t i = MAX_DEVICES;
while (i-->0) {
map->state[i] = STATE_UNUSED;
map->var1[i] = 0U;
map->var2[i] = 0U;
}
}
}
/* Parse (limit - *dataptr) bytes at (*dataptr), i.e. from (*dataptr) up to but not including (limit).
* Record structure is
* 1 byte Device ID
* 2 bytes Network endian var1
* 2 bytes Network endian var2
* dataptr will be advanced to first un-decoded byte.
* If the function returns zero, check errno for errors:
* 0 No errors, at least one packet was decoded
* EAGAIN Incomplete data/no data, need more
* EDOM Invalid device ID
* ENOSPC set() callback failed
* ENOENT add() callback failed
*
* For each device in the data:
* - If the map state for that device is STATE_UNUSED, the
* add(map, id, var1, var2)
* callback will be called.
* If the callback returns nonzero, the function will abort.
* - If the map state for that device is not STATE_UNUSED,
* and either var1 or var2 or both differ from the mapped state, the
* set(map, id, var1, var2)
* callback will be called.
* If the callback returns nonzero, the function will abort.
*/
int decode(const unsigned char **const dataptr, const unsigned char *const limit, device_map_t *const map,
int (*add)(device_map_t *const, const unsigned char, const unsigned short, const unsigned short),
int (*set)(device_map_t *const, const unsigned char, const unsigned short, const unsigned short))
{
if (!dataptr || !limit || !map) {
errno = EINVAL; /* EINVAL: Invalid parameters. */
return 0;
} else
if (*dataptr + 5 < limit) {
errno = EAGAIN; /* EAGAIN: Not enough data yet. */
return 0;
} else {
const unsigned char *data = *dataptr;
int retval;
while (data + 5 <= limit) {
const unsigned char id = data[0];
const unsigned int var1 = data[1] * 256U + data[2];
const unsigned int var2 = data[3] * 256U + data[4];
#if MAX_DEVICES <= UCHAR_MAX
/* Make sure device ID is not too large. */
if (id >= MAX_DEVICES) {
errno = EDOM; /* EDOM: Invalid device ID. */
return 0;
}
#endif
if (map->state[id] == STATE_UNUSED) {
if (add != NULL) {
retval = add(map, id, var1, var2);
if (retval) {
errno = ENOSPC; /* ENOSPC: add() failed */
return retval;
}
}
} else
if (map->var1[id] != var1 || map->var2[id] != var2) {
if (set != NULL) {
retval = set(map, id, var1, var2);
if (retval) {
errno = ENOENT; /* ENOENT: set() failed */
return retval;
}
}
}
/* Advance pointer to next possible data structure */
data += 5;
*dataptr = data;
}
errno = 0; /* OK */
return 0;
}
}
/* Example add callback. */
int example_add(device_map_t *const map,
const unsigned char id,
const unsigned short var1,
const unsigned short var2)
{
map->state[id] = STATE_DEVICE;
map->var1[id] = var1;
map->var2[id] = var2;
fprintf(stdout, "Added device %d: var1 = %d, var2 = %d\n", id, var1, var2);
fflush(stdout);
return 0;
}
/* Example set callback. */
int example_set(device_map_t *const map,
const unsigned char id,
const unsigned short var1,
const unsigned short var2)
{
fprintf(stdout, "Changed device %d: var1 = %d (was %d), var2 = %d (was %d)\n",
id, var1, map->var1[id], var2, map->var2[id]);
fflush(stdout);
map->var1[id] = var1;
map->var2[id] = var2;
return 0;
}
int main(void)
{
device_map_t map;
unsigned char data_buffer[16]; /* Anything larger than 2 packets is ok. */
unsigned char *data_next = data_buffer;
unsigned char *data_ends = data_buffer;
int c, result;
/* Clear device map. */
map_init(&map);
while (1) {
result = decode((const unsigned char **)&data_next, data_ends, &map, example_add, example_set);
if (result)
switch (errno) {
case ENOSPC:
fprintf(stderr, "add() callback failed, return value %d.\n", result);
return EXIT_FAILURE;
case ENOENT:
fprintf(stderr, "set() callback failed, return value %d.\n", result);
return EXIT_FAILURE;
default:
fprintf(stderr, "Bug in decode(), return value %d.\n", result);
return EXIT_FAILURE;
}
/* Result is 0, so check errno too */
if (errno != 0 && errno != EAGAIN) {
fprintf(stderr, "decode() failed: %s.\n", strerror(errno));
return EXIT_FAILURE;
}
/* Buffer empty? Need to repack the buffer? */
if (data_next >= data_ends) {
data_next = data_buffer;
data_ends = data_buffer;
} else
if (data_next >= data_buffer + (sizeof data_buffer) / 2) {
memmove(data_buffer, data_next, (size_t)(data_ends - data_next));
data_ends = data_buffer + (data_ends - data_next);
data_next = data_buffer;
}
#ifdef PARANOID
if (data_ends >= data_buffer + sizeof data_buffer) {
/* Buffer is full already! */
fprintf(stderr, "decode() left a full buffer!\n");
return EXIT_FAILURE;
}
#endif
/* decode() has processed all data it could from the buffer,
* so we definitely need more. */
c = getc(stdin);
if (c == EOF)
break;
/* Append to buffer. */
*(data_ends++) = c;
}
if (data_ends > data_next)
fprintf(stderr, "No more data; last %d bytes ignored.\n", (int)(data_ends - data_next));
else
fprintf(stderr, "No more data; all data processed.\n");
return 0;
}
Compile using e.g.
