Diff: xstp.c

Revision:

0:6015e28a6fb1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/xstp.c	Fri Jun 17 16:22:34 2016 +0000
@@ -0,0 +1,120 @@
+/** @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;
+}
+