1385 lines
38 KiB
C++
1385 lines
38 KiB
C++
//
|
|
// begin license header
|
|
//
|
|
// This file is part of Pixy CMUcam5 or "Pixy" for short
|
|
//
|
|
// All Pixy source code is provided under the terms of the
|
|
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
|
|
// Those wishing to use Pixy source code, software and/or
|
|
// technologies under different licensing terms should contact us at
|
|
// cmucam@cs.cmu.edu. Such licensing terms are available for
|
|
// all portions of the Pixy codebase presented here.
|
|
//
|
|
// end license header
|
|
//
|
|
|
|
#include <string.h>
|
|
#include <new>
|
|
#include "chirp.hpp"
|
|
#include "debuglog.h"
|
|
|
|
|
|
// todo yield, sleep() while waiting for sync response
|
|
// todo
|
|
|
|
// assume that destination is aligned on the correct boundary and copy the source byte by byte
|
|
void copyAlign(char *dest, const char *src, int size)
|
|
{
|
|
int i;
|
|
for (i=0; i<size; i++)
|
|
dest[i] = src[i];
|
|
}
|
|
|
|
Chirp::Chirp(bool hinterested, bool client, Link *link)
|
|
{
|
|
log("pixydebug: Chirp::Chirp()\n");
|
|
m_link = NULL;
|
|
m_errorCorrected = false;
|
|
m_sharedMem = false;
|
|
m_buf = NULL;
|
|
m_bufSave = NULL;
|
|
|
|
m_maxNak = CRP_MAX_NAK;
|
|
m_retries = CRP_RETRIES;
|
|
m_headerTimeout = CRP_HEADER_TIMEOUT;
|
|
m_dataTimeout = CRP_DATA_TIMEOUT;
|
|
m_idleTimeout = CRP_IDLE_TIMEOUT;
|
|
m_sendTimeout = CRP_SEND_TIMEOUT;
|
|
m_call = false;
|
|
m_connected = false;
|
|
m_hinformer = false;
|
|
m_hinterested = hinterested;
|
|
m_client = client;
|
|
|
|
m_procTableSize = CRP_PROCTABLE_LEN;
|
|
m_procTable = new (std::nothrow) ProcTableEntry[m_procTableSize];
|
|
memset(m_procTable, 0, sizeof(ProcTableEntry)*m_procTableSize);
|
|
|
|
if (link)
|
|
setLink(link);
|
|
log("pixydebug: Chirp::Chirp() returned\n");
|
|
}
|
|
|
|
Chirp::~Chirp()
|
|
{
|
|
log("pixydebug: Chirp::~Chirp()\n");
|
|
// if we're a client, disconnect (let server know)
|
|
if (m_client)
|
|
remoteInit(false);
|
|
if (!m_sharedMem)
|
|
{
|
|
restoreBuffer();
|
|
delete[] m_buf;
|
|
}
|
|
delete[] m_procTable;
|
|
log("pixydebug: Chirp::~Chirp() returned\n");
|
|
}
|
|
|
|
int Chirp::init(bool connect)
|
|
{
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::setLink(Link *link)
|
|
{
|
|
int return_value;
|
|
|
|
log("pixydebug: Chirp::setLink()\n");
|
|
m_link = link;
|
|
m_errorCorrected = m_link->getFlags()&LINK_FLAG_ERROR_CORRECTED;
|
|
m_sharedMem = m_link->getFlags()&LINK_FLAG_SHARED_MEM;
|
|
m_blkSize = m_link->blockSize();
|
|
|
|
if (m_errorCorrected)
|
|
m_headerLen = 12; // startcode (uint32_t), type (uint8_t), (pad), proc (uint16_t), len (uint32_t)
|
|
else
|
|
m_headerLen = 8; // type (uint8_t), (pad), proc (uint16_t), len (uint32_t)
|
|
|
|
if (m_sharedMem)
|
|
{
|
|
m_buf = (uint8_t *)m_link->getFlags(LINK_FLAG_INDEX_SHARED_MEMORY_LOCATION);
|
|
m_bufSize = m_link->getFlags(LINK_FLAG_INDEX_SHARED_MEMORY_SIZE);
|
|
}
|
|
else
|
|
{
|
|
m_bufSize = CRP_BUFSIZE;
|
|
m_buf = new (std::nothrow) uint8_t[m_bufSize];
|
|
}
|
|
|
|
// link is set up, need to call init
|
|
if (m_client) {
|
|
return_value = remoteInit(true);
|
|
log("pixydebug: remoteInit() = %d\n", return_value);
|
|
log("pixydebug: setLink() returned %d\n", return_value);
|
|
return return_value;
|
|
}
|
|
|
|
log("pixydebug: setLink() returned %d\n", CRP_RES_OK);
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
|
|
int Chirp::assemble(uint8_t type, ...)
|
|
{
|
|
int res;
|
|
va_list args;
|
|
bool save = m_call;
|
|
uint32_t saveLen = m_len;
|
|
|
|
if (type==CRP_XDATA)
|
|
m_call = false;
|
|
|
|
va_start(args, type);
|
|
res = vassemble(&args);
|
|
va_end(args);
|
|
|
|
if (type==CRP_XDATA || (!m_call && res==CRP_RES_OK)) // if we're not a call, we're extra data, so we need to send
|
|
{
|
|
res = sendChirpRetry(CRP_XDATA, 0);
|
|
m_len = saveLen;
|
|
}
|
|
|
|
m_call = save;
|
|
|
|
return res;
|
|
}
|
|
|
|
int Chirp::vassemble(va_list *args)
|
|
{
|
|
int len;
|
|
|
|
len = vserialize(this, m_buf, m_bufSize, args);
|
|
// check for error
|
|
if (len<0)
|
|
return len;
|
|
|
|
// set length (don't include header)
|
|
m_len = len - m_headerLen;
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
bool Chirp::connected()
|
|
{
|
|
return m_connected;
|
|
}
|
|
|
|
int Chirp::useBuffer(uint8_t *buf, uint32_t len)
|
|
{
|
|
int res;
|
|
|
|
if (m_bufSave==NULL)
|
|
{
|
|
m_bufSave = m_buf;
|
|
m_buf = buf;
|
|
}
|
|
else if (buf!=m_buf)
|
|
return CRP_RES_ERROR_MEMORY;
|
|
|
|
m_len = len-m_headerLen;
|
|
if (!m_call) // if we're not a call, we're extra data, so we need to send
|
|
{
|
|
res = sendChirpRetry(CRP_XDATA, 0);
|
|
restoreBuffer(); // restore buffer immediately!
|
|
if (res!=CRP_RES_OK) // convert call into response
|
|
return res;
|
|
}
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
void Chirp::restoreBuffer()
|
|
{
|
|
if (m_bufSave)
|
|
{
|
|
m_buf = m_bufSave;
|
|
m_bufSave = NULL;
|
|
}
|
|
}
|
|
|
|
|
|
int Chirp::serialize(Chirp *chirp, uint8_t *buf, uint32_t bufSize, ...)
|
|
{
|
|
int res;
|
|
va_list args;
|
|
|
|
va_start(args, bufSize);
|
|
res = vserialize(chirp, buf, bufSize, &args);
|
|
va_end(args);
|
|
|
|
return res;
|
|
}
|
|
|
|
#define RESIZE_BUF(size) \
|
|
if (size > bufSize-CRP_BUFPAD) \
|
|
{ \
|
|
if (!chirp) \
|
|
return CRP_RES_ERROR_MEMORY; \
|
|
else \
|
|
{ \
|
|
if ((res=chirp->realloc(size))<0) \
|
|
return res; \
|
|
buf = chirp->m_buf; \
|
|
bufSize = chirp->m_bufSize; \
|
|
} \
|
|
} \
|
|
|
|
|
|
int Chirp::vserialize(Chirp *chirp, uint8_t *buf, uint32_t bufSize, va_list *args)
|
|
{
|
|
int res;
|
|
uint8_t type, origType;
|
|
uint32_t i, si;
|
|
bool copy = true;
|
|
|
|
if (chirp)
|
|
{
|
|
if (chirp->m_call) // reserve an extra 4 for responseint
|
|
i = chirp->m_headerLen+4;
|
|
else // if it's a chirp call, just reserve the header
|
|
i = chirp->m_headerLen;
|
|
}
|
|
else
|
|
i = 0;
|
|
|
|
bufSize -= i;
|
|
|
|
while(1)
|
|
{
|
|
#if 1
|
|
type = va_arg(*args, int);
|
|
#else
|
|
type = va_arg(*args, uint8_t);
|
|
#endif
|
|
|
|
if (type==END)
|
|
break;
|
|
|
|
si = i; // save index so we can skip over data if needed
|
|
buf[i++] = type;
|
|
|
|
// treat hints like other types for now
|
|
// but if gotoe (guy on the other end) isn't interested in hints (m_hinformer=false),
|
|
// we'll restore index to si and effectively skip data.
|
|
origType = type;
|
|
type &= ~CRP_HINT;
|
|
|
|
if (type==CRP_INT8)
|
|
{
|
|
#if 1
|
|
int8_t val = va_arg(*args, int);
|
|
#else
|
|
int8_t val = va_arg(*args, int8_t);
|
|
#endif
|
|
*(int8_t *)(buf+i) = val;
|
|
i += 1;
|
|
}
|
|
else if (type==CRP_INT16)
|
|
{
|
|
#if 1
|
|
int16_t val = va_arg(*args, int);
|
|
#else
|
|
int16_t val = va_arg(*args, int16_t);
|
|
#endif
|
|
ALIGN(i, 2);
|
|
// rewrite type so getType will work (even though we might add padding between type and data)
|
|
buf[i-1] = origType;
|
|
*(int16_t *)(buf+i) = val;
|
|
i += 2;
|
|
}
|
|
else if (type==CRP_INT32 || origType==CRP_TYPE_HINT) // CRP_TYPE_HINT is a special case...
|
|
{
|
|
int32_t val = va_arg(*args, int32_t);
|
|
ALIGN(i, 4);
|
|
buf[i-1] = origType;
|
|
*(int32_t *)(buf+i) = val;
|
|
i += 4;
|
|
}
|
|
else if (type==CRP_FLT32)
|
|
{
|
|
#if 1
|
|
float val = va_arg(*args, double);
|
|
#else
|
|
float val = va_arg(*args, float);
|
|
#endif
|
|
ALIGN(i, 4);
|
|
buf[i-1] = origType;
|
|
*(float *)(buf+i) = val;
|
|
i += 4;
|
|
}
|
|
else if (type==CRP_STRING)
|
|
{
|
|
int8_t *s = va_arg(*args, int8_t *);
|
|
uint32_t len = strlen((char *)s)+1; // include null
|
|
|
|
RESIZE_BUF(len+i);
|
|
|
|
memcpy(buf+i, s, len);
|
|
i += len;
|
|
}
|
|
else if (type&CRP_ARRAY)
|
|
{
|
|
uint8_t size = type&0x0f;
|
|
uint32_t len = va_arg(*args, int32_t);
|
|
|
|
// deal with no copy case (use our own buffer)
|
|
if ((type&CRP_NO_COPY)==CRP_NO_COPY)
|
|
{
|
|
// rewrite type so as not to confuse gotoe
|
|
origType = type &= ~CRP_NO_COPY;
|
|
buf[i-1] = origType;
|
|
copy = false;
|
|
}
|
|
|
|
ALIGN(i, 4);
|
|
buf[i-1] = origType;
|
|
*(uint32_t *)(buf+i) = len;
|
|
i += 4;
|
|
ALIGN(i, size);
|
|
|
|
if (copy)
|
|
{
|
|
len *= size; // scale by size of array elements
|
|
|
|
RESIZE_BUF(len+i);
|
|
|
|
int8_t *ptr = va_arg(*args, int8_t *);
|
|
memcpy(buf+i, ptr, len);
|
|
i += len;
|
|
}
|
|
}
|
|
else
|
|
return CRP_RES_ERROR_PARSE;
|
|
|
|
// skip hint data if we're not a source
|
|
if (chirp && !chirp->m_hinformer && origType&CRP_HINT)
|
|
i = si;
|
|
|
|
RESIZE_BUF(i);
|
|
}
|
|
|
|
// return length
|
|
return i;
|
|
}
|
|
|
|
|
|
// this isn't completely necessary, but it makes things a lot easier to use.
|
|
// passing a pointer to a pointer and then having to dereference is just confusing....
|
|
// so for scalars (ints, floats) you don't need to pass in ** pointers, just * pointers so
|
|
// chirp can write the value into the * pointer and hand it back.
|
|
// But for arrays you need ** pointers, so chirp doesn't need to copy the whole array into your buffer---
|
|
// chirp will write the * pointer value into your ** pointer.
|
|
int Chirp::loadArgs(va_list *args, void *recvArgs[])
|
|
{
|
|
int i;
|
|
uint8_t type, size;
|
|
void **recvArg;
|
|
|
|
for (i=0; recvArgs[i]!=NULL && i<CRP_MAX_ARGS; i++)
|
|
{
|
|
type = getType(recvArgs[i]);
|
|
recvArg = va_arg(*args, void **);
|
|
if (recvArg==NULL)
|
|
return CRP_RES_ERROR_PARSE;
|
|
|
|
if (!(type&CRP_ARRAY)) // if we're a scalar
|
|
{
|
|
size = type&0x0f;
|
|
if (size==1) *(uint8_t *)recvArg = *(uint8_t *)recvArgs[i];
|
|
else if (size==2) *(uint16_t *)recvArg = *(uint16_t *)recvArgs[i];
|
|
else if (size==4) *(uint32_t *)recvArg = *(uint32_t *)recvArgs[i];
|
|
//else if (size==8) *recvArg = *(double *)recvArgs[i];
|
|
else return CRP_RES_ERROR_PARSE;
|
|
}
|
|
else // we're an array
|
|
{
|
|
if (type==CRP_STRING)
|
|
*(char **)recvArg = (char *)recvArgs[i];
|
|
else
|
|
{
|
|
*(uint32_t *)recvArg = *(uint32_t *)recvArgs[i++];
|
|
recvArg = va_arg(*args, void **);
|
|
if (recvArg==NULL)
|
|
return CRP_RES_ERROR_PARSE;
|
|
*(void **)recvArg = recvArgs[i];
|
|
}
|
|
}
|
|
}
|
|
// check to see if last arg is NULL, if not, we have a parse error
|
|
// if the arg isn't null, it means the caller is expecting data to be
|
|
// put there. If data isn't put there, and the caller dereferences, segfault
|
|
if (va_arg(*args, void **)!=NULL)
|
|
return CRP_RES_ERROR_PARSE;
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::call(uint8_t service, ChirpProc proc, va_list args)
|
|
{
|
|
int res, i;
|
|
uint8_t type;
|
|
va_list arguments;
|
|
|
|
va_copy(arguments, args);
|
|
|
|
// if it's just a regular call (not init or enumerate), we need to be connected
|
|
if (!(service&CRP_CALL) && !m_connected)
|
|
return CRP_RES_ERROR_NOT_CONNECTED;
|
|
|
|
// parse arguments and assemble in m_buf
|
|
m_len = 0;
|
|
// restore buffer in case it was changed
|
|
restoreBuffer();
|
|
if ((res=vassemble(&arguments))<0)
|
|
{
|
|
va_end(arguments);
|
|
return res;
|
|
}
|
|
|
|
if (service&CRP_CALL) // special case for enumerate and init (internal calls)
|
|
{
|
|
type = service;
|
|
service = SYNC;
|
|
}
|
|
else
|
|
type = CRP_CALL;
|
|
|
|
// send call data
|
|
if ((res=sendChirpRetry(type, proc))!=CRP_RES_OK) // convert call into response
|
|
{
|
|
va_end(arguments);
|
|
return res;
|
|
}
|
|
|
|
|
|
// if the service is synchronous, receive response while servicing other calls
|
|
if (!(service&ASYNC))
|
|
{
|
|
ChirpProc recvProc;
|
|
void *recvArgs[CRP_MAX_ARGS+1];
|
|
m_link->setTimer(); // set timer, so we can check to see if we're taking too much time
|
|
|
|
while(1)
|
|
{
|
|
if ((res=recvChirp(&type, &recvProc, recvArgs, true))==CRP_RES_OK)
|
|
{
|
|
if (type&CRP_RESPONSE)
|
|
break;
|
|
else // handle calls as they come in
|
|
handleChirp(type, recvProc, (const void **)recvArgs);
|
|
}
|
|
else
|
|
{
|
|
va_end(arguments);
|
|
return res;
|
|
}
|
|
if (m_link->getTimer()>m_headerTimeout) // we could receive XDATA (for example) and never exit this while loop
|
|
return CRP_RES_ERROR_RECV_TIMEOUT;
|
|
}
|
|
|
|
// deal with arguments
|
|
if (service&RETURN_ARRAY) // copy array of arguments
|
|
{
|
|
void **recvArray;
|
|
while(1)
|
|
{
|
|
recvArray = va_arg(arguments, void **);
|
|
if (recvArray!=NULL)
|
|
break;
|
|
}
|
|
for (i=0; recvArgs[i]; i++)
|
|
recvArray[i] = recvArgs[i];
|
|
recvArray[i] = NULL;
|
|
}
|
|
else if ((res=loadArgs(&arguments, recvArgs))<0)
|
|
{
|
|
va_end(arguments);
|
|
return res;
|
|
}
|
|
}
|
|
|
|
|
|
va_end(arguments);
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::call(uint8_t service, ChirpProc proc, ...)
|
|
{
|
|
int result;
|
|
|
|
va_list arguments;
|
|
|
|
va_start(arguments, proc);
|
|
result = call(service, proc, arguments);
|
|
va_end(arguments);
|
|
|
|
return result;
|
|
}
|
|
|
|
int Chirp::sendChirpRetry(uint8_t type, ChirpProc proc)
|
|
{
|
|
int i, res=-1;
|
|
|
|
if (!m_connected && !(type&CRP_INTRINSIC))
|
|
return CRP_RES_ERROR_NOT_CONNECTED;
|
|
// deal with case where there is no actual data (e.g. it's all hint data and gotoe isn't hinterested)
|
|
// but chirp calls can have no data of course
|
|
if (m_len==0 && !(type&CRP_CALL))
|
|
return CRP_RES_OK;
|
|
for (i=0; i<m_retries; i++)
|
|
{
|
|
res = sendChirp(type, proc);
|
|
if (res==CRP_RES_OK)
|
|
break;
|
|
}
|
|
|
|
// if sending the chirp fails after retries, we should assume we're no longer connected
|
|
if (res<0)
|
|
m_connected = false;
|
|
|
|
return res;
|
|
}
|
|
|
|
int Chirp::sendChirp(uint8_t type, ChirpProc proc)
|
|
{
|
|
int res;
|
|
if (m_errorCorrected)
|
|
res = sendFull(type, proc);
|
|
else
|
|
{
|
|
// we'll send forever as long as we get naks
|
|
// we rely on receiver to give up
|
|
while((res=sendHeader(type, proc))==CRP_RES_ERROR_CRC);
|
|
if (res!=CRP_RES_OK)
|
|
return res;
|
|
res = sendData();
|
|
}
|
|
if (res!=CRP_RES_OK)
|
|
return res;
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::handleChirp(uint8_t type, ChirpProc proc, const void *args[])
|
|
{
|
|
int res;
|
|
int32_t responseInt = 0;
|
|
uint8_t n;
|
|
|
|
// default case, we return one integer (responseint)
|
|
m_len = 4;
|
|
|
|
// check for intrinsic calls
|
|
if (type&CRP_INTRINSIC)
|
|
{
|
|
m_call = true; // indicate to ourselves that this is a chirp call
|
|
if (type==CRP_CALL_ENUMERATE)
|
|
responseInt = handleEnumerate((char *)args[0], (ChirpProc *)args[1]);
|
|
else if (type==CRP_CALL_INIT)
|
|
responseInt = handleInit((uint16_t *)args[0], (uint8_t *)args[1]);
|
|
else if (type==CRP_CALL_ENUMERATE_INFO)
|
|
responseInt = handleEnumerateInfo((ChirpProc *)args[0]);
|
|
else
|
|
responseInt = CRP_RES_ERROR;
|
|
m_call = false;
|
|
}
|
|
else if (type==CRP_XDATA)
|
|
{
|
|
handleXdata(args);
|
|
return CRP_RES_OK;
|
|
}
|
|
else // normal call
|
|
{
|
|
if (proc>=m_procTableSize || proc<0)
|
|
return CRP_RES_ERROR; // index exceeded
|
|
|
|
ProcPtr ptr = m_procTable[proc].procPtr;
|
|
if (ptr==NULL)
|
|
return CRP_RES_ERROR; // some chirps are not meant to be called in both directions
|
|
|
|
// count args
|
|
for (n=0; args[n]!=NULL; n++);
|
|
|
|
m_call = true; // indicate to ourselves that this is a chirp call
|
|
// this is probably overkill....
|
|
if (n==0)
|
|
responseInt = (*ptr)(this);
|
|
else if (n==1)
|
|
responseInt = (*(uint32_t(*)(const void*,Chirp*))ptr)(args[0],this);
|
|
else if (n==2)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,Chirp*))ptr)(args[0],args[1],this);
|
|
else if (n==3)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],this);
|
|
else if (n==4)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],this);
|
|
else if (n==5)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],this);
|
|
else if (n==6)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],this);
|
|
else if (n==7)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],args[6],this);
|
|
else if (n==8)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],args[6],args[7],this);
|
|
else if (n==9)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],args[6],args[7],args[8],this);
|
|
else if (n==10)
|
|
responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],args[6],args[7],args[8],args[9],this);
|
|
else
|
|
responseInt = CRP_RES_ERROR;
|
|
m_call = false;
|
|
}
|
|
|
|
// if it's a chirp call, we need to send back the result
|
|
// result is in m_buf
|
|
if (type&CRP_CALL)
|
|
{
|
|
// write responseInt
|
|
*(uint32_t *)(m_buf+m_headerLen) = responseInt;
|
|
// send response
|
|
res = sendChirpRetry(CRP_RESPONSE | (type&~CRP_CALL), m_procTable[proc].chirpProc); // convert call into response
|
|
restoreBuffer(); // restore buffer immediately!
|
|
if (res!=CRP_RES_OK)
|
|
return res;
|
|
}
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::reallocTable()
|
|
{
|
|
ProcTableEntry *newProcTable;
|
|
int newProcTableSize;
|
|
|
|
// allocate new table, zero
|
|
newProcTableSize = m_procTableSize+CRP_PROCTABLE_LEN;
|
|
newProcTable = new (std::nothrow) ProcTableEntry[newProcTableSize];
|
|
if (newProcTable==NULL)
|
|
return CRP_RES_ERROR_MEMORY;
|
|
memset(newProcTable, 0, sizeof(ProcTableEntry)*newProcTableSize);
|
|
// copy to new table
|
|
memcpy(newProcTable, m_procTable, sizeof(ProcTableEntry)*m_procTableSize);
|
|
// delete old table
|
|
delete [] m_procTable;
|
|
// set to new
|
|
m_procTable = newProcTable;
|
|
m_procTableSize = newProcTableSize;
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
ChirpProc Chirp::lookupTable(const char *procName)
|
|
{
|
|
ChirpProc i;
|
|
|
|
for(i=0; i<m_procTableSize; i++)
|
|
{
|
|
if (m_procTable[i].procName!=NULL && strcmp(m_procTable[i].procName, procName)==0)
|
|
return i;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
|
|
ChirpProc Chirp::updateTable(const char *procName, ProcPtr procPtr)
|
|
{
|
|
// if it exists already, update,
|
|
// if it doesn't exist, add it
|
|
if (procName==NULL)
|
|
return -1;
|
|
|
|
ChirpProc proc = lookupTable(procName);
|
|
if (proc<0) // next empty entry
|
|
{
|
|
for (proc=0; proc<m_procTableSize && m_procTable[proc].procName; proc++);
|
|
if (proc==m_procTableSize)
|
|
{
|
|
reallocTable();
|
|
return updateTable(procName, procPtr);
|
|
}
|
|
}
|
|
|
|
// add to table
|
|
m_procTable[proc].procName = procName;
|
|
m_procTable[proc].procPtr = procPtr;
|
|
|
|
return proc;
|
|
}
|
|
|
|
ChirpProc Chirp::getProc(const char *procName, ProcPtr callback)
|
|
{
|
|
uint32_t res;
|
|
ChirpProc cproc = -1;
|
|
|
|
if (callback)
|
|
cproc = updateTable(procName, callback);
|
|
|
|
if (call(CRP_CALL_ENUMERATE, 0,
|
|
STRING(procName), // send name
|
|
INT16(cproc), // send local index
|
|
END_OUT_ARGS,
|
|
&res, // get remote index
|
|
END_IN_ARGS
|
|
)>=0)
|
|
return res;
|
|
|
|
// a negative ChirpProc is an error
|
|
return -1;
|
|
}
|
|
|
|
int Chirp::remoteInit(bool connect)
|
|
{
|
|
int res;
|
|
uint32_t responseInt;
|
|
uint8_t hinformer;
|
|
|
|
res = call(CRP_CALL_INIT, 0,
|
|
UINT16(connect ? m_blkSize : 0), // send block size
|
|
UINT8(m_hinterested), // send whether we're interested in hints or not
|
|
END_OUT_ARGS,
|
|
&responseInt,
|
|
&hinformer, // receive whether we should send hints
|
|
END_IN_ARGS
|
|
);
|
|
if (res>=0)
|
|
{
|
|
m_connected = connect;
|
|
m_hinformer = hinformer;
|
|
return responseInt;
|
|
}
|
|
return res;
|
|
}
|
|
|
|
int Chirp::getProcInfo(ChirpProc proc, ProcInfo *info)
|
|
{
|
|
uint32_t responseInt;
|
|
int res;
|
|
|
|
res = call(CRP_CALL_ENUMERATE_INFO, 0,
|
|
UINT16(proc),
|
|
END_OUT_ARGS,
|
|
&responseInt,
|
|
&info->procName,
|
|
&info->argTypes,
|
|
&info->procInfo,
|
|
END_IN_ARGS
|
|
);
|
|
|
|
if (res==CRP_RES_OK)
|
|
return responseInt;
|
|
return res;
|
|
}
|
|
|
|
int Chirp::setProc(const char *procName, ProcPtr proc, ProcTableExtension *extension)
|
|
{
|
|
ChirpProc cProc = updateTable(procName, proc);
|
|
if (cProc<0)
|
|
return CRP_RES_ERROR;
|
|
|
|
m_procTable[cProc].extension = extension;
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::registerModule(const ProcModule *module)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; module[i].procName; i++)
|
|
setProc(module[i].procName, module[i].procPtr, (ProcTableExtension *)module[i].argTypes);
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
void Chirp::setSendTimeout(uint32_t timeout)
|
|
{
|
|
m_sendTimeout = timeout;
|
|
}
|
|
|
|
void Chirp::setRecvTimeout(uint32_t timeout)
|
|
{
|
|
m_headerTimeout = timeout;
|
|
}
|
|
|
|
int32_t Chirp::handleEnumerate(char *procName, ChirpProc *callback)
|
|
{
|
|
ChirpProc proc;
|
|
// lookup in table
|
|
proc = lookupTable(procName);
|
|
// set remote index in table
|
|
m_procTable[proc].chirpProc = *callback;
|
|
|
|
return proc;
|
|
}
|
|
|
|
int32_t Chirp::handleInit(uint16_t *blkSize, uint8_t *hinformer)
|
|
{
|
|
int32_t responseInt;
|
|
|
|
bool connect = *blkSize ? true : false;
|
|
responseInt = init(connect);
|
|
m_connected = connect;
|
|
m_blkSize = *blkSize; // get block size, write it
|
|
m_hinformer = *hinformer;
|
|
|
|
CRP_RETURN(this, UINT8(m_hinterested), END);
|
|
|
|
return responseInt;
|
|
}
|
|
|
|
int32_t Chirp::handleEnumerateInfo(ChirpProc *proc)
|
|
{
|
|
const ProcTableExtension *extension;
|
|
uint8_t null = '\0';
|
|
|
|
if (*proc>=m_procTableSize || m_procTable[*proc].procName==NULL)
|
|
extension = NULL;
|
|
else
|
|
extension = m_procTable[*proc].extension;
|
|
|
|
if (extension)
|
|
{
|
|
CRP_RETURN(this, STRING(m_procTable[*proc].procName), STRING(extension->argTypes),
|
|
STRING(extension->procInfo), END);
|
|
return CRP_RES_OK;
|
|
}
|
|
else // no extension, just send procedure name
|
|
{
|
|
CRP_RETURN(this, STRING(m_procTable[*proc].procName), STRING(&null),
|
|
STRING(&null), END);
|
|
return CRP_RES_ERROR;
|
|
}
|
|
}
|
|
|
|
int Chirp::realloc(uint32_t min)
|
|
{
|
|
if (m_sharedMem)
|
|
return CRP_RES_ERROR_MEMORY;
|
|
|
|
if (!min)
|
|
min = m_bufSize+CRP_BUFSIZE;
|
|
else
|
|
min += CRP_BUFSIZE;
|
|
uint8_t *newbuf = new (std::nothrow) uint8_t[min];
|
|
if (newbuf==NULL)
|
|
return CRP_RES_ERROR_MEMORY;
|
|
memcpy(newbuf, m_buf, m_bufSize);
|
|
delete[] m_buf;
|
|
m_buf = newbuf;
|
|
m_bufSize = min;
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
// service deals with calls and callbacks
|
|
int Chirp::service(bool all)
|
|
{
|
|
int i;
|
|
uint8_t type;
|
|
ChirpProc recvProc;
|
|
void *args[CRP_MAX_ARGS+1];
|
|
|
|
for (i=0; true; i++)
|
|
{
|
|
if (recvChirp(&type, &recvProc, args)==CRP_RES_OK)
|
|
handleChirp(type, recvProc, (const void **)args);
|
|
else
|
|
break;
|
|
if (!all)
|
|
break;
|
|
}
|
|
|
|
return i;
|
|
}
|
|
|
|
int Chirp::recvChirp(uint8_t *type, ChirpProc *proc, void *args[], bool wait) // null pointer terminates
|
|
{
|
|
int res;
|
|
uint32_t i, offset;
|
|
|
|
restoreBuffer();
|
|
|
|
// receive
|
|
if (m_errorCorrected)
|
|
res = recvFull(type, proc, wait);
|
|
else
|
|
{
|
|
for (i=0; true; i++)
|
|
{
|
|
res = recvHeader(type, proc, wait);
|
|
if (res==CRP_RES_ERROR_CRC)
|
|
{
|
|
if (i<m_maxNak)
|
|
continue;
|
|
else
|
|
return CRP_RES_ERROR_MAX_NAK;
|
|
}
|
|
else if (res==CRP_RES_OK)
|
|
break;
|
|
else
|
|
return res;
|
|
}
|
|
res = recvData();
|
|
}
|
|
if (res!=CRP_RES_OK)
|
|
return res;
|
|
|
|
// get responseInt from response
|
|
if (*type&CRP_RESPONSE)
|
|
{
|
|
// fake responseInt so it gets inserted
|
|
*(m_buf+m_headerLen-4) = CRP_UINT32; // write type so it parses correctly
|
|
*(m_buf+m_headerLen-1) = CRP_UINT32;
|
|
// increment pointer
|
|
offset = m_headerLen-4;
|
|
m_len+=4;
|
|
}
|
|
else // call has no responseInt
|
|
offset = m_headerLen;
|
|
|
|
return deserializeParse(m_buf+offset, m_len, args);
|
|
}
|
|
|
|
int Chirp::deserialize(uint8_t *buf, uint32_t len, ...)
|
|
{
|
|
int res;
|
|
va_list args;
|
|
void *recvArgs[CRP_MAX_ARGS+1];
|
|
|
|
if ((res=deserializeParse(buf, len, recvArgs))<0)
|
|
return res;
|
|
|
|
va_start(args, len);
|
|
res = loadArgs(&args, recvArgs);
|
|
va_end(args);
|
|
|
|
return res;
|
|
}
|
|
|
|
int Chirp::vdeserialize(uint8_t *buf, uint32_t len, va_list *args)
|
|
{
|
|
int res;
|
|
void *recvArgs[CRP_MAX_ARGS+1];
|
|
|
|
if ((res=deserializeParse(buf, len, recvArgs))<0)
|
|
return res;
|
|
|
|
return loadArgs(args, recvArgs);
|
|
}
|
|
|
|
int Chirp::getArgList(uint8_t *buf, uint32_t len, uint8_t *argList)
|
|
{
|
|
uint8_t dataType, size, a;
|
|
uint32_t i;
|
|
|
|
// parse remaining args
|
|
for(i=0, a=0; i<len; a++)
|
|
{
|
|
if (a==CRP_MAX_ARGS)
|
|
return CRP_RES_ERROR;
|
|
|
|
dataType = buf[i++];
|
|
argList[a] = dataType;
|
|
size = dataType&0x0f;
|
|
if (!(dataType&CRP_ARRAY)) // if we're a scalar
|
|
{
|
|
ALIGN(i, size);
|
|
i += dataType&0x0f; // extract size of scalar, add it
|
|
}
|
|
else // we're an array
|
|
{
|
|
if (dataType==CRP_STRING || dataType==CRP_HSTRING) // string is a special case
|
|
i += strlen((char *)(buf+i))+1; // +1 include null character
|
|
else
|
|
{
|
|
ALIGN(i, 4);
|
|
uint32_t len = *(uint32_t *)(buf+i);
|
|
i += 4;
|
|
ALIGN(i, size);
|
|
i += len*size;
|
|
}
|
|
}
|
|
}
|
|
argList[a] = '\0'; // terminate list
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
|
|
int Chirp::deserializeParse(uint8_t *buf, uint32_t len, void *args[])
|
|
{
|
|
uint8_t dataType, size, a;
|
|
uint32_t i;
|
|
|
|
// parse remaining args
|
|
for(i=0, a=0; i<len; a++)
|
|
{
|
|
if (a==CRP_MAX_ARGS)
|
|
return CRP_RES_ERROR;
|
|
|
|
dataType = buf[i++];
|
|
size = dataType&0x0f;
|
|
if (!(dataType&CRP_ARRAY)) // if we're a scalar
|
|
{
|
|
ALIGN(i, size);
|
|
args[a] = (void *)(buf+i);
|
|
i += dataType&0x0f; // extract size of scalar, add it
|
|
}
|
|
else // we're an array
|
|
{
|
|
if (dataType==CRP_STRING || dataType==CRP_HSTRING) // string is a special case
|
|
{
|
|
args[a] = (void *)(buf+i);
|
|
i += strlen((char *)(buf+i))+1; // +1 include null character
|
|
}
|
|
else
|
|
{
|
|
ALIGN(i, 4);
|
|
uint32_t len = *(uint32_t *)(buf+i);
|
|
args[a++] = (void *)(buf+i);
|
|
i += 4;
|
|
ALIGN(i, size);
|
|
args[a] = (void *)(buf+i);
|
|
i += len*size;
|
|
}
|
|
}
|
|
}
|
|
args[a] = NULL; // terminate list
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
uint8_t Chirp::getType(const void *arg)
|
|
{
|
|
return *((uint8_t *)arg - 1);
|
|
}
|
|
|
|
uint16_t Chirp::calcCrc(uint8_t *buf, uint32_t len)
|
|
{
|
|
uint32_t i;
|
|
uint16_t crc;
|
|
|
|
// this isn't a real crc, but it's cheap and prob good enough
|
|
for (i=0, crc=0; i<len; i++)
|
|
crc += buf[i];
|
|
crc += len;
|
|
|
|
return crc;
|
|
}
|
|
|
|
|
|
int Chirp::sendFull(uint8_t type, ChirpProc proc)
|
|
{
|
|
int res;
|
|
|
|
*(uint32_t *)m_buf = CRP_START_CODE;
|
|
*(uint8_t *)(m_buf+4) = type;
|
|
*(ChirpProc *)(m_buf+6) = proc;
|
|
*(uint32_t *)(m_buf+8) = m_len;
|
|
// send header
|
|
if ((res=m_link->send(m_buf, CRP_MAX_HEADER_LEN, m_sendTimeout))<0)
|
|
return res;
|
|
// if we haven't sent everything yet....
|
|
if (m_len+m_headerLen>CRP_MAX_HEADER_LEN && !m_sharedMem)
|
|
{
|
|
if ((res=m_link->send(m_buf+CRP_MAX_HEADER_LEN, m_len-(CRP_MAX_HEADER_LEN-m_headerLen), m_sendTimeout))<0)
|
|
return res;
|
|
}
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::sendHeader(uint8_t type, ChirpProc proc)
|
|
{
|
|
int res;
|
|
bool ack;
|
|
uint32_t chunk, startCode = CRP_START_CODE;
|
|
uint16_t crc;
|
|
|
|
if ((res=m_link->send((uint8_t *)&startCode, 4, m_sendTimeout))<0)
|
|
return res;
|
|
|
|
*(uint8_t *)m_buf = type;
|
|
*(uint16_t *)(m_buf+2) = proc;
|
|
*(uint32_t *)(m_buf+4) = m_len;
|
|
if ((res=m_link->send(m_buf, m_headerLen, m_sendTimeout))<0)
|
|
return res;
|
|
crc = calcCrc(m_buf, m_headerLen);
|
|
|
|
if (m_len>=CRP_MAX_HEADER_LEN)
|
|
chunk = CRP_MAX_HEADER_LEN;
|
|
else
|
|
chunk = m_len;
|
|
if (m_link->send(m_buf, chunk, m_sendTimeout)<0)
|
|
return CRP_RES_ERROR_SEND_TIMEOUT;
|
|
|
|
// send crc
|
|
crc += calcCrc(m_buf, chunk);
|
|
if (m_link->send((uint8_t *)&crc, 2, m_sendTimeout)<0)
|
|
return CRP_RES_ERROR_SEND_TIMEOUT;
|
|
|
|
if ((res=recvAck(&ack, m_headerTimeout))<0)
|
|
return res;
|
|
|
|
if (ack)
|
|
m_offset = chunk;
|
|
else
|
|
return CRP_RES_ERROR_CRC;
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::sendData()
|
|
{
|
|
uint16_t crc;
|
|
uint32_t chunk;
|
|
uint8_t sequence;
|
|
bool ack;
|
|
int res;
|
|
|
|
for (sequence=0; m_offset<m_len; )
|
|
{
|
|
if (m_len-m_offset>=m_blkSize)
|
|
chunk = m_blkSize;
|
|
else
|
|
chunk = m_len-m_offset;
|
|
// send data
|
|
if (m_link->send(m_buf+m_offset, chunk, m_sendTimeout)<0)
|
|
return CRP_RES_ERROR_SEND_TIMEOUT;
|
|
// send sequence
|
|
if (m_link->send((uint8_t *)&sequence, 1, m_sendTimeout)<0)
|
|
return CRP_RES_ERROR_SEND_TIMEOUT;
|
|
// send crc
|
|
crc = calcCrc(m_buf+m_offset, chunk) + calcCrc((uint8_t *)&sequence, 1);
|
|
if (m_link->send((uint8_t *)&crc, 2, m_sendTimeout)<0)
|
|
return CRP_RES_ERROR_SEND_TIMEOUT;
|
|
|
|
if ((res=recvAck(&ack, m_dataTimeout))<0)
|
|
return res;
|
|
if (ack)
|
|
{
|
|
m_offset += chunk;
|
|
sequence++;
|
|
}
|
|
}
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::sendAck(bool ack) // false=nack
|
|
{
|
|
uint8_t c;
|
|
|
|
if (ack)
|
|
c = CRP_ACK;
|
|
else
|
|
c = CRP_NACK;
|
|
|
|
if (m_link->send(&c, 1, m_sendTimeout)<0)
|
|
return CRP_RES_ERROR_SEND_TIMEOUT;
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::recvHeader(uint8_t *type, ChirpProc *proc, bool wait)
|
|
{
|
|
uint8_t c;
|
|
uint32_t chunk, startCode = 0;
|
|
uint16_t crc, rcrc;
|
|
|
|
int return_value;
|
|
|
|
return_value = m_link->receive(&c, 1, wait?m_headerTimeout:0);
|
|
|
|
if (return_value < 0) {
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
if (return_value == 0) {
|
|
return_value = CRP_RES_ERROR;
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
|
|
// find start code
|
|
while(1)
|
|
{
|
|
startCode >>= 8;
|
|
startCode |= (uint32_t)c<<24;
|
|
if (startCode==CRP_START_CODE)
|
|
break;
|
|
|
|
return_value = m_link->receive(&c, 1, m_idleTimeout);
|
|
|
|
if (return_value < 0) {
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
if (return_value == 0) {
|
|
return_value = CRP_RES_ERROR;
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
}
|
|
// receive rest of header
|
|
if (m_link->receive(m_buf, m_headerLen, m_idleTimeout) < 0) {
|
|
return_value = CRP_RES_ERROR_RECV_TIMEOUT;
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
|
|
if (return_value < (int) m_headerLen) {
|
|
return_value = CRP_RES_ERROR;
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
|
|
*type = *(uint8_t *)m_buf;
|
|
*proc = *(ChirpProc *)(m_buf+2);
|
|
m_len = *(uint32_t *)(m_buf+4);
|
|
crc = calcCrc(m_buf, m_headerLen);
|
|
|
|
if (m_len>=CRP_MAX_HEADER_LEN-m_headerLen)
|
|
chunk = CRP_MAX_HEADER_LEN-m_headerLen;
|
|
else
|
|
chunk = m_len;
|
|
|
|
return_value = m_link->receive(m_buf, chunk+2, m_idleTimeout);
|
|
|
|
if (return_value < 0) { // +2 for crc
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
if (return_value < (int) (chunk + 2)) {
|
|
return_value = CRP_RES_ERROR;
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
copyAlign((char *)&rcrc, (char *)(m_buf+chunk), 2);
|
|
if (rcrc==crc+calcCrc(m_buf, chunk))
|
|
{
|
|
m_offset = chunk;
|
|
sendAck(true);
|
|
}
|
|
else
|
|
{
|
|
sendAck(false); // send nack
|
|
return_value = CRP_RES_ERROR_CRC;
|
|
goto chirp_recvheader__exit;
|
|
}
|
|
|
|
return_value = CRP_RES_OK;
|
|
|
|
chirp_recvheader__exit:
|
|
|
|
return return_value;
|
|
}
|
|
|
|
int Chirp::recvFull(uint8_t *type, ChirpProc *proc, bool wait)
|
|
{
|
|
int res;
|
|
uint32_t startCode;
|
|
uint32_t len, recvd;
|
|
|
|
// receive header, with startcode check to make sure we're synced
|
|
while(1)
|
|
{
|
|
if ((res=m_link->receive(m_buf, CRP_MAX_HEADER_LEN, wait?m_headerTimeout:0))<0)
|
|
return res;
|
|
// check to see if we received less data than expected
|
|
if (res<sizeof(uint32_t))
|
|
continue;
|
|
recvd = res;
|
|
startCode = *(uint32_t *)m_buf;
|
|
if (startCode==CRP_START_CODE)
|
|
break;
|
|
}
|
|
*type = *(uint8_t *)(m_buf+4);
|
|
*proc = *(ChirpProc *)(m_buf+6);
|
|
m_len = *(uint32_t *)(m_buf+8);
|
|
|
|
if (m_len+m_headerLen>m_bufSize && (res=realloc(m_len+m_headerLen))<0)
|
|
return res;
|
|
|
|
if (m_len+m_headerLen>recvd && !m_sharedMem)
|
|
{
|
|
len = m_len+m_headerLen;
|
|
while(recvd<len)
|
|
{
|
|
if ((res=m_link->receive(m_buf+recvd, len-recvd, m_idleTimeout))<0)
|
|
return res;
|
|
recvd += res;
|
|
}
|
|
}
|
|
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
// We assume that the probability that we send a nack and the receiver interprets a nack is 100%
|
|
// We can't assume that the probability that we send an ack and the receiver interprets it is 100%
|
|
// Scenario
|
|
// 1) we receive packet 0, redo is 0, crc is good, we increment offset, send ack. (inc) (inc) rs=0, s=0, inc
|
|
// 2) we receive packet 1, crc is bad, we send nack (!inc) (!inc) rs=1, s=1
|
|
// 3) sender gets nack, so it resends
|
|
// 4) we receive packet 1, redo is 1, crc is good, we don't increment offset, send ack (inc) (inc) rs=1, s=1
|
|
// 5) we receive packet 2, redo is 0, crc is bad, we send nack (!inc) (!inc) rs=2, s=2
|
|
// 6) we receive packet 2, redo is 1, crc is good, we send ack (inc) (inc) rs=2, s=2
|
|
// 7) we receive packet 3, redo is 0, crc is good, we send ack (inc) (inc)
|
|
// different scenario
|
|
// 1) we receive packet 0, redo is 0, crc is good, we increment offset, send ack. (inc) (inc) rs=0, s=0
|
|
// 2) sender thinks it gets a nack, so it resends
|
|
// 3) we receive packet 0 again, but crc is bad, we send nack (!inc) (!inc) rs=1, s=0
|
|
// 4) sender gets nack, so it resends
|
|
// 5) we receive packet 0, redo is 1, crc is good, we don't increment offset, send ack (!inc) (inc) rs=1, s=0
|
|
// (we've essentially thrown out this packet, but that's ok, because we have a good packet 0)
|
|
// 6) we receive packet 1, redo is 0, crc is bad, we send nack (!inc) (!inc) rs=1, s=1
|
|
// 7) we receive packet 1, redo is 1, crc is good, we send ack (inc) (inc) rs=1, s=1
|
|
// 8) we receive packet 2, redo is 0, crc is good, we send ack (inc) (inc) rs=2, s=2
|
|
// a redo flag is not sufficient to communicate which packet we're on because the sender can misinterpret
|
|
// any number of nacks
|
|
int Chirp::recvData()
|
|
{
|
|
int res;
|
|
uint32_t chunk;
|
|
uint16_t crc;
|
|
uint8_t sequence, rsequence, naks;
|
|
|
|
if (m_len+3+m_headerLen>m_bufSize && (res=realloc(m_len+3+m_headerLen))<0) // +3 to read sequence, crc
|
|
return res;
|
|
|
|
for (rsequence=0, naks=0; m_offset<m_len; )
|
|
{
|
|
if (m_len-m_offset>=m_blkSize)
|
|
chunk = m_blkSize;
|
|
else
|
|
chunk = m_len-m_offset;
|
|
if (m_link->receive(m_buf+m_offset, chunk+3, m_dataTimeout)<0) // +3 to read sequence, crc
|
|
return CRP_RES_ERROR_RECV_TIMEOUT;
|
|
if (res<(int)chunk+3)
|
|
return CRP_RES_ERROR;
|
|
sequence = *(uint8_t *)(m_buf+m_offset+chunk);
|
|
copyAlign((char *)&crc, (char *)(m_buf+m_offset+chunk+1), 2);
|
|
if (crc==calcCrc(m_buf+m_offset, chunk+1))
|
|
{
|
|
if (rsequence==sequence)
|
|
{
|
|
m_offset += chunk;
|
|
rsequence++;
|
|
}
|
|
sendAck(true);
|
|
naks = 0;
|
|
}
|
|
else
|
|
{
|
|
sendAck(false);
|
|
naks++;
|
|
if (naks<m_maxNak)
|
|
naks++;
|
|
else
|
|
return CRP_RES_ERROR_MAX_NAK;
|
|
}
|
|
}
|
|
return CRP_RES_OK;
|
|
}
|
|
|
|
int Chirp::recvAck(bool *ack, uint16_t timeout) // false=nack
|
|
{
|
|
int res;
|
|
uint8_t c;
|
|
if ((res=m_link->receive(&c, 1, timeout))<0)
|
|
return CRP_RES_ERROR_RECV_TIMEOUT;
|
|
if (res<1)
|
|
return CRP_RES_ERROR;
|
|
|
|
if (c==CRP_ACK)
|
|
*ack = true;
|
|
else
|
|
*ack = false;
|
|
|
|
return CRP_RES_OK;
|
|
}
|