/*****************************************************************************
* FUNCTION: ConvertASCIIHexToBinary
*
* RETURNS: true if conversion successful, else false
*
* PARAMS: p_ascii -- ascii string to be converted
* p_binary -- binary value if conversion successful
*
* NOTES: Converts an input ascii hex string to binary value (up to 32-bit value)
*****************************************************************************/
tZGBool ConvertASCIIHexToBinary(tZGS8 *p_ascii, tZGU16 *p_binary)
{
tZGS8 i;
tZGU32 multiplier = 1;
*p_binary = 0;
// not allowed to have a string of more than 4 nibbles
if (strlen((char*)p_ascii) > 8u)
{
return kZGBoolFalse;
}
// first, ensure all characters are a hex digit
for (i = (tZGU8)strlen((char *)p_ascii) - 1; i >= 0 ; --i)
{
if (!isxdigit(p_ascii[i]))
{
return kZGBoolFalse;
}
*p_binary += multiplier * HexToBin(p_ascii[i]);
multiplier *= 16;
}
return kZGBoolTrue;
}
void ChangeRegisterValue()
{
cout << "\tEnter 'c' to skip this mode and continue \n";
while ( true )
{
string t1="",t2="";
cout << "Register (r0-r11) : ";
cin >> t1;
if (t1 == "c")
break ;
if (RG.Registers.find(t1) == RG.Registers.end())
{
cout << "\tError in register value . Not changing value . ";
}
else
{
cout << "New Contents (in Hex) : ";
cin >> t2;
if (CheckHex(t2) == false)
{
cout << "Error in value . Did not change .\n";
}
else
{
RG.value_of_register[RG.Registers[t1]] = HexToBin(t2);
}
}
}
}
/*****************************************************************************
* FUNCTION: ConvertASCIIHexToBinary
*
* RETURNS: true if conversion successful, else false
*
* PARAMS: p_ascii -- ascii string to be converted
* p_binary -- binary value if conversion successful
*
* NOTES: Converts an input ascii hex string to binary value (up to 32-bit value)
*****************************************************************************/
bool ConvertASCIIHexToBinary(int8_t *p_ascii, uint16_t *p_binary)
{
int8_t i;
uint32_t multiplier = 1;
*p_binary = 0;
// not allowed to have a string of more than 4 nibbles
if (strlen((char*)p_ascii) > 8u)
{
return false;
}
// first, ensure all characters are a hex digit
for (i = (uint8_t)strlen((char *)p_ascii) - 1; i >= 0 ; --i)
{
if (!isxdigit(p_ascii[i]))
{
return false;
}
*p_binary += multiplier * HexToBin(p_ascii[i]);
multiplier *= 16;
}
return true;
}
/*****************************************************************************
* FUNCTION: ConvertASCIIHexToBinary
*
* RETURNS: TRUE if conversion successful, else FALSE
*
* PARAMS: p_ascii -- ascii string to be converted
* p_binary -- binary value if conversion successful
*
* NOTES: Converts an input ascii hex string to binary value (up to 32-bit value)
*****************************************************************************/
BOOL ConvertASCIIHexToBinary(INT8 *p_ascii, UINT16 *p_binary)
{
INT8 i;
UINT32 multiplier = 1;
*p_binary = 0;
// not allowed to have a string of more than 4 nibbles
if (strlen((char*)p_ascii) > 8u)
{
return FALSE;
}
// first, ensure all characters are a hex digit
for (i = (UINT8)strlen((char *)p_ascii) - 1; i >= 0 ; --i)
{
if (!isxdigit(p_ascii[i]))
{
return FALSE;
}
*p_binary += multiplier * HexToBin(p_ascii[i]);
multiplier *= 16;
}
return TRUE;
}
//converte os caracteres lidos do arquivo para binário
void ArquivoToBin(char * bin, char * arquivo){
char hex[2];
for (int i=0; i<fileLen; i++)
{
sprintf(hex,"%02x", (unsigned int) arquivo[i] & 0xff);
for (int j=0; j<2; j++)
{
strncat(bin,HexToBin(hex[j]),4);
}
}
}
int __fastcall TMainForm::Hex2Bytes(String byteChars, PUCHAR buf) {
// Remove non-hex chars
String tmpStr = byteChars.UpperCase().Trim();
int bytes = tmpStr.Length() / 2;
// Stuff the output buffer with the byte values
if (bytes)
for (int i=0; i<bytes; i++) {
String s = tmpStr.SubString(1+ i*2, 2);
HexToBin(s.c_str(), &buf[i], 1);
}
return bytes;
}
//---------------------------------------------------------------------------
bool TForm1::Hex2Bin(String filename, bool is_work)
{
TStrings * hex_file = new TStringList();
hex_file->LoadFromFile(filename);
//int address_offset = 0;
for (int line_no=0;line_no<hex_file->Count;line_no++)
{
String line_str = hex_file->Strings[line_no];
if (line_str[1] == ':')
{
LineData line_data;
HexToBin(line_str.c_str()+1, (char*)&line_data, line_str.Length()/2);
unsigned __int8 check_sum = 0;
for (int i=0; i<1+2+1+line_data.length+1; i++)
{
check_sum += ((char*)&line_data)[i];
}
if (check_sum == 0)
{
if (line_data.type == 1)
{
return false;
}
else if (line_data.type == 0)
{
;
memmove(& (file_buf[/*address_offset +*/ ntohs(line_data.address)]),
line_data.data,
line_data.length);
max_address = std::max(max_address, ntohs(line_data.address) + line_data.length);
}
/*else if (line_data.type == 4 && is_work)
{
address_offset = line_data.data[0];
address_offset = address_offset * 256 * 256;
}*/
}
else
{
return false;
}
}
}
delete hex_file;
return true;
}
bool SetBreakPoints()
{
string tmp ;
int in,c=0;
cout << "Enter -1 as address of break-point to quit from this mode\n";
while ( true )
{
cout << "bp"<<c++<<" : ";
cin >> tmp; // Hex Code
if (tmp == "-1")
break;
if (CheckHex(tmp) == false )
{
cout << "Invalid Hex value . Quitting . \n";
return false ;
}
breaks.insert(Value(HexToBin(tmp).c_str()));
}
return true ;
}
static int
sms_sim_html (struct shttpd_arg_t *arg)
{
int n = 0, ret = 0;
n += snprintf (arg->buf + n, arg->buflen - n, "%s",
"HTTP/1.1 200 OK\r\n"
"Content-Type: text/html\r\n\r\n"
"<html><head><title>SMS in SIM Card</title>"
"<meta http-equiv=\"Content-Type\" content=\"text/html; charset=utf8\"/>"
"<meta name=\"viewport\" content=\"width=320, initial-scale=1.0\" />"
"</head><body>");
n += snprintf(arg->buf + n, arg->buflen - n, "<div>");
char *cmgr0 = ReadSMS(gsm_modem,0);
if(cmgr0 != NULL)
{
/*+CMGR: 0,,22
* 0891683108100005F0040D91683186517521F8000870015030224423026D4B
*/
char hexStr[1024];
int state;
int ret = sscanf(cmgr0, "\r\n+CMGR: %d,,%*d\r\n%s",&state,hexStr);
if (ret == 2) {
printf("Decode cmgr0, state = %d\n",state);
unsigned char *smsc=NULL,*oa=NULL,*text=NULL,*date=NULL;
unsigned char *bin = HexToBin(hexStr,strlen(hexStr));
DecodeSMS(bin,&smsc,&oa,&text,&date);
if(oa != NULL && text != NULL)
{
char encoded_url[1024];
urlencode(oa, encoded_url);
n += snprintf(arg->buf + n, arg->buflen - n, "<p>LastReceived:<br/><a href=\"/?phone_num=%s\">%s</a> %s ",encoded_url,oa,date);
n += snprintf(arg->buf + n, arg->buflen - n, "<span>%s</span>",text);
n += snprintf(arg->buf + n, arg->buflen - n, " <a href=\"/?text=%s\">smsto</a> <a href=\"mailto:?body=%s\">mailto</a><br/>------------------------------</p>",text,text);
}
if(smsc != NULL)
free(smsc);
if(oa != NULL)
free(oa);
//text don't need free;
if(date != NULL)
free(date);
free(bin);
}else{
n += snprintf(arg->buf + n, arg->buflen - n, "<p>LastReceived:<br/>Read Error!</p>");
}
free(cmgr0);
}else
n += snprintf(arg->buf + n, arg->buflen - n, "<p>LastReceived:<br/>Read Error!</p>");
char *sms = ReadSMSList(gsm_modem);
if(sms)
{
int len = strlen(sms);
char * current = NULL;
for (current = strstr(sms, "\r\n+CMGL: ");
current != NULL && current < sms + len - 1;
current = strstr(current + 1, "\r\n+CMGL: ")) {
int idx = 0;
int s;
int l;
char hexStr[1024];
sscanf(current, "\r\n+CMGL: %d,%d,,%d\r\n%s", &idx, &s,&l,hexStr);
if (idx == 0) {
printf("Could not read SMS number, skipping...\n");
} else {
printf("Decode %d, state = %d\n",idx,s);
unsigned char *smsc=NULL,*oa=NULL,*text=NULL,*date=NULL;
unsigned char *bin = HexToBin(hexStr,strlen(hexStr));
DecodeSMS(bin,&smsc,&oa,&text,&date);
if(oa != NULL && text != NULL)
{
char encoded_url[1024];
urlencode(oa, encoded_url);
n += snprintf(arg->buf + n, arg->buflen - n, "<p><a href=\"/?phone_num=%s\">%s</a> %s ",encoded_url,oa,date);
n += snprintf(arg->buf + n, arg->buflen - n, "<span>%s</span>",text);
n += snprintf(arg->buf + n, arg->buflen - n, " <a href=\"/?text=%s\">smsto</a> <a href=\"mailto:?body=%s\">mailto</a></p>",text,text);
}
if(smsc != NULL)
free(smsc);
if(oa != NULL)
free(oa);
//text don't need free;
if(date != NULL)
free(date);
free(bin);
}
}
free(sms);
}else
n += snprintf (arg->buf + n, arg->buflen - n, "<h1>empty</h1>");
n += snprintf (arg->buf + n, arg->buflen - n, "</div></body></html>");
assert (n < (int) arg->buflen);
arg->last = 1;
return (n);
}
static int
simpb_html (struct shttpd_arg_t *arg)
{
int n = 0, ret = 0;
char * current = NULL;
char *sr;
char *line;
int sr_len;
n += snprintf (arg->buf + n, arg->buflen - n, "%s",
"HTTP/1.1 200 OK\r\n"
"Content-Type: text/html\r\n\r\n"
"<html><head><title>PhoneBook in SIM Card</title>"
"<meta http-equiv=\"Content-Type\" content=\"text/html; charset=utf8\"/>"
"<meta name=\"viewport\" content=\"width=320, initial-scale=1.0\" />"
"</head><body>");
sr = sqlite_exec("/var/root/Library/AddressBook/AddressBook.sqlitedb","select ROWID,first from ABPerson");
sr_len = strlen(sr);
n += snprintf(arg->buf + n, arg->buflen - n, "<table border=\"0\" cellpadding=\"5\" align=\"center\">");
for(line = sr; line < (sr + sr_len-1); line = strstr(line,"|\n")+2)
{
int id;
char name[128];
char *phone;
char sql[256];
sscanf(line,"%d|%[^|]",&id,name);
fprintf(stderr,"id:%d,name:%s\n",id,name);
sprintf(sql,"select value from ABMultiValue where record_id=%d",id);
phone = sqlite_exec("/var/root/Library/AddressBook/AddressBook.sqlitedb",sql);
fprintf(stderr,"phone:%s\n",phone);
if(phone != NULL)
{
phone[strlen(phone)-2] = 0;
n += snprintf(arg->buf + n, arg->buflen - n, "<tr><td><a href=\"/?phone_num=%s\">%s</a></td><td>%s</td></tr>",
name,name,phone
);
free(phone);
}
}
if(sr)
free(sr);
n += snprintf (arg->buf + n, arg->buflen - n, "</table>");
n += snprintf (arg->buf + n, arg->buflen - n, "<p align=\"center\">--------- SIM Card ---------</p>");
if(cached_pb_len == 0)
{
int pbs_len = ReadPB(gsm_modem);
memcpy(cached_pb,readbuf,pbs_len);
cached_pb_len = pbs_len;
}
fprintf(stderr,"cached_pb_len: %d\n",cached_pb_len);
n += snprintf(arg->buf + n, arg->buflen - n, "<table border=\"0\" cellpadding=\"5\" align=\"center\">");
for (current = strstr(cached_pb, "\r\n+CPBR: ");
current != NULL && current < cached_pb + cached_pb_len - 1;
current = strstr(current + 1, "\r\n+CPBR: ")) {
int pos,i;
unsigned char phone_num[32];
unsigned char name[64];
unsigned short *ucs2;
unsigned char *utf8;
//"\r\n+CPBR: 1,"111111",129,"a"\r\n"
sscanf(current, "\r\n+CPBR: %d,\"%[^\"]\",%*d,\"%[^\"]", &pos,phone_num,name);
// fprintf(stderr,"%d, %s, %s\n",pos,phone_num,name);
// for(i=0; i<strlen(phone_num); i++)
// fprintf(stderr,"%02X ",phone_num[i]);
// fprintf(stderr,"\n");
// for(i=0; i<strlen(name); i++)
// fprintf(stderr,"%02X ",name[i]);
// fprintf(stderr,"\n");
ucs2 = (unsigned short *)HexToBin(name,strlen(name));
// fprintf(stderr,"ucs2: ");
// for(i=0; i<(strlen(name)/4); i++)
// fprintf(stderr,"%04X ",ucs2[i]);
// fprintf(stderr,"\n");
utf8 = ucs2_to_utf8(ucs2,strlen(name)/4);
free(ucs2);
// for(i=0; i<strlen(utf8); i++)
// fprintf(stderr,"%02X ",utf8[i]);
// fprintf(stderr,"\n");
n += snprintf(arg->buf + n, arg->buflen - n, "<tr><td><a href=\"/?phone_num=%s\">%s</a></td><td>%s</td></tr>",
phone_num,utf8,phone_num
);
}
n += snprintf (arg->buf + n, arg->buflen - n, "</table></body></html>");
assert (n < (int) arg->buflen);
arg->last = 1;
return (n);
}
bool Javad::readLog(string fname,int mjd)
{
Timer timer;
timer.start();
DBGMSG(debugStream,INFO,"reading " << fname);
ifstream infile (fname.c_str());
string line;
int linecount=0;
string msgid,currpctime,pctime,msg,gpstime;
U4 gpsTOD;
F8 rxTimeOffset;
F4 sawtooth;
vector<string> rxid;
vector<SVMeasurement *> gpsmeas;
gotIonoData = false;
gotUTCdata=false;
U1 uint8buf;
I1 sint8buf;
I2 sint16buf;
I4 sint32buf;
U4 u4buf;
unsigned int currMsgs=0,reqdMsgs;
unsigned int nSats;
unsigned char trackedSVs[MAX_CHANNELS];
unsigned char navStatus[MAX_CHANNELS];
unsigned char elevs[MAX_CHANNELS];
unsigned char azimuths[MAX_CHANNELS];
F8 CApr[MAX_CHANNELS];
unsigned char CAlockFlags[MAX_CHANNELS*2];
F8 P1pr[MAX_CHANNELS];
F8 relP1pr[MAX_CHANNELS];
unsigned char P1lockFlags[MAX_CHANNELS*2];
F8 P2pr[MAX_CHANNELS];
F8 relP2pr[MAX_CHANNELS];
unsigned char P2lockFlags[MAX_CHANNELS*2];
I2 i2bufarray[MAX_CHANNELS];
I4 i4bufarray[MAX_CHANNELS];
F4 f4bufarray[MAX_CHANNELS];
F8 f8bufarray[MAX_CHANNELS];
F8 smoothingOffset;
int rcCnt=0,RCcnt=0;
int R1cnt=0,r1Cnt=0,m1RCnt=0,m1rCnt=0;
int R2cnt=0,r2Cnt=0,m2RCnt=0,m2rCnt=0;
unsigned int errorCount=0,badC1Measurements=0,badP1Measurements=0,badP2Measurements=0,numSVmeasurements=0;
U2 RDyyyy;
U1 RDmm,RDdd;
reqdMsgs = AZ_MSG | EL_MSG | FC_MSG | RC_rc_MSG| RT_MSG | SI_MSG | SS_MSG | TO_MSG | YA_MSG| ZA_MSG;
if (dualFrequency)
reqdMsgs |= R1_r1_1R_1r_MSG | R2_r2_2R_2r_MSG | F1_MSG | F2_MSG;
if (infile.is_open()){
while ( getline (infile,line) ){
linecount++;
if (line.size()==0) continue; // skip empty line
if ('#' == line.at(0)) continue; // skip comments
if ('%' == line.at(0)) continue;
if ('@' == line.at(0)){
size_t pos;
if (string::npos != (pos = line.find("RXID",2 )) ){
rxid.push_back(line.substr(pos+4,line.size()-pos-4));
}
continue;
}
stringstream sstr(line);
// Basic check on the format
if ( (line.size() < 16) || // too short
(line.at(2) != ' ') || // missing delimiter
(line.at(5) != ':') || // missing delimiter
(line.at(8) != ':') ||
(line.at(11) != ' ')){
errorCount++;
continue;
}
sstr >> msgid >> currpctime >> msg;
if (sstr.fail()){
DBGMSG(debugStream,WARNING," bad data at line " << linecount);
errorCount++;
continue;
}
// FIXME could improve parser by converting the msgid to a hex value?
// Some messages we just don't want
if (msgid == "NP"){
continue;
}
// FIXME Valid checksum?
// If we compute a valid checksum, then checking the message size is a bit paranoid
//HexToBin(hexdata,count/2,rawdata);
//sprintf(temp,"%s%03X",command,count/2);
//for (i=0;i<count/2;i++) temp[i+5] = rawdata[i];
//if (cs(temp,count/2 + 5 - 1 ) != rawdata[count/2 - 1]) continue;
//
// Messages that occur once per second
//
//
// The Receiver Date message starts each second
//
if(msgid=="RD"){ // Receiver Date (RD) message
if ((currMsgs == reqdMsgs) && (rcCnt <= 1) && (RCcnt <= 1)){ // save measurements
if ((dualFrequency &&
!((R1cnt<=1) && (r1Cnt<=1) && (m1RCnt<=1) && (m1rCnt<=1) &&
(R2cnt<=1) && (r2Cnt<=1) && (m2RCnt<=1) && (m2rCnt<=1))))
{
DBGMSG(debugStream,INFO,"Too many P1/2 messages");
pctime=currpctime;
currMsgs=0;
rcCnt=RCcnt=0;
R1cnt=r1Cnt=m1RCnt=m1rCnt=0;
R2cnt=r2Cnt=m2RCnt=m2rCnt=0;
continue;
}
ReceiverMeasurement *rmeas = new ReceiverMeasurement();
numSVmeasurements += nSats;
// For each tracked satellite, get the pseudorange for each code
for (unsigned int chan=0; chan < nSats; chan++){
// FIXME ignore GLONASS for the present
// GPS USID 1..37, GLONASS 38..69,70
if (trackedSVs[chan] > 37){
continue;
}
if (codes & GNSSSystem::C1){
bool ok=true;
// Check that PLLs are locked for each channel before we use the data
if (CAlockFlags[chan*2] != 83){
DBGMSG(debugStream,WARNING," C/A unlocked at line " << linecount << "(prn=" << (int) trackedSVs[chan] << ")");
badC1Measurements++;
ok=false;
}
// Simple sanity check in case pseudoranges or the receiver time offset are wild
if (((CApr[chan]-rxTimeOffset)<0.05) || ((CApr[chan]-rxTimeOffset)>0.10)){
DBGMSG(debugStream,WARNING," C/A pseudorange too large at line " << linecount << "(" << CApr[chan]-rxTimeOffset << ")");
if (ok) badC1Measurements++; // don't count it twice
ok=false;
}
if (ok){
SVMeasurement *svm = new SVMeasurement(trackedSVs[chan],CApr[chan]-rxTimeOffset,rmeas); // pseudorange is corrected for rx offset
svm->dbuf3 = CApr[chan];
rmeas->gps.push_back(svm);
}
} // if codes & C1
if (codes & GNSSSystem::P1){
bool ok=true;
if (P1lockFlags[chan*2] != 83){
DBGMSG(debugStream,WARNING," P1 unlocked at line " << linecount << "(prn=" << (int) trackedSVs[chan] << ")");
badP1Measurements++;
ok=false;
}
// FIXME better sanity check
ok = ok && !isnan(P1pr[chan]);
if (ok){
SVMeasurement *svm = new SVMeasurement(trackedSVs[chan],P1pr[chan]-rxTimeOffset,rmeas); // pseudorange is corrected for rx offset
rmeas->gpsP1.push_back(svm);
}
} // if codes & P1
if (codes & GNSSSystem::P2){
bool ok = true;
if (P2lockFlags[chan*2] != 83){
DBGMSG(debugStream,WARNING," P2 unlocked at line " << linecount << "(prn=" << (int) trackedSVs[chan] << ")");
badP2Measurements++;
ok=false;
}
// FIXME better sanity check
ok = ok && !isnan(P2pr[chan]);
if (ok){
SVMeasurement *svm = new SVMeasurement(trackedSVs[chan],P2pr[chan]-rxTimeOffset,rmeas); // pseudorange is corrected for rx offset
rmeas->gpsP2.push_back(svm);
}
} // if codes & P2
}
if (rmeas->gps.size() > 0){ // FIXME check other codes
int pchh,pcmm,pcss;
if ((3==sscanf(pctime.c_str(),"%d:%d:%d",&pchh,&pcmm,&pcss))){
rmeas->pchh=pchh;
rmeas->pcmm=pcmm;
rmeas->pcss=pcss;
// One more check
if ((gpsTOD/1000.0+rxTimeOffset) > -0.1 && (gpsTOD/1000.0+rxTimeOffset < 86400.1)){
int igpsTOD = gpsTOD/1000;
int hh = (int) (igpsTOD/3600);
int mm = (int) (igpsTOD - hh*3600)/60;
rmeas->tmGPS.tm_sec= igpsTOD - hh*3600 - mm*60;
rmeas->tmGPS.tm_min= mm;
rmeas->tmGPS.tm_hour=hh ;
rmeas->tmGPS.tm_mday=RDdd;
rmeas->tmGPS.tm_mon=RDmm-1;
rmeas->tmGPS.tm_year=RDyyyy-1900;
rmeas->tmGPS.tm_isdst=0;
mktime(&(rmeas->tmGPS)); // this sets wday (note: TZ=UTC enforced in Main.cpp) so DST stays correct
rmeas->gpstow = 86400*rmeas->tmGPS.tm_wday+igpsTOD;
rmeas->tmfracs = rxTimeOffset;
// The time offset can be negative so have to account for rollovers
// time_t ttGPS = (time_t)(mktime(&(rmeas->tmGPS)) + rxTimeOffset);
// struct tm *tmGPS = gmtime(&ttGPS);
// rmeas->tmGPS=*tmGPS;
// rmeas->tmfracs = rxTimeOffset;
// if (rmeas->tmfracs < 0)
// rmeas->tmfracs += 1.0;
// YA and TO messages can roll over at different times - handle this case
// Typically, the difference between the smoothed and receiver time offsets is <= 1 ns
if ((smoothingOffset - rxTimeOffset)> 5e-4) smoothingOffset-=1e-3;
if ((smoothingOffset - rxTimeOffset)<-5e-4) smoothingOffset+=1e-3;
rmeas->sawtooth=sawtooth-(smoothingOffset-rxTimeOffset); // added to the counter measurement
// the term (moothingOffset-rxTimeOffset) is typically zero
rmeas->timeOffset = rxTimeOffset; // just used for diagnostics
// All OK
measurements.push_back(rmeas);
DBGMSG(debugStream,TRACE,rmeas->gps.size() << " measurements at " << (int) gpsTOD << " "
<< hh << ":" << mm << ":" << (int) rmeas->tmGPS.tm_sec << " (GPS), "
<< pchh << ":" << pcmm << ":" << pcss << " (PC)");
}
else{
errorCount++;
DBGMSG(debugStream,WARNING,"GPS TOD out of range at " << pchh << ":" << pcmm << ":" << pcss << " (PC)");
}
}
else{
errorCount++;
DBGMSG(debugStream,WARNING,"Unreadable PC time " << currpctime);
}
}
else{
errorCount++;
DBGMSG(debugStream,WARNING,"No useable GPS measurements at " << currpctime);
delete rmeas;
}
}
if (msg.size() == 6*2){
HexToBin((char *) msg.c_str(),sizeof(U2),(unsigned char *) &RDyyyy);
HexToBin((char *) msg.substr(2*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &RDmm);
HexToBin((char *) msg.substr(3*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &RDdd);
HexToBin((char *) msg.substr(4*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &uint8buf);
DBGMSG(debugStream,TRACE," RD " << (int) RDyyyy << ":" << (int) RDmm << ":" << (int) RDdd << " rx ref time=" << (int) uint8buf);
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," RD msg wrong size at line " << linecount);
}
pctime=currpctime;
currMsgs=0;
rcCnt=RCcnt=0;
R1cnt=r1Cnt=m1RCnt=m1rCnt=0;
R2cnt=r2Cnt=m2RCnt=m2rCnt=0;
continue;
}
if (msgid == "~~"){
if (msg.size() == 5*2 ){
HexToBin((char *) msg.substr(0,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &gpsTOD);
currMsgs |= RT_MSG;
DBGMSG(debugStream,TRACE," RT " << (int) gpsTOD);
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," bad data at line " << linecount);
}
continue;
}
if(msgid=="SI"){ // Satellite Indices (SI) message
if (currMsgs & SI_MSG){
currMsgs=0; // unexpected SI message
rcCnt=RCcnt=0;
R1cnt=r1Cnt=m1RCnt=m1rCnt=0;
R2cnt=r2Cnt=m2RCnt=m2rCnt=0;
continue;
}
// Can't check the message size!
currMsgs |= SI_MSG;
nSats=(msg.size() - 2) / 2;
HexToBin((char *) msg.c_str(),nSats,trackedSVs);
continue;
}
if(msgid=="TO"){ // Reference Time to Receiver Time Offset (TO) message
if (msg.size() == 9*2){
HexToBin((char *) msg.c_str(),sizeof(F8),(unsigned char *) &rxTimeOffset);
// Discard outliers
if ((fabs(rxTimeOffset)>0.001) || (fabs(rxTimeOffset)<1E-10)){
badC1Measurements++;
DBGMSG(debugStream,WARNING," TO outlier at line " << linecount << ":" << rxTimeOffset);
}
else{
currMsgs |= TO_MSG;
DBGMSG(debugStream,TRACE," TO " << rxTimeOffset *1.0E9 << " ns");
}
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," TO msg wrong size at line " << linecount);
}
continue;
}
if(msgid=="YA"){ // smoothing offset (YA) message - assuming we are using pps A
if (msg.size() == 10*2){
HexToBin((char *) msg.c_str(),sizeof(F8),(unsigned char *) &smoothingOffset);
// Discard outliers. YA is occasionally reported as zero following a tracking glitch.
if ((fabs(smoothingOffset)>0.001) || (smoothingOffset==0)){
badC1Measurements++;
DBGMSG(debugStream,WARNING," YA outlier at line " << linecount << ":" << smoothingOffset);
}
else{
currMsgs |= YA_MSG;
DBGMSG(debugStream,TRACE," YA " << smoothingOffset *1.0E9 << " ns");
}
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," YA msg wrong size at line " << linecount << ":" << msg.size());
}
continue;
}
if(msgid=="ZA"){ // PPS offset (ZA) message - assuming we are using pps A
if (msg.size() == 5*2){
HexToBin((char *) msg.c_str(),sizeof(F4),(unsigned char *) &sawtooth); // units are ns
// Discard outliers
if (fabs(sawtooth)> 50.0){
badC1Measurements++;
DBGMSG(debugStream,WARNING," ZA outlier at line " << linecount << ":" << sawtooth);
}
else{
DBGMSG(debugStream,TRACE," ZA " << sawtooth << " ns");
sawtooth *= 1.0E-9;
currMsgs |= ZA_MSG;
}
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," ZA msg wrong size at line " << linecount);
}
continue;
}
// Need the SI message to parse the following messages
if (!(currMsgs & SI_MSG)) continue;
if(msgid == "SS"){ // Navigation Status (SS) message
unsigned int ssnSats = (msg.size() - 4) / 2;
if (ssnSats == nSats){
HexToBin((char *) msg.c_str(),nSats,navStatus);
currMsgs |= SS_MSG;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," SS msg wrong size at line " << linecount << ":" << ssnSats << " " << nSats);
}
continue;
}
if(msgid == "EL"){ // Satellite Elevations (EL) message
unsigned int elnSats = (msg.size() - 2) / 2;
if (elnSats == nSats){
HexToBin((char *) msg.c_str(),nSats,elevs);
currMsgs |= EL_MSG;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," EL msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "AZ"){ // Satellite Azimuths (AZ) message
unsigned int aznSats = (msg.size() - 2) / 2;
if (aznSats == nSats){
HexToBin((char *) msg.c_str(),nSats,azimuths);
currMsgs |= AZ_MSG;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," AZ msg wrong size at line " << linecount);
}
continue;
}
// L1C measurements
if(msgid == "rc"){ // Delta C/A Pseudoranges (rc) message
if (RCcnt) continue; // full pseudoranges take precedence
unsigned int rcnSats = (msg.size() - 2) / 8;
if (rcnSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(I4),(unsigned char *) (i4bufarray));
for (unsigned int i=0;i<nSats;i++)
CApr[i] = (double)(i4bufarray[i])*1e-11 + 0.075;
currMsgs |= RC_rc_MSG;
rcCnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," rc msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "RC"){ // Full C/A Pseudoranges (RC) message
unsigned int RCnSats = (msg.size() - 2) / 16;
if (RCnSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(F8),(unsigned char *) (f8bufarray));
for (unsigned int i=0;i<nSats;i++)
CApr[i] = (double) f8bufarray[i];
currMsgs |= RC_rc_MSG;
RCcnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," RC msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "FC"){ // F/A Signal Lock Flags (FC) message
unsigned int FCnSats = (msg.size() - 2) / 4;
if (FCnSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(U2),(unsigned char *) (CAlockFlags));
currMsgs |= FC_MSG;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," FC msg wrong size at line " << linecount);
}
continue;
}
// P1 & P2 messages
if (dualFrequency){
// Four pseudorange messages for each signal
if(msgid == "R1"){ // Full P1 pseudorange (R1) message
unsigned int msgSats = (msg.size() - 2) / (2*sizeof(F8));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(F8),(unsigned char *) (f8bufarray));
for (unsigned int i=0;i<nSats;i++)
P1pr[i] = (double) f8bufarray[i];
currMsgs |= R1_r1_1R_1r_MSG;
R1cnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," R1 msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "r1"){ // Short P1 pseudoranges (r1) message
if (R1cnt) continue; // full pseudoranges take precedence
unsigned int msgSats = (msg.size() - 2) / (2*sizeof(I4));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(I4),(unsigned char *) (i4bufarray));
for (unsigned int i=0;i<nSats;i++)
P1pr[i] = (double)(i4bufarray[i])*1e-11 + 0.075;
currMsgs |= R1_r1_1R_1r_MSG;
r1Cnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," r1 msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "1R"){ // Relative P1 pseudoranges (1R) message
unsigned int msgSats = (msg.size() - 2) / (2*sizeof(F4));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(F4),(unsigned char *) (f4bufarray));
for (unsigned int i=0;i<nSats;i++)
relP1pr[i] = (double) f4bufarray[i];
currMsgs |= R1_r1_1R_1r_MSG;
m1RCnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," 1R msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "1r"){ // Short relative P1 pseudoranges (1r) message
if (m1RCnt) continue; // full relative pseudoranges take precedence
unsigned int msgSats = (msg.size() - 2) / (2*sizeof(I2));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(I2),(unsigned char *) (i2bufarray));
for (unsigned int i=0;i<nSats;i++)
relP1pr[i] = (double)(i2bufarray[i])*1e-11 + 2.0e-7;
currMsgs |= R1_r1_1R_1r_MSG;
m1rCnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," 1r msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "R2"){ // Full P2 pseudorange (R2) message
unsigned int msgSats = (msg.size() - 2) / (2*sizeof(F8));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(F8),(unsigned char *) (f8bufarray));
for (unsigned int i=0;i<nSats;i++)
P2pr[i] = (double) f8bufarray[i];
currMsgs |= R2_r2_2R_2r_MSG;
R2cnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," R2 msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "r2"){ // Short P2 pseudoranges (r2) message
if (R2cnt) continue; // full pseudoranges take precedence
unsigned int msgSats = (msg.size() - 2) / (2*sizeof(I4));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(I4),(unsigned char *) (i4bufarray));
for (unsigned int i=0;i<nSats;i++)
P2pr[i] = (double)(i4bufarray[i])*1e-11 + 0.075;
currMsgs |= R2_r2_2R_2r_MSG;
r2Cnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," r2 msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "2R"){ // Relative P2 pseudoranges (2R) message
unsigned int msgSats = (msg.size() - 2) / (2*sizeof(F4));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(F4),(unsigned char *) (f4bufarray));
for (unsigned int i=0;i<nSats;i++)
relP2pr[i] = (double) f4bufarray[i];
currMsgs |= R2_r2_2R_2r_MSG;
m2RCnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," 2R msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "2r"){ // Short relative P2 pseudoranges (2r) message
if (m2RCnt) continue; // full delta pseudoranges take precedence
unsigned int msgSats = (msg.size() - 2) / (2*sizeof(I2));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(I2),(unsigned char *) (i2bufarray));
for (unsigned int i=0;i<nSats;i++)
relP2pr[i] = (double)(i2bufarray[i])*1e-11 + 2.0e-7;
currMsgs |= R2_r2_2R_2r_MSG;
m2rCnt++;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," 2r msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "F1"){ // P1 Lock Flags (F1) message
unsigned int msgSats = (msg.size() - 2) /(2*sizeof(U2));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(U2),(unsigned char *) (P1lockFlags));
currMsgs |= F1_MSG;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," F1 msg wrong size at line " << linecount);
}
continue;
}
if(msgid == "F2"){ // P2 Lock Flags (F2) message
unsigned int msgSats = (msg.size() - 2) /(2*sizeof(U2));
if (msgSats == nSats){
HexToBin((char *) msg.c_str(),nSats*sizeof(U2),(unsigned char *) (P2lockFlags));
currMsgs |= F2_MSG;
}
else{
errorCount++;
DBGMSG(debugStream,WARNING," F2 msg wrong size at line " << linecount);
}
continue;
}
} // if dualFrequency
//
// Intermittent messages - parse last
//
if (!gotIonoData){
if (msgid=="IO"){
if (msg.size()==39*2){
HexToBin((char *) msg.substr(6*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.a0));
HexToBin((char *) msg.substr(10*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.a1));
HexToBin((char *) msg.substr(14*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.a2));
HexToBin((char *) msg.substr(18*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.a3));
HexToBin((char *) msg.substr(22*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.B0));
HexToBin((char *) msg.substr(26*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.B1));
HexToBin((char *) msg.substr(30*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.B2));
HexToBin((char *) msg.substr(34*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.B3));
gotIonoData=true;
DBGMSG(debugStream,TRACE,"ionosphere parameters: a0=" << gps.ionoData.a0);
}
else{
errorCount++;
DBGMSG(debugStream,WARNING,"Bad I0 message size");
}
continue;
}
}
if (!gotUTCdata){
if (msgid=="UO"){
if (msg.size()==24*2){
HexToBin((char *) msg.substr(0*2,2*sizeof(F8)).c_str(),sizeof(F8),(unsigned char *) &(gps.UTCdata.A0));
HexToBin((char *) msg.substr(8*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.UTCdata.A1));
HexToBin((char *) msg.substr(12*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
gps.UTCdata.t_ot=u4buf;
HexToBin((char *) msg.substr(16*2,2*sizeof(U2)).c_str(),sizeof(U2),(unsigned char *) &(gps.UTCdata.WN_t));
HexToBin((char *) msg.substr(18*2,2*sizeof(I1)).c_str(),sizeof(I1),(unsigned char *) &(sint8buf));
gps.UTCdata.dtlS=sint8buf;
HexToBin((char *) msg.substr(19*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &uint8buf);
gps.UTCdata.DN=uint8buf;
HexToBin((char *) msg.substr(20*2,2*sizeof(U2)).c_str(),sizeof(U2),(unsigned char *) &(gps.UTCdata.WN_LSF));
HexToBin((char *) msg.substr(22*2,2*sizeof(I1)).c_str(),sizeof(I1),(unsigned char *) &sint8buf);
gps.UTCdata.dt_LSF=sint8buf;
DBGMSG(debugStream,TRACE,"UTC parameters: dtLS=" << gps.UTCdata.dtlS << ",dt_LSF=" << gps.UTCdata.dt_LSF);
gotUTCdata = gps.currentLeapSeconds(mjd,&leapsecs);
}
else{
errorCount++;
DBGMSG(debugStream,WARNING,"Bad U0 message size");
}
continue;
}
}
if(msgid == "GE"){ // GPS ephemeris
if (msg.length() == 123*2){
GPS::EphemerisData *ed = new GPS::EphemerisData;
HexToBin((char *) msg.substr(0*2,2*sizeof(UINT8)).c_str(), sizeof(UINT8), (unsigned char *) &(ed->SVN));
HexToBin((char *) msg.substr(1*2,2*sizeof(UINT32)).c_str(),sizeof(UINT32), (unsigned char *) &(u4buf));
ed->t_ephem=u4buf;
HexToBin((char *) msg.substr(6*2,2*sizeof(SINT16)).c_str(),sizeof(SINT16), (unsigned char *) &(sint16buf));
ed-> IODC=sint16buf;
HexToBin((char *) msg.substr(8*2,2*sizeof(SINT32)).c_str(),sizeof(SINT32), (unsigned char *) &(sint32buf));
ed->t_OC=sint32buf;
HexToBin((char *) msg.substr(12*2,2*sizeof(SINT8)).c_str(), sizeof(SINT8), (unsigned char *) &(sint8buf));
ed->SV_accuracy_raw=sint8buf;
HexToBin((char *) msg.substr(13*2,2*sizeof(UINT8)).c_str(), sizeof(UINT8), (unsigned char *) &(ed->SV_health));
HexToBin((char *) msg.substr(14*2,2*sizeof(SINT16)).c_str(),sizeof(SINT16), (unsigned char *) &sint16buf);
ed->week_number=(UINT16) sint16buf;
HexToBin((char *) msg.substr(16*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->t_GD));
// HexToBin((char *) msg.substr(37*2+2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->SV_accuracy));
HexToBin((char *) msg.substr(20*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->a_f2));
HexToBin((char *) msg.substr(24*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->a_f1));
HexToBin((char *) msg.substr(28*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->a_f0));
HexToBin((char *) msg.substr(32*2,2*sizeof(SINT32)).c_str(),sizeof(SINT32), (unsigned char *) &sint32buf);
ed->t_oe=sint32buf;
HexToBin((char *) msg.substr(36*2,2*sizeof(SINT16)).c_str(), sizeof(SINT16), (unsigned char *) &sint16buf);
ed->IODE=(UINT8) sint16buf; // WARNING! Truncated
HexToBin((char *) msg.substr(38*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->sqrtA));
HexToBin((char *) msg.substr(46*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->e));
HexToBin((char *) msg.substr(54*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->M_0));
ed->M_0 *= M_PI;
HexToBin((char *) msg.substr(62*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->OMEGA_0));
ed->OMEGA_0 *= M_PI;
HexToBin((char *) msg.substr(70*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->i_0));
ed->i_0 *= M_PI;
HexToBin((char *) msg.substr(78*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->OMEGA));
ed->OMEGA *= M_PI;
HexToBin((char *) msg.substr(86*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->delta_N));
ed->delta_N *=M_PI;
HexToBin((char *) msg.substr(90*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->OMEGADOT));
ed->OMEGADOT *= M_PI;
HexToBin((char *) msg.substr(94*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->IDOT));
ed->IDOT *= M_PI;
HexToBin((char *) msg.substr(98*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_rc));
HexToBin((char *) msg.substr(102*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_rs));
HexToBin((char *) msg.substr(106*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_uc));
HexToBin((char *) msg.substr(110*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_us));
HexToBin((char *) msg.substr(114*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_ic));
HexToBin((char *) msg.substr(118*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_is));
DBGMSG(debugStream,TRACE,"Ephemeris: SVN="<< (unsigned int) ed->SVN << ",toe="<< ed->t_oe << ",IODE=" << (int) ed->IODE);
gps.addEphemeris(ed);
}
else{
errorCount++;
DBGMSG(debugStream,WARNING,"Bad GE message size");
}
continue;
}
}
}
else{
bool __fastcall TMainForm::LoadHexToRAM(String fName, bool bLow)
{
// Load all the text lines from the .hex file into a String List for easy
// manipulation
TStringList *sList = new TStringList;
if (fName.Pos("VendAX")) {
GetVendAX(sList);
} else {
TFileStream *srcStream = new TFileStream(fName,fmOpenRead | fmShareDenyWrite);
if (srcStream == NULL) return false;
sList->LoadFromStream(srcStream);
delete srcStream;
}
String tmp;
int v;
// Delete non-data records
for (int i=sList->Count - 1; i>=0; i--) {
if (sList->Strings[i].Length()) {
tmp = sList->Strings[i].SubString(8,2); // Get the Record Type into v
v = 0; HexToBin(tmp.c_str(),(char *)&v,1); v *= 2;
if (v) sList->Delete(i); // Data records are type == 0
}
}
// Re-construct the strings to only contain the offset followed by the data
for (int i=0; i<sList->Count; i++) {
// Remove comments
v = sList->Strings[i].Pos("//");
if (v) sList->Strings[i].SetLength(v-1);
// Build string that just contains the offset followed by the data bytes
if (sList->Strings[i].Length()) {
// Get the offset
String sOffset = sList->Strings[i].SubString(4,4);
// Get the string of data chars
tmp = sList->Strings[i].SubString(2,2);
v = 0; HexToBin(tmp.c_str(),(char *)&v,1); v *= 2;
String s = sList->Strings[i].SubString(10,v);
// Replace the string in the list
sList->Strings[i] = sOffset + s;
}
}
if (bLow) ResetFX2(1); // Stop the processor
FX2Device->ControlEndPt->ReqCode = bLow ? 0xA0 : 0xA3;
FX2Device->ControlEndPt->Index = 0;
FX2Device->ControlEndPt->Value = 0;
// Go through the list, loading data into RAM
String DataString = "";
WORD nxtoffset = 0;
LONG xferLen = 0;
WORD offset;
int RamSize = 0x2000; // 8KB
UCHAR buf[MAX_CTLXFER_SIZE];
char c;
PCHAR pc;
for (int i=0; i<sList->Count; i++) {
// Get the offset
tmp = sList->Strings[i].SubString(1,4);
HexToBin(tmp.c_str(),(char *)&offset,2);
pc = (char *) &offset; c = pc[0]; pc[0]=pc[1]; pc[1]=c; // Swap the bytes
int sLen = sList->Strings[i].Length();
// Handle a line that straddles the 8KB boundary
int bytes = (sLen - 4)/2;
LONG lastAddr = offset + bytes;
// This case is the last segment to be sent to low memory
if (bLow && (offset < RamSize) && (lastAddr > RamSize))
bytes = RamSize - offset;
// In this case, we found the first segment to be sent to the high memory
if (!bLow && (offset < RamSize) && (lastAddr > RamSize)) {
bytes = lastAddr - RamSize;
String s = "xxxx"+sList->Strings[i].SubString(sLen - (bytes*2)+1,bytes*2);
sList->Strings[i] = s;
offset = RamSize;
}
if ((bLow && (offset < RamSize)) || // Below 8KB - internal RAM
(!bLow && (offset >= RamSize)) ) {
xferLen += bytes;
if ((offset == nxtoffset) && (xferLen < MAX_CTLXFER_SIZE)) {
DataString += sList->Strings[i].SubString(5,bytes*2);
} else {
LONG len = DataString.Length() / 2;
if (len) {
Hex2Bytes(DataString,buf);
FX2Device->ControlEndPt->Write(buf, len);
}
FX2Device->ControlEndPt->Value = offset; // The destination address
DataString = sList->Strings[i].SubString(5,bytes*2);
xferLen = bytes;
}
nxtoffset = offset + bytes; // Where next contiguous data would sit
}
}
// Send the last segment of bytes
LONG len = DataString.Length() / 2;
if (len) {
Hex2Bytes(DataString,buf);
FX2Device->ControlEndPt->Write(buf, len);
}
if (bLow) ResetFX2(0); // Start running this new code
return true;
}
static void rijndaelCBC_MCT (FILE *fp, const char *initKey, int keyLength,
const char *initIV, const char *initBlock, int blockLength, BYTE direction)
{
int i, j, r, t;
BYTE inBlock[256/8], outBlock[256/8], binKey[256/8], cv[256/8];
BYTE keyMaterial[320];
BYTE iv[64+1];
keyInstance keyInst;
cipherInstance cipherInst;
#ifdef TRACE_KAT_MCT
int width = 0;
clock_t elapsed = -clock();
printf ("Executing CBC MCT (%s, key %d): ",
direction == DIR_ENCRYPT ? "ENCRYPT" : "DECRYPT", keyLength);
fflush (stdout);
#endif /* ?TRACE_KAT_MCT */
fprintf (fp,
"\n"
"==========\n"
"\n"
"KEYSIZE=%d\n", keyLength);
fflush (fp);
HexToBin (inBlock, initBlock, blockLength); /* this is either PT0 or CT0 */
HexToBin (cv, initIV, blockLength);
HexToBin (binKey, initKey, keyLength);
for (i = 0; i < 400; i++) {
#ifdef TRACE_KAT_MCT
while (width-- > 0) putchar ('\b'); width = printf ("%d", i); fflush (stdout);
#endif /* ?TRACE_KAT_MCT */
fprintf (fp, "\nI=%d\n", i);
/* prepare key: */
for (j = 0; j < keyLength/8; j++) {
sprintf (&keyMaterial[2*j], "%02X", binKey[j]);
}
keyMaterial[keyLength/4] = 0;
fprintf (fp, "KEY=%s\n", keyMaterial);
keyInst.blockLen = blockLength;
r = makeKey(&keyInst, direction, keyLength, keyMaterial);
if (TRUE != r) {
fprintf(stderr,"makeKey error %d\n",r);
exit(-1);
}
/* do encryption/decryption: */
blockPrint (fp, cv, blockLength, "IV");
blockPrint (fp, inBlock, blockLength, direction == DIR_ENCRYPT ? "PT" : "CT");
if (direction == DIR_ENCRYPT) {
for (j = 0; j < 10000; j++) {
for(t = 0; t < blockLength/8; t++) {
sprintf(iv+2*t,"%02x",cv[t]);
}
cipherInst.blockLen = blockLength;
r = cipherInit (&cipherInst, MODE_CBC, iv);
if (TRUE != r) {
fprintf(stderr,"cipherInit error %d\n",r);
exit(-1);
}
r = blockEncrypt(&cipherInst, &keyInst, inBlock, blockLength, outBlock);
if (blockLength != r) {
fprintf(stderr,"blockEncrypt error %d\n",r);
exit(-1);
}
memcpy (inBlock, cv, blockLength/8);
memcpy (cv, outBlock, blockLength/8);
}
} else {
for (j = 0; j < 10000; j++) {
for(t = 0; t < blockLength/8; t++) {
sprintf(iv+2*t,"%02x",cv[t]);
}
cipherInst.blockLen = blockLength;
cipherInit (&cipherInst, MODE_CBC, iv);
blockDecrypt(&cipherInst, &keyInst, inBlock, blockLength, outBlock);
memcpy (cv, inBlock, blockLength/8);
memcpy (inBlock, outBlock, blockLength/8);
}
}
blockPrint (fp, outBlock, blockLength, direction == DIR_ENCRYPT ? "CT" : "PT");
/* prepare new key: */
switch (keyLength) {
case 128:
for (j = 0; j < 128/8; j++) {
binKey[j] ^= outBlock[j];
}
break;
case 192:
for (j = 0; j < 64/8; j++) {
if (direction == DIR_ENCRYPT)
binKey[j] ^= inBlock[j + 64/8];
else
binKey[j] ^= cv[j + 64/8];
}
for (j = 0; j < 128/8; j++) {
binKey[j + 64/8] ^= outBlock[j];
}
break;
case 256:
for (j = 0; j < 128/8; j++) {
if (direction == DIR_ENCRYPT)
binKey[j] ^= inBlock[j];
else
binKey[j] ^= cv[j];
}
for (j = 0; j < 128/8; j++) {
binKey[j + 128/8] ^= outBlock[j];
}
break;
}
}
#ifdef TRACE_KAT_MCT
elapsed += clock();
printf (" done (%.1f s).\n", (float)elapsed/CLOCKS_PER_SEC);
#endif /* ?TRACE_KAT_MCT */
} /* rijndaelCBC_MCT */
static void rijndaelECB_MCT (FILE *fp, const char *initKey, int keyLength,
const char *initBlock, int blockLength, BYTE direction)
{
int i, j;
BYTE inBlock[4*MAXBC], outBlock[4*MAXBC], binKey[4*MAXKC];
BYTE keyMaterial[320];
keyInstance keyInst;
cipherInstance cipherInst;
#ifdef TRACE_KAT_MCT
int width = 0;
clock_t elapsed = -clock();
printf ("Executing ECB MCT (%s, key %d): ",
direction == DIR_ENCRYPT ? "ENCRYPT" : "DECRYPT", keyLength);
fflush (stdout);
#endif /* ?TRACE_KAT_MCT */
fprintf (fp,
"\n"
"=========================\n"
"\n"
"KEYSIZE=%d\n", keyLength);
fflush (fp);
HexToBin (outBlock, initBlock, blockLength);
HexToBin (binKey, initKey, keyLength);
for (i = 0; i < 400; i++) {
#ifdef TRACE_KAT_MCT
while (width-- > 0) putchar ('\b'); width = printf ("%d", i); fflush (stdout);
#endif /* ?TRACE_KAT_MCT */
fprintf (fp, "\nI=%d\n", i);
/* prepare key: */
for (j = 0; j < keyLength/8; j++) {
sprintf (&keyMaterial[2*j], "%02X", binKey[j]);
}
keyMaterial[keyLength/4] = 0;
fprintf (fp, "KEY=%s\n", keyMaterial);
keyInst.blockLen = blockLength;
makeKey(&keyInst, direction, keyLength, keyMaterial);
/* do encryption/decryption: */
blockPrint (fp, outBlock, blockLength, direction == DIR_ENCRYPT ? "PT" : "CT");
cipherInst.blockLen = blockLength;
cipherInit (&cipherInst, MODE_ECB, NULL);
if (direction == DIR_ENCRYPT) {
for (j = 0; j < 10000; j++) {
memcpy (inBlock, outBlock, blockLength/8);
blockEncrypt(&cipherInst, &keyInst, inBlock, blockLength, outBlock);
}
} else {
for (j = 0; j < 10000; j++) {
memcpy (inBlock, outBlock, blockLength/8);
blockDecrypt(&cipherInst, &keyInst, inBlock, blockLength, outBlock);
}
}
blockPrint (fp, outBlock, blockLength, direction == DIR_ENCRYPT ? "CT" : "PT");
/* prepare new key: */
switch (keyLength) {
case 128:
for (j = 0; j < 128/8; j++) {
binKey[j] ^= outBlock[j];
}
break;
case 192:
for (j = 0; j < 64/8; j++) {
binKey[j] ^= inBlock[j + 64/8];
}
for (j = 0; j < 128/8; j++) {
binKey[j + 64/8] ^= outBlock[j];
}
break;
case 256:
for (j = 0; j < 128/8; j++) {
binKey[j] ^= inBlock[j];
}
for (j = 0; j < 128/8; j++) {
binKey[j + 128/8] ^= outBlock[j];
}
break;
}
}
#ifdef TRACE_KAT_MCT
elapsed += clock();
printf (" done (%.1f s).\n", (float)elapsed/CLOCKS_PER_SEC);
#endif /* ?TRACE_KAT_MCT */
} /* rijndaelECB_MCT */
