BootloaderInterface.cpp
Go to the documentation of this file.
00001 /*
00002         Aseba - an event-based framework for distributed robot control
00003         Copyright (C) 2007--2012:
00004                 Stephane Magnenat <stephane at magnenat dot net>
00005                 (http://stephane.magnenat.net)
00006                 and other contributors, see authors.txt for details
00007         
00008         This program is free software: you can redistribute it and/or modify
00009         it under the terms of the GNU Lesser General Public License as published
00010         by the Free Software Foundation, version 3 of the License.
00011         
00012         This program is distributed in the hope that it will be useful,
00013         but WITHOUT ANY WARRANTY; without even the implied warranty of
00014         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015         GNU Lesser General Public License for more details.
00016         
00017         You should have received a copy of the GNU Lesser General Public License
00018         along with this program. If not, see <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 #include "BootloaderInterface.h"
00022 #include "../common/consts.h"
00023 #include "../msg/msg.h"
00024 #include "utils.h"
00025 #include "HexFile.h"
00026 #include "FormatableString.h"
00027 #include <dashel/dashel.h>
00028 #include <memory>
00029 #include <unistd.h>
00030 
00031 namespace Aseba 
00032 {
00033         using namespace Dashel;
00034         using namespace std;
00035         
00036         BootloaderInterface::BootloaderInterface(Stream* stream, int dest) :
00037                 stream(stream),
00038                 dest(dest),
00039                 pageSize(0),
00040                 pagesStart(0),
00041                 pagesCount(0)
00042         {
00043                 
00044         }
00045         
00046         bool BootloaderInterface::readPage(unsigned pageNumber, uint8* data)
00047         {
00048                 if ((pageNumber < pagesStart) || (pageNumber >= pagesStart + pagesCount))
00049                 {
00050                         throw Error(FormatableString("Error, page index %0 out of page range [%1:%2]").arg(pageNumber).arg(pagesStart).arg(pagesStart+pagesCount));
00051                 }
00052                 
00053                 // send command
00054                 BootloaderReadPage message;
00055                 message.dest = dest;
00056                 message.pageNumber = pageNumber;
00057                 message.serialize(stream);
00058                 stream->flush();
00059                 unsigned dataRead = 0;
00060                 
00061                 // get data
00062                 while (true)
00063                 {
00064                         auto_ptr<Message> message(Message::receive(stream));
00065                         
00066                         // handle ack
00067                         BootloaderAck *ackMessage = dynamic_cast<BootloaderAck *>(message.get());
00068                         if (ackMessage && (ackMessage->source == dest))
00069                         {
00070                                 uint16 errorCode = ackMessage->errorCode;
00071                                 if (errorCode == BootloaderAck::SUCCESS)
00072                                 {
00073                                         if (dataRead < pageSize)
00074                                                 cerr << "Warning, got acknowledgement but page not fully read (" << dataRead << "/" << pageSize << ") bytes.\n";
00075                                         return true;
00076                                 }
00077                                 else
00078                                         return false;
00079                         }
00080                         
00081                         // handle data
00082                         BootloaderDataRead *dataMessage = dynamic_cast<BootloaderDataRead *>(message.get());
00083                         if (dataMessage && (dataMessage->source == dest))
00084                         {
00085                                 if (dataRead >= pageSize)
00086                                         cerr << "Warning, reading oversized page (" << dataRead << "/" << pageSize << ") bytes.\n";
00087                                 copy(dataMessage->data, dataMessage->data + sizeof(dataMessage->data), data);
00088                                 data += sizeof(dataMessage->data);
00089                                 dataRead += sizeof(dataMessage->data);
00090                                 cout << "Page read so far (" << dataRead << "/" << pageSize << ") bytes.\n";
00091                         }
00092                 }
00093                 
00094                 return true;
00095         }
00096         
00097         bool BootloaderInterface::readPageSimple(unsigned pageNumber, uint8 * data)
00098         {
00099                 BootloaderReadPage message;
00100                 message.dest = dest;
00101                 message.pageNumber = pageNumber;
00102                 message.serialize(stream);
00103                 stream->flush();
00104                 
00105                 stream->read(data, 2048);
00106                 return true;
00107         }
00108         
00109         bool BootloaderInterface::writePage(unsigned pageNumber, const uint8 *data, bool simple)
00110         {
00111                 writePageStart(pageNumber, data, simple);
00112                 
00113                 // send command
00114                 BootloaderWritePage writePage;
00115                 writePage.dest = dest;
00116                 writePage.pageNumber = pageNumber;
00117                 writePage.serialize(stream);
00118                 
00119                 if (simple)
00120                 {
00121                         // just write the complete page at ounce
00122                         stream->write(data,pageSize);
00123                 }
00124                 else
00125                 {
00126                         // flush command
00127                         stream->flush();
00128                         
00129                         // wait ACK
00130                         while (true)
00131                         {
00132                                 auto_ptr<Message> message(Message::receive(stream));
00133                                 
00134                                 // handle ack
00135                                 BootloaderAck *ackMessage = dynamic_cast<BootloaderAck *>(message.get());
00136                                 if (ackMessage && (ackMessage->source == dest))
00137                                 {
00138                                         uint16 errorCode = ackMessage->errorCode;
00139                                         if(errorCode == BootloaderAck::SUCCESS)
00140                                                 break;
00141                                         else
00142                                                 return false;
00143                                 }
00144                         }
00145                         
00146                         // write data
00147                         for (unsigned dataWritten = 0; dataWritten < pageSize;)
00148                         {
00149                                 BootloaderPageDataWrite pageData;
00150                                 pageData.dest = dest;
00151                                 copy(data + dataWritten, data + dataWritten + sizeof(pageData.data), pageData.data);
00152                                 pageData.serialize(stream);
00153                                 dataWritten += sizeof(pageData.data);
00154                                 //cout << "." << std::flush;
00155                                 
00156                                 /*
00157                                 while (true)
00158                                 {
00159                                         auto_ptr<Message> message(Message::receive(stream));
00160                                         
00161                                         // handle ack
00162                                         BootloaderAck *ackMessage = dynamic_cast<BootloaderAck *>(message);
00163                                         if (ackMessage && (ackMessage->source == dest))
00164                                         {
00165                                                 uint16 errorCode = ackMessage->errorCode;
00166                                                 if(errorCode == BootloaderAck::SUCCESS)
00167                                                         break;
00168                                                 else
00169                                                         return false;
00170                                         }
00171                                 }
00172                                 */
00173                         }
00174                         
00175                 }
00176                 
00177                 // flush only here to save bandwidth
00178                 stream->flush();
00179                 
00180                 while (true)
00181                 {
00182                         writePageWaitAck();
00183                         auto_ptr<Message> message(Message::receive(stream));
00184                         
00185                         // handle ack
00186                         BootloaderAck *ackMessage = dynamic_cast<BootloaderAck *>(message.get());
00187                         if (ackMessage && (ackMessage->source == dest))
00188                         {
00189                                 uint16 errorCode = ackMessage->errorCode;
00190                                 if(errorCode == BootloaderAck::SUCCESS)
00191                                 {
00192                                         writePageSuccess();
00193                                         return true;
00194                                 }
00195                                 else
00196                                 {
00197                                         writePageFailure();
00198                                         return false;
00199                                 }
00200                         }
00201                 }
00202                 
00203                 // should not happen
00204                 return true;
00205         }
00206         
00207         void BootloaderInterface::writeHex(const string &fileName, bool reset, bool simple)
00208         {
00209                 // Load hex file
00210                 HexFile hexFile;
00211                 hexFile.read(fileName);
00212                 
00213                 writeHexStart(fileName, reset, simple);
00214                 
00215                 if (reset) 
00216                 {
00217                         Reboot msg(dest);
00218                         msg.serialize(stream);
00219                         stream->flush();
00220                         
00221                         writeHexEnteringBootloader();
00222                 }
00223         
00224                 // Get page layout
00225                 if (simple)
00226                 {
00227                         /* 
00228                         This would be cleaner for non-broken robot, but would not work for recovery.
00229                         Waiting works for robots in any states
00230                         // wait for disconnected message
00231                         while (true)
00232                         {
00233                                 auto_ptr<Message> message(Message::receive(stream));
00234                                 Disconnected* disconnectedMessage(dynamic_cast<Disconnected*>(message.get()));
00235                                 if (disconnectedMessage)
00236                                         break;
00237                         }*/
00238                         // give 10 ms to the robot the time to reset
00239                         usleep(10000);
00240                         pageSize = 2048;
00241                 }
00242                 else
00243                 {
00244                         // get bootloader description
00245                         while (true)
00246                         {
00247                                 auto_ptr<Message> message(Message::receive(stream));
00248                                 BootloaderDescription *bDescMessage = dynamic_cast<BootloaderDescription *>(message.get());
00249                                 if (bDescMessage && (bDescMessage->source == dest))
00250                                 {
00251                                         pageSize = bDescMessage->pageSize;
00252                                         pagesStart = bDescMessage->pagesStart;
00253                                         pagesCount = bDescMessage->pagesCount;
00254                                         break;
00255                                 }
00256                         }
00257                 }
00258                 
00259                 // Build a map of pages out of the map of addresses
00260                 typedef map<uint32, vector<uint8> > PageMap;
00261                 PageMap pageMap;
00262                 for (HexFile::ChunkMap::iterator it = hexFile.data.begin(); it != hexFile.data.end(); it ++)
00263                 {
00264                         // get page number
00265                         unsigned chunkAddress = it->first;
00266                         // index inside data chunk
00267                         unsigned chunkDataIndex = 0;
00268                         // size of chunk in bytes
00269                         unsigned chunkSize = it->second.size();
00270                         
00271                         // copy data from chunk to page
00272                         do
00273                         {
00274                                 // get page number
00275                                 unsigned pageIndex = (chunkAddress + chunkDataIndex) / pageSize;
00276                                 // get address inside page
00277                                 unsigned byteIndex = (chunkAddress + chunkDataIndex) % pageSize;
00278                         
00279                                 // if page does not exists, create it
00280                                 if (pageMap.find(pageIndex) == pageMap.end())
00281                                 {
00282                                 //      std::cout << "New page N° " << pageIndex << " for address 0x" << std::hex << chunkAddress << endl;
00283                                         pageMap[pageIndex] = vector<uint8>(pageSize, (uint8)0);
00284                                 }
00285                                 // copy data
00286                                 unsigned amountToCopy = min(pageSize - byteIndex, chunkSize - chunkDataIndex);
00287                                 copy(it->second.begin() + chunkDataIndex, it->second.begin() + chunkDataIndex + amountToCopy, pageMap[pageIndex].begin() + byteIndex);
00288                                 
00289                                 // increment chunk data pointer
00290                                 chunkDataIndex += amountToCopy;
00291                         }
00292                         while (chunkDataIndex < chunkSize);
00293                 }
00294                 
00295                 writeHexGotDescription(pageMap.size());
00296                 
00297                 if (simple)
00298                 {
00299                         // Write pages
00300                         for (PageMap::iterator it = pageMap.begin(); it != pageMap.end(); it ++)
00301                         {
00302                                 unsigned pageIndex = it->first;
00303                                 if (pageIndex != 0)
00304                                         if (!writePage(pageIndex, &it->second[0], true))
00305                                                 errorWritePageNonFatal(pageIndex);
00306                         }
00307                         // Now look for the index 0 page
00308                         for (PageMap::iterator it = pageMap.begin(); it != pageMap.end(); it ++)
00309                         {
00310                                 unsigned pageIndex = it->first;
00311                                 if (pageIndex == 0)
00312                                         if (!writePage(pageIndex, &it->second[0], true))
00313                                                 errorWritePageNonFatal(pageIndex);
00314                         }
00315                 }
00316                 else
00317                 {
00318                         // Write pages
00319                         for (PageMap::iterator it = pageMap.begin(); it != pageMap.end(); it ++)
00320                         {
00321                                 unsigned pageIndex = it->first;
00322                                 if ((pageIndex >= pagesStart) && (pageIndex < pagesStart + pagesCount))
00323                                         if (!writePage(pageIndex, &it->second[0], false))
00324                                                 throw Error(FormatableString("Error while writing page %0").arg(pageIndex));
00325                         }
00326                 }
00327                 
00328                 writeHexWritten();
00329 
00330                 if (reset) 
00331                 {
00332                         BootloaderReset msg(dest);
00333                         msg.serialize(stream);
00334                         stream->flush();
00335                         
00336                         writeHexExitingBootloader();
00337                 }
00338         }
00339         
00340         void BootloaderInterface::readHex(const string &fileName)
00341         {
00342                 HexFile hexFile;
00343                 
00344                 // Create memory
00345                 unsigned address = pagesStart * pageSize;
00346                 hexFile.data[address] = vector<uint8>();
00347                 hexFile.data[address].reserve(pagesCount * pageSize);
00348                 
00349                 // Read pages
00350                 for (unsigned page = pagesStart; page < pagesCount; page++)
00351                 {
00352                         vector<uint8> buffer((uint8)0, pageSize);
00353                         
00354                         if (!readPage(page, &buffer[0]))
00355                                 throw Error(FormatableString("Error, cannot read page %0").arg(page));
00356                         
00357                         copy(&buffer[0], &buffer[pageSize], back_inserter(hexFile.data[address]));
00358                 }
00359                 
00360                 // Write hex file
00361                 hexFile.strip(pageSize);
00362                 hexFile.write(fileName);
00363         }
00364         
00365 } // namespace Aseba


aseba
Author(s): Stéphane Magnenat
autogenerated on Sun Oct 5 2014 23:46:38