Guillermo García / xstp

ddsfdggfa

xstp.c@0:6015e28a6fb1, 2016-06-17 (annotated)

Committer:
ggarcia
Date:
Fri Jun 17 16:22:34 2016 +0000
Revision:
0:6015e28a6fb1
?kl

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ggarcia 0:6015e28a6fb1 1 /** @file xstp.c
ggarcia 0:6015e28a6fb1 2 * @brief Implementación del porotocolo XSTP
ggarcia 0:6015e28a6fb1 3 */
ggarcia 0:6015e28a6fb1 4 #include "xstp.h"
ggarcia 0:6015e28a6fb1 5 #include <stdlib.h>
ggarcia 0:6015e28a6fb1 6 #include <string.h>
ggarcia 0:6015e28a6fb1 7
ggarcia 0:6015e28a6fb1 8 uint8_t* escapeData(uint8_t *destlen, uint8_t *src, uint8_t srclen){
ggarcia 0:6015e28a6fb1 9 /* Destino de igual tamaño que el origen */
ggarcia 0:6015e28a6fb1 10 uint8_t *dest = (uint8_t*) malloc(sizeof(uint8_t) * srclen);
ggarcia 0:6015e28a6fb1 11 *destlen = srclen;
ggarcia 0:6015e28a6fb1 12
ggarcia 0:6015e28a6fb1 13 /* Copiar nueva data */
ggarcia 0:6015e28a6fb1 14 uint8_t sidx = 0;
ggarcia 0:6015e28a6fb1 15 uint8_t didx = 0;
ggarcia 0:6015e28a6fb1 16 uint8_t from = 0;
ggarcia 0:6015e28a6fb1 17 uint8_t to = 0;
ggarcia 0:6015e28a6fb1 18 uint8_t mustescape = 0;
ggarcia 0:6015e28a6fb1 19
ggarcia 0:6015e28a6fb1 20 while (sidx < srclen) {
ggarcia 0:6015e28a6fb1 21 /* Buscar bytes a escapar */
ggarcia 0:6015e28a6fb1 22 for (; sidx < srclen && !(mustescape = (src[sidx] == 0x7c)); sidx++);
ggarcia 0:6015e28a6fb1 23
ggarcia 0:6015e28a6fb1 24 /* Copiar bytes */
ggarcia 0:6015e28a6fb1 25 to = sidx;
ggarcia 0:6015e28a6fb1 26 memcpy(dest + didx, src + from, to - from);
ggarcia 0:6015e28a6fb1 27 didx += to - from;
ggarcia 0:6015e28a6fb1 28 from = sidx;
ggarcia 0:6015e28a6fb1 29
ggarcia 0:6015e28a6fb1 30 if (mustescape) {
ggarcia 0:6015e28a6fb1 31 /* Agregar escape */
ggarcia 0:6015e28a6fb1 32 (*destlen)++;
ggarcia 0:6015e28a6fb1 33 dest = (uint8_t*) realloc(dest, sizeof(uint8_t) * *destlen);
ggarcia 0:6015e28a6fb1 34 dest[didx] = 0x7b;
ggarcia 0:6015e28a6fb1 35 didx++;
ggarcia 0:6015e28a6fb1 36
ggarcia 0:6015e28a6fb1 37 /* Agregar byte reservado */
ggarcia 0:6015e28a6fb1 38 dest[didx] = src[sidx] ^ 0x20;
ggarcia 0:6015e28a6fb1 39 didx++;
ggarcia 0:6015e28a6fb1 40
ggarcia 0:6015e28a6fb1 41 /* Avanzar byte reservado */
ggarcia 0:6015e28a6fb1 42 sidx++;
ggarcia 0:6015e28a6fb1 43 from = sidx;
ggarcia 0:6015e28a6fb1 44 }
ggarcia 0:6015e28a6fb1 45 }
ggarcia 0:6015e28a6fb1 46 return dest;
ggarcia 0:6015e28a6fb1 47 }
ggarcia 0:6015e28a6fb1 48
ggarcia 0:6015e28a6fb1 49 int buildSensorPacket(XSTPPacket *packet, uint8_t ssv){
ggarcia 0:6015e28a6fb1 50 packet->data = (uint8_t*) malloc(sizeof(uint8_t) * 2);
ggarcia 0:6015e28a6fb1 51
ggarcia 0:6015e28a6fb1 52 /* Packet type */
ggarcia 0:6015e28a6fb1 53 *(packet->data) = 0x01;
ggarcia 0:6015e28a6fb1 54
ggarcia 0:6015e28a6fb1 55 /* SSV */
ggarcia 0:6015e28a6fb1 56 *(packet->data+1) = ssv;
ggarcia 0:6015e28a6fb1 57 packet->len = 2;
ggarcia 0:6015e28a6fb1 58
ggarcia 0:6015e28a6fb1 59 return 0;
ggarcia 0:6015e28a6fb1 60 }
ggarcia 0:6015e28a6fb1 61
ggarcia 0:6015e28a6fb1 62 int buildErrorPacket(XSTPPacket *packet, uint16_t ecode){
ggarcia 0:6015e28a6fb1 63 packet->data = (uint8_t*) malloc(sizeof(uint8_t) * 3);
ggarcia 0:6015e28a6fb1 64
ggarcia 0:6015e28a6fb1 65 /* Packet type */
ggarcia 0:6015e28a6fb1 66 *(packet->data) = 0x02;
ggarcia 0:6015e28a6fb1 67
ggarcia 0:6015e28a6fb1 68 /* Error code */
ggarcia 0:6015e28a6fb1 69 *(packet->data+1) = (uint8_t) (ecode >> 8);
ggarcia 0:6015e28a6fb1 70 *(packet->data+2) = (uint8_t) ecode;
ggarcia 0:6015e28a6fb1 71 packet->len = 3;
ggarcia 0:6015e28a6fb1 72
ggarcia 0:6015e28a6fb1 73 return 0;
ggarcia 0:6015e28a6fb1 74 }
ggarcia 0:6015e28a6fb1 75
ggarcia 0:6015e28a6fb1 76 int attachSensor(XSTPPacket *packet, uint16_t sid, uint8_t *data, uint8_t len){
ggarcia 0:6015e28a6fb1 77 uint8_t hsid = (uint8_t) (sid >> 8);
ggarcia 0:6015e28a6fb1 78 uint8_t lsid = (uint8_t) sid;
ggarcia 0:6015e28a6fb1 79
ggarcia 0:6015e28a6fb1 80 /* Escapar sid y data */
ggarcia 0:6015e28a6fb1 81 uint8_t hesidlen, lesidlen, edatalen;
ggarcia 0:6015e28a6fb1 82 uint8_t* hesid = escapeData(&hesidlen, &hsid, 1);
ggarcia 0:6015e28a6fb1 83 uint8_t* lesid = escapeData(&lesidlen, &lsid, 1);
ggarcia 0:6015e28a6fb1 84 uint8_t* edata = escapeData(&edatalen, data, len);
ggarcia 0:6015e28a6fb1 85
ggarcia 0:6015e28a6fb1 86 /* Error al intentar agregar más de XSTP_MAXDATA bytes */
ggarcia 0:6015e28a6fb1 87 if (packet->len + 1 + hesidlen + lesidlen + edatalen > XSTP_MAXDATA) return 1;
ggarcia 0:6015e28a6fb1 88
ggarcia 0:6015e28a6fb1 89 /* Reasignar espacio de memoria */
ggarcia 0:6015e28a6fb1 90 packet->data = (uint8_t*) realloc(packet->data,
ggarcia 0:6015e28a6fb1 91 sizeof(uint8_t) * (packet->len + 1 + hesidlen + lesidlen + edatalen) );
ggarcia 0:6015e28a6fb1 92
ggarcia 0:6015e28a6fb1 93 /* Agregar delimitador */
ggarcia 0:6015e28a6fb1 94 *(packet->data + packet->len) = 0x7c;
ggarcia 0:6015e28a6fb1 95 packet->len += 1;
ggarcia 0:6015e28a6fb1 96
ggarcia 0:6015e28a6fb1 97 /* Agregar SID */
ggarcia 0:6015e28a6fb1 98 memcpy(packet->data + packet->len, hesid, hesidlen);
ggarcia 0:6015e28a6fb1 99 packet->len += hesidlen;
ggarcia 0:6015e28a6fb1 100 memcpy(packet->data + packet->len, lesid, lesidlen);
ggarcia 0:6015e28a6fb1 101 packet->len += lesidlen;
ggarcia 0:6015e28a6fb1 102
ggarcia 0:6015e28a6fb1 103 /* Agregar data */
ggarcia 0:6015e28a6fb1 104 memcpy(packet->data + packet->len, edata, edatalen);
ggarcia 0:6015e28a6fb1 105 packet->len += edatalen;
ggarcia 0:6015e28a6fb1 106
ggarcia 0:6015e28a6fb1 107 free(hesid);
ggarcia 0:6015e28a6fb1 108 free(lesid);
ggarcia 0:6015e28a6fb1 109 free(edata);
ggarcia 0:6015e28a6fb1 110
ggarcia 0:6015e28a6fb1 111 return 0;
ggarcia 0:6015e28a6fb1 112 }
ggarcia 0:6015e28a6fb1 113
ggarcia 0:6015e28a6fb1 114 int freePacket(XSTPPacket *packet){
ggarcia 0:6015e28a6fb1 115 /* Liberar la data y la estructura*/
ggarcia 0:6015e28a6fb1 116 free(packet->data);
ggarcia 0:6015e28a6fb1 117
ggarcia 0:6015e28a6fb1 118 return 0;
ggarcia 0:6015e28a6fb1 119 }
ggarcia 0:6015e28a6fb1 120