216 lines
5.6 KiB
C
216 lines
5.6 KiB
C
#include "device.h"
|
|
#include <ctype.h>
|
|
#include <errno.h>
|
|
#include <inttypes.h>
|
|
#include <limits.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
uint32_t read_le32(const unsigned char *raw)
|
|
{
|
|
uint32_t value = (uint32_t)raw[0];
|
|
value |= (uint32_t)raw[1] << 8;
|
|
value |= (uint32_t)raw[2] << 16;
|
|
value |= (uint32_t)raw[3] << 24;
|
|
return value;
|
|
}
|
|
|
|
void write_le32(unsigned char *raw, uint32_t value)
|
|
{
|
|
raw[0] = (unsigned char)value;
|
|
raw[1] = (unsigned char)(value >> 8);
|
|
raw[2] = (unsigned char)(value >> 16);
|
|
raw[3] = (unsigned char)(value >> 24);
|
|
}
|
|
|
|
uint32_t bytes_crc(const unsigned char *raw, size_t size)
|
|
{
|
|
uint32_t crc = UINT32_C(0xffffffff);
|
|
for (size_t i = 0; i < size; ++i) {
|
|
crc ^= raw[i];
|
|
for (unsigned bit = 0; bit < 8; ++bit) {
|
|
uint32_t mask = 0u - (crc & 1u);
|
|
crc = (crc >> 1) ^ (UINT32_C(0xedb88320) & mask);
|
|
}
|
|
}
|
|
return ~crc;
|
|
}
|
|
|
|
void record_encode(const DeviceRecord *record, unsigned char out[16])
|
|
{
|
|
write_le32(out, record->sequence);
|
|
write_le32(out + 4, record->channel);
|
|
write_le32(out + 8, (uint32_t)record->value);
|
|
write_le32(out + 12, bytes_crc(out, 12));
|
|
}
|
|
|
|
int record_decode(const unsigned char *raw, size_t size, DeviceRecord *out)
|
|
{
|
|
if (size != 16) {
|
|
return 0;
|
|
}
|
|
if (read_le32(raw + 12) != bytes_crc(raw, 12)) {
|
|
return 0;
|
|
}
|
|
uint32_t channel = read_le32(raw + 4);
|
|
if (channel >= DEVICE_CHANNELS) {
|
|
return 0;
|
|
}
|
|
out->sequence = read_le32(raw);
|
|
out->channel = channel;
|
|
out->value = (int32_t)read_le32(raw + 8);
|
|
return 1;
|
|
}
|
|
|
|
int parse_u32(const char *text, uint32_t *out)
|
|
{
|
|
char *end = NULL;
|
|
if (text == NULL || *text == '\0' || *text == '-') {
|
|
return 0;
|
|
}
|
|
errno = 0;
|
|
unsigned long long value = strtoull(text, &end, 10);
|
|
if (errno != 0 || *end != '\0' || value > UINT32_MAX) {
|
|
return 0;
|
|
}
|
|
*out = (uint32_t)value;
|
|
return 1;
|
|
}
|
|
|
|
int parse_i32(const char *text, int32_t *out)
|
|
{
|
|
char *end = NULL;
|
|
if (text == NULL || *text == '\0') {
|
|
return 0;
|
|
}
|
|
errno = 0;
|
|
long long value = strtoll(text, &end, 10);
|
|
if (errno != 0 || *end != '\0' || value < INT32_MIN || value > INT32_MAX) {
|
|
return 0;
|
|
}
|
|
*out = (int32_t)value;
|
|
return 1;
|
|
}
|
|
|
|
static int hex_nibble(char digit)
|
|
{
|
|
if (digit >= '0' && digit <= '9') {
|
|
return digit - '0';
|
|
}
|
|
if (digit >= 'a' && digit <= 'f') {
|
|
return digit - 'a' + 10;
|
|
}
|
|
if (digit >= 'A' && digit <= 'F') {
|
|
return digit - 'A' + 10;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int hex_decode(const char *text, unsigned char *out, size_t limit, size_t *size)
|
|
{
|
|
size_t length = strlen(text);
|
|
if (length % 2 || length / 2 > limit) {
|
|
return 0;
|
|
}
|
|
for (size_t i = 0; i < length / 2; ++i) {
|
|
int high = hex_nibble(text[i * 2]);
|
|
int low = hex_nibble(text[i * 2 + 1]);
|
|
if (high < 0 || low < 0) {
|
|
return 0;
|
|
}
|
|
out[i] = (unsigned char)((high << 4) | low);
|
|
}
|
|
*size = length / 2;
|
|
return 1;
|
|
}
|
|
|
|
static size_t tokens_copy(const char *line, char storage[DEVICE_LINE], char **tokens)
|
|
{
|
|
size_t length = strlen(line);
|
|
if (length >= DEVICE_LINE) {
|
|
return 9;
|
|
}
|
|
memcpy(storage, line, length + 1);
|
|
char *next = storage;
|
|
size_t count = 0;
|
|
while (*next != '\0') {
|
|
while (isspace((unsigned char)*next)) {
|
|
++next;
|
|
}
|
|
if (*next == '\0') {
|
|
break;
|
|
}
|
|
if (count == 8) {
|
|
return 9;
|
|
}
|
|
tokens[count++] = next;
|
|
while (*next != '\0' && !isspace((unsigned char)*next)) {
|
|
++next;
|
|
}
|
|
if (*next != '\0') {
|
|
*next++ = '\0';
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
int text_request(const char *line, Request *out)
|
|
{
|
|
char storage[DEVICE_LINE];
|
|
char *token[8];
|
|
size_t count = tokens_copy(line, storage, token);
|
|
memset(out, 0, sizeof(*out));
|
|
out->kind = REQUEST_INVALID;
|
|
if (count == 0 || count > 8) {
|
|
return 0;
|
|
}
|
|
if (strcmp(token[0], "RESET") == 0 && count == 2) {
|
|
out->kind = REQUEST_RESET;
|
|
return parse_u32(token[1], &out->argument);
|
|
}
|
|
if (strcmp(token[0], "PUT") == 0 && count == 4) {
|
|
out->kind = REQUEST_PUT;
|
|
return parse_u32(token[1], &out->record.sequence) &&
|
|
parse_u32(token[2], &out->record.channel) &&
|
|
parse_i32(token[3], &out->record.value);
|
|
}
|
|
if (strcmp(token[0], "DRAIN") == 0 && count == 2) {
|
|
out->kind = REQUEST_DRAIN;
|
|
return parse_u32(token[1], &out->argument);
|
|
}
|
|
if (strcmp(token[0], "DROP") == 0 && count == 2) {
|
|
out->kind = REQUEST_DROP;
|
|
return parse_u32(token[1], &out->argument);
|
|
}
|
|
if (strcmp(token[0], "STATE") == 0 && count == 1) {
|
|
out->kind = REQUEST_STATE;
|
|
return 1;
|
|
}
|
|
if (strcmp(token[0], "WIRE") == 0 && count == 2) {
|
|
unsigned char raw[32];
|
|
size_t size = 0;
|
|
if (!hex_decode(token[1], raw, sizeof(raw), &size) ||
|
|
!record_decode(raw, size, &out->record)) {
|
|
return 0;
|
|
}
|
|
out->kind = REQUEST_PUT;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void text_response(const Response *response)
|
|
{
|
|
printf("R %d %" PRIu32 " %" PRIu32 " %" PRIu32 " %" PRIu32,
|
|
response->status, response->epoch, response->accepted,
|
|
response->dropped, response->pending);
|
|
printf(" %" PRIu32 " %d %d %d %zu", response->digest,
|
|
response->backend, response->feature, response->version,
|
|
response->count);
|
|
for (size_t i = 0; i < response->count; ++i) {
|
|
printf(" %" PRId64, response->values[i]);
|
|
}
|
|
putchar('\n');
|
|
}
|