00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <assert.h>
00038 #include <iostream>
00039 #include <fstream>
00040 #include <memory>
00041
00042 #include <string.h>
00043 #include <stdlib.h>
00044
00045 #include <ros/time.h>
00046
00047 #include <wge100_camera/ipcam_packet.h>
00048 #include <wge100_camera/host_netutil.h>
00049 #include <wge100_camera/wge100lib.h>
00050
00051 #include <boost/format.hpp>
00052
00053
00054
00055 #define FLASH_SIZE (FLASH_PAGE_SIZE * (FLASH_MAX_PAGENO + 1) / 2)
00056 uint8_t firmware[FLASH_SIZE];
00057 int firmwarelen = 0;
00058
00059 int hexval(char c)
00060 {
00061 assert(isxdigit(c));
00062
00063 if (isdigit(c))
00064 return c - '0';
00065 if (isupper(c))
00066 return c - 'A' + 10;
00067 if (islower(c))
00068 return c - 'a' + 10;
00069
00070 return 0;
00071 }
00072
00073 int read_mcs(std::string fname)
00074 {
00075 memset(firmware, 0xFF, FLASH_SIZE);
00076 firmwarelen = 0;
00077 std::ifstream file;
00078 file.open(fname.c_str());
00079 std::auto_ptr<std::vector<uint8_t> > data;
00080 int curseg = -1;
00081 int linenum = 0;
00082
00083 if (!file.is_open())
00084 {
00085 fprintf(stderr, "Failed to open file %s\n", fname.c_str());
00086 return -1;
00087 }
00088
00089 while (!file.eof())
00090 {
00091 linenum++;
00092 char line[100];
00093 file.getline(line, sizeof(line));
00094 unsigned char *curchar = (unsigned char *)line;
00095
00096 if (*curchar++ != ':')
00097 {
00098 fprintf(stderr, "Line %i did not start with a colon.\n", linenum);
00099 return -1;
00100 }
00101
00102
00103 int bytes[100];
00104 int bytecount = 0;
00105 unsigned char checksum = 0;
00106
00107 while (isxdigit(curchar[0]) && isxdigit(curchar[1]))
00108 {
00109 bytes[bytecount] = 16 * hexval(*curchar++);
00110 bytes[bytecount] += hexval(*curchar++);
00111 checksum += bytes[bytecount++];
00112 }
00113
00114 while (*curchar && isspace(*curchar))
00115 curchar++;
00116
00117 if (*curchar)
00118 {
00119 fprintf(stderr, "Unexpected character 0x%02X at end of line %i.\n",
00120 (int) *curchar, linenum);
00121 return -1;
00122 }
00123
00124
00125
00126
00127
00128 if (bytecount < 5)
00129 {
00130 fprintf(stderr, "Line %i was too short (%i < 5).\n", linenum, bytecount);
00131 return -1;
00132 }
00133
00134 int len = bytes[0];
00135
00136 if (len != bytecount - 5)
00137 {
00138 fprintf(stderr, "Line %i contained %i bytes, but reported %i bytes.\n",
00139 linenum, bytecount, bytes[0]);
00140 return -1;
00141 }
00142
00143 int addr = (bytes[1] << 8) + bytes[2];
00144
00145 switch (bytes[3])
00146 {
00147 case 0:
00148 if (curseg + addr != firmwarelen)
00149 {
00150 fprintf(stderr, "Non contiguous address (%08x != %08x), line %i\n",
00151 curseg + addr, firmwarelen, linenum);
00152 return -1;
00153 }
00154 if (curseg + addr + len >= FLASH_SIZE)
00155 {
00156 fprintf(stderr, "Exceeded reserved flash size (note: upload_mcs code can be edited to allow larger values) at line %i.\n", linenum);
00157 return -1;
00158 }
00159 for (int i = 0; i < len; i++)
00160 firmware[firmwarelen++] = bytes[i + 4];
00161 break;
00162
00163 case 1:
00164 while (true)
00165 {
00166 char c;
00167 file.get(c);
00168 if (file.eof())
00169 return 0;
00170 if (!isspace(c))
00171 {
00172 fprintf(stderr, "EOF record on line %i was followed by character %02X.\n",
00173 linenum, (int) c);
00174 return -1;
00175 }
00176 }
00177
00178 case 4:
00179 if (len != 2)
00180 {
00181 fprintf(stderr, "Extended record had wrong length (%i != 2) on line %i\n",
00182 len, linenum);
00183 return -1;
00184 }
00185 curseg = ((bytes[4] << 8) + bytes[5]) << 16;
00186 break;
00187
00188 default:
00189 fprintf(stderr, "Unknown record type %i at line %i.\n", bytes[3], linenum);
00190 return -1;
00191 }
00192 }
00193
00194 fprintf(stderr, "Unexpected EOF after line %i.\n", linenum);
00195 return -1;
00196 }
00197
00198
00199 void bitswap()
00200 {
00201
00202 uint8_t swapped[256];
00203
00204 for (int i = 0; i < 256; i++)
00205 {
00206 uint8_t shift = i;
00207 swapped[i] = 0;
00208 for (int j = 0; j < 8; j++)
00209 {
00210 swapped[i] = (swapped[i] << 1) | (shift & 1);
00211 shift >>= 1;
00212 }
00213 }
00214
00215
00216 for (int i = 0; i < FLASH_SIZE; i++)
00217 firmware[i] = swapped[firmware[i]];
00218 }
00219
00220 int write_flash(char *camera_url)
00221 {
00222 IpCamList camera;
00223 const char *errmsg;
00224
00225 fprintf(stderr, "Searching for camera: %s", camera_url);
00226 for (int i = 0; i < 5; i++)
00227 {
00228 fprintf(stderr, ".");
00229 int outval = wge100FindByUrl(camera_url, &camera, SEC_TO_USEC(0.1), &errmsg);
00230 if (!outval)
00231 {
00232 fprintf(stderr, "\n");
00233 goto found;
00234 }
00235 sleep(1);
00236 }
00237
00238 fprintf(stderr, "\nMatching URL %s : %s\n", camera_url, errmsg);
00239 return -1;
00240 found:
00241
00242
00243 int retval = wge100Configure(&camera, camera.ip_str, SEC_TO_USEC(0.5));
00244 if (retval != 0) {
00245 if (retval == ERR_CONFIG_ARPFAIL) {
00246 fprintf(stderr, "Unable to create ARP entry (are you root?), continuing anyway\n");
00247 } else {
00248 fprintf(stderr, "IP address configuration failed\n");
00249 return -1;
00250 }
00251 }
00252
00253 if ( wge100TriggerControl( &camera, TRIG_STATE_INTERNAL ) != 0) {
00254 fprintf(stderr, "Could not communicate with camera after configuring IP. Is ARP set? Is %s accessible from %s?\n", camera.ip_str, camera.ifName);
00255 return -1;
00256 }
00257
00258 fprintf(stderr, "******************************************************************\n");
00259 fprintf(stderr, "Firmware file parsed successfully. Will start writing to flash in 5 seconds.\n");
00260 fprintf(stderr, "Make sure that no other software is accessing the camera during reflashing.\n");
00261 fprintf(stderr, "Hit CTRL+C to abort now.\n");
00262 fprintf(stderr, "******************************************************************\n");
00263 sleep(5);
00264
00265 fprintf(stderr, "Writing to flash. DO NOT ABORT OR TURN OFF CAMERA!!\n");
00266
00267 unsigned char buff[FLASH_PAGE_SIZE];
00268
00269 for (int page = 0; page < (firmwarelen + FLASH_PAGE_SIZE - 1) / FLASH_PAGE_SIZE;
00270 page++)
00271 {
00272 if (page % 100 == 0)
00273 {
00274 fprintf(stderr, ".");
00275 fflush(stderr);
00276 }
00277
00278 int addr = page * FLASH_PAGE_SIZE;
00279 int startretries = 10;
00280 int retries = startretries;
00281
00282 if (wge100ReliableFlashWrite(&camera, page, (uint8_t *) firmware + addr, &retries) != 0)
00283 {
00284 fprintf(stderr, "\nFlash write error on page %i.\n", page);
00285 fprintf(stderr, "If you reset the camera it will probably not come up.\n");
00286 fprintf(stderr, "Try reflashing NOW, to possibly avoid a hard JTAG reflash.\n");
00287 fprintf(stderr, "Be sure that you are not streaming images. Reflash will not\n");
00288 fprintf(stderr, "work while streaming.\n");
00289 return -2;
00290 }
00291
00292 if (retries < startretries)
00293 {
00294 fprintf(stderr, "x(%i)\n", startretries - retries);
00295 }
00296 }
00297
00298 fprintf(stderr, "\n");
00299
00300 fprintf(stderr, "Verifying flash.\n");
00301
00302 for (int page = 0; page < (firmwarelen + FLASH_PAGE_SIZE - 1) / FLASH_PAGE_SIZE;
00303 page++)
00304 {
00305 int addr = page * FLASH_PAGE_SIZE;
00306 if (page % 100 == 0)
00307 {
00308 fprintf(stderr, ".");
00309 fflush(stderr);
00310 }
00311
00312 if (wge100ReliableFlashRead(&camera, page, (uint8_t *) buff, NULL) != 0)
00313 {
00314 fprintf(stderr, "\nFlash read error on page %i.\n", page);
00315 fprintf(stderr, "If you reset the camera it will probably not come up.\n");
00316 fprintf(stderr, "Try reflashing NOW, to possibly avoid a hard JTAG reflash.\n");
00317 return -2;
00318 }
00319
00320 if (memcmp(buff, (uint8_t *) firmware + addr, FLASH_PAGE_SIZE))
00321 {
00322 fprintf(stderr, "\nFlash compare error on page %i.\n", page);
00323 fprintf(stderr, "If you reset the camera it will probably not come up.\n");
00324 fprintf(stderr, "Try reflashing NOW, to possibly avoid a hard JTAG reflash.\n");
00325 return -3;
00326 }
00327 }
00328 fprintf(stderr, "\n");
00329
00330 fprintf(stderr, "Success! Restarting camera, should take about 10 seconds to come back up after this.\n");
00331
00332 wge100ReconfigureFPGA(&camera);
00333
00334 return 0;
00335 }
00336
00337 int main(int argc, char **argv)
00338 {
00339 if ((argc != 3 && argc != 2) || !strcmp(argv[1], "--help")) {
00340 fprintf(stderr, "Usage: %s <file.mcs> <camera_url>\n", argv[0]);
00341 fprintf(stderr, " %s <file.mcs> > dump.bin\n", argv[0]);
00342 fprintf(stderr, "Reads a .mcs file and uploads it to the camera or dumps it as binary to stdout.\n");
00343 return -1;
00344 }
00345
00346 if (read_mcs(argv[1]))
00347 return -1;
00348
00349 if (firmware[4] == 0x55)
00350 bitswap();
00351 else if (firmware[4] != 0xaa)
00352 {
00353 fprintf(stderr, "Unexpected value at position 4. Don't know whether to bit-swap.\n");
00354 return -1;
00355 }
00356
00357 if (argc == 2)
00358 {
00359 fwrite(firmware, FLASH_SIZE, 1, stdout);
00360 return 0;
00361 }
00362
00363 char* camera_url = argv[2];
00364
00365 int retval = 1;
00366 const int max_retries = 5;
00367 for (int i = 0; i < max_retries; i++)
00368 {
00369 retval = -write_flash(camera_url);
00370 if (!retval)
00371 break;
00372 fprintf(stderr, "\nUpload failed. Retrying... %u/%u\n\n", i+1, max_retries);
00373 sleep(1);
00374 }
00375
00376 return retval;
00377 }