bitstuff.cpp

//
// Simple bit stuffing and framing prototype for HDLC implementations
//
//  Needs c++ for i/o as-is, but its really a C-based implementation
//
//  This is a quick and dirty solution, if flow control or synchronous
//  behavior is necessary, look into using other libs..
//
//  note: needs crc still, basically I just wanted to make a quick and 
//      droppable implementation of bit stuffing
//

#include <iostream>
#include <bitset>

#define DEBUG

#define LOBYTE(in)  (uint8_t)(in & 0xff)
#define HIBYTE(in)  (uint8_t)(in >> 8 & 0xff)
#define LOBIT(in)   (uint8_t)(in & 0x01)
#define MARKER 0b01111110
#define MAX_ONE_CNT 5

#define IN_BUF_SIZE 16
#define OUT_BUF_SIZE IN_BUF_SIZE * 2


typedef struct stuff_state_t {
    uint8_t onecnt;
    uint8_t bitcnt;
    uint32_t bitbuf;
};


void add_flag(uint8_t in_flag, stuff_state_t* in_state) {
    // shift current bit into buffer
    in_state->bitbuf |= (uint32_t)(in_flag << in_state->bitcnt);
    in_state->bitcnt += 8;
}


void stuff_byte(uint8_t in_byte, stuff_state_t* in_state) {
    for (int i = 0; i < 8; i++) {
        uint8_t this_bit = LOBIT(in_byte >> i);
        if (this_bit)
            in_state->onecnt++;
        else
            in_state->onecnt = 0;

        // shift current bit into buffer
        in_state->bitbuf |= (uint32_t)(this_bit << in_state->bitcnt++);

        // inject 0s if max amount of consecutive 1s detected
        if (in_state->onecnt == MAX_ONE_CNT) {
            in_state->bitcnt++;
            in_state->onecnt = 0;
        }
    }
}


int commit_cached_bytes(uint8_t* out_bytes, stuff_state_t* in_state) {
    int num_out = 0;
    while (in_state->bitcnt >= 8) {
        in_state->bitcnt -= 8;
        out_bytes[num_out++] = LOBYTE(in_state->bitbuf);
        // shift high 16b down 1B & mask to zero the top
        in_state->bitbuf = (uint32_t)(in_state->bitbuf >> 8 & 0x00ffff);
    }
    return num_out;
}


int do_frame(uint8_t* in_bytes, int in_num, uint8_t* out_bytes, stuff_state_t* in_state) {
    int num_out = 0;

    // stuff each inputted byte and commit the solidified output
    for (int i = 0; i < in_num; i++) {
        stuff_byte(in_bytes[i], in_state);
        num_out += commit_cached_bytes(&out_bytes[num_out], in_state);
    }
    // TODO: needs crc...
    add_flag(MARKER, in_state);
    num_out += commit_cached_bytes(&out_bytes[num_out], in_state);
    add_flag(MARKER, in_state);
    num_out += commit_cached_bytes(&out_bytes[num_out], in_state);

    return num_out;
}


void unstuff_byte(uint8_t in_byte, stuff_state_t* in_state) {
    // TODO: needs crc...
    for (int i = 0; i < 8; i++) {
        uint8_t this_bit = LOBIT(in_byte >> i);

        // shift current bit into buffer
        in_state->bitbuf |= (uint32_t)(this_bit << in_state->bitcnt++);
        
        if (in_state->onecnt == MAX_ONE_CNT) {
            in_state->onecnt = 0;
            // if yet another 1, means we have a flag, remove it...
            if (this_bit) {
                in_state->bitcnt -= 7;
                return;
            }
            // eject one 0, max count reached...
            if (!this_bit)
                in_state->bitcnt--;
        }
        else {
            if (this_bit)
                in_state->onecnt++;
            else
                in_state->onecnt = 0;
        }
    }
}


int de_frame(uint8_t* in_bytes, int in_num, uint8_t* out_bytes, stuff_state_t* in_state) {
    int num_out = 0;
    // unstuff each inputted byte and commit the solidified output
    for (int i = 0; i < in_num; i++) {
        unstuff_byte(in_bytes[i], in_state);
        num_out += commit_cached_bytes(&out_bytes[num_out], in_state);
    }
    return num_out;
}


void debug_print_bytes(const char* label, uint8_t* in_bytes, int in_num) {
#ifdef DEBUG
    for (int i = 0; i < in_num; i++)
        std::cout << label << std::bitset<8>(in_bytes[i]) << " (" << 
            (int)in_bytes[i] << ")" <<  std::endl;
#endif
}


int main()
{
    stuff_state_t i_st; // frame
    memset(&i_st, 0, sizeof(i_st));
    stuff_state_t o_st; // deframe
    memset(&o_st, 0, sizeof(o_st));

    uint8_t in_bytes[IN_BUF_SIZE];
    uint8_t framed_bytes[OUT_BUF_SIZE];
    uint8_t out_bytes[OUT_BUF_SIZE];

    memset(&in_bytes[0], 0, sizeof(IN_BUF_SIZE));
    memset(&framed_bytes[0], 0, sizeof(OUT_BUF_SIZE));
    memset(&out_bytes[0], 0, sizeof(OUT_BUF_SIZE));

    int to_in = 2; // IN_BUF_SIZE

    for (int i = 1; i < to_in + 1; i++)
        in_bytes[i-1] = (uint8_t)(i * 31 % 255);
    debug_print_bytes("ADDED: ", in_bytes, to_in);

    int n_framed = do_frame(in_bytes, to_in, framed_bytes, &i_st);
    debug_print_bytes("FRAMED: ", framed_bytes, n_framed);

    int n_deframed = de_frame(framed_bytes, n_framed, out_bytes, &o_st);
    debug_print_bytes("DEFRAMED: ", out_bytes, n_deframed);
}