void AodvPacket::toSerial() {
switch (cmd) {
case 0:
printPacketHeader(*this,"DATA");
printRawData(payload);
break;
case 1:
printPacketHeader(*this,"ARP");
printRawData(payload);
break;
case 2:
printPacketHeader(*this,"DESCRIBE");
printRawData(payload);
break;
case 3:
printPacketHeader(*this,"ROUTE REQUEST");
printRouteRequest(*this);
break;
case 4:
printPacketHeader(*this,"ROUTE RESPONSE");
printRouteRequest(*this);
break;
default:
printPacketHeader(*this,String("")+cmd);
printRawData(payload);
break;
}
Serial.println("<EOM>");
}
void* sendThread(void* i_SqsockInfo)
{
pid_t myPid;
QSOCKINFO* SqsockInfo = (QSOCKINFO*)i_SqsockInfo;
QData* qData;
int iUartFd = SqsockInfo->iUartFd;
status_t status;
unsigned int uiCount;
unsigned char uchWriteBuff[WRITE_BUFF_SIZE]={0x0};
int iWriteSize;
myPid = pthread_self();
SqsockInfo->sendThreadId= myPid;
status=EOK;
while(1){
uiCount = qGetCountMutex(SqsockInfo->writeQueue);
if(uiCount > 0){
qData = qPopMutex(SqsockInfo->writeQueue, &status);
if(qData != NULL){
bzero((void*)uchWriteBuff, WRITE_BUFF_SIZE);
memcpy((void*)uchWriteBuff, (void*)qData->pvData, qData->iLength);
printRawData(uchWriteBuff, qData->iLength, SEND);
//fprintf(stderr,"### %s():%d ###\n",__func__, __LINE__);
while((SqsockInfo->iUartState & 0x0F) == 0x01)
usleep(500);
//fprintf(stderr,"### %s():%d ###\n",__func__, __LINE__);
SqsockInfo->iUartState &= 0x0F;
iWriteSize = write(iUartFd, uchWriteBuff, qData->iLength);
tcflush(iUartFd, TCOFLUSH);
qDataDestroy(qData);
qData=NULL;
}else{
printLog( "by pcw : gThread.c : %d... \n",__LINE__);
break;
}
}else
usleep(100);
}
//command thread end
//recv thread end
pthread_cancel(SqsockInfo->recvThreadId);
status = qDestroyMutex (SqsockInfo->readQueue);
SqsockInfo->readQueue=NULL;
if(status != EOK)
printLog( "by pcw : error qDestroyMutex-readQueu\n");
status = qDestroyMutex (SqsockInfo->writeQueue);
SqsockInfo->writeQueue=NULL;
if(status != EOK)
printLog( "by pcw : error qDestroyMutex-writeQueu\n");
return NULL;
}
/*
* packRILcommandString
* Used for packing standard AT command string to RIL command.
*
* IN *inStr : AT command string with NULL terminate
* IN *prefix : prefix of AT response
* OUT **outCmd : RAW RIL command out. Caller is responsible to free this resource.
* RETUURN : Length of outCmd data.
*/
size_t packRILCommand(char *inStr, char *prefix, char **outCmd)
{
/* |Request Enum |Request Token|Number of Strings|Srings.....|
* |<--4 bytes-->|<--4 bytes-->|<--- 4 bytes --->|<------ ->|
*/
size_t length = 0;
char *cmdStr[2] = {NULL,NULL};
char *pData = NULL;
Parcel p;
static int s_token = 0;
if ((NULL == inStr)||(NULL == outCmd)) {
return 0;
}
cmdStr[0] = inStr;
cmdStr[1] = prefix;
// p.writeInt32(length); /* fake write to reserve space */
p.writeInt32(RIL_REQUEST_OEM_HOOK_STRINGS);
p.writeInt32(s_token++);
packStrings(p,cmdStr,2*sizeof(char *)); /* ONLY support 1 string now */
length = p.dataSize();
#if 0
offset = p.dataPosition(); /* Store data position */
p.setDataPosition(0); /* Move to the buffer pointer to head */
p.writeInt32(length - 4); /* Update data length */
p.setDataPosition(offset); /* Restore data position */
#endif /* 0 */
pData = (char *) malloc(length);
if (NULL != pData) {
memcpy(pData,p.data(),length);
*outCmd = pData;
LOGI("packRILCommand: %d bytes\n",length);
printRawData((const void *) pData, length);
} else {
return 0;
}
return length;
}
int main(int argc, char **argv) {
// #if TEST_MODE
// // Initialize test data
// int data[MAXSIZE] = { 2, 20, 37, 22, 6, 3 };
// #else
// // Receive input from user or open file
// int data[MAXSIZE];
// int cnt = getData(data);
// printf("Inputted data count = %d\n", cnt);
// #endif
// Handle data and processing
RESULT set = { 0, 0, 0, 0, 0, 0, 0 }; /*Initialize results to zero*/
INT_NODE top;
top.next = NULL;
printf("Address of top = [%p]\n", &top);
printf("Address of top.next = [%p]\n", top.next);
getRawData(&top);
updateResult(&set, top.next);
printRawData(top.next);
printResult(&set);
// updateResult(&set, top.next);
// printResult(&set);
// updateResult(&set, top.next);
// printResult(&set);
// Output to screen
print_h1st0gram(top.next);
return 0;
}
//status_t status;
void* cmdProcessingThread(void* i_SqsockInfo)
{
pid_t myPid;
QSOCKINFO* SqsockInfo = (QSOCKINFO*)i_SqsockInfo;
QData* qData;
status_t status;
int iSize=0;
unsigned int uiCount;
char chReadBuff[READ_BUFF_SIZE]={0x0};
struct MsgHead* pstMsgHead;
char chIcdCode=0x0;
int iAz, iEl;
char chChangeValue[4];
float fAz, fEl;
// 쓰레드 종료를 하기 위해서 필요한 쓰레드ID를 저장.
myPid = pthread_self();
SqsockInfo->commandThreadId = myPid;
status=EOK;
while(1){
uiCount = qGetCountMutex(SqsockInfo->readQueue);
if(uiCount > 0){
qData = qPopMutex(SqsockInfo->readQueue, &status);
if(qData == NULL){
printLog( "by pcw : readQueue pop Mutex error(no data)\n");
usleep(1000);
break;
}
bzero((void*)chReadBuff, READ_BUFF_SIZE);
iSize = qData->iLength;
memcpy((void*)chReadBuff, qData->pvData, iSize);
fprintf(stderr,"### %s():%d count : %d [iSize:%d]###\n",__func__,__LINE__,gCnt++,iSize);
printRawData(chReadBuff, iSize, RECV);
if(iSize > 5){
chIcdCode = chReadBuff[6];
fprintf(stderr,"### %s():%d icd:%02x ###\n",__func__,__LINE__, chIcdCode);
if(chIcdCode == 0x42){
chChangeValue[3] = chReadBuff[7];
chChangeValue[2] = chReadBuff[8];
chChangeValue[1] = chReadBuff[9];
chChangeValue[0] = chReadBuff[10];
memcpy(&iAz, chChangeValue, sizeof(int));
fAz = iAz / 4096.0;
}
chIcdCode = chReadBuff[11];
fprintf(stderr,"### %s():%d icd:%02x ###\n",__func__,__LINE__, chIcdCode);
if(chIcdCode == 0x43){
chChangeValue[3] = chReadBuff[12];
chChangeValue[2] = chReadBuff[13];
chChangeValue[1] = chReadBuff[14];
chChangeValue[0] = chReadBuff[15];
memcpy(&iEl, chChangeValue, sizeof(int));
fEl = iEl / 4096.0;
}
fprintf(stderr,"Az : %f, El : %f\n",fAz, fEl);
}
// delete qData
qDataDestroy(qData);
qData=NULL;
}else{
usleep(1000);
}
}
//sender thread end
pthread_cancel(SqsockInfo->sendThreadId);
status = qDestroyMutex (SqsockInfo->writeQueue);
SqsockInfo->writeQueue=NULL;
if(status != EOK)
printLog( "by pcw : error qDestroyMutex-writeQueu\n");
//recv thread end
pthread_cancel(SqsockInfo->recvThreadId);
status = qDestroyMutex (SqsockInfo->readQueue);
SqsockInfo->readQueue = NULL;
if(status != EOK)
printLog( "by pcw : error qDestroyMutex-readQueu\n");
return NULL;
}
void* recvThread(void* i_SqsockInfo)
{
pid_t myPid;
QSOCKINFO* SqsockInfo = (QSOCKINFO*)i_SqsockInfo;
int iUartFd = SqsockInfo->iUartFd;
int iRecvSize=0;
int iTotalSize=0;
int iRemainderSize=0;
int iCopySize=0;
int iDeleteSize;
unsigned char uchReadBuff[READ_BUFF_SIZE]={0x0};
unsigned char uchTmpBuff[READ_BUFF_SIZE]={0x0};
//struct TcpMsgHead* readMsgHead;
status_t status;
static QData* qData;
unsigned int uiCount;
int errnum;
int iDataLen;
struct MsgHead* pstMsgHead;
myPid = pthread_self();
SqsockInfo->recvThreadId= myPid;
status=EOK;
tcflush(iUartFd, TCIFLUSH);
while(1){
SqsockInfo->iUartState |= 0x01;
bzero((void*)uchReadBuff, READ_BUFF_SIZE);
//fprintf(stderr,"### %s():%d tot:%d ###\n",__func__, __LINE__, iTotalSize);
iRecvSize = read(iUartFd, (void*)uchReadBuff+iTotalSize, READ_BUFF_SIZE-iTotalSize);
if(iRecvSize <= 0){
//0.5초 이내 데이터가 안들어오는 경우
SqsockInfo->iUartState &= 0xF0;
while((SqsockInfo->iUartState & 0xF0) == 0x10)
usleep(500);
}else{
iTotalSize += iRecvSize;
//fprintf(stderr,"### %s():%d tot:%d ###\n",__func__, __LINE__, iTotalSize);
//iTotalSize = 0;
while(iTotalSize > 4){
//헤더 검사
//fprintf(stderr,"### %s():%d tot:%d ###\n",__func__, __LINE__, iTotalSize);
iDeleteSize = checkHeader(uchReadBuff, iTotalSize);
if(iDeleteSize > 0){
//전송받은 데이터의 처음이 헤더가 아닌 경우 다음 헤더를 찾아 헤더 이전데이터를 삭제한다.
//fprintf(stderr,"### %s():%d tot:%d del:%d###\n",__func__, __LINE__, iTotalSize, iDeleteSize);
iTotalSize = iTotalSize - iDeleteSize;
memset(uchTmpBuff, 0x0, sizeof(uchTmpBuff));
memcpy(uchTmpBuff, uchReadBuff+iDeleteSize, iTotalSize);
memset(uchReadBuff, 0x0, sizeof(uchReadBuff));
memcpy(uchReadBuff, uchTmpBuff, iTotalSize);
}else{
//fprintf(stderr,"### %s():%d tot:%d del:%d###\n",__func__, __LINE__, iTotalSize, iDeleteSize);
pstMsgHead = (struct MsgHead*)uchReadBuff;
iDataLen = pstMsgHead->uchDataSize; // Data 크기
iCopySize = iDataLen + sizeof(struct MsgHead)+CHECK_SUM_LENTH;
if(iCopySize > iTotalSize){
//헤더는 정상이지만 아직 모든 데이터를 못 받은 경우
//fprintf(stderr,"### %s():%d tot:%d cpy:%d###\n",__func__, __LINE__, iTotalSize, iCopySize);
break;
}
//데이터 무결성 확인
if(check_integrity(uchReadBuff, iCopySize)){
qData = qDataCreate(iCopySize);
if (qData == NULL)
printLog( "by pcw : %s():%d error create queue data",__func__,__LINE__);
else{
iRemainderSize = iTotalSize - iCopySize;
iTotalSize = iTotalSize - iRemainderSize;
memcpy(qData->pvData, (void*)uchReadBuff, iCopySize);
qData->iLength = iCopySize;
memset(uchTmpBuff, 0x0, sizeof(uchTmpBuff));
memcpy(uchTmpBuff, uchReadBuff+iCopySize, iRemainderSize);
memset(uchReadBuff, 0x0, sizeof(uchReadBuff));
memcpy(uchReadBuff, uchTmpBuff+iCopySize, iRemainderSize);
status = qPushMutex(SqsockInfo->readQueue, &qData->link);
if (status != EOK)
printLog( "by pcw : %s():%d qPushMutex Error(status : %d)",__func__,__LINE__,status);
}
}else{
//무결성이 깨진 경우 무결성 검사한 데이터를 모두 삭제한다.
fprintf(stderr,"### %s():%d tot:%d cpy:%d###\n",__func__, __LINE__, iTotalSize, iCopySize);
printRawData(uchReadBuff, iCopySize, RECV);
iTotalSize = iTotalSize - iCopySize;
memset(uchTmpBuff, 0x0, sizeof(uchTmpBuff));
memcpy(uchTmpBuff, uchReadBuff+iCopySize, iTotalSize);
memset(uchReadBuff, 0x0, sizeof(uchReadBuff));
memcpy(uchReadBuff, uchTmpBuff, iTotalSize);
fprintf(stderr,"### %s():%d tot:%d cpy:%d###\n",__func__, __LINE__, iTotalSize, iCopySize);
}
}
}
}
}
//command thread end
pthread_cancel(SqsockInfo->commandThreadId);
status = qDestroyMutex (SqsockInfo->readQueue);
SqsockInfo->readQueue=NULL;
if(status != EOK)
printLog( "by pcw : error qDestroyMutex-readQueu\n");
//sender thread end
pthread_cancel(SqsockInfo->sendThreadId);
pthread_exit(SqsockInfo->sendThreadId);
status = qDestroyMutex (SqsockInfo->writeQueue);
SqsockInfo->writeQueue = NULL;
if(status != EOK)
printLog( "by pcw : error qDestroyMutex-writeQueu\n");
return NULL;
}
