Stephen Davis
/
BT_Hub
Library set up as dummy module on mbed to mimic Nordic.
packetformatter.cpp@2:9ab591cf81b8, 2016-12-13 (annotated)
- Committer:
- Stephen_NewVistas
- Date:
- Tue Dec 13 00:20:29 2016 +0000
- Revision:
- 2:9ab591cf81b8
- Parent:
- 1:d6b18299a715
everything commented . now going to strip out mbed stuff.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Stephen_NewVistas | 0:226550611f0d | 1 | #include <stdio.h> |
Stephen_NewVistas | 0:226550611f0d | 2 | #include <string.h> |
Stephen_NewVistas | 0:226550611f0d | 3 | #include "packetformatter.h" |
Stephen_NewVistas | 0:226550611f0d | 4 | #include "charactercode.h" |
Stephen_NewVistas | 0:226550611f0d | 5 | //#define DEBUG |
Stephen_NewVistas | 0:226550611f0d | 6 | |
Stephen_NewVistas | 0:226550611f0d | 7 | static unsigned int checkSum; |
Stephen_NewVistas | 0:226550611f0d | 8 | static unsigned int packetSize; |
Stephen_NewVistas | 0:226550611f0d | 9 | static unsigned char buffer[1024]; |
Stephen_NewVistas | 0:226550611f0d | 10 | |
Stephen_NewVistas | 0:226550611f0d | 11 | /****************************************************************************** |
Stephen_NewVistas | 0:226550611f0d | 12 | * Function ConvertToPacket( Packet *_packet , unsigned char *_buffer ) |
Stephen_NewVistas | 0:226550611f0d | 13 | * |
Stephen_NewVistas | 0:226550611f0d | 14 | * This function is passed an empty packet a buffer with a message. The message |
Stephen_NewVistas | 0:226550611f0d | 15 | * is broken down and placed into the packet. |
Stephen_NewVistas | 0:226550611f0d | 16 | * |
Stephen_NewVistas | 0:226550611f0d | 17 | * |
Stephen_NewVistas | 0:226550611f0d | 18 | * PreCondition: None |
Stephen_NewVistas | 0:226550611f0d | 19 | * |
Stephen_NewVistas | 0:226550611f0d | 20 | * Input: '_packet' - empty packet , '_buffer' - message |
Stephen_NewVistas | 0:226550611f0d | 21 | * |
Stephen_NewVistas | 0:226550611f0d | 22 | * Output: |
Stephen_NewVistas | 0:226550611f0d | 23 | * |
Stephen_NewVistas | 0:226550611f0d | 24 | * Side Effects: None |
Stephen_NewVistas | 0:226550611f0d | 25 | * |
Stephen_NewVistas | 0:226550611f0d | 26 | *****************************************************************************/ |
Stephen_NewVistas | 0:226550611f0d | 27 | int ConvertToPacket( Packet *_packet , unsigned char *_buffer ){ |
Stephen_NewVistas | 0:226550611f0d | 28 | memset( _packet , '\0' , sizeof( Packet ) ); |
Stephen_NewVistas | 0:226550611f0d | 29 | |
Stephen_NewVistas | 0:226550611f0d | 30 | _packet->deviceID = (int)_buffer[1] ; |
Stephen_NewVistas | 1:d6b18299a715 | 31 | _packet->sourceID = (int)_buffer[2] ; |
Stephen_NewVistas | 0:226550611f0d | 32 | _packet->command = _buffer[3]; |
Stephen_NewVistas | 0:226550611f0d | 33 | _packet->packetLength = _buffer[4]; |
Stephen_NewVistas | 0:226550611f0d | 34 | _packet->packetLength = _packet->packetLength << 8; |
Stephen_NewVistas | 0:226550611f0d | 35 | _packet->packetLength |= _buffer[5]; |
Stephen_NewVistas | 0:226550611f0d | 36 | |
Stephen_NewVistas | 0:226550611f0d | 37 | int i; |
Stephen_NewVistas | 0:226550611f0d | 38 | for( i = 0 ; i < _packet->packetLength ; i++ ){ |
Stephen_NewVistas | 0:226550611f0d | 39 | _packet->packetData[i] = *( _buffer + i + 6 ); |
Stephen_NewVistas | 0:226550611f0d | 40 | } |
Stephen_NewVistas | 0:226550611f0d | 41 | return 0; |
Stephen_NewVistas | 0:226550611f0d | 42 | } |
Stephen_NewVistas | 0:226550611f0d | 43 | |
Stephen_NewVistas | 0:226550611f0d | 44 | /****************************************************************************** |
Stephen_NewVistas | 0:226550611f0d | 45 | * Function unsigned short escapedToSpecialChar(unsigned char) |
Stephen_NewVistas | 0:226550611f0d | 46 | * |
Stephen_NewVistas | 0:226550611f0d | 47 | * This function is called to convert a data if it is a special character (0x7F, 0x8F, 0x8E) |
Stephen_NewVistas | 0:226550611f0d | 48 | * to equivalent escaped character. |
Stephen_NewVistas | 0:226550611f0d | 49 | * |
Stephen_NewVistas | 0:226550611f0d | 50 | * e.g. Special Character Escaped Character |
Stephen_NewVistas | 0:226550611f0d | 51 | * 0x7F = 0x8E 0x01 |
Stephen_NewVistas | 0:226550611f0d | 52 | * 0x8F = 0x8E 0x02 |
Stephen_NewVistas | 0:226550611f0d | 53 | * 0x8E = 0x8E 0x8E |
Stephen_NewVistas | 0:226550611f0d | 54 | * |
Stephen_NewVistas | 0:226550611f0d | 55 | * PreCondition: None |
Stephen_NewVistas | 0:226550611f0d | 56 | * |
Stephen_NewVistas | 0:226550611f0d | 57 | * Input: 'c' - character to convert if it is an special character. |
Stephen_NewVistas | 0:226550611f0d | 58 | * |
Stephen_NewVistas | 0:226550611f0d | 59 | * Output: Equivalent escaped character |
Stephen_NewVistas | 0:226550611f0d | 60 | * |
Stephen_NewVistas | 0:226550611f0d | 61 | * Side Effects: None |
Stephen_NewVistas | 0:226550611f0d | 62 | * |
Stephen_NewVistas | 0:226550611f0d | 63 | *****************************************************************************/ |
Stephen_NewVistas | 0:226550611f0d | 64 | unsigned short |
Stephen_NewVistas | 0:226550611f0d | 65 | specialToEscapedChar(unsigned char c) |
Stephen_NewVistas | 0:226550611f0d | 66 | { |
Stephen_NewVistas | 0:226550611f0d | 67 | |
Stephen_NewVistas | 0:226550611f0d | 68 | unsigned short escapedChar = 0; |
Stephen_NewVistas | 0:226550611f0d | 69 | char isSpecialChar = ((STARTPOLL == c) || (STARTPACKET == c) || (FLAGBYTE == c)); |
Stephen_NewVistas | 0:226550611f0d | 70 | |
Stephen_NewVistas | 0:226550611f0d | 71 | if(isSpecialChar) |
Stephen_NewVistas | 0:226550611f0d | 72 | { |
Stephen_NewVistas | 0:226550611f0d | 73 | escapedChar = (FLAGBYTE << 8); |
Stephen_NewVistas | 0:226550611f0d | 74 | switch(c) |
Stephen_NewVistas | 0:226550611f0d | 75 | { |
Stephen_NewVistas | 0:226550611f0d | 76 | case STARTPACKET: |
Stephen_NewVistas | 0:226550611f0d | 77 | escapedChar |= 0x01; |
Stephen_NewVistas | 0:226550611f0d | 78 | break; |
Stephen_NewVistas | 0:226550611f0d | 79 | case STARTPOLL: |
Stephen_NewVistas | 0:226550611f0d | 80 | escapedChar |= 0x02; |
Stephen_NewVistas | 0:226550611f0d | 81 | break; |
Stephen_NewVistas | 0:226550611f0d | 82 | case FLAGBYTE: |
Stephen_NewVistas | 0:226550611f0d | 83 | escapedChar |= FLAGBYTE; |
Stephen_NewVistas | 0:226550611f0d | 84 | break; |
Stephen_NewVistas | 0:226550611f0d | 85 | } |
Stephen_NewVistas | 0:226550611f0d | 86 | } |
Stephen_NewVistas | 0:226550611f0d | 87 | else |
Stephen_NewVistas | 0:226550611f0d | 88 | { |
Stephen_NewVistas | 0:226550611f0d | 89 | escapedChar = (unsigned char) c; |
Stephen_NewVistas | 0:226550611f0d | 90 | } |
Stephen_NewVistas | 0:226550611f0d | 91 | |
Stephen_NewVistas | 0:226550611f0d | 92 | return escapedChar; |
Stephen_NewVistas | 0:226550611f0d | 93 | } |
Stephen_NewVistas | 0:226550611f0d | 94 | |
Stephen_NewVistas | 0:226550611f0d | 95 | /****************************************************************************** |
Stephen_NewVistas | 0:226550611f0d | 96 | * Function static void insertValueToBuffer(unsigned char) |
Stephen_NewVistas | 0:226550611f0d | 97 | * |
Stephen_NewVistas | 0:226550611f0d | 98 | * This function is called to insert the value to buffer. |
Stephen_NewVistas | 0:226550611f0d | 99 | * |
Stephen_NewVistas | 0:226550611f0d | 100 | * PreCondition: None |
Stephen_NewVistas | 0:226550611f0d | 101 | * |
Stephen_NewVistas | 0:226550611f0d | 102 | * Input: 'value' - value to insert in the buffer. |
Stephen_NewVistas | 0:226550611f0d | 103 | * |
Stephen_NewVistas | 0:226550611f0d | 104 | * Output: None |
Stephen_NewVistas | 0:226550611f0d | 105 | * |
Stephen_NewVistas | 0:226550611f0d | 106 | * Side Effects: Increment the packetSize variable |
Stephen_NewVistas | 0:226550611f0d | 107 | * |
Stephen_NewVistas | 0:226550611f0d | 108 | *****************************************************************************/ |
Stephen_NewVistas | 0:226550611f0d | 109 | static void |
Stephen_NewVistas | 0:226550611f0d | 110 | insertValueToBuffer(unsigned char value) |
Stephen_NewVistas | 0:226550611f0d | 111 | { |
Stephen_NewVistas | 0:226550611f0d | 112 | buffer[packetSize] = value; |
Stephen_NewVistas | 0:226550611f0d | 113 | packetSize++; |
Stephen_NewVistas | 0:226550611f0d | 114 | } |
Stephen_NewVistas | 0:226550611f0d | 115 | |
Stephen_NewVistas | 0:226550611f0d | 116 | /****************************************************************************** |
Stephen_NewVistas | 0:226550611f0d | 117 | * Function static void processPacketData(unsigned char) |
Stephen_NewVistas | 0:226550611f0d | 118 | * |
Stephen_NewVistas | 0:226550611f0d | 119 | * This function is called to process the packet if it is a special character it will |
Stephen_NewVistas | 0:226550611f0d | 120 | * convert to escaped character e.g. 0x7F = 0x8E 0x01, 0x8E = 0x8E 0x8E, 0x8F = 0x8E 0x02. |
Stephen_NewVistas | 0:226550611f0d | 121 | * The size of return value of specialToEscapedChar function is 2 bytes so it must be inserted |
Stephen_NewVistas | 0:226550611f0d | 122 | * in the buffer twice. |
Stephen_NewVistas | 0:226550611f0d | 123 | * |
Stephen_NewVistas | 0:226550611f0d | 124 | * PreCondition: None |
Stephen_NewVistas | 0:226550611f0d | 125 | * |
Stephen_NewVistas | 0:226550611f0d | 126 | * Input: 'packetData' - value to convert if it is a special character. |
Stephen_NewVistas | 0:226550611f0d | 127 | * |
Stephen_NewVistas | 0:226550611f0d | 128 | * Output: None |
Stephen_NewVistas | 0:226550611f0d | 129 | * |
Stephen_NewVistas | 0:226550611f0d | 130 | * Side Effects: None |
Stephen_NewVistas | 0:226550611f0d | 131 | * |
Stephen_NewVistas | 0:226550611f0d | 132 | *****************************************************************************/ |
Stephen_NewVistas | 0:226550611f0d | 133 | static void |
Stephen_NewVistas | 0:226550611f0d | 134 | processPacketData(unsigned char packetData) |
Stephen_NewVistas | 0:226550611f0d | 135 | { |
Stephen_NewVistas | 0:226550611f0d | 136 | |
Stephen_NewVistas | 0:226550611f0d | 137 | unsigned short packetDataResult = specialToEscapedChar(packetData); |
Stephen_NewVistas | 0:226550611f0d | 138 | |
Stephen_NewVistas | 0:226550611f0d | 139 | if( (packetDataResult >> 8) != 0) |
Stephen_NewVistas | 0:226550611f0d | 140 | { |
Stephen_NewVistas | 0:226550611f0d | 141 | insertValueToBuffer((unsigned char)(packetDataResult>> 8)); |
Stephen_NewVistas | 0:226550611f0d | 142 | } |
Stephen_NewVistas | 0:226550611f0d | 143 | |
Stephen_NewVistas | 0:226550611f0d | 144 | insertValueToBuffer((unsigned char) packetDataResult); |
Stephen_NewVistas | 0:226550611f0d | 145 | |
Stephen_NewVistas | 0:226550611f0d | 146 | } |
Stephen_NewVistas | 0:226550611f0d | 147 | |
Stephen_NewVistas | 0:226550611f0d | 148 | /****************************************************************************** |
Stephen_NewVistas | 0:226550611f0d | 149 | * Function void getFormattedPacket(Packet *, unsigned char *) |
Stephen_NewVistas | 0:226550611f0d | 150 | * |
Stephen_NewVistas | 0:226550611f0d | 151 | * This function is called to process the packet in to a series of bytes that |
Stephen_NewVistas | 0:226550611f0d | 152 | * conforms into the protocol of the communication. |
Stephen_NewVistas | 0:226550611f0d | 153 | * Protocol Format Example: |
Stephen_NewVistas | 0:226550611f0d | 154 | * 0x7F - Start Packet |
Stephen_NewVistas | 0:226550611f0d | 155 | * 0xFE - Device ID |
Stephen_NewVistas | 0:226550611f0d | 156 | * 0x01 - Source ID |
Stephen_NewVistas | 0:226550611f0d | 157 | * 0x05 - Command type |
Stephen_NewVistas | 0:226550611f0d | 158 | * 0x00 - Payload length 1st byte |
Stephen_NewVistas | 0:226550611f0d | 159 | * 0x01 - Payload length 2nd byte |
Stephen_NewVistas | 0:226550611f0d | 160 | * 0x04 - Payload or data |
Stephen_NewVistas | 0:226550611f0d | 161 | * 0x76 - checksum |
Stephen_NewVistas | 0:226550611f0d | 162 | * |
Stephen_NewVistas | 0:226550611f0d | 163 | * |
Stephen_NewVistas | 0:226550611f0d | 164 | * PreCondition: None |
Stephen_NewVistas | 0:226550611f0d | 165 | * |
Stephen_NewVistas | 0:226550611f0d | 166 | * Input: 'packet' - instance of Packet data structure that contains the |
Stephen_NewVistas | 0:226550611f0d | 167 | * following information: |
Stephen_NewVistas | 0:226550611f0d | 168 | * + device id |
Stephen_NewVistas | 0:226550611f0d | 169 | * + source id |
Stephen_NewVistas | 0:226550611f0d | 170 | * + command |
Stephen_NewVistas | 0:226550611f0d | 171 | * + packet or payload length |
Stephen_NewVistas | 0:226550611f0d | 172 | * + packet or payload |
Stephen_NewVistas | 0:226550611f0d | 173 | * |
Stephen_NewVistas | 0:226550611f0d | 174 | * 'packetBuffer' - empty buffer that will be the holder of the process data or packet. |
Stephen_NewVistas | 0:226550611f0d | 175 | * |
Stephen_NewVistas | 0:226550611f0d | 176 | * Output: Total size of data inserted in the buffer. |
Stephen_NewVistas | 0:226550611f0d | 177 | * |
Stephen_NewVistas | 0:226550611f0d | 178 | * Side Effects: None |
Stephen_NewVistas | 0:226550611f0d | 179 | * |
Stephen_NewVistas | 0:226550611f0d | 180 | *****************************************************************************/ |
Stephen_NewVistas | 0:226550611f0d | 181 | int getFormattedPacket(Packet * packet, unsigned char * packetBuffer) |
Stephen_NewVistas | 0:226550611f0d | 182 | { |
Stephen_NewVistas | 0:226550611f0d | 183 | |
Stephen_NewVistas | 0:226550611f0d | 184 | checkSum = 0; |
Stephen_NewVistas | 0:226550611f0d | 185 | packetSize = 0; |
Stephen_NewVistas | 0:226550611f0d | 186 | |
Stephen_NewVistas | 0:226550611f0d | 187 | insertValueToBuffer(STARTPACKET); |
Stephen_NewVistas | 0:226550611f0d | 188 | checkSum += STARTPACKET; |
Stephen_NewVistas | 0:226550611f0d | 189 | |
Stephen_NewVistas | 0:226550611f0d | 190 | #ifdef DEBUG |
Stephen_NewVistas | 0:226550611f0d | 191 | printf("Start Packet: %x\n", STARTPACKET); |
Stephen_NewVistas | 0:226550611f0d | 192 | #endif |
Stephen_NewVistas | 0:226550611f0d | 193 | |
Stephen_NewVistas | 0:226550611f0d | 194 | processPacketData(packet->deviceID); |
Stephen_NewVistas | 0:226550611f0d | 195 | checkSum += packet->deviceID; |
Stephen_NewVistas | 0:226550611f0d | 196 | |
Stephen_NewVistas | 0:226550611f0d | 197 | #ifdef DEBUG |
Stephen_NewVistas | 0:226550611f0d | 198 | printf("Device ID: %x\n", packet->deviceID); |
Stephen_NewVistas | 0:226550611f0d | 199 | #endif |
Stephen_NewVistas | 0:226550611f0d | 200 | |
Stephen_NewVistas | 0:226550611f0d | 201 | processPacketData(packet->sourceID); |
Stephen_NewVistas | 0:226550611f0d | 202 | checkSum += packet->sourceID; |
Stephen_NewVistas | 0:226550611f0d | 203 | |
Stephen_NewVistas | 0:226550611f0d | 204 | #ifdef DEBUG |
Stephen_NewVistas | 0:226550611f0d | 205 | printf("Source ID: %x\n", packet->sourceID); |
Stephen_NewVistas | 0:226550611f0d | 206 | #endif |
Stephen_NewVistas | 0:226550611f0d | 207 | |
Stephen_NewVistas | 0:226550611f0d | 208 | processPacketData(packet->command); |
Stephen_NewVistas | 0:226550611f0d | 209 | checkSum += packet->command; |
Stephen_NewVistas | 0:226550611f0d | 210 | |
Stephen_NewVistas | 0:226550611f0d | 211 | #ifdef DEBUG |
Stephen_NewVistas | 0:226550611f0d | 212 | printf("Command: %x\n", packet->command); |
Stephen_NewVistas | 0:226550611f0d | 213 | #endif |
Stephen_NewVistas | 0:226550611f0d | 214 | |
Stephen_NewVistas | 0:226550611f0d | 215 | unsigned char mostSignificantBytePacketLength = packet->packetLength >> 8; |
Stephen_NewVistas | 0:226550611f0d | 216 | unsigned char leastSignificantBytePacketLength = (unsigned char) packet->packetLength; |
Stephen_NewVistas | 0:226550611f0d | 217 | processPacketData(mostSignificantBytePacketLength); |
Stephen_NewVistas | 0:226550611f0d | 218 | checkSum += mostSignificantBytePacketLength; |
Stephen_NewVistas | 0:226550611f0d | 219 | processPacketData(leastSignificantBytePacketLength); |
Stephen_NewVistas | 0:226550611f0d | 220 | checkSum += leastSignificantBytePacketLength; |
Stephen_NewVistas | 0:226550611f0d | 221 | |
Stephen_NewVistas | 0:226550611f0d | 222 | #ifdef DEBUG |
Stephen_NewVistas | 0:226550611f0d | 223 | printf("Packet length MSB: %x\n", mostSignificantBytePacketLength); |
Stephen_NewVistas | 0:226550611f0d | 224 | printf("Packet length LSB: %x\n", leastSignificantBytePacketLength); |
Stephen_NewVistas | 0:226550611f0d | 225 | #endif |
Stephen_NewVistas | 0:226550611f0d | 226 | |
Stephen_NewVistas | 0:226550611f0d | 227 | int i; |
Stephen_NewVistas | 0:226550611f0d | 228 | for(i = 0; i < packet->packetLength; i++) |
Stephen_NewVistas | 0:226550611f0d | 229 | { |
Stephen_NewVistas | 0:226550611f0d | 230 | processPacketData(packet->packetData[i]); |
Stephen_NewVistas | 0:226550611f0d | 231 | checkSum += packet->packetData[i]; |
Stephen_NewVistas | 0:226550611f0d | 232 | |
Stephen_NewVistas | 0:226550611f0d | 233 | #ifdef DEBUG |
Stephen_NewVistas | 0:226550611f0d | 234 | printf("Data %d: %x\n", i, packet->packetData[i]); |
Stephen_NewVistas | 0:226550611f0d | 235 | #endif |
Stephen_NewVistas | 0:226550611f0d | 236 | } |
Stephen_NewVistas | 0:226550611f0d | 237 | |
Stephen_NewVistas | 0:226550611f0d | 238 | unsigned char checksumLSB = (unsigned char)(checkSum & 0xFF); |
Stephen_NewVistas | 0:226550611f0d | 239 | unsigned char twosComplementCheckSum = ~checksumLSB + 1; |
Stephen_NewVistas | 0:226550611f0d | 240 | processPacketData(twosComplementCheckSum); |
Stephen_NewVistas | 0:226550611f0d | 241 | |
Stephen_NewVistas | 0:226550611f0d | 242 | #ifdef DEBUG |
Stephen_NewVistas | 0:226550611f0d | 243 | printf("Checksum: %x\n", twosComplementCheckSum); |
Stephen_NewVistas | 0:226550611f0d | 244 | #endif |
Stephen_NewVistas | 0:226550611f0d | 245 | |
Stephen_NewVistas | 0:226550611f0d | 246 | memcpy(packetBuffer, buffer, packetSize); |
Stephen_NewVistas | 0:226550611f0d | 247 | |
Stephen_NewVistas | 0:226550611f0d | 248 | return packetSize; |
Stephen_NewVistas | 0:226550611f0d | 249 | } |
Stephen_NewVistas | 0:226550611f0d | 250 | |
Stephen_NewVistas | 0:226550611f0d | 251 | /****************************************************************************** |
Stephen_NewVistas | 0:226550611f0d | 252 | * Function void getFormattedBootloaderPacket(unsigned char *, unsigned char *, int) |
Stephen_NewVistas | 0:226550611f0d | 253 | * |
Stephen_NewVistas | 0:226550611f0d | 254 | * This function is called to process the data in to a series of bytes that |
Stephen_NewVistas | 0:226550611f0d | 255 | * conforms into the bootloader protocol. |
Stephen_NewVistas | 0:226550611f0d | 256 | * Protocol Format Example: |
Stephen_NewVistas | 0:226550611f0d | 257 | * 0x55 - STX or Start of TeXt |
Stephen_NewVistas | 0:226550611f0d | 258 | * 0x55 - STX or Start of TeXt |
Stephen_NewVistas | 0:226550611f0d | 259 | * 0x01 - Data |
Stephen_NewVistas | 0:226550611f0d | 260 | * 0x03 - Data |
Stephen_NewVistas | 0:226550611f0d | 261 | * 0x0C - Checksum |
Stephen_NewVistas | 0:226550611f0d | 262 | * 0x04 - ETX or End of TeXt |
Stephen_NewVistas | 0:226550611f0d | 263 | * |
Stephen_NewVistas | 0:226550611f0d | 264 | * |
Stephen_NewVistas | 0:226550611f0d | 265 | * PreCondition: None |
Stephen_NewVistas | 0:226550611f0d | 266 | * |
Stephen_NewVistas | 0:226550611f0d | 267 | * Input: |
Stephen_NewVistas | 0:226550611f0d | 268 | * 'buffer' - empty buffer that will be the holder of the process. |
Stephen_NewVistas | 0:226550611f0d | 269 | * 'data' - values to be send in bootloader. |
Stephen_NewVistas | 0:226550611f0d | 270 | * 'size' - size of data to be send. |
Stephen_NewVistas | 0:226550611f0d | 271 | * |
Stephen_NewVistas | 0:226550611f0d | 272 | * Output: Total size of data inserted in the buffer. |
Stephen_NewVistas | 0:226550611f0d | 273 | * |
Stephen_NewVistas | 0:226550611f0d | 274 | * Side Effects: None |
Stephen_NewVistas | 0:226550611f0d | 275 | * |
Stephen_NewVistas | 0:226550611f0d | 276 | *****************************************************************************/ |
Stephen_NewVistas | 0:226550611f0d | 277 | int |
Stephen_NewVistas | 0:226550611f0d | 278 | getFormattedBootloaderPacket(unsigned char *buffer, unsigned char *data, int size) |
Stephen_NewVistas | 0:226550611f0d | 279 | { |
Stephen_NewVistas | 0:226550611f0d | 280 | int i; |
Stephen_NewVistas | 0:226550611f0d | 281 | int j; |
Stephen_NewVistas | 0:226550611f0d | 282 | int sum; |
Stephen_NewVistas | 0:226550611f0d | 283 | |
Stephen_NewVistas | 0:226550611f0d | 284 | i = 0; |
Stephen_NewVistas | 0:226550611f0d | 285 | buffer[i++] = STX; |
Stephen_NewVistas | 0:226550611f0d | 286 | buffer[i++] = STX; |
Stephen_NewVistas | 0:226550611f0d | 287 | sum = 0; |
Stephen_NewVistas | 0:226550611f0d | 288 | |
Stephen_NewVistas | 0:226550611f0d | 289 | for(j = 0; j < size; j++) |
Stephen_NewVistas | 0:226550611f0d | 290 | { |
Stephen_NewVistas | 0:226550611f0d | 291 | if(data[j] == ETX || data[j] == STX || data[j] == ENQ) |
Stephen_NewVistas | 0:226550611f0d | 292 | { |
Stephen_NewVistas | 0:226550611f0d | 293 | buffer[j+i++] = ENQ; |
Stephen_NewVistas | 0:226550611f0d | 294 | } |
Stephen_NewVistas | 0:226550611f0d | 295 | |
Stephen_NewVistas | 0:226550611f0d | 296 | buffer[j+i] = data[j]; |
Stephen_NewVistas | 0:226550611f0d | 297 | sum += data[j]; |
Stephen_NewVistas | 0:226550611f0d | 298 | } |
Stephen_NewVistas | 0:226550611f0d | 299 | |
Stephen_NewVistas | 0:226550611f0d | 300 | sum = ((sum * -1) & 0xFF); |
Stephen_NewVistas | 0:226550611f0d | 301 | if (sum == ETX || sum == STX || sum == ENQ) |
Stephen_NewVistas | 0:226550611f0d | 302 | buffer[j+i++] = ENQ; |
Stephen_NewVistas | 0:226550611f0d | 303 | |
Stephen_NewVistas | 0:226550611f0d | 304 | buffer[j+i] = sum; |
Stephen_NewVistas | 0:226550611f0d | 305 | j++; |
Stephen_NewVistas | 0:226550611f0d | 306 | buffer[j+i] = ETX; |
Stephen_NewVistas | 0:226550611f0d | 307 | j++; |
Stephen_NewVistas | 0:226550611f0d | 308 | |
Stephen_NewVistas | 0:226550611f0d | 309 | //printf("getFormattedBootloaderPacket return = %d\n",j+i); |
Stephen_NewVistas | 0:226550611f0d | 310 | |
Stephen_NewVistas | 0:226550611f0d | 311 | return (j+i); |
Stephen_NewVistas | 0:226550611f0d | 312 | } |
Stephen_NewVistas | 0:226550611f0d | 313 | |
Stephen_NewVistas | 0:226550611f0d | 314 |