t_stat doFaultInstructionPair(DCDstruct *i, word24 fltAddress)
{
// XXX stolen from xed instruction
DCDstruct _xip; // our decoded instruction struct
EISstruct _eis;
word36 insPair[2];
Read2(i, fltAddress, &insPair[0], &insPair[1], InstructionFetch, 0);
_xip.IWB = insPair[0];
_xip.e = &_eis;
DCDstruct *xec = decodeInstruction(insPair[0], &_xip); // fetch instruction into current instruction
t_stat ret = executeInstruction(xec);
if (ret)
return (ret);
_xip.IWB = insPair[1];
_xip.e = &_eis;
xec = decodeInstruction(insPair[1], &_xip); // fetch instruction into current instruction
ret = executeInstruction(xec);
//if (ret)
// return (ret);
//
//return SCPE_OK;
return ret;
}
bool RoboClaw::GetConfig(uint8_t address, uint16_t &config){
bool valid;
uint16_t value = Read2(address,GETCONFIG,&valid);
if(valid){
config = value;
}
return valid;
}
bool RoboClaw::ReadEncoderModes(uint8_t address, uint8_t &M1mode, uint8_t &M2mode){
bool valid;
uint16_t value = Read2(address,GETENCODERMODE,&valid);
if(valid){
M1mode = value>>8;
M2mode = value;
}
return valid;
}
bool RoboClaw::GetDeadBand(uint8_t address, uint8_t &Min, uint8_t &Max){
bool valid;
uint16_t value = Read2(address,GETDEADBAND,&valid);
if(valid){
Min = value>>8;
Max = value;
}
return valid;
}
bool RoboClaw::ReadBuffers(uint8_t address, uint8_t &depth1, uint8_t &depth2){
bool valid;
uint16_t value = Read2(address,GETBUFFERS,&valid);
if(valid){
depth1 = value>>8;
depth2 = value;
}
return valid;
}
static int read_short(dest_t *dest, glui16 *val)
{
unsigned char buf[2];
int res = read_buffer(dest, buf, 2);
if (res)
return res;
*val = Read2(buf);
return 0;
}
bool RoboClaw::ReadCurrents(uint8_t address, uint8_t ¤t1, uint8_t ¤t2){
bool valid;
uint16_t value = Read2(address,GETCURRENTS,&valid);
if(valid){
current1 = value>>8;
current2 = value;
}
return valid;
}
unsigned char* ReadZip(const char* filename,
int* num_chunks, ImageChunk** chunks,
int include_pseudo_chunk) {
struct stat st;
if (stat(filename, &st) != 0) {
printf("failed to stat \"%s\": %s\n", filename, strerror(errno));
return NULL;
}
unsigned char* img = malloc(st.st_size);
FILE* f = fopen(filename, "rb");
if (fread(img, 1, st.st_size, f) != st.st_size) {
printf("failed to read \"%s\" %s\n", filename, strerror(errno));
fclose(f);
return NULL;
}
fclose(f);
// look for the end-of-central-directory record.
int i;
for (i = st.st_size-20; i >= 0 && i > st.st_size - 65600; --i) {
if (img[i] == 0x50 && img[i+1] == 0x4b &&
img[i+2] == 0x05 && img[i+3] == 0x06) {
break;
}
}
// double-check: this archive consists of a single "disk"
if (!(img[i+4] == 0 && img[i+5] == 0 && img[i+6] == 0 && img[i+7] == 0)) {
printf("can't process multi-disk archive\n");
return NULL;
}
int cdcount = Read2(img+i+8);
int cdoffset = Read4(img+i+16);
ZipFileEntry* temp_entries = malloc(cdcount * sizeof(ZipFileEntry));
int entrycount = 0;
unsigned char* cd = img+cdoffset;
for (i = 0; i < cdcount; ++i) {
if (!(cd[0] == 0x50 && cd[1] == 0x4b && cd[2] == 0x01 && cd[3] == 0x02)) {
printf("bad central directory entry %d\n", i);
return NULL;
}
int clen = Read4(cd+20); // compressed len
int ulen = Read4(cd+24); // uncompressed len
int nlen = Read2(cd+28); // filename len
int xlen = Read2(cd+30); // extra field len
int mlen = Read2(cd+32); // file comment len
int hoffset = Read4(cd+42); // local header offset
char* filename = malloc(nlen+1);
memcpy(filename, cd+46, nlen);
filename[nlen] = '\0';
int method = Read2(cd+10);
cd += 46 + nlen + xlen + mlen;
if (method != 8) { // 8 == deflate
free(filename);
continue;
}
unsigned char* lh = img + hoffset;
if (!(lh[0] == 0x50 && lh[1] == 0x4b && lh[2] == 0x03 && lh[3] == 0x04)) {
printf("bad local file header entry %d\n", i);
return NULL;
}
if (Read2(lh+26) != nlen || memcmp(lh+30, filename, nlen) != 0) {
printf("central dir filename doesn't match local header\n");
return NULL;
}
xlen = Read2(lh+28); // extra field len; might be different from CD entry?
temp_entries[entrycount].data_offset = hoffset+30+nlen+xlen;
temp_entries[entrycount].deflate_len = clen;
temp_entries[entrycount].uncomp_len = ulen;
temp_entries[entrycount].filename = filename;
++entrycount;
}
qsort(temp_entries, entrycount, sizeof(ZipFileEntry), fileentry_compare);
#if 1
printf("found %d deflated entries\n", entrycount);
for (i = 0; i < entrycount; ++i) {
printf("off %10zd len %10zd unlen %10zd %p %s\n",
temp_entries[i].data_offset,
temp_entries[i].deflate_len,
temp_entries[i].uncomp_len,
temp_entries[i].filename,
temp_entries[i].filename);
}
#endif
*num_chunks = 0;
*chunks = malloc((entrycount*2+2) * sizeof(ImageChunk));
ImageChunk* curr = *chunks;
if (include_pseudo_chunk) {
curr->type = CHUNK_NORMAL;
curr->start = 0;
curr->len = st.st_size;
curr->data = img;
curr->filename = NULL;
curr->I = NULL;
++curr;
++*num_chunks;
}
int pos = 0;
int nextentry = 0;
while (pos < st.st_size) {
if (nextentry < entrycount && pos == temp_entries[nextentry].data_offset) {
curr->type = CHUNK_DEFLATE;
curr->start = pos;
curr->deflate_len = temp_entries[nextentry].deflate_len;
curr->deflate_data = img + pos;
curr->filename = temp_entries[nextentry].filename;
curr->I = NULL;
curr->len = temp_entries[nextentry].uncomp_len;
curr->data = malloc(curr->len);
z_stream strm;
strm.zalloc = Z_NULL;
strm.zfree = Z_NULL;
strm.opaque = Z_NULL;
strm.avail_in = curr->deflate_len;
strm.next_in = curr->deflate_data;
// -15 means we are decoding a 'raw' deflate stream; zlib will
// not expect zlib headers.
int ret = inflateInit2(&strm, -15);
strm.avail_out = curr->len;
strm.next_out = curr->data;
ret = inflate(&strm, Z_NO_FLUSH);
if (ret != Z_STREAM_END) {
printf("failed to inflate \"%s\"; %d\n", curr->filename, ret);
return NULL;
}
inflateEnd(&strm);
pos += curr->deflate_len;
++nextentry;
++*num_chunks;
++curr;
continue;
}
// use a normal chunk to take all the data up to the start of the
// next deflate section.
curr->type = CHUNK_NORMAL;
curr->start = pos;
if (nextentry < entrycount) {
curr->len = temp_entries[nextentry].data_offset - pos;
} else {
curr->len = st.st_size - pos;
}
curr->data = img + pos;
curr->filename = NULL;
curr->I = NULL;
pos += curr->len;
++*num_chunks;
++curr;
}
free(temp_entries);
return img;
}
uint16_t RoboClaw::ReadError(uint8_t address,bool *valid){
return Read2(address,GETERROR,valid);
}
bool RoboClaw::ReadTemp2(uint8_t address, uint16_t &temp){
bool valid;
temp = Read2(address,GETTEMP2,&valid);
return valid;
}
uint16_t RoboClaw::ReadLogicBatteryVoltage(uint8_t address,bool *valid){
return Read2(address,GETLBATT,valid);
}
static glui32 read_stackstate(dest_t *dest, glui32 chunklen, int portable)
{
glui32 res;
glui32 frameend, frm, frm2, frm3, locpos, frlen, numlocals;
if (chunklen > stacksize)
return 1;
stackptr = chunklen;
frameptr = 0;
valstackbase = 0;
localsbase = 0;
if (!portable) {
res = read_buffer(dest, stack, stackptr);
if (res)
return res;
return 0;
}
/* This isn't going to be pleasant; we're going to read the data in
as a block, and then convert it in-place. */
res = read_buffer(dest, stack, stackptr);
if (res)
return res;
frameend = stackptr;
while (frameend != 0) {
/* Read the beginning-of-frame pointer. Remember, right now, the
whole frame is stored big-endian. So we have to read with the
Read*() macros, and then write with the StkW*() macros. */
frm = Read4(stack+(frameend-4));
frm2 = frm;
frlen = Read4(stack+frm2);
StkW4(frm2, frlen);
frm2 += 4;
locpos = Read4(stack+frm2);
StkW4(frm2, locpos);
frm2 += 4;
/* The locals-format list is in bytes, so we don't have to convert it. */
frm3 = frm2;
frm2 = frm+locpos;
numlocals = 0;
while (1) {
unsigned char loctype, loccount;
loctype = Read1(stack+frm3);
frm3 += 1;
loccount = Read1(stack+frm3);
frm3 += 1;
if (loctype == 0 && loccount == 0)
break;
/* Skip up to 0, 1, or 3 bytes of padding, depending on loctype. */
while (frm2 & (loctype-1)) {
StkW1(frm2, 0);
frm2++;
}
/* Convert this set of locals. */
switch (loctype) {
case 1:
do {
/* Don't need to convert bytes. */
frm2 += 1;
loccount--;
} while (loccount);
break;
case 2:
do {
glui16 loc = Read2(stack+frm2);
StkW2(frm2, loc);
frm2 += 2;
loccount--;
} while (loccount);
break;
case 4:
do {
glui32 loc = Read4(stack+frm2);
StkW4(frm2, loc);
frm2 += 4;
loccount--;
} while (loccount);
break;
}
numlocals++;
}
if ((numlocals & 1) == 0) {
StkW1(frm3, 0);
frm3++;
StkW1(frm3, 0);
frm3++;
}
if (frm3 != frm+locpos) {
return 1;
}
while (frm2 & 3) {
StkW1(frm2, 0);
frm2++;
}
if (frm2 != frm+frlen) {
return 1;
}
/* Now, the values pushed on the stack after the call frame itself.
This includes the stub. */
while (frm2 < frameend) {
glui32 loc = Read4(stack+frm2);
StkW4(frm2, loc);
frm2 += 4;
}
frameend = frm;
}
return 0;
}