HexFile.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 "HexFile.h"
00022 #include <istream>
00023 #include <ostream>
00024 #include <fstream>
00025 #include <iostream>
00026 #include <iomanip>
00027 #include <cassert>
00028 #include <algorithm>
00029 #include <valarray>
00030 
00031 namespace Aseba
00032 {
00033         std::string HexFile::EarlyEOF::toString() const
00034         {
00035                 return FormatableString("Early end of file after %0 lines").arg(line);
00036         }
00037         
00038         std::string HexFile::InvalidRecord::toString() const
00039         {
00040                 return FormatableString("Invalid record at line %0").arg(line);
00041         }
00042         
00043         std::string HexFile::WrongCheckSum::toString() const
00044         {
00045                 return FormatableString("Wrong checksum (%0 instead of %1) at line %2").arg(computedCheckSum, 0, 16).arg(recordCheckSum, 0, 16).arg(line);
00046         }
00047         
00048         std::string HexFile::UnknownRecordType::toString() const
00049         {
00050                 return FormatableString("Unknown record type (%1) at line %0").arg(line).arg(recordType, 0, 16);
00051         }
00052         
00053         std::string HexFile::FileOpeningError::toString() const
00054         {
00055                 return FormatableString("Can't open file %0").arg(fileName);
00056         }
00057         
00058         unsigned HexFile::getUint4(std::istream &stream)
00059         {
00060                 int c = stream.get();
00061                 if (c <= '9')
00062                         return c - '0';
00063                 else if (c <= 'F')
00064                         return c - 'A' + 10;
00065                 else
00066                         return c - 'a' + 10;
00067         }
00068         
00069         unsigned HexFile::getUint8(std::istream &stream)
00070         {
00071                 return (getUint4(stream) << 4) | getUint4(stream);
00072         }
00073         
00074         unsigned HexFile::getUint16(std::istream &stream)
00075         {
00076                 return (getUint8(stream) << 8) | getUint8(stream);
00077         }
00078         
00079         void HexFile::read(const std::string &fileName)
00080         {
00081                 std::ifstream ifs(fileName.c_str());
00082                 
00083                 if (ifs.bad())
00084                         throw FileOpeningError(fileName);
00085                 
00086                 int lineCounter = 0;
00087                 uint32 baseAddress = 0;
00088                 
00089                 while (ifs.good())
00090                 {
00091                         // leading ":" character
00092                         char c;
00093                         ifs >> c;
00094                         if (c != ':')
00095                                 throw InvalidRecord(lineCounter);
00096                         
00097                         uint8 computedCheckSum = 0;
00098                         
00099                         // record data length
00100                         uint8 dataLength = getUint8(ifs);
00101                         computedCheckSum += dataLength;
00102                         
00103                         // short address
00104                         uint16 lowAddress = getUint16(ifs);
00105                         computedCheckSum += lowAddress;
00106                         computedCheckSum += lowAddress >> 8;
00107                         
00108                         // record type
00109                         uint8 recordType = getUint8(ifs);
00110                         computedCheckSum += recordType;
00111                         
00112                         switch (recordType)
00113                         {
00114                                 case 0:
00115                                 // data record
00116                                 {
00117                                         // read data
00118                                         std::vector<uint8> recordData;
00119                                         for (int i = 0; i != dataLength; i++)
00120                                         {
00121                                                 uint8 d = getUint8(ifs);
00122                                                 computedCheckSum += d;
00123                                                 recordData.push_back(d);
00124                                                 //std::cout << "data " << std::hex << (unsigned)d << "\n";
00125                                         }
00126                                         
00127                                         // verify checksum
00128                                         uint8 checkSum = getUint8(ifs);
00129                                         computedCheckSum = 1 + ~computedCheckSum;
00130                                         if (checkSum != computedCheckSum)
00131                                                 throw WrongCheckSum(lineCounter, checkSum, computedCheckSum);
00132                                         
00133                                         // compute address
00134                                         uint32 address = lowAddress;
00135                                         address += baseAddress;
00136                                         //std::cout << "data record at address 0x" << std::hex << address << "\n";
00137                                         
00138                                         // is some place to add
00139                                         bool found = false;
00140                                         for (ChunkMap::iterator it = data.begin(); it != data.end(); ++it)
00141                                         {
00142                                                 size_t chunkSize = it->second.size();
00143                                                 if (address == it->first + chunkSize)
00144                                                 {
00145                                                         // copy new
00146                                                         std::copy(recordData.begin(), recordData.end(), std::back_inserter(it->second));
00147                                                         found = true;
00148                                                         //std::cout << "tail fusable chunk found\n";
00149                                                         break;
00150                                                 }
00151                                                 else if (address + recordData.size() == it->first)
00152                                                 {
00153                                                         // resize
00154                                                         it->second.resize(chunkSize + recordData.size());
00155                                                         // move
00156                                                         std::copy_backward(it->second.begin(), it->second.begin() + chunkSize, it->second.end());
00157                                                         // copy new
00158                                                         std::copy(recordData.begin(), recordData.end(), it->second.begin());
00159                                                         found = true;
00160                                                         //std::cout << "head fusable chunk found\n";
00161                                                         break;
00162                                                 }
00163                                         }
00164                                         if (!found)
00165                                                 data[address] = recordData;
00166                                 }
00167                                 break;
00168                                 
00169                                 case 1:
00170                                 // end of file record
00171                                 for (ChunkMap::iterator it = data.begin(); it != data.end(); ++it)
00172                                 {
00173                                         //std::cout << "End of file found. Address " << it->first << " size " << it->second.size() << "\n";
00174                                 }
00175                                 ifs.close();
00176                                 return;
00177                                 break;
00178                                 
00179                                 case 2:
00180                                 // extended segment address record
00181                                 {
00182                                         if (dataLength != 2)
00183                                                 throw InvalidRecord(lineCounter);
00184                                         
00185                                         // read data
00186                                         uint16 highAddress = getUint16(ifs);
00187                                         computedCheckSum += highAddress;
00188                                         computedCheckSum += highAddress >> 8;
00189                                         baseAddress = highAddress;
00190                                         baseAddress <<= 4;
00191                                         //std::cout << "Extended segment address record (?!): 0x" << std::hex << baseAddress << "\n";
00192                                         
00193                                         // verify checksum
00194                                         uint8 checkSum = getUint8(ifs);
00195                                         computedCheckSum = 1 + ~computedCheckSum;
00196                                         if (checkSum != computedCheckSum)
00197                                                 throw WrongCheckSum(lineCounter, checkSum, computedCheckSum);
00198                                 }
00199                                 break;
00200                                 
00201                                 case 4:
00202                                 // extended linear address record
00203                                 {
00204                                         if (dataLength != 2)
00205                                                 throw InvalidRecord(lineCounter);
00206                                         
00207                                         // read data
00208                                         uint16 highAddress = getUint16(ifs);
00209                                         computedCheckSum += highAddress;
00210                                         computedCheckSum += highAddress >> 8;
00211                                         baseAddress = highAddress;
00212                                         baseAddress <<= 16;
00213                                         //std::cout << "Linear address record: 0x" << std::hex << baseAddress << "\n";
00214                                         
00215                                         // verify checksum
00216                                         uint8 checkSum = getUint8(ifs);
00217                                         computedCheckSum = 1 + ~computedCheckSum;
00218                                         if (checkSum != computedCheckSum)
00219                                                 throw WrongCheckSum(lineCounter, checkSum, computedCheckSum);
00220                                 }
00221                                 break;
00222                                 
00223                                 default:
00224                                 throw UnknownRecordType(lineCounter, recordType);
00225                                 break;
00226                         }
00227                         
00228                         lineCounter++;
00229                 }
00230                 
00231                 throw EarlyEOF(lineCounter);
00232         }
00233         
00234         void HexFile::writeExtendedLinearAddressRecord(std::ofstream &stream, unsigned addr16) const
00235         {
00236                 assert(addr16 <= 65535);
00237                 
00238                 uint8 checkSum = 0;
00239                 
00240                 stream << ":02000004";
00241                 checkSum += 0x02;
00242                 checkSum += 0x00;
00243                 checkSum += 0x00;
00244                 checkSum += 0x04;
00245                 
00246                 stream << std::setw(2);
00247                 stream << (addr16 >> 8);
00248                 checkSum += (addr16 >> 8);
00249                 
00250                 stream << std::setw(2);
00251                 stream << (addr16 & 0xFF);
00252                 checkSum += (addr16 & 0xFF);
00253                 
00254                 checkSum = (~checkSum) + 1;
00255                 stream << std::setw(2);
00256                 stream << (unsigned)checkSum;
00257                 
00258                 stream << "\n";
00259         }
00260         
00261         void HexFile::writeData(std::ofstream &stream, unsigned addr16, unsigned count8, uint8 *data) const
00262         {
00263                 assert(addr16 <= 65535);
00264                 assert(count8 <= 255);
00265                 
00266                 uint8 checkSum = 0;
00267                 
00268                 stream << ":";
00269                 
00270                 stream << std::setw(2);
00271                 stream << count8;
00272                 checkSum += count8;
00273                 
00274                 stream << std::setw(2);
00275                 stream << (addr16 >> 8);
00276                 checkSum += (addr16 >> 8);
00277                 
00278                 stream << std::setw(2);
00279                 stream << (addr16 & 0xFF);
00280                 checkSum += (addr16 & 0xFF);
00281                 
00282                 stream << "00";
00283                 checkSum += 0x00;
00284                 
00285                 for (unsigned i = 0; i < count8; i++)
00286                 {
00287                         stream << std::setw(2);
00288                         stream << (unsigned)data[i];
00289                         checkSum += data[i];
00290                 }
00291                 
00292                 checkSum = (~checkSum) + 1;
00293                 stream << std::setw(2);
00294                 stream << (unsigned)checkSum;
00295                 
00296                 stream << "\n";
00297         }
00298 
00299         void HexFile::strip(unsigned pageSize)
00300         {
00301                 // Build a page map.
00302                 typedef std::map<uint32, std::vector<uint8> > PageMap;
00303                 PageMap pageMap;
00304                 for (ChunkMap::iterator it = data.begin(); it != data.end(); it ++)
00305                 {
00306                         // get page number
00307                         unsigned chunkAddress = it->first;
00308                         // index inside data chunk
00309                         unsigned chunkDataIndex = 0;
00310                         // size of chunk in bytes
00311                         unsigned chunkSize = it->second.size();
00312 
00313                         // copy data from chunk to page
00314                         do
00315                         {
00316                                 // get page number
00317                                 unsigned pageIndex = (chunkAddress + chunkDataIndex) / pageSize;
00318                                 // get address inside page
00319                                 unsigned byteIndex = (chunkAddress + chunkDataIndex) % pageSize;
00320 
00321                                 // if page does not exists, create it
00322                                 if (pageMap.find(pageIndex) == pageMap.end())
00323                                 {
00324                                 //      std::cout << "New page N° " << pageIndex << " for address 0x" << std::hex << chunkAddress << endl;
00325                                         pageMap[pageIndex] = std::vector<uint8>(pageSize, (uint8)0xFF); // New page is created uninitialized
00326                                 }
00327                                 // copy data
00328                                 unsigned amountToCopy = std::min(pageSize - byteIndex, chunkSize - chunkDataIndex);
00329                                 copy(it->second.begin() + chunkDataIndex, it->second.begin() + chunkDataIndex + amountToCopy, pageMap[pageIndex].begin() + byteIndex);
00330 
00331                                 // increment chunk data pointer
00332                                 chunkDataIndex += amountToCopy;
00333                         }
00334                         while (chunkDataIndex < chunkSize);
00335                 }
00336                 
00337                 // Now, for each page, drop it if empty
00338                 data.clear();
00339                 
00340                 for(PageMap::iterator it = pageMap.begin(); it != pageMap.end(); it++)
00341                 {
00342                         int isempty = 1;
00343                         unsigned int i;
00344                         for(i = 0; i < pageSize; i+=4)
00345                                 if(it->second[i] != 0xff || it->second[i+1] != 0xff || it->second[i+2] != 0xff) {
00346                                         isempty = 0;
00347                                         break;
00348                                 }
00349                         if(!isempty)
00350                                 data[it->first * pageSize] = it->second;
00351                 }
00352 
00353 
00354         }
00355         
00356         void HexFile::write(const std::string &fileName) const
00357         {
00358                 int first = 1;
00359                 unsigned highAddress = 0;
00360                 std::ofstream ofs(fileName.c_str());
00361                 
00362                 if (ofs.bad())
00363                         throw FileOpeningError(fileName);
00364                 
00365                 // set format
00366                 ofs.flags(std::ios::hex | std::ios::uppercase);
00367                 ofs.fill('0');
00368                 
00369                 // for each chunk
00370                 for (ChunkMap::const_iterator it = data.begin(); it != data.end(); it++)
00371                 {
00372                         // split address
00373                         unsigned address = it->first;
00374                         unsigned amount = it->second.size();
00375                         
00376                         for (unsigned count = 0; count < amount;)
00377                         {
00378                                 // check if we have changed 64 K boundary, if so, write new high address
00379                                 unsigned newHighAddress = (address + count) >> 16;
00380                                 if (newHighAddress != highAddress || first)
00381                                         writeExtendedLinearAddressRecord(ofs, newHighAddress);
00382                                 first = 0;
00383                                 highAddress = newHighAddress;
00384                                 
00385                                 // write data
00386                                 unsigned rowCount = std::min(amount - count, (unsigned)16);
00387                                 unsigned lowAddress = (address + count) & 0xFFFF;
00388                                 std::valarray<uint8> buffer(rowCount);
00389                                 std::copy(it->second.begin() + count, it->second.begin() + count + rowCount, &buffer[0]);
00390                                 writeData(ofs, lowAddress, rowCount , &buffer[0]);
00391                                 
00392                                 // increment counters
00393                                 count += rowCount;
00394                         }
00395                 }
00396                 
00397                 // write EOF
00398                 ofs << ":00000001FF";
00399         }
00400 }
00401 


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