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