upload_mcs.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009-2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 // Author: Blaise Gassend
00036 
00037 #include <assert.h>
00038 #include <iostream>
00039 #include <fstream>
00040 #include <memory> // for std::auto_ptr
00041  
00042 #include <string.h> // for memset(3)
00043 #include <stdlib.h> // for atoi(3)
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 // We are assuming that the firmware will fit in the first half of the
00054 // pages. This may not turn out to be true later...
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; // This will work for the forearm camera, but will break for larger address spaces.
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     // Declare bytes as int to avoid casting later.
00103     int bytes[100]; // Big enough because the line is only 100 characters. 
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     /*for (int i = 0; i < bytecount; i++)
00125       printf("%02X ", bytes[i]);
00126     printf("\n");*/
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: // Data
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: // EOF
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: // Extended address record
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 // For a serial flash, the bits in each byte need to be swapped around.
00199 void bitswap()
00200 {
00201   // Precompute bit-swap table
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   // Bit-swap the whole firmware.
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   // Configure the camera with its IP address, wait up to 500ms for completion
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 }


wge100_camera
Author(s): Blaise Gassend, Patrick Mihelich, Eric MacIntosh, David Palchak
autogenerated on Sat Jun 8 2019 20:51:24