ddsfdggfa
xstp.c
- Committer:
- ggarcia
- Date:
- 2016-06-17
- Revision:
- 0:6015e28a6fb1
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; }