File content as of revision 0:6015e28a6fb1:

/** @file xstp.c
 * @brief Implementación del porotocolo XSTP
 */
#include "xstp.h"
#include <stdlib.h>
#include <string.h>

uint8_t* escapeData(uint8_t *destlen, uint8_t *src, uint8_t srclen){
    /* Destino de igual tamaño que el origen */
    uint8_t *dest = (uint8_t*) malloc(sizeof(uint8_t) * srclen);
    *destlen = srclen;

     /* Copiar nueva data */
    uint8_t sidx = 0;
    uint8_t didx = 0;
    uint8_t from = 0;
    uint8_t to = 0;
    uint8_t mustescape = 0;

    while (sidx < srclen) {
        /* Buscar bytes a escapar */
        for (; sidx < srclen && !(mustescape = (src[sidx] == 0x7c)); sidx++);

        /* Copiar bytes */
        to = sidx;
        memcpy(dest + didx, src + from, to - from);
        didx += to - from;
        from = sidx;

        if (mustescape) {
            /* Agregar escape */
            (*destlen)++;
            dest = (uint8_t*) realloc(dest, sizeof(uint8_t) * *destlen);
            dest[didx] = 0x7b;
            didx++;

            /* Agregar byte reservado */
            dest[didx] = src[sidx] ^ 0x20;
            didx++;

            /* Avanzar byte reservado */
            sidx++;
            from = sidx;
        }
    }
    return dest;
}

int buildSensorPacket(XSTPPacket *packet, uint8_t ssv){
    packet->data = (uint8_t*) malloc(sizeof(uint8_t) * 2);

    /* Packet type */
    *(packet->data) = 0x01;

    /* SSV */
    *(packet->data+1) = ssv;
    packet->len = 2;

    return 0;
}

int buildErrorPacket(XSTPPacket *packet, uint16_t ecode){
    packet->data = (uint8_t*) malloc(sizeof(uint8_t) * 3);

    /* Packet type */
    *(packet->data) = 0x02;

    /* Error code */
    *(packet->data+1) = (uint8_t) (ecode >> 8);
    *(packet->data+2) = (uint8_t) ecode;
    packet->len = 3;

    return 0;
}

int attachSensor(XSTPPacket *packet, uint16_t sid, uint8_t *data, uint8_t len){
    uint8_t hsid = (uint8_t) (sid >> 8);
    uint8_t lsid = (uint8_t) sid;

    /* Escapar sid y data */
    uint8_t hesidlen, lesidlen, edatalen;
    uint8_t* hesid = escapeData(&hesidlen, &hsid, 1);
    uint8_t* lesid = escapeData(&lesidlen, &lsid, 1);
    uint8_t* edata = escapeData(&edatalen, data, len);

    /* Error al intentar agregar más de XSTP_MAXDATA bytes */
    if (packet->len + 1 +  hesidlen + lesidlen + edatalen > XSTP_MAXDATA) return 1;

    /* Reasignar espacio de memoria */
    packet->data = (uint8_t*) realloc(packet->data,
            sizeof(uint8_t) * (packet->len + 1 + hesidlen + lesidlen + edatalen) );

    /* Agregar delimitador */
    *(packet->data + packet->len) = 0x7c;
    packet->len += 1;

    /* Agregar SID */
    memcpy(packet->data + packet->len, hesid, hesidlen);
    packet->len += hesidlen;
    memcpy(packet->data + packet->len, lesid, lesidlen);
    packet->len += lesidlen;

    /* Agregar data */
    memcpy(packet->data + packet->len, edata, edatalen);
    packet->len += edatalen;

    free(hesid);
    free(lesid);
    free(edata);

    return 0;
}

int freePacket(XSTPPacket *packet){
    /* Liberar la data y la estructura*/
    free(packet->data);

    return 0;
}