companion/src/storage/hexinterface.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "hexinterface.h"
  22
  23 HexInterface::HexInterface(QTextStream &stream):
  24   stream(stream)
  25 {
  26 }
  27
  28 int HexInterface::getValueFromLine(const QString &line, int pos, int len)
  29 {
  30     bool ok;
  31     int hex = line.mid(pos,len).toInt(&ok, 16);
  32     return ok ? hex : -1;
  33 }
  34
  35 int HexInterface::load(uint8_t *data, int maxsize)
  36 {
  37   int result = 0;
  38   int offset = 0;
  39   while (!stream.atEnd()) {
  40     QString line = stream.readLine();
  41
  42     if(line.left(1)!=":") continue;
  43
  44     int byteCount = getValueFromLine(line,1);
  45     int address = getValueFromLine(line,3,4);
  46     int recType = getValueFromLine(line,7);
  47     if (recType==0x02) {
  48         offset+=0x010000;
  49     }
  50     if(byteCount<0 || address<0 || recType<0)
  51       return 0;
  52
  53     QByteArray ba;
  54     ba.clear();
  55
  56     quint8 chkSum = 0;
  57     chkSum -= byteCount;
  58     chkSum -= recType;
  59     chkSum -= address & 0xFF;
  60     chkSum -= address >> 8;
  61     for(int i=0; i<byteCount; i++)
  62     {
  63         quint8 v = getValueFromLine(line,(i*2)+9) & 0xFF;
  64         chkSum -= v;
  65         ba.append(v);
  66     }
  67
  68     quint8 retV = getValueFromLine(line,(byteCount*2)+9) & 0xFF;
  69     if(chkSum!=retV)
  70       return 0;
  71
  72     if (address+offset + byteCount <= maxsize) {
  73       if (recType == 0x00) { //data record - ba holds record
  74         memcpy(&data[address+offset],ba.data(),byteCount);
  75         result = std::max(result, address+offset+byteCount);
  76       }
  77     }
  78     else {
  79       return 0;
  80     }
  81   }
  82
  83   return result;
  84 }
  85
  86
  87 bool HexInterface::save(const uint8_t * data, const int size)
  88 {
  89   int addr = 0;
  90   int nextbank = 1;
  91   while (addr < size) {
  92     if (addr>(nextbank*0x010000)-1) {
  93       stream << iHEXExtRec(nextbank) << "\n";
  94       nextbank++;
  95     }
  96     int llen = 32;
  97     if ((size - addr) < llen)
  98       llen = size - addr;
  99     stream << iHEXLine(data, addr, llen) << "\n";
 100     addr += llen;
 101   }
 102   stream << ":00000001FF\n"; // write EOF
 103   return true;
 104 }
 105
 106 QString HexInterface::iHEXLine(const quint8 * data, quint32 addr, quint8 len)
 107 {
 108   unsigned int bankaddr;
 109   bankaddr=addr&0xffff;
 110   QString str = QString(":%1%2000").arg(len, 2, 16, QChar('0')).arg(bankaddr, 4, 16, QChar('0')); //write start, bytecount (32), address and record type
 111   quint8 chkSum = 0;
 112   chkSum = -len; //-bytecount; recordtype is zero
 113   chkSum -= bankaddr & 0xFF;
 114   chkSum -= bankaddr >> 8;
 115   for (int j = 0; j < len; j++) {
 116     str += QString("%1").arg(data[addr + j], 2, 16, QChar('0'));
 117     chkSum -= data[addr + j];
 118   }
 119
 120   str += QString("%1").arg(chkSum, 2, 16, QChar('0'));
 121   return str.toUpper(); // output to file and lf;
 122 }
 123
 124 QString HexInterface::iHEXExtRec(quint8 bank)
 125 {
 126   QString str = QString(":02000002"); //write record type 2 record
 127   quint8 chkSum = 0;
 128   chkSum = -2; //-bytecount; recordtype is zero
 129   chkSum -= 2; // type 2 record type
 130   str += QString("%1000").arg((bank&0x0f)<<4,1,16,QChar('0'));
 131   chkSum -= ((bank&0x0f)<<4); // type 2 record type
 132   str += QString("%1").arg(chkSum, 2, 16, QChar('0'));
 133   return str.toUpper(); // output to file and lf;
 134 }
 135