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?
User | Revision | Line number | New 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 |