Joaquin Verastegui / jro

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   JRO_CR2 frdm_test

Fork of jro by Miguel Urco

ipserver.cpp@1:7c424a3e12ea, 2014-12-04 (annotated)

Committer:
miguelcordero191
Date:
Thu Dec 04 14:35:52 2014 +0000
Revision:
1:7c424a3e12ea
Parent:
0:b444ea725ba7 
First release: write and read process working very well

Who changed what in which revision?

UserRevisionLine numberNew contents of line
miguelcordero191 0:b444ea725ba7 1 #include "ipserver.h"
miguelcordero191 0:b444ea725ba7 2
miguelcordero191 1:7c424a3e12ea 3 void printCadena(char* cad, int lenc){
miguelcordero191 1:7c424a3e12ea 4
miguelcordero191 1:7c424a3e12ea 5 printf("\r\n");
miguelcordero191 1:7c424a3e12ea 6 for (int i=0; i<lenc; i++){
miguelcordero191 1:7c424a3e12ea 7
miguelcordero191 1:7c424a3e12ea 8 printf("cad[%d] = %d\r\n", i, cad[i]);
miguelcordero191 1:7c424a3e12ea 9 }
miguelcordero191 1:7c424a3e12ea 10
miguelcordero191 1:7c424a3e12ea 11 }
miguelcordero191 1:7c424a3e12ea 12
miguelcordero191 1:7c424a3e12ea 13 IpData::IpData(char* tx_buffer){
miguelcordero191 1:7c424a3e12ea 14
miguelcordero191 1:7c424a3e12ea 15 this->id = ID_DEV;
miguelcordero191 1:7c424a3e12ea 16 this->isValidData = false;
miguelcordero191 1:7c424a3e12ea 17
miguelcordero191 1:7c424a3e12ea 18 this->len_tx_data = 0;
miguelcordero191 1:7c424a3e12ea 19 this->tx_buff = tx_buffer;
miguelcordero191 1:7c424a3e12ea 20
miguelcordero191 1:7c424a3e12ea 21 }
miguelcordero191 1:7c424a3e12ea 22
miguelcordero191 0:b444ea725ba7 23 char* IpData::__findHeader(char* buffer){
miguelcordero191 0:b444ea725ba7 24 /*
miguelcordero191 0:b444ea725ba7 25 Find header and get ip data (without header)
miguelcordero191 0:b444ea725ba7 26
miguelcordero191 0:b444ea725ba7 27 Input:
miguelcordero191 0:b444ea725ba7 28
miguelcordero191 0:b444ea725ba7 29 Return:
miguelcordero191 0:b444ea725ba7 30 valid_ipdata: ip data without header
miguelcordero191 0:b444ea725ba7 31
miguelcordero191 0:b444ea725ba7 32 Example:
miguelcordero191 0:b444ea725ba7 33
miguelcordero191 0:b444ea725ba7 34 buffer = [ B0 B1 $ J R O $ B7 B8 B9 B10 ..... ]
miguelcordero191 0:b444ea725ba7 35
miguelcordero191 0:b444ea725ba7 36 return
miguelcordero191 0:b444ea725ba7 37 valid_ip_data = [ B7 B8 B9 B10 ..... ]
miguelcordero191 0:b444ea725ba7 38
miguelcordero191 0:b444ea725ba7 39
miguelcordero191 0:b444ea725ba7 40 */
miguelcordero191 0:b444ea725ba7 41 char* valid_ip_data;
miguelcordero191 0:b444ea725ba7 42
miguelcordero191 1:7c424a3e12ea 43 //printf("\r\nSearching header in %s\r\n", buffer);
miguelcordero191 1:7c424a3e12ea 44
miguelcordero191 0:b444ea725ba7 45 //Finding header
miguelcordero191 0:b444ea725ba7 46 valid_ip_data = strstr(buffer, HEADER);
miguelcordero191 0:b444ea725ba7 47
miguelcordero191 1:7c424a3e12ea 48 if (valid_ip_data == NULL){
miguelcordero191 1:7c424a3e12ea 49 printf("\r\nHeader was not found\r\n");
miguelcordero191 0:b444ea725ba7 50 return NULL;
miguelcordero191 1:7c424a3e12ea 51 }
miguelcordero191 1:7c424a3e12ea 52
miguelcordero191 1:7c424a3e12ea 53 //printf("\r\nHeader was found %s\r\n", valid_ip_data);
miguelcordero191 1:7c424a3e12ea 54
miguelcordero191 1:7c424a3e12ea 55 //printf("\r\nskipping header ...\r\n");
miguelcordero191 0:b444ea725ba7 56
miguelcordero191 0:b444ea725ba7 57 valid_ip_data += 5; //skip header
miguelcordero191 1:7c424a3e12ea 58
miguelcordero191 0:b444ea725ba7 59 return valid_ip_data;
miguelcordero191 0:b444ea725ba7 60 }
miguelcordero191 1:7c424a3e12ea 61
miguelcordero191 1:7c424a3e12ea 62 char IpData::__getXor( char* data, unsigned long len_data){
miguelcordero191 0:b444ea725ba7 63
miguelcordero191 1:7c424a3e12ea 64 char new_xor = 0;
miguelcordero191 1:7c424a3e12ea 65
miguelcordero191 1:7c424a3e12ea 66 for(unsigned long i = 0; i < len_data; i++){
miguelcordero191 1:7c424a3e12ea 67 new_xor = new_xor xor data[i];
miguelcordero191 1:7c424a3e12ea 68 }
miguelcordero191 1:7c424a3e12ea 69
miguelcordero191 1:7c424a3e12ea 70 return new_xor;
miguelcordero191 1:7c424a3e12ea 71 }
miguelcordero191 1:7c424a3e12ea 72
miguelcordero191 1:7c424a3e12ea 73 int IpData::__verifyData( char* ip_data, unsigned long len_data){
miguelcordero191 0:b444ea725ba7 74
miguelcordero191 0:b444ea725ba7 75 char new_xor = 0;
miguelcordero191 0:b444ea725ba7 76
miguelcordero191 1:7c424a3e12ea 77 new_xor = this->__getXor(ip_data, len_data);
miguelcordero191 1:7c424a3e12ea 78
miguelcordero191 1:7c424a3e12ea 79 if (new_xor != 0){
miguelcordero191 1:7c424a3e12ea 80 printf("\r\nInvalid XOR: %d\r\n", new_xor);
miguelcordero191 1:7c424a3e12ea 81 return 0;
miguelcordero191 0:b444ea725ba7 82 }
miguelcordero191 0:b444ea725ba7 83
miguelcordero191 1:7c424a3e12ea 84 //printf("\r\nXOR verified successfully\r\n");
miguelcordero191 1:7c424a3e12ea 85 return 1;
miguelcordero191 1:7c424a3e12ea 86 }
miguelcordero191 1:7c424a3e12ea 87
miguelcordero191 1:7c424a3e12ea 88 int IpData::__getParameters(){
miguelcordero191 0:b444ea725ba7 89 return 1;
miguelcordero191 0:b444ea725ba7 90 }
miguelcordero191 0:b444ea725ba7 91
miguelcordero191 1:7c424a3e12ea 92 char* IpData::encode(unsigned short cmd, char* payload, unsigned long len_payload){
miguelcordero191 0:b444ea725ba7 93
miguelcordero191 1:7c424a3e12ea 94 int head_size= strlen(HEADER);
miguelcordero191 1:7c424a3e12ea 95 char xor_wr;
miguelcordero191 1:7c424a3e12ea 96
miguelcordero191 1:7c424a3e12ea 97 //Copy header
miguelcordero191 1:7c424a3e12ea 98 strcpy(tx_buff, HEADER);
miguelcordero191 1:7c424a3e12ea 99 len_tx_data = len_payload + 5; //Incluye la longitud de la data + 1B (id_class) + 1B (id_dev) + 2B (cmd) + 1B (xor)
miguelcordero191 1:7c424a3e12ea 100 len_tx_buffer = len_tx_data + 9; //Incluye 5 bytes del Header + 4 de la longitud
miguelcordero191 1:7c424a3e12ea 101
miguelcordero191 1:7c424a3e12ea 102 tx_buff[0+head_size] = (len_tx_data >> 24) & 0xff;
miguelcordero191 1:7c424a3e12ea 103 tx_buff[1+head_size] = (len_tx_data >> 16) & 0xff;
miguelcordero191 1:7c424a3e12ea 104 tx_buff[2+head_size] = (len_tx_data >> 8) & 0xff;
miguelcordero191 1:7c424a3e12ea 105 tx_buff[3+head_size] = len_tx_data & 0xff;
miguelcordero191 1:7c424a3e12ea 106
miguelcordero191 1:7c424a3e12ea 107 tx_buff[4+head_size] = ID_CLASS;
miguelcordero191 1:7c424a3e12ea 108 tx_buff[5+head_size] = ID_DEV;
miguelcordero191 0:b444ea725ba7 109
miguelcordero191 1:7c424a3e12ea 110 tx_buff[6+head_size] = (cmd >> 8) & 0xff;
miguelcordero191 1:7c424a3e12ea 111 tx_buff[7+head_size] = cmd & 0xff;
miguelcordero191 1:7c424a3e12ea 112
miguelcordero191 1:7c424a3e12ea 113 for (unsigned int i=0; i<len_payload; i++)
miguelcordero191 1:7c424a3e12ea 114 tx_buff[8+head_size+i] = payload[i];
miguelcordero191 1:7c424a3e12ea 115
miguelcordero191 1:7c424a3e12ea 116 xor_wr = this->__getXor(&tx_buff[0+head_size], len_tx_data+4-1); //Incluir todos los bytes de datos mas los 4B de la longitud menos 1B del xor
miguelcordero191 1:7c424a3e12ea 117
miguelcordero191 1:7c424a3e12ea 118 tx_buff[8+head_size+len_payload] = xor_wr;
miguelcordero191 1:7c424a3e12ea 119
miguelcordero191 1:7c424a3e12ea 120 printf("\r\nTx Buffer = ");
miguelcordero191 1:7c424a3e12ea 121 for(unsigned long i=0; i< len_tx_buffer; i++)
miguelcordero191 1:7c424a3e12ea 122 printf("0x%x ", tx_buff[i]);
miguelcordero191 1:7c424a3e12ea 123
miguelcordero191 1:7c424a3e12ea 124 return tx_buff;
miguelcordero191 0:b444ea725ba7 125
miguelcordero191 0:b444ea725ba7 126 }
miguelcordero191 0:b444ea725ba7 127
miguelcordero191 1:7c424a3e12ea 128
miguelcordero191 1:7c424a3e12ea 129 int IpData::decode( char* buffer, unsigned long len_buffer){
miguelcordero191 0:b444ea725ba7 130
miguelcordero191 0:b444ea725ba7 131 char* ip_data;
miguelcordero191 0:b444ea725ba7 132 unsigned long len_data;
miguelcordero191 0:b444ea725ba7 133
miguelcordero191 1:7c424a3e12ea 134 this->isValidData = false;
miguelcordero191 1:7c424a3e12ea 135
miguelcordero191 1:7c424a3e12ea 136 printf("\r\nRx Buffer = ");
miguelcordero191 1:7c424a3e12ea 137 for(unsigned long i=0; i< len_buffer; i++)
miguelcordero191 1:7c424a3e12ea 138 printf("0x%x ", buffer[i]);
miguelcordero191 1:7c424a3e12ea 139
miguelcordero191 1:7c424a3e12ea 140 if (len_buffer < 13){
miguelcordero191 1:7c424a3e12ea 141 printf("\r\nLongitud de la data insuficiente\r\n");
miguelcordero191 0:b444ea725ba7 142 return 0;
miguelcordero191 1:7c424a3e12ea 143 }
miguelcordero191 0:b444ea725ba7 144
miguelcordero191 0:b444ea725ba7 145 //Finding header and get ip data (without header)
miguelcordero191 0:b444ea725ba7 146 ip_data = this->__findHeader(buffer);
miguelcordero191 0:b444ea725ba7 147
miguelcordero191 0:b444ea725ba7 148 if (ip_data == NULL)
miguelcordero191 0:b444ea725ba7 149 return 0;
miguelcordero191 0:b444ea725ba7 150
miguelcordero191 1:7c424a3e12ea 151 len_data = ip_data[0]*65536*256 + ip_data[1]*65536 + ip_data[2]*256 + ip_data[3];
miguelcordero191 0:b444ea725ba7 152
miguelcordero191 1:7c424a3e12ea 153 //printf("\r\nLen data = %d\r\n", len_data);
miguelcordero191 1:7c424a3e12ea 154
miguelcordero191 1:7c424a3e12ea 155 this->rx_buff = ip_data;
miguelcordero191 0:b444ea725ba7 156
miguelcordero191 1:7c424a3e12ea 157 //len_data + 4 = longitud de toda la data (incluyendo los 4 bytes del campo len)
miguelcordero191 1:7c424a3e12ea 158 if (not this->__verifyData(ip_data, len_data+4))
miguelcordero191 0:b444ea725ba7 159 return 0;
miguelcordero191 1:7c424a3e12ea 160
miguelcordero191 1:7c424a3e12ea 161 //head = ip_data[0:5];
miguelcordero191 1:7c424a3e12ea 162 this->id_class = ip_data[4];
miguelcordero191 1:7c424a3e12ea 163 this->id_dev = ip_data[5];
miguelcordero191 1:7c424a3e12ea 164 this->cmd = ip_data[6]*256 + ip_data[7];
miguelcordero191 1:7c424a3e12ea 165 this->payload = ip_data + 8;
miguelcordero191 1:7c424a3e12ea 166 this->len_payload = len_data - 5; //1B id_class, 1B id_dev, 2B cmd y 1B xor
miguelcordero191 1:7c424a3e12ea 167 this->xor_rd = ip_data[4+len_data-1];
miguelcordero191 1:7c424a3e12ea 168
miguelcordero191 1:7c424a3e12ea 169 printf("\r\nID_CLASS = 0x%x, ID_DEV = 0x%x, CMD = 0x%x, XOR = 0x%x", id_class, id_dev, cmd, xor_rd);
miguelcordero191 1:7c424a3e12ea 170 printf("\r\nPAYLOAD[0] = 0x%x, PYLD_LEN = 0x%x\r\n", payload[0], len_payload);
miguelcordero191 1:7c424a3e12ea 171
miguelcordero191 1:7c424a3e12ea 172 if (this->id_class == ID_CLASS){
miguelcordero191 1:7c424a3e12ea 173 this->isValidData = true;
miguelcordero191 1:7c424a3e12ea 174
miguelcordero191 1:7c424a3e12ea 175 return 1;
miguelcordero191 0:b444ea725ba7 176 }
miguelcordero191 0:b444ea725ba7 177
miguelcordero191 1:7c424a3e12ea 178 return 0;
miguelcordero191 0:b444ea725ba7 179
miguelcordero191 0:b444ea725ba7 180 }
miguelcordero191 0:b444ea725ba7 181
miguelcordero191 0:b444ea725ba7 182 char IpData::getIdClass(){
miguelcordero191 1:7c424a3e12ea 183
miguelcordero191 1:7c424a3e12ea 184 if (not this->isValidData)
miguelcordero191 1:7c424a3e12ea 185 return 0;
miguelcordero191 1:7c424a3e12ea 186
miguelcordero191 0:b444ea725ba7 187 return this->id_class;
miguelcordero191 0:b444ea725ba7 188 }
miguelcordero191 0:b444ea725ba7 189
miguelcordero191 0:b444ea725ba7 190 char IpData::getIdDevice(){
miguelcordero191 1:7c424a3e12ea 191
miguelcordero191 1:7c424a3e12ea 192 if (not this->isValidData)
miguelcordero191 1:7c424a3e12ea 193 return 0;
miguelcordero191 1:7c424a3e12ea 194
miguelcordero191 0:b444ea725ba7 195 return this->id_dev;
miguelcordero191 0:b444ea725ba7 196 }
miguelcordero191 0:b444ea725ba7 197
miguelcordero191 1:7c424a3e12ea 198 unsigned short IpData::getCmd(){
miguelcordero191 1:7c424a3e12ea 199
miguelcordero191 1:7c424a3e12ea 200 if (not this->isValidData)
miguelcordero191 1:7c424a3e12ea 201 return 0;
miguelcordero191 1:7c424a3e12ea 202
miguelcordero191 0:b444ea725ba7 203 return this->cmd;
miguelcordero191 0:b444ea725ba7 204 }
miguelcordero191 0:b444ea725ba7 205
miguelcordero191 0:b444ea725ba7 206 unsigned long IpData::getPayloadLen(){
miguelcordero191 1:7c424a3e12ea 207
miguelcordero191 1:7c424a3e12ea 208 if (not this->isValidData)
miguelcordero191 1:7c424a3e12ea 209 return 0;
miguelcordero191 1:7c424a3e12ea 210
miguelcordero191 1:7c424a3e12ea 211 return this->len_payload;
miguelcordero191 0:b444ea725ba7 212 }
miguelcordero191 0:b444ea725ba7 213
miguelcordero191 0:b444ea725ba7 214 char* IpData::getPayload(){
miguelcordero191 0:b444ea725ba7 215
miguelcordero191 1:7c424a3e12ea 216 if (not this->isValidData)
miguelcordero191 1:7c424a3e12ea 217 return NULL;
miguelcordero191 1:7c424a3e12ea 218
miguelcordero191 0:b444ea725ba7 219 return this->payload;
miguelcordero191 1:7c424a3e12ea 220 }
miguelcordero191 1:7c424a3e12ea 221
miguelcordero191 1:7c424a3e12ea 222 char* IpData::getTxData(){
miguelcordero191 1:7c424a3e12ea 223
miguelcordero191 1:7c424a3e12ea 224 return this->tx_buff;
miguelcordero191 1:7c424a3e12ea 225 }
miguelcordero191 1:7c424a3e12ea 226 unsigned long IpData::getTxDataLen(){
miguelcordero191 1:7c424a3e12ea 227
miguelcordero191 1:7c424a3e12ea 228 return this->len_tx_buffer;
miguelcordero191 0:b444ea725ba7 229 }