xbee_encoder.c

#include <assert.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

struct xbee_enc_state {
    uint8_t const * input;
    size_t size;
    size_t pos;

    bool delim;
    bool msb;
    bool lsb;
    bool checksum;

    bool escape;
    uint8_t esc_byte;

    uint8_t rolling_sum;
};

#define XBEE_MASK (0x20)

#define XBEE_DELIM  (0x7E)
#define XBEE_ESCAPE (0x7D)
#define XBEE_XON    (0x11)
#define XBEE_XOFF   (0x13)

#define NEEDS_ESCAPE(B) ((XBEE_DELIM == (B))  || (XBEE_ESCAPE == (B)) ||   \
                         (XBEE_XON == (B))    || (XBEE_XOFF == (B)))
#define MASK_BYTE(B) ((B) ^ XBEE_MASK)

static void xbee_enc_init(struct xbee_enc_state * s,
                          void const * input,
                          size_t size);

static uint8_t xbee_setup_escape(struct xbee_enc_state * s,
                                 uint8_t b);

static bool xbee_get1(struct xbee_enc_state * s,
                      uint8_t * r);

static size_t xbee_get(struct xbee_enc_state * s,
                       void * output,
                       size_t size);

static uint8_t buffer[128] = { 0 };
static char const * data = "hi my name is john \x7e";

int main(int argc, char * argv[]) {
    (void)argc;
    (void)argv;

    struct xbee_enc_state s;
    xbee_enc_init(&s, data, strlen(data));

    size_t const enclen = xbee_get(&s, buffer, sizeof(buffer));
    fwrite(buffer, 1, enclen, stdout);

    return 0;
}

static void xbee_enc_init(struct xbee_enc_state * s,
                          void const * input,
                          size_t size) {
    assert(s);
    assert(input);

    *s = (struct xbee_enc_state) {
        .input = input,
        .size = size,
    };
}

static uint8_t xbee_setup_escape(struct xbee_enc_state * s,
                                 uint8_t b) {
    assert(!s->escape);

    if (NEEDS_ESCAPE(b)) {
        s->esc_byte = MASK_BYTE(b);
        s->escape = true;

        return XBEE_ESCAPE;
    } else {
        return b;
    }
}

static bool xbee_get1(struct xbee_enc_state * s,
                      uint8_t * r) {
    assert(s);
    assert(r);
    assert(s->input);
    assert(s->size <= UINT16_MAX);

    // If the delimiter hasn't been set yet, send that.
    if (!s->delim) {
        *r = XBEE_DELIM;
        s->delim = true;
        return true;
    }

    if (s->escape) {
        // If we need an escape, send it now and clear the escape
        // status.
        *r = s->esc_byte;
        s->escape = false;
    } else if (!s->msb) {
        // Send the MSB if we haven't yet.
        *r = xbee_setup_escape(s, 0xFF & (s->size >> 8));
        s->msb = true;
    } else if (!s->lsb) {
        // Send the LSB if we haven't yet.
        *r = xbee_setup_escape(s, 0xFF & s->size);
        s->lsb = true;
    } else if (s->pos < s->size) {
        // Send the next byte as long as we have one.
        uint8_t const b = s->input[s->pos];

        s->rolling_sum += b;
        *r = xbee_setup_escape(s, b);

        s->pos += 1;
    } else if (!s->checksum) {
        *r = xbee_setup_escape(s, 0xFF - s->rolling_sum);
        s->checksum = true;
    } else {
        return false;
    }

    return true;
}

static size_t xbee_get(struct xbee_enc_state * s,
                       void * output,
                       size_t size) {
    uint8_t * const output_bytes = output;
    size_t i;

    for (i = 0; i < size; i++) {
        if(!xbee_get1(s, &output_bytes[i])) {
            break;
        }
    }

    return i;
}