#include "analyzer.h"
// Converts a hexadecimal character [0-9A-F] into number 0..15, returns negative value otherwise
static int OneDigit(char c) {
c-='0';
if ((unsigned char)c<10) return c; // numbers
c-='A'-'0';
if (c<0) return c; // invalid
c+=10;
if (c<16) return c; // capital letters (small letters are not allowed here!)
c-=16;
return c; // invalid
}
// Takes two hexadecimal characters to form a byte, returns negative value on error
// Low-significant nibble comes first!
static int TwoDigits(const char *s) {
return OneDigit(s[0])|OneDigit(s[1])<<4;
}
// Extracts the 0..16 bytes out of STRING message (received data)
// Returns the length of data, in bytes, or a negative error code
int string_decode(const char*s, int l, int*a, void*dv, int dl) {
if (l&1) return wrong_length; // must be an even character count
l-=10;
if ((unsigned)l>42-10) return wrong_length; // must be in range 10..42
char cs=':';
if (*s++!=cs) return wrong_header;
if (s[l+7]!='\r') return wrong_footer;
if (s[l+8]!='\n') return wrong_footer;
const char *ss=s;
int i=l+6;
do cs+=*ss++; while (--i);
cs^=cs>>4;
if (OneDigit(*ss)!=(cs&0x0F)) return wrong_check;
i=TwoDigits(s), s+=2;
if (i!=l) return wrong_header; // length doesn't match
i=TwoDigits(s), s+=2;
i|=TwoDigits(s)<<8, s+=2;
if (i<0) return wrong_hex;
if (a) *a=i;
l>>=1;
char *d=(char*)dv;
for(i=0;i<l;i++) {
int i=TwoDigits(s); s+=2;
if (i<0) return wrong_hex;
if (dl>0) *d++=i;
dl--;
}
return l; // length of data
}
// Generates one hexadecimal character
static char OneHex(int n) {
n&=0x0F;
n+='0';
if (n>'9') n+='A'-'9'+1;
return n;
}
// Generates two hexadecimal characters
// Low-significant nibble comes first!
static void TwoHex(char*d, int n) {
d[0]=OneHex(n);
d[1]=OneHex(n>>4);
}
// Generates a STRING message out of given address and data
// Returns the string length, in characters, or a negative error code
int string_encode(const void*sv, int l, int a, char*d, int dl) {
if ((unsigned)l>16) return wrong_length;
int ret=(l<<1)+10;
if (dl < ret) return buffer_too_small;
char cs=':';
*d++=cs;
char *dd=d;
TwoHex(dd,ret-4), dd+=2;
TwoHex(dd,a), dd+=2;
TwoHex(dd,a>>8), dd+=2;
const char *s=(const char*)sv;
for (;l;l--) TwoHex(dd,*s++), dd+=2;
do cs+=*d++; while (d!=dd);
cs^=cs>>4;
*d++=OneHex(cs);
*d++='\r';
*d='\n';
return ret;
}
int string_recv(void*dv, int dlen, int(*sendrecv)(const char*,int,char*,int), BYTE*delim, int dl, int ae) {
int len=0; // byte counter
BYTE sum=delim&&!dl?*delim++:0;
char*d=(char*)dv;
for(;;){
char r[42]; // maximum 1+6+16*2+1+2 = 42 Bytes
static const char s[4]={0x1B,'O','\r','\n'};
int l=sendrecv(s,sizeof(s),r,sizeof(r));
if (l<0) return l+sendrecv_errors;
int a;
int e=string_decode(r,l,&a,d,dlen);
if (e<0) return e; // return this error code
if (delim) {
if (dl) { // Write the packed recordsize array
sum+=e;
if (e!=16 || sum==240) {
if (!--dl) return cannot_record;
*delim++=sum;
sum=0;
}
}else{ // Check received record size against packed recordsize array
if (e==16) {
if (sum<e) return unmatched_recordsize;
if (!(sum-=e)) goto nextdelim;
}else{
if (sum!=e) return unmatched_recordsize; // Must match exactly to the remainding length (even at EOF)
nextdelim:
sum=*delim++;
}
}
}
if (!e /*&& a==0x1000*/) goto okay; // EOF block: ready for cleanup
if (a!=ae) return -7; // sequencing error
d+=e; len+=e;
dlen-=e; // remaining space in target buffer
ae++;
if (dlen<0) {
if (dl && delim) return buffer_too_small;
static const char s[4]={0x1B,'A','\r','\n'}; // cancel transmission if buffer full
l=sendrecv(s,sizeof(s),r,sizeof(r));
if (l<0) return l+sendrecv_errors;
if (l==4 && *(DWORD*)r==*(DWORD*)s) goto okay;
return wrong_echo;
}
}
okay:
if (dl && delim) *delim=0;
return len;
}
// Packed recordsize arrays, zero-terminated
// Every "full" record containing 16 bytes is concatenated (i.e. run-length-encoded)
// with the next, maybe full, record, for up to 255 bytes
// (i.e. 15 contiguous "full" records and one 15-byte non-full record)
const BYTE RecordListP[]={5,22,6,15,11,63,13,141,45,91,71,77,93,240,240,94,206,30,199,32,0};
const BYTE RecordList0[]={14,15,13,15,15,15,15,47,12,9,9,9,9,9,9,15,12,12,12,8,0};
const BYTE RecordListT[]={14,15,9,9,9,9,9,9,9,0};
const BYTE RecordList6[]={15,15,6,0};
const BYTE RecordList9[]={24,0};
const BYTE RecordListD[]={10,10,16,0};
// Another interpretation: This packed list counts full (16-byte) records in the high nibble
// and partial byte counts (less than 16) in the low nibble of each byte.
// Calculates the needed records to read beyond <offset>
int offsettorecord(const BYTE*delim, int offset) {
if (offset<=0) return 0; // must be >= 1
int r,s,b;
for (r=0;b=*delim++;) do{
s=b; if (s>16) s=16; // unpack the packed list
r++; // count the records
if (offset<=s) return r; // at least one record
offset-=s;
b-=s;
}while (b);
return 0; // reached end of list (offset too large)
}
// counts the number of records of packed <delim> array (not the same as strlen() due to packing)
int numberofrecords(const BYTE*delim) {
int r=0;
BYTE b;
do{
b=*delim++;
r+=b>>4; // count full records
if (b&0x0F) r++; // count partial records
}while (b);
return r;
}
Detected encoding: ASCII (7 bit) | 2
|