//---------------------------------------------------------------------------
void RMC_Parse( char *head)
{
// $GPRMC,073446.000,A,2446.3752,N,12101.3708,E,0.002,22.08,121006,,,A*6C
char *start, result[20], tmp[20], *point;
short len=0;
// check checksum
if(CheckSum(head, strlen(head)))
{
// UTC time : 161229.487
start = strstr( head, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, tmp))
{
// Hour
strncpy( result, tmp, 2);
result[2]='\0';
g_gpsInfo.hour = atoi(result);
// Min
strncpy( result, tmp+2, 2);
result[2]='\0';
g_gpsInfo.min = atoi(result);
// Sec
strncpy( result, tmp+4, strlen(tmp)-4);
result[strlen(tmp)-4]='\0';
g_gpsInfo.sec = (float)(atof(result));
}
// valid
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
if(*result == 'A')
{
if(g_gpsInfo.FixType == 0)
g_gpsInfo.FixType = 1; // Assume 2D
if(g_gpsInfo.FixService == 0)
g_gpsInfo.FixService = 1; // Assume SPS
}
else
{
g_gpsInfo.FixType = 0; // NoFix
g_gpsInfo.FixService = 0; // NoFix
}
// Position(Lat) : 3723.2475(N)
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
point = strstr( result, ".");
len = (point-2)-result;
strncpy(tmp, result, len);
tmp[len]='\0';
g_gpsInfo.Lat = (float)(atoi(tmp));
strncpy(tmp, result+len, strlen(result)-len);
tmp[strlen(result)-len]='\0';
g_gpsInfo.Lat += (float)(atof(tmp)/60.0);
}
else //Can not fetch Lat field
{
g_gpsInfo.Lat = 0;
}
// N or S
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result) && g_gpsInfo.Lat!=0)
{
if(*result=='S')
g_gpsInfo.Lat = -g_gpsInfo.Lat;
}
// Position(Lon) : 12158.3416(W)
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
point = strstr( result, ".");
len = (point-2)-result;
strncpy(tmp, result, len);
tmp[len]='\0';
g_gpsInfo.Lon = (float)(atoi(tmp));
strncpy(tmp, result+len, strlen(result)-len);
tmp[strlen(result)-len]='\0';
g_gpsInfo.Lon += (float)(atof(tmp)/60.0);
}
else //Can not fetch Lat field
{
g_gpsInfo.Lon = 0;
}
// E or W
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result) && g_gpsInfo.Lat!=0)
{
if(*result=='W')
g_gpsInfo.Lon = -g_gpsInfo.Lon;
}
// Speed : 0.13
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
g_gpsInfo.Speed = (float)(atof(result) * Knot2Kmhr);
}
// Track : 309.62
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
g_gpsInfo.Track = (float)(atof(result));
}
// Date : 120598
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
// Day
strncpy(tmp, result, 2);
tmp[2]='\0';
g_gpsInfo.day=atoi(tmp);
// Month
strncpy(tmp, result+2, 2);
tmp[2]='\0';
g_gpsInfo.mon=atoi(tmp);
// Year
strncpy(tmp, result+4, 2);
tmp[2]='\0';
g_gpsInfo.year=atoi(tmp)+2000;
}
// skip Magnetic variation
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
// mode indicator
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
if(g_gpsInfo.FixType > 0)
{
switch(result[0])
{
case 'A':
{
g_gpsInfo.FixService = 1;
break;
}
case 'D':
{
g_gpsInfo.FixService = 2;
break;
}
case 'E':
{
g_gpsInfo.FixService = 6;
break;
}
}
}
}
}
int main(int argc, char *argv[])
{
long i;
long c;
extern char *optarg;
long m1;
long factor;
long pages;
unsigned long start;
CLOCK(start);
while ((c = getopt(argc, argv, "p:m:n:l:stoh")) != -1) {
switch(c) {
case 'p': P = atoi(optarg);
if (P < 1) {
printerr("P must be >= 1\n");
exit(-1);
}
if (log_2(P) == -1) {
printerr("P must be a power of 2\n");
exit(-1);
}
break;
case 'm': M = atoi(optarg);
m1 = M/2;
if (2*m1 != M) {
printerr("M must be even\n");
exit(-1);
}
break;
case 'n': num_cache_lines = atoi(optarg);
orig_num_lines = num_cache_lines;
if (num_cache_lines < 1) {
printerr("Number of cache lines must be >= 1\n");
exit(-1);
}
break;
case 'l': log2_line_size = atoi(optarg);
if (log2_line_size < 0) {
printerr("Log base 2 of cache line length in bytes must be >= 0\n");
exit(-1);
}
break;
case 's': dostats = !dostats;
break;
case 't': test_result = !test_result;
break;
case 'o': doprint = !doprint;
break;
case 'h': printf("Usage: FFT <options>\n\n");
printf("options:\n");
printf(" -mM : M = even integer; 2**M total complex data points transformed.\n");
printf(" -pP : P = number of processors; Must be a power of 2.\n");
printf(" -nN : N = number of cache lines.\n");
printf(" -lL : L = Log base 2 of cache line length in bytes.\n");
printf(" -s : Print individual processor timing statistics.\n");
printf(" -t : Perform FFT and inverse FFT. Test output by comparing the\n");
printf(" integral of the original data to the integral of the data that\n");
printf(" results from performing the FFT and inverse FFT.\n");
printf(" -o : Print out complex data points.\n");
printf(" -h : Print out command line options.\n\n");
printf("Default: FFT -m%1d -p%1d -n%1d -l%1d\n",
DEFAULT_M,DEFAULT_P,NUM_CACHE_LINES,LOG2_LINE_SIZE);
exit(0);
break;
}
}
MAIN_INITENV(,80000000);
N = 1<<M;
rootN = 1<<(M/2);
rowsperproc = rootN/P;
if (rowsperproc == 0) {
printerr("Matrix not large enough. 2**(M/2) must be >= P\n");
exit(-1);
}
line_size = 1 << log2_line_size;
if (line_size < 2*sizeof(double)) {
printf("WARNING: Each element is a complex double (%ld bytes)\n",2*sizeof(double));
printf(" => Less than one element per cache line\n");
printf(" Computing transpose blocking factor\n");
factor = (2*sizeof(double)) / line_size;
num_cache_lines = orig_num_lines / factor;
}
if (line_size <= 2*sizeof(double)) {
pad_length = 1;
} else {
pad_length = line_size / (2*sizeof(double));
}
if (rowsperproc * rootN * 2 * sizeof(double) >= PAGE_SIZE) {
pages = (2 * pad_length * sizeof(double) * rowsperproc) / PAGE_SIZE;
if (pages * PAGE_SIZE != 2 * pad_length * sizeof(double) * rowsperproc) {
pages ++;
}
pad_length = (pages * PAGE_SIZE) / (2 * sizeof(double) * rowsperproc);
} else {
pad_length = (PAGE_SIZE - (rowsperproc * rootN * 2 * sizeof(double))) /
(2 * sizeof(double) * rowsperproc);
if (pad_length * (2 * sizeof(double) * rowsperproc) !=
(PAGE_SIZE - (rowsperproc * rootN * 2 * sizeof(double)))) {
printerr("Padding algorithm unsuccessful\n");
exit(-1);
}
}
Global = (struct GlobalMemory *) G_MALLOC(sizeof(struct GlobalMemory));
x = (double *) G_MALLOC(2*(N+rootN*pad_length)*sizeof(double)+PAGE_SIZE);
trans = (double *) G_MALLOC(2*(N+rootN*pad_length)*sizeof(double)+PAGE_SIZE);
umain = (double *) G_MALLOC(2*rootN*sizeof(double));
umain2 = (double *) G_MALLOC(2*(N+rootN*pad_length)*sizeof(double)+PAGE_SIZE);
Global->transtimes = (long *) G_MALLOC(P*sizeof(long));
Global->totaltimes = (long *) G_MALLOC(P*sizeof(long));
if (Global == NULL) {
printerr("Could not malloc memory for Global\n");
exit(-1);
} else if (x == NULL) {
printerr("Could not malloc memory for x\n");
exit(-1);
} else if (trans == NULL) {
printerr("Could not malloc memory for trans\n");
exit(-1);
} else if (umain == NULL) {
printerr("Could not malloc memory for umain\n");
exit(-1);
} else if (umain2 == NULL) {
printerr("Could not malloc memory for umain2\n");
exit(-1);
}
x = (double *) (((unsigned long) x) + PAGE_SIZE - ((unsigned long) x) % PAGE_SIZE);
trans = (double *) (((unsigned long) trans) + PAGE_SIZE - ((unsigned long) trans) % PAGE_SIZE);
umain2 = (double *) (((unsigned long) umain2) + PAGE_SIZE - ((unsigned long) umain2) % PAGE_SIZE);
/* In order to optimize data distribution, the data structures x, trans,
and umain2 have been aligned so that each begins on a page boundary.
This ensures that the amount of padding calculated by the program is
such that each processor's partition ends on a page boundary, thus
ensuring that all data from these structures that are needed by a
processor can be allocated to its local memory */
/* POSSIBLE ENHANCEMENT: Here is where one might distribute the x,
trans, and umain2 data structures across physically distributed
memories as desired.
One way to place data is as follows:
double *base;
long i;
i = ((N/P)+(rootN/P)*pad_length)*2;
base = &(x[0]);
for (j=0;j<P;j++) {
Place all addresses x such that (base <= x < base+i) on node j
base += i;
}
The trans and umain2 data structures can be placed in a similar manner.
*/
printf("\n");
printf("FFT with Blocking Transpose\n");
printf(" %ld Complex Doubles\n",N);
printf(" %ld Processors\n",P);
if (num_cache_lines != orig_num_lines) {
printf(" %ld Cache lines\n",orig_num_lines);
printf(" %ld Cache lines for blocking transpose\n",num_cache_lines);
} else {
printf(" %ld Cache lines\n",num_cache_lines);
}
printf(" %d Byte line size\n",(1 << log2_line_size));
printf(" %d Bytes per page\n",PAGE_SIZE);
printf("\n");
BARINIT(Global->start, P);
LOCKINIT(Global->idlock);
Global->id = 0;
InitX(x); /* place random values in x */
if (test_result) {
ck1 = CheckSum(x);
}
if (doprint) {
printf("Original data values:\n");
PrintArray(N, x);
}
InitU(N,umain); /* initialize u arrays*/
InitU2(N,umain2,rootN);
/* fire off P processes */
CREATE(SlaveStart, P);
WAIT_FOR_END(P);
if (doprint) {
if (test_result) {
printf("Data values after inverse FFT:\n");
} else {
printf("Data values after FFT:\n");
}
PrintArray(N, x);
}
transtime = Global->transtimes[0];
printf("\n");
printf(" PROCESS STATISTICS\n");
printf(" Computation Transpose Transpose\n");
printf(" Proc Time Time Fraction\n");
printf(" 0 %10ld %10ld %8.5f\n",
Global->totaltimes[0],Global->transtimes[0],
((double)Global->transtimes[0])/Global->totaltimes[0]);
if (dostats) {
transtime2 = Global->transtimes[0];
avgtranstime = Global->transtimes[0];
avgcomptime = Global->totaltimes[0];
maxtotal = Global->totaltimes[0];
mintotal = Global->totaltimes[0];
maxfrac = ((double)Global->transtimes[0])/Global->totaltimes[0];
minfrac = ((double)Global->transtimes[0])/Global->totaltimes[0];
avgfractime = ((double)Global->transtimes[0])/Global->totaltimes[0];
for (i=1;i<P;i++) {
if (Global->transtimes[i] > transtime) {
transtime = Global->transtimes[i];
}
if (Global->transtimes[i] < transtime2) {
transtime2 = Global->transtimes[i];
}
if (Global->totaltimes[i] > maxtotal) {
maxtotal = Global->totaltimes[i];
}
if (Global->totaltimes[i] < mintotal) {
mintotal = Global->totaltimes[i];
}
if (((double)Global->transtimes[i])/Global->totaltimes[i] > maxfrac) {
maxfrac = ((double)Global->transtimes[i])/Global->totaltimes[i];
}
if (((double)Global->transtimes[i])/Global->totaltimes[i] < minfrac) {
minfrac = ((double)Global->transtimes[i])/Global->totaltimes[i];
}
printf(" %3ld %10ld %10ld %8.5f\n",
i,Global->totaltimes[i],Global->transtimes[i],
((double)Global->transtimes[i])/Global->totaltimes[i]);
avgtranstime += Global->transtimes[i];
avgcomptime += Global->totaltimes[i];
avgfractime += ((double)Global->transtimes[i])/Global->totaltimes[i];
}
printf(" Avg %10.0f %10.0f %8.5f\n",
((double) avgcomptime)/P,((double) avgtranstime)/P,avgfractime/P);
printf(" Max %10ld %10ld %8.5f\n",
maxtotal,transtime,maxfrac);
printf(" Min %10ld %10ld %8.5f\n",
mintotal,transtime2,minfrac);
}
Global->starttime = start;
printf("\n");
printf(" TIMING INFORMATION\n");
printf("Start time : %16lu\n",
Global->starttime);
printf("Initialization finish time : %16lu\n",
Global->initdonetime);
printf("Overall finish time : %16lu\n",
Global->finishtime);
printf("Total time with initialization : %16lu\n",
Global->finishtime-Global->starttime);
printf("Total time without initialization : %16lu\n",
Global->finishtime-Global->initdonetime);
printf("Overall transpose time : %16ld\n",
transtime);
printf("Overall transpose fraction : %16.5f\n",
((double) transtime)/(Global->finishtime-Global->initdonetime));
printf("\n");
if (test_result) {
ck3 = CheckSum(x);
printf(" INVERSE FFT TEST RESULTS\n");
printf("Checksum difference is %.3f (%.3f, %.3f)\n",
ck1-ck3, ck1, ck3);
if (fabs(ck1-ck3) < 0.001) {
printf("TEST PASSED\n");
} else {
printf("TEST FAILED\n");
}
}
MAIN_END;
}
/*
*********************************************************************************************************
* App_TaskNoah_Ruler()
*
* Description : Process Comminucation between Ruler module and Audio Bridge.
* Noah protocol layer related data parsing and processing.
* This task wait for message event from App_TaskUART_Rx(). Check if the Noah layer data is
* valid in the message.
*
* Argument(s) : p_arg Argument passed to 'App_TaskNoah_Ruler()' by 'OSTaskCreate()'.
*
* Return(s) : none.
*
* Note(s) : (1) The first line of code is used to prevent a compiler warning because 'p_arg' is not
* used. The compiler should not generate any code for this statement.
*********************************************************************************************************
*/
void App_TaskNoah_Ruler( void *p_arg )
{
(void)p_arg;
NOAH_CMD *pPcCmd ;
CPU_INT08U *pCmdBuf ;
CPU_INT08U rxID ;
CPU_INT08U PcCmdRxID_Ruler[4];
CPU_INT08U sum ;
CPU_INT08U SessionDone;
CPU_INT08U err ;
CPU_INT08U *pTaskMsgIN ;
CPU_INT08U *pMsg ;
pTaskMsgIN = NULL;
pMsg = NULL;
SessionDone = 0 ;
while( DEF_TRUE ) {
pTaskMsgIN = (INT8U *)OSQPend( EVENT_MsgQ_RulerUART2Noah, 0, &err );
if( pTaskMsgIN != NULL && OS_ERR_NONE == err ) {
pCmdBuf = pTaskMsgIN; // char point to the data buffer
pPcCmd = (NOAH_CMD *)pCmdBuf ; //change to NOAH_CMD type
rxID = GET_FRAME_ID( pPcCmd->head ) ; //get frame ID, index
sum = pPcCmd->checkSum; //get check sum data
LED_Toggle(LED_DS2);
APP_TRACE_DBG(("\r\n<<"));
switch( GET_FRAME_TYPE( pPcCmd->head ) ) {// GET_FRAME_TYPE(pPcCmd->head) get frame type
case FRAM_TYPE_DATA :
if( (sum == 0) || ( sum == CheckSum(0,pCmdBuf, pPcCmd->DataLen + 2)) ) {
// APP_TRACE_INFO(("R[%d]:[0x%2x>0x%2x][",Global_Ruler_Index,PcCmdRxID_Ruler[Global_Ruler_Index],rxID));
// for(unsigned int i = 0; i<pPcCmd->DataLen; i++){
// APP_TRACE_INFO((" %2X", *(unsigned char*)(pPcCmd->Data+i) ));
// }
// APP_TRACE_INFO((" ]\r\n"));
pcSendDateToBuf( EVENT_MsgQ_Noah2RulerUART, SET_FRAME_HEAD(rxID,FRAM_TYPE_ACK), NULL, 0, 0, NULL, 0 ) ; // ACK
if( (CPU_INT08U)(PcCmdRxID_Ruler[Global_Ruler_Index]+0x40) == rxID ) { // chweck if frameID repeat
PcCmdRxID_Ruler[Global_Ruler_Index] = rxID ; //save this frameID
err = Noah_CMD_Parse_Ruler( pPcCmd, &SessionDone); // jump to CMD parse
if( (err != OS_ERR_NONE) || (SessionDone == 1) ) {
SessionDone = 0;
Ruler_CMD_Result = err ;
OSTimeDly(2); //wait enough time for ACK to be sent
APP_TRACE_DBG((" Res[0x%2x]", Ruler_CMD_Result));
OSSemPost( Done_Sem_RulerUART );
}
} else {
APP_TRACE_DBG(("PcCmdRxID_Ruler[%d] Err: expect 0x%X, get 0x%X",Global_Ruler_Index,PcCmdRxID_Ruler[Global_Ruler_Index]+0x40,rxID));
}
} else {
pcSendDateToBuf( EVENT_MsgQ_Noah2RulerUART, SET_FRAME_HEAD(rxID,FRAM_TYPE_NAK), NULL, 0, 0, NULL, 0 ) ; // NAK
}
break ;
case FRAM_TYPE_ESTA :
PcCmdRxID_Ruler[Global_Ruler_Index] = 0xC0 ; // ? why 0x40: make sure there can be many same setup frame
PcCmdTxID_Ruler[Global_Ruler_Index] = 0x00 ; //
OSSemPost( ACK_Sem_RulerUART );
OSSemPost( Done_Sem_RulerUART ); //end the resend pending--
OSTimeDly(1); //wait for the TX buffer is empty
//Reset all UART CMD related buffer and release mem
do{ //reset mem used by EVENT_MsgQ_RulerUART2Noah
pMsg = (INT8U *)OSQAccept( EVENT_MsgQ_Noah2RulerUART, &err );
OSMemPut( pMEM_Part_MsgUART, pMsg );
} while ( pMsg != NULL && OS_ERR_NONE == err ) ;
do{ //reset mem used by EVENT_MsgQ_RulerUART2Noah
pMsg = (INT8U *)OSQAccept( EVENT_MsgQ_RulerUART2Noah, &err );
OSMemPut( pMEM_Part_MsgUART, pMsg );
} while ( pMsg != NULL && OS_ERR_NONE == err ) ;
Queue_Flush( pUART_Send_Buf[RULER_UART] ); //clear uart send&rec data queue
Queue_Flush( pUART_Rece_Buf[RULER_UART] );
SessionDone = 0; //init
OSSemSet( ACK_Sem_RulerUART, 0, &err ); // clear the sem
OSSemSet( Done_Sem_RulerUART, 0, &err );
test_counter4++;
APP_TRACE_DBG(("EST/ESTA"));
break ;
case FRAM_TYPE_ACK :
if( rxID == PcCmdTxID_Ruler[Global_Ruler_Index] ) {
OSSemPost( ACK_Sem_RulerUART );
test_counter2++;
} else {
APP_TRACE_INFO_T(("\r\nACK: Got %X, Expect %X\r\n",rxID,PcCmdTxID_Ruler[Global_Ruler_Index] ));
}
APP_TRACE_DBG(("ACK"));
test_counter3++;
break ;
case FRAM_TYPE_NAK :
// dismiss NAK, there will be a resend if no ACK got
// OSSemPost( Done_Sem_RulerUART );
test_counter1++;
APP_TRACE_DBG(("NAK"));
break;
default :
break ;
}
//release mem
OSMemPut( pMEM_Part_MsgUART, pTaskMsgIN );
}
////OSTimeDly(2); //use OSQPend(), no delay needed!!!
}
}
/*******************************************************************************
* Function Name : KeyHandle
* Description : The key response function
* Input : None
* Output : None
* Return : None
* Attention : None
*******************************************************************************/
void KeyHandle(void)
{
if(Key_Return & PRESS_KEY1)
{
if(Key_Return & KEY_LONG)
{
m_pro_commonCmd.head_part.cmd = CMD_RESET_MODULE;
m_pro_commonCmd.head_part.sn = ++SN;
m_pro_commonCmd.sum = CheckSum((uint8_t *)&m_pro_commonCmd, sizeof(pro_commonCmd));
SendToUart((uint8_t *)&m_pro_commonCmd, sizeof(pro_commonCmd), 1);
LED_RGB_Control(0,0,50);
LED_RGB_Control(0,0,0);
Key_Return = 0;
}
}
if(Key_Return & PRESS_KEY2)
{
if(Key_Return & KEY_LONG)
{
m_m2w_setModule.config_info = 0x01; //soft ap
LED_RGB_Control(50,0,0);
wait_wifi_status = 1;
}
else if(Key_Return & KEY_UP)
{
m_m2w_setModule.config_info = 0x02; //air link
LED_RGB_Control(0,50,0);
wait_wifi_status = 1;
}
else
{
Key_Return = 0;
return ;
}
m_m2w_setModule.head_part.sn = ++SN;
m_m2w_setModule.sum = CheckSum((uint8_t *)&m_m2w_setModule, sizeof(m2w_setModule));
SendToUart((uint8_t *)&m_m2w_setModule, sizeof(m2w_setModule), 1);
delay_ms(500);
Key_Return = 0;
}
if(Key_Return & PRESS_KEY3)
{
if(Key_Return & KEY_LONG)
{
m_m2w_mcuStatus.status_r.alert_byte = 0x01; //alert 1
LED_RGB_Control(50,0,0);
LED_RGB_Control(0,0,0);
}
else if(Key_Return & KEY_UP)
{
m_m2w_mcuStatus.status_r.fault_byte = 0x03; //fault 1 and 2
LED_RGB_Control(0,50,0);
LED_RGB_Control(0,0,0);
}
else
{
Key_Return = 0;
return ;
}
ReportStatus(REPORT_STATUS);
m_m2w_mcuStatus.status_r.alert_byte = 0x00; //clean the alert
m_m2w_mcuStatus.status_r.fault_byte = 0x00; //clean the fault
delay_ms(500);
Key_Return = 0;
}
if(Key_Return & PRESS_KEY4)
{
if(Key_Return & KEY_LONG)
{
m_m2w_mcuStatus.status_r.alert_byte = 0x02; //alert 2
LED_RGB_Control(50,0,0);
LED_RGB_Control(0,0,0);
}
else if(Key_Return & KEY_UP)
{
m_m2w_mcuStatus.status_r.fault_byte = 0x0C; //fault 3 and 4
LED_RGB_Control(0,50,0);
LED_RGB_Control(0,0,0);
}
else
{
Key_Return = 0;
return ;
}
ReportStatus(REPORT_STATUS);
m_m2w_mcuStatus.status_r.alert_byte = 0x00; //clean the alert
m_m2w_mcuStatus.status_r.fault_byte = 0x00; //clean the fault
delay_ms(500);
Key_Return = 0;
}
}
BOOL clsCMM::ParseBuffer(char *pBuffer, int &nBuffer, ADDRESSEDPACKAGE *pPackage)
{
//ParseBuffer analysis if there is a package in buffer,
//if yes, extract it to pPackage and reset the pBuffer and nBuffer and return true,
//otherwise do nothing and return false
char *pChar = pBuffer;
char *pCharMax = pBuffer + nBuffer - 1;
enum { BEGIN, HEADER, SIZE, PACKAGE, CHECK } state = BEGIN;
char from, to;
short size;
//pointers to identify packages in buffer
//pThisPackage point to the first package in buffer, pNextPackage point the the next package
//if there is no package in buffer, pThisPackage is null, if there is package in buffer, but not completed, then pThisPackage point to the package, but pNextPackage is null
//if buffer contains a complete package, then pThisPackage point to the package and pNextPackage point to the first char after this package
char *pThisPackage = NULL;
char *pNextPackage = NULL;
while (pChar <= pCharMax) {
if (state == BEGIN) {
if ((*pChar >= 0 && *pChar <= 99) || *pChar & 0x80) {
from = pChar[0];
pThisPackage = pChar;
if (from & 0x80) from &= ~0x80; //eliminate the leading 1, new protocol
state = HEADER; //from
}
pChar++; continue;
}
if (state == HEADER) {
if ((*pChar >= 0 && *pChar <= 99) || *pChar & 0x80) {
to = pChar[0];
to &= ~0x80; //eliminate the leading 1
state = SIZE;
}
else {
pThisPackage = NULL;
state = BEGIN;
}
pChar ++; continue;
}
if (state == SIZE) {
if (pChar + 1 > pCharMax) break; //non-complete break
size = (short &)pChar[0];
if (size < 1 || size > MAXSIZE_PACKAGE) { //improper package, reset
pThisPackage = NULL;
state = BEGIN;
continue;
}
else {
state = PACKAGE;
pChar += 2;
continue;
}
}
if (state == PACKAGE) {
if (pChar + size + 1 > pCharMax) break; //non-complete break;
unsigned short sum = CheckSum(pChar, size);
unsigned short check = GETWORD(pChar+size);
if (sum != check) { //improper package, reset
pThisPackage = NULL;
state = BEGIN;
continue;
}
//success
pPackage->from = from;
pPackage->to = to;
pPackage->package.size = size;
::memcpy(pPackage->package.content, pChar, size);
pNextPackage = pChar + size + 2;
break;
}
}
if (pThisPackage == NULL) { //no package in buffer, clear buffer
nBuffer = 0;
return FALSE;
}
if (pNextPackage == NULL) return FALSE; //package not complete, also return false
if (pNextPackage > pCharMax)
nBuffer = 0; //buffer exactly end at this package, clear buffer
else { //otherwise, shift left content to the head of buffer (clear this package)
nBuffer = pCharMax - pNextPackage + 1;
::memcpy(pBuffer, pNextPackage, nBuffer);
}
return TRUE;
}
/*******************************************************************************
* Function Name : MessageHandle
* Description :
* Input : None
* Output : None
* Return : None
* Attention : None
*******************************************************************************/
uint8_t GizWits_MessageHandle(uint8_t * Message_Buf, uint8_t Length_buf)
{
Pro_HeadPartTypeDef * Recv_HeadPart = NULL;
uint8_t ret = 0;
//抓取一包
Pro_GetFrame();
//ACK超时重发机制
ret = GizWits_D2W_Resend_AckCmdHandle();
if(ret == 1)
{
#if(DEBUG==1)
Serial.println(F("1 Give up Resend!"));
#endif
}
else if(ret == 2)
{
#if(DEBUG==1)
Serial.print(F("[1 Resend ACK --> Time / Num: "));Serial.print(Wait_AckStruct.SendTime,DEC);Serial.print(" / ");Serial.print(Wait_AckStruct.SendNum,DEC);Serial.print("]");
Serial.println("");
#endif
}
if(packageFlag)
{
//验证校验码
if(CheckSum(UART_HandleStruct.Message_Buf, UART_HandleStruct.Message_Len) !=
UART_HandleStruct.Message_Buf[UART_HandleStruct.Message_Len - 1])
{
Pro_W2D_ErrorCmdHandle(Error_AckSum, 0);
packageFlag = 0;
return 2;
}
//检测返回ACK状态
ret = GizWits_W2D_AckCmdHandle();
if(ret == 1)
{
#if(DEBUG==1)
Serial.println(F("1 ACK: SUCCESS! ..."));
#endif
}
else if(ret == 2)
{
#if(DEBUG==1)
Serial.println(F("2 ACK: Report Again! ..."));
#endif
}
else if(ret == 3)
{
#if(DEBUG==1)
Serial.println(F("3 ACK: SUCCESS--but--Time out! ..."));
#endif
}
Recv_HeadPart = (Pro_HeadPartTypeDef *)UART_HandleStruct.Message_Buf;
switch (Recv_HeadPart->Cmd)
{
case Pro_W2D_GetDeviceInfo_Cmd:
Pro_W2D_GetMcuInfo();
break;
case Pro_W2D_P0_Cmd:
{
switch(UART_HandleStruct.Message_Buf[sizeof(Pro_HeadPartTypeDef)])
{
case P0_W2D_Control_Devce_Action:
{
Pro_W2D_CommonCmdHandle();
memcpy(Message_Buf, UART_HandleStruct.Message_Buf+sizeof(Pro_HeadPartP0CmdTypeDef), Length_buf);
memset(&UART_HandleStruct.Message_Buf, 0, UART_HandleStruct.Message_Len);
packageFlag = 0;
return 0;
}
case P0_W2D_ReadDevStatus_Action:
Pro_W2D_ReadDevStatusHandle();
break;
default:
break;
}
}
break;
case Pro_W2D_P0_Ack_Cmd:
break;
case Pro_W2D_Heartbeat_Cmd:
Pro_W2D_CommonCmdHandle();
break;
case Pro_W2D_ControlWifi_Config_Ack_Cmd:
break;
case Pro_W2D_ResetWifi_Ack_Cmd:
break;
case Pro_W2D_ReportWifiStatus_Cmd:
Pro_W2D_WifiStatusHandle();
break;
case Pro_W2D_ReportMCUReset_Cmd:
Pr0_W2D_RequestResetDeviceHandle();
break;
case Pro_W2D_ErrorPackage_Cmd:
Pro_W2D_ErrorCmdHandle(Error_Other, 1);
break;
default:
Pro_W2D_ErrorCmdHandle(Error_Cmd, 0);
break;
}
memset(&UART_HandleStruct.Message_Buf, 0, UART_HandleStruct.Message_Len);
packageFlag = 0;
}
return 1;
}
//*****************************************************************************
//
//! SendPacket() sends a data packet.
//!
//! \param pucData is the location of the data to be sent to the device.
//! \param ucSize is the number of bytes to send from puData.
//! \param bAck is a boolean that is true if an ACK/NAK packet should be
//! received in response to this packet.
//!
//! This function sends a packet of data to the device.
//!
//! \returns The function returns zero to indicated success while any non-zero
//! value indicates a failure.
//
//*****************************************************************************
int
SendPacket(unsigned char *pucData, unsigned char ucSize, unsigned long bAck)
{
unsigned char ucCheckSum;
unsigned char ucAck;
ucCheckSum = CheckSum(pucData, ucSize);
//
// Make sure that we add the bytes for the size and checksum to the total.
//
ucSize += 2;
//
// Send the Size in bytes.
//
if(UARTSendData(&ucSize, 1))
{
return(-1);
}
//
// Send the CheckSum
//
if(UARTSendData(&ucCheckSum, 1))
{
return(-1);
}
//
// Now send the remaining bytes out.
//
ucSize -= 2;
//
// Send the Data
//
if(UARTSendData(pucData, ucSize))
{
return(-1);
}
//
// Return immediately if no ACK/NAK is expected.
//
if(!bAck)
{
return(0);
}
//
// Wait for the acknoledge from the device.
//
do
{
if(UARTReceiveData(&ucAck, 1))
{
return(-1);
}
}
while(ucAck == 0);
if(ucAck != COMMAND_ACK)
{
return(-1);
}
return(0);
}
int main(int argc,char* argv[])
{
int opt;
int fd=0;
char *device=NULL;
char *baudrate=NULL;
while ((opt = getopt(argc, argv, "d:b:-:")) != -1)
{
switch (opt)
{
case 'd':
device=optarg;
dbg_printf("-d %s\n",device);
break;
case 'b':
baudrate=optarg;
dbg_printf("-b %s\n",baudrate);
break;
case '-':
if(strcmp(optarg,"help")==0)
{
printf( "Usage : [-d device] [-b] baudrate\n");
printf( " %s -d /dev/ttyUSB0 -b 38400 \n",argv[0]);
return 0;
}
break;
case '?':
default: /* '?' */
fprintf(stderr, "Usage: %s [-d device] [-b] baudrate\n", argv[0]);
exit(EXIT_FAILURE);
}
}
fd = uartInit(fd, device==NULL ? serial_name: device
, baudrate==NULL ? 19200 : atoi(baudrate));
while ( 1 )
{
int i;
short readLen = 0;
memset( rcvUartBuf, 0, sizeof( rcvUartBuf ) );
memset( input, 0, sizeof( input ) );
printf( "Atcmd no include start, length, checkSum, end \n" );
printf( "Enter Atcmd : " );
scanf( "%s", input );
if((input[0]=='q' || input[0]=='Q') && input[1]==0 )
{
printf(" enter quit \n");
return 0;
}
else if(input[0]=='s'|| (input[0]=='S'&&input[1]==0) )
{
printf(" enter Show \n");
dataDump(fd,rcvUartBuf);
continue;
}
/*DEBUG*/
#ifdef RYAN_DEBUG
len = ( strlen( input ) / 2 );
inlen = strlen( input );
printf( "inlen = %X\n", inlen );
#endif
printf( "[T]: " );
for ( i = 0; i < inlen; i++ )
{
if ( input[i] < 58 ) /*0~9 字元轉數字 0~9*/
input[i] -= 48;
if ( input[i] > 64 && input[i] < 71 ) /*A~F 字元轉數字 A~F*/
input[i] -= 55;
if ( input[i] > 96 && input[i] < 103 ) /*a~f 字元轉數字 a~f*/
input[i] -= 87;
output[i] = input[i];
#ifdef RYAN_DEBUG
printf( "%X", output[i] ); //debug
#endif
}
outlen = inlen;
for ( i = 0; i < outlen/2; i++ )
{
output[i] = ( output[i * 2] << 4 ) | ( output[( i * 2 ) + 1] );
printf( "%02X", output[i] );
}
printf( "\n" );
printf( "outlen = %X ", outlen );
sum = CheckSum ( output, outlen, len );
printf( "len= %x ", len );
printf( "checksum= %x \n", sum );
sendUartBuf[0] = 0x02;
sendUartBuf[1] = 0x00;
sendUartBuf[2] = outlen;
for ( i = 0, j = 0; i < outlen + 5; i++, j++ )
{
sendUartBuf[i + 3] = output[j];
}
sendUartBuf[i - 2] = sum;
sendUartBuf[i - 1] = 0x03;
printf( "sendUartBuf = : " );
for ( i = 0; i < len + 5; i++ )
{
printf( "%02X", sendUartBuf[i] );
}
#ifdef RYAN_DEBUG
printf( "\n sendbuf outlen = %d\n", outlen + 5 );
write( fd, &sendUartBuf, outlen + 5 );
#endif
printf( "\n" );
sleep( 5 );
/* read reponse */
readLen = read ( fd, rcvUartBuf, uartAck );
printf( "***********************************Start****************************\n\n" );
printf( "[R]: " );
for ( i = 0; i < uartAck; i++ )
{
printf( "%02X", rcvUartBuf[i] );
}
printf( "\n\n" );
printf( "[Parse]: " );
for ( i = 0; i < uartAck; i++ )
{
printf( "%C", rcvUartBuf[i] );
}
printf( "\n**********************************End*****************************\n\n" );
}
return 0;
}
int main(int argc,char *argv[])
{
double *Anext;
double *A0;
int nx,ny,nz,tx,ty,tz,timesteps;
int i;
ticks t1, t2;
double spt;
/* parse command line options */
if (argc < 8) {
printf("\nUSAGE:\n%s <grid x> <grid y> <grid z> <block x> <block y> <block z> <timesteps>\n", argv[0]);
printf("\nTIME SKEWING CONSTRAINTS:\nIn each dimension, <grid size - 2> should be a multiple of <block size>.\n");
printf("\nCIRCULAR QUEUE CONSTRAINTS:\n<grid y - 2> should be a multiple of <block y>. The block sizes in the other dimensions are ignored.\n\n");
return EXIT_FAILURE;
}
nx = atoi(argv[1]);
ny = atoi(argv[2]);
nz = atoi(argv[3]);
tx = atoi(argv[4]);
ty = atoi(argv[5]);
tz = atoi(argv[6]);
timesteps = atoi(argv[7]);
printf("%dx%dx%d, blocking: %dx%dx%d, timesteps: %d\n",
nx,ny,nz,tx,ty,tz,timesteps);
#ifdef HAVE_PAPI
PAPI_library_init(PAPI_VER_CURRENT);
#endif
/* find conversion factor from ticks to seconds */
spt = seconds_per_tick();
/* allocate arrays */
Anext=(double*)malloc(sizeof(double)*nx*ny*nz);
A0=(double*)malloc(sizeof(double)*nx*ny*nz);
printf("USING TIMER: %s \t SECONDS PER TICK:%g \n", TIMER_DESC, spt);
for (i=0;i<NUM_TRIALS;i++) {
/* initialize arrays to all ones */
StencilInit(nx,ny,nz,Anext);
StencilInit(nx,ny,nz,A0);
#ifdef CIRCULARQUEUEPROBE
if (timesteps > 1) {
CircularQueueInit(nx, ty, timesteps);
}
#endif
// clear_cache();
t1 = getticks();
/* stencil function */
StencilProbe(A0, Anext, nx, ny, nz, tx, ty, tz, timesteps);
t2 = getticks();
printf("elapsed ticks: %g time:%g \n", elapsed(t2, t1), spt * elapsed(t2,t1));
#ifdef COMPUTECHECKSUM
extern double CheckSum(int, int, int, double*);
double r1=CheckSum(nx, ny, nz, A0);
double r2=CheckSum(nx, ny, nz, Anext);
printf("Checksums: A0=%g, Anext=%g\n", r1, r2);
#endif
}
/* free arrays */
free(Anext);
free(A0);
return EXIT_SUCCESS;
}
void *defaultScan(void *arg)
{
int protocolNumber;
pthread_mutex_lock(&lockProto);
args *myarg = (args *)arg;
// memcpy(myarg,arg,sizeof(args));
string destIp = myarg->ip;
protocolNumber= myarg->protocolNum;
pthread_mutex_unlock(&lockProto);
//cout << "prno " << protocolNumber;
bool isIp4;
if ( protocolNumber == NO_PROTO )
pthread_exit(NULL);
int version = ipVersion(destIp);
if (version == IPV4 )
{
isIp4 = true;
}
else if (version == IPV6 )
{
isIp4 = false;
}
else if ( version == NO_IP_FORMAT )
{
cout << "invalid IP " << destIp;
pthread_exit(NULL);
}
//WHERE
struct ps_iphdr *iphdrSend , *iphdrRecv;
struct ps_ip6hdr *ip6hdrSend , *ip6hdrRecv;
struct sockaddr_in srvaddr;
//need to change variable name
struct hostent *hstent, *servTcpIpAddr;
struct sockaddr_in clientsin;
struct servent *serv,*poss_serv;
struct sockaddr_storage recvStorage;
struct sockaddr_in6 clientsin6;
int sockfd;
int IPHEADER ;
if ( isIp4)
IPHEADER = IPHEADER_SIZE;
else
IPHEADER = IPV6HEADER_SIZE;
int packetLen = IPHEADER + ICMPHEADER_SIZE;
unsigned char *packetData = new unsigned char[packetLen];
unsigned char recvIcmpPacketData[packetLen];
buildIpHeader ipPkt;
buildIp6Header ip6Pkt;
int portNumber = 8971;
map<string,string> p;
// pthread_mutex_lock(&lockProto);
// portNumber= myOpt.getPort(portIndex);
//portIndex++;
// pthread_mutex_unlock(&lockProto);
int icmpStatus;
/// while ( portNumber)
// {
//printf("The ID of this thread is: %u and Protocol is: %d\n", (unsigned int)pthread_self(),protocolNumber);
setZeroMemory(packetData,packetLen);
if ( isIp4)
sockfd = socket(AF_INET, SOCK_RAW, IPPROTO_RAW);
else
sockfd = socket(AF_INET6, SOCK_RAW, IPPROTO_UDP);
if(sockfd < 0 )
{
delete[] packetData;
perror("sockfd create : ");
pthread_exit(NULL);
}
int sockicmpfd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
if ( isIp4 )
{
ipPkt.setIpHeader(myOpt.getSourceIp(),destIp,protocolNumber);
iphdrSend = ipPkt.getIpHeader();
memcpy(packetData,(void *)iphdrSend, IPHEADER);
struct ps_iphdr *tmphdr = (struct ps_iphdr *)packetData;
tmphdr->ip_sum = CheckSum(packetData,IPHEADER_SIZE);
if ( protocolNumber == IPPROTO_ICMP )
{
struct icmp *icmphdr = ( struct icmp *)(packetData+IPHEADER);
icmphdr->icmp_type = ICMP_ECHO;
icmphdr->icmp_code = 0;
icmphdr->icmp_seq = htons(0);
icmphdr->icmp_id = htons(100);
icmphdr->icmp_cksum = (CheckSum(packetData,ICMPHEADER_SIZE+IPHEADER_SIZE));
}
}
//add Here for IPV6
if ( isIp4)
{
clientsin.sin_family = AF_INET;
servTcpIpAddr = gethostbyname(destIp.c_str());
bcopy((char *)servTcpIpAddr->h_addr, (char *)&clientsin.sin_addr.s_addr, servTcpIpAddr->h_length);
clientsin.sin_port=htons(portNumber);
}
//add here for IPV6
icmpStatus = NO_ICMP;
for(int countRe =0;countRe< NUMBER_RETRANSMISSIONS && ( icmpStatus == NO_ICMP);countRe++)
{
const int on = 1;
int setsockStatus;
if ( isIp4)
{
setsockStatus = setsockopt(sockfd, IPPROTO_IP, IP_HDRINCL, &on, sizeof (on));
if (setsockStatus < 0 )
{
cerr<< "Cannot set HDRINCL port";
delete[] packetData;
pthread_exit(NULL);
}
}
int sendStatus;
if ( isIp4)
sendStatus = sendto(sockfd, packetData, iphdrSend->ip_len, 0, (struct sockaddr *)&clientsin, sizeof(clientsin)) ;
else
{
//sendStatus = sendto(sockfd, packetData, ip6hdrSend->ip_len, 0, (struct sockaddr *)&clientsin6, sizeof(clientsin6)) ;
}
if (sendStatus == -1 && errno != EINTR )
{
//perror(" error in sendto: ");
delete[] packetData;
pthread_exit(NULL);
}
//analysePacket
fcntl(sockicmpfd, F_SETFL, O_NONBLOCK);
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(sockfd, &fds);
FD_SET(sockicmpfd,&fds);
tv.tv_sec = TIMEOUT;
tv.tv_usec = 0;
int selectStatus = select(sockicmpfd+1, &fds, NULL, NULL, &tv);
setZeroMemory(recvIcmpPacketData,0);
socklen_t fromlen = sizeof (recvStorage);
int numbytesIcmpRecvd = recvfrom(sockicmpfd,recvIcmpPacketData, packetLen , 0,(struct sockaddr *)&recvStorage, &fromlen);
if( numbytesIcmpRecvd > 0 )
icmpStatus = parseIcmpHeader(recvIcmpPacketData,numbytesIcmpRecvd,(void *)iphdrSend,version);
}
string protocolName;
struct protoent *servptr;
if(!(servptr = getprotobynumber(protocolNumber)))
protocolName = "N/A";
else
protocolName = servptr->p_name;
if ( icmpStatus == ICMP_REPLY )
{
//cout << "open";
p.insert(pair<string,string>(protocolName,"open"));
}
else if ( icmpStatus == ICMP_PROTO_UNREACHABLE )
{
//cout << "closed";
p.insert(pair<string,string>(protocolName,"closed"));
}
else if ( icmpStatus == NO_ICMP )
{
//cout << "open|filtered";
p.insert(pair<string,string>(protocolName,"open|filtered"));
}
else if ( icmpStatus == ICMP_UNREACHABLE || icmpStatus == ICMP_CLOSE )
{
//cout << "filtered";
p.insert(pair<string,string>(protocolName,"filtered"));
}
//int previousPort;
//previousPort = portNumber;
cout<<"\nProtocol: "<<protocolNumber<<"\n";
result.insert(pair<int,map<string,string> >(protocolNumber,p));
map<int,map<string,string> >::iterator outerIter;
map<string,string>::iterator innerIter;
outerIter = result.find(protocolNumber);
//cout<<"\nProtocol: "<<protocolNumber<<"\n";
for(innerIter = outerIter->second.begin(); innerIter!= outerIter->second.end(); innerIter++)
{
cout<<innerIter->first<<"--->"<<innerIter->second<<"\n";
}
result.erase(protocolNumber);
//pthread_mutex_lock(&lockPort);
//result.insert(pair<int,map<string,string> >(protocolNumber,p));
// portNumber= myOpt.getPort(portIndex);
// portIndex++;
//pthread_mutex_unlock(&lockPort);
// }
delete[] packetData;
}
void *tcpScan(void *arg)
{
//printf("The ID of this thread is: %u \n", (unsigned int)pthread_self());
args *myarg = (args *)arg;
int protocolNumber;
protocolNumber= myarg->protocolNum;
string destIp = myarg->ip;
bool isIp4;
int version = ipVersion(destIp);
if (version == IPV4 )
{
isIp4 = true;
}
else if (version == IPV6 )
{
isIp4 = false;
}
else if ( version == NO_IP_FORMAT )
{
cout << "invalid IP";
pthread_exit(NULL);
}
//extract details from the myOpt class
int numOfScans = myOpt.getScanOptionLength() ;
int sockfd, sockicmpfd;
//different header class
buildIpHeader ipPkt;
buildTcpHeader tcpPkt;
buildIp6Header ip6Pkt;
struct ps_tcphdr *tcphdrSend , *tcphdrRecv;
struct ps_iphdr *iphdrSend , *iphdrRecv;
struct ps_ip6hdr *ip6hdrSend , *ip6hdrRecv;
struct sockaddr_in srvaddr;
struct hostent *hstent, *servTcpIpAddr;
struct sockaddr_in clientsin;
struct servent *serv,*poss_serv;
struct sockaddr_storage recvStorage;
struct sockaddr_in6 clientsin6;
int IPHEADER;
if ( isIp4)
IPHEADER = IPHEADER_SIZE;
else
IPHEADER = IPV6HEADER_SIZE;
int packetLen = PSEUDOHEADER_SIZE+TCPHEADER_SIZE+IPHEADER;
unsigned char *packetData = new unsigned char[packetLen];
struct ps_pseudohdr *pseudohdr;
struct ps_pseudo6hdr *pseudo6hdr;
unsigned char segment[TCPHEADER_SIZE+PSEUDOHEADER_SIZE+DATA_SIZE];
unsigned char recvTcpPacketData[packetLen],recvIcmpPacketData[packetLen];
int portNumber;
pthread_mutex_lock(&lockPort);
portNumber= myOpt.getPort(portIndex);
portIndex++;
pthread_mutex_unlock(&lockPort);
//printf("The ID of this thread is: %u and Portnumber is: %d\n", (unsigned int)pthread_self(),portNumber);
while ( portNumber)
{
if ( protocolNumber != NO_PROTO)
cout<<"\nProtocol: "<<protocolNumber<<"\n";
map<string, string> p;
for ( int index = 0; index < numOfScans; index++ )
{
setZeroMemory(packetData,packetLen);
setZeroMemory(segment,TCPHEADER_SIZE+PSEUDOHEADER_SIZE);
//Declare socket TCP
if ( isIp4)
sockfd = socket(AF_INET, SOCK_RAW, IPPROTO_TCP);
else
sockfd = socket(AF_INET6, SOCK_STREAM, 0);
if(sockfd < 0 )
{
delete[] packetData;
perror("sockfd create : ");
pthread_exit(NULL);
}
sockicmpfd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
if (sockicmpfd < 0 )
{
perror("socket ICMP connect Error()\n");
delete[] packetData;
pthread_exit(NULL);
}
//set TCP header
tcpPkt.setTcpHeader(myOpt.getScanOptions(index),portNumber);
tcphdrSend = tcpPkt.getTcpHeader();
//set IP header
if ( isIp4)
{
ipPkt.setIpHeader(myOpt.getSourceIp(),destIp,IPPROTO_TCP);
iphdrSend = ipPkt.getIpHeader();
createPseudoHeader(&pseudohdr ,iphdrSend);
memcpy(segment,(void *)pseudohdr,PSEUDOHEADER_SIZE);
memcpy(segment+PSEUDOHEADER_SIZE,(void *)tcphdrSend,TCPHEADER_SIZE);
tcphdrSend->th_sum = (CheckSum(segment,TCPHEADER_SIZE+PSEUDOHEADER_SIZE));
}
else
{
ip6Pkt.setIp6Header(myOpt.getSourceIp(),destIp,IPPROTO_TCP);
ip6hdrSend = ip6Pkt.getIp6Header();
createPseudoHeader6(&pseudo6hdr ,ip6hdrSend);
memcpy(segment,(void *)pseudo6hdr,PSEUDO6HEADER_SIZE);
memcpy(segment+PSEUDO6HEADER_SIZE,(void *)tcphdrSend,TCPHEADER_SIZE);
tcphdrSend->th_sum = (CheckSum(segment,TCPHEADER_SIZE+PSEUDO6HEADER_SIZE));
}
if ( isIp4)
memcpy(packetData,(void *)iphdrSend, IPHEADER);
else
memcpy(packetData,(void *)ip6hdrSend, IPHEADER);
memcpy(packetData+IPHEADER,(void *)tcphdrSend,TCPHEADER_SIZE);
if ( isIp4)
free(pseudohdr);
else
free(pseudo6hdr);
//Time to follow socket API and connect
//see richard stevens chapter-28
if ( isIp4)
{
clientsin.sin_family = AF_INET;
servTcpIpAddr = gethostbyname(destIp.c_str());
bcopy((char *)servTcpIpAddr->h_addr, (char *)&clientsin.sin_addr.s_addr, servTcpIpAddr->h_length);
clientsin.sin_port=htons(portNumber);
}
else
{
clientsin6.sin6_family = AF_INET6;
clientsin6.sin6_flowinfo = 0;
servTcpIpAddr = gethostbyname2(destIp.c_str(),AF_INET6);
bind(sockfd,(struct sockaddr *) &clientsin6 , sizeof(clientsin6));
inet_pton(AF_INET6,destIp.c_str(), &(clientsin6.sin6_addr));
clientsin6.sin6_port=htons(portNumber);
}
const int on = 1;
int setsockStatus;
if ( isIp4) {
setsockStatus = setsockopt(sockfd, IPPROTO_IP, IP_HDRINCL, &on, sizeof (on));
if (setsockStatus < 0 )
{
cerr<< "Cannot set HDRINCL port";
delete[] packetData;
pthread_exit(NULL);
}
}
int retransStatus= NO_RESPONSE , icmpStatus = NO_ICMP;
for(int countRe =0;countRe< NUMBER_RETRANSMISSIONS && (retransStatus == NO_RESPONSE) && ( icmpStatus == NO_ICMP);countRe++)
{
fcntl(sockicmpfd, F_SETFL, O_NONBLOCK);
int sendStatus;
if ( isIp4)
sendStatus = sendto(sockfd, packetData, iphdrSend->ip_len, 0, (struct sockaddr *)&clientsin, sizeof(clientsin));
else
{
sendStatus = sendto(sockfd, packetData, ip6hdrSend->ip6_len, 0, (struct sockaddr *)&clientsin6, sizeof(clientsin6));
}
if (sendStatus == -1 && errno != EINTR )
{
//perror(" error in sendto: ");
delete[] packetData;
pthread_exit(NULL);
}
setZeroMemory(recvTcpPacketData,0);
setZeroMemory(recvIcmpPacketData,0);
socklen_t fromlen = sizeof (recvStorage);
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(sockfd, &fds);
FD_SET(sockicmpfd,&fds);
tv.tv_sec = TIMEOUT;
tv.tv_usec = 0;
int selectStatus = select(sockfd+1, &fds, NULL, NULL, &tv);
int icmpSelectStatus = select(sockicmpfd+1, &fds, NULL, NULL, &tv);
int numbytesTcpRecvd = recvfrom(sockfd, recvTcpPacketData, packetLen , 0,(struct sockaddr *)&recvStorage, &fromlen);
int numbytesIcmpRecvd = recvfrom(sockicmpfd,recvIcmpPacketData, packetLen , 0,(struct sockaddr *)&recvStorage, &fromlen);
//bool premoved = false;
if ( numbytesTcpRecvd > 0 )
{
if ( isIp4)
{
retransStatus = parseTcpHeader(recvTcpPacketData,numbytesTcpRecvd,myOpt.getScanOptions(index),iphdrSend,tcphdrSend, p);
//premoved = true;
}
else
{
retransStatus = parseTcpHeader6(recvTcpPacketData,numbytesTcpRecvd,myOpt.getScanOptions(index),ip6hdrSend,tcphdrSend,p);
}
}
else if ( numbytesIcmpRecvd > 0 )
{
//cout << "icmp\n";
if ( isIp4)
{
icmpStatus = parseIcmpHeader(recvIcmpPacketData,numbytesIcmpRecvd,(void *)iphdrSend,ipVersion(destIp));
}
else
{
icmpStatus = parseIcmpHeader(recvIcmpPacketData,numbytesIcmpRecvd,(void *)ip6hdrSend,ipVersion(destIp));
}
}
}
string scans = myOpt.getScanOptions(index);
if ( scans.compare("ACK") == 0)
{
if ( retransStatus == NO_RESPONSE || icmpStatus == ICMP_PROTO_UNREACHABLE || icmpStatus == ICMP_UNREACHABLE || icmpStatus == ICMP_CLOSE)
{
//cout << "filtered";
p.insert(pair<string,string>(scans.c_str(),"filtered"));
}
}
else if ( scans.compare("XMAS") == 0 || scans.compare("NULL") == 0 || scans.compare("FIN") == 0)
{
if ( retransStatus == NO_RESPONSE && icmpStatus == NO_ICMP )
{
//cout << "open|filtered";
p.insert(pair<string,string>(scans.c_str(),"open|filtered"));
}
if ( icmpStatus == ICMP_UNREACHABLE || icmpStatus == ICMP_PROTO_UNREACHABLE || icmpStatus == ICMP_CLOSE)
{
//cout << "filtered";
p.insert(pair<string,string>(scans.c_str(),"filtered"));
}
}
else if ( scans.compare("SYN") == 0)
{
// WHERE
//cout << "retransStatus " << retransStatus;
if ( retransStatus == NO_RESPONSE || icmpStatus == ICMP_UNREACHABLE || icmpStatus == ICMP_PROTO_UNREACHABLE || icmpStatus == ICMP_CLOSE)
{
//cout << "filtered\n";
p.insert(pair<string,string>(scans.c_str(),"filtered"));
}
}
close(sockfd);
close(sockicmpfd);
}
pthread_mutex_lock(&lockLog);
int previousPort;
previousPort = portNumber;
result.insert(pair<int,map<string,string> >(previousPort,p));
map<int,map<string,string> >::iterator outerIter;
map<string,string>::iterator innerIter;
outerIter = result.find(previousPort);
bool portStatus = false;
cout<<"\nPort:"<<portNumber<<"\n";
for(innerIter = outerIter->second.begin(); innerIter!= outerIter->second.end(); innerIter++)
{
cout<<innerIter->first<<"--->"<<innerIter->second<<"\n";
if(strcmp(innerIter->first.c_str(),"SYN") == 0 && strcmp(innerIter->second.c_str(),"open") == 0)
portStatus = true;
}
if(portStatus)
cout<<"Port "<<previousPort<<" Status: open"<<endl;
result.erase(previousPort);
pthread_mutex_unlock(&lockLog);
pthread_mutex_lock(&lockPort);
portNumber= myOpt.getPort(portIndex);
portIndex++;
pthread_mutex_unlock(&lockPort);
close(sockfd);
close(sockicmpfd);
}
delete[] packetData;
}
void *udpScan(void *arg)
{
args *myarg = (args *)arg;
int protocolNumber;
protocolNumber= myarg->protocolNum;
string destIp = myarg->ip;
bool isIp4;
int version = ipVersion(destIp);
if (version == IPV4 )
{
isIp4 = true;
}
else if (version == IPV6 )
{
isIp4 = false;
}
else if ( version == NO_IP_FORMAT )
{
cout << "invalid IP";
pthread_exit(NULL);
}
//extract details from the myOpt class
int numOfScans = myOpt.getScanOptionLength();
int sockfd, sockicmpfd;
//different header class
buildIpHeader ipPkt;
buildUdpHeader udpPkt;
buildIp6Header ip6Pkt;
struct ps_udphdr *udphdrSend , *udphdrRecv;
struct ps_iphdr *iphdrSend , *iphdrRecv;
struct ps_ip6hdr *ip6hdrSend , *ip6hdrRecv;
struct sockaddr_in srvaddr;
struct hostent *hstent, *servTcpIpAddr;
struct sockaddr_in clientsin;
struct servent *serv,*poss_serv;
struct sockaddr_storage recvStorage;
struct sockaddr_in6 clientsin6;
int IPHEADER ;
if ( isIp4)
IPHEADER = IPHEADER_SIZE;
else
IPHEADER = IPV6HEADER_SIZE;
int packetLen = PSEUDOHEADER_SIZE+UDPHEADER_SIZE+IPHEADER;
unsigned char *packetData = new unsigned char[packetLen];
struct ps_pseudohdr *pseudohdr;
struct ps_pseudo6hdr *pseudo6hdr;
unsigned char segment[UDPHEADER_SIZE+PSEUDOHEADER_SIZE+DATA_SIZE];
unsigned char recvUdpPacketData[packetLen],recvIcmpPacketData[packetLen];
int portNumber;
pthread_mutex_lock(&lockPort);
portNumber= myOpt.getPort(portIndex);
portIndex++;
pthread_mutex_unlock(&lockPort);
if ( portNumber == 0 )
{
delete[] packetData;
pthread_exit(NULL);
}
while( portNumber)
{
if ( protocolNumber != NO_PROTO)
cout<<"\nProtocol: "<<protocolNumber<<"\n";
setZeroMemory(packetData,packetLen);
setZeroMemory(segment,TCPHEADER_SIZE+PSEUDOHEADER_SIZE);
map<string,string> p;
//Declare socket TCP
if ( isIp4)
sockfd = socket(AF_INET, SOCK_RAW, IPPROTO_UDP);
else
sockfd = socket(AF_INET6, SOCK_DGRAM, 0);
if(sockfd < 0 )
{
delete[] packetData;
perror("sockfd create : ");
pthread_exit(NULL);
}
sockicmpfd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
if (sockicmpfd < 0 )
{
perror("socket ICMP connect Error()\n");
delete[] packetData;
pthread_exit(NULL);
}
//set UDP header
udpPkt.setUdpHeader(portNumber);
udphdrSend = udpPkt.getUdpHeader();
if ( isIp4 )
{
ipPkt.setIpHeader(myOpt.getSourceIp(),destIp,IPPROTO_UDP);
iphdrSend = ipPkt.getIpHeader();
//cout << "IP while sending pkt : = " << inet_ntoa(iphdrSend->ip_src) << " : \n";
createPseudoHeader(&pseudohdr ,iphdrSend);
memcpy(segment,(void *)pseudohdr,PSEUDOHEADER_SIZE);
memcpy(segment+PSEUDOHEADER_SIZE,(void *)udphdrSend,UDPHEADER_SIZE);
udphdrSend->uh_sum = (CheckSum(segment,UDPHEADER_SIZE+PSEUDOHEADER_SIZE));
//copy packet
memcpy(packetData,(void *)iphdrSend, IPHEADER);
memcpy(packetData+IPHEADER_SIZE,(void *)udphdrSend,UDPHEADER_SIZE);
free(pseudohdr);
}
else
{
ip6Pkt.setIp6Header(myOpt.getSourceIp(),destIp,IPPROTO_TCP);
ip6hdrSend = ip6Pkt.getIp6Header();
createPseudoHeader6(&pseudo6hdr ,ip6hdrSend);
memcpy(segment,(void *)pseudo6hdr,PSEUDO6HEADER_SIZE);
memcpy(segment+PSEUDO6HEADER_SIZE,(void *)udphdrSend,UDPHEADER_SIZE);
udphdrSend->uh_sum = (CheckSum(segment,UDPHEADER_SIZE+PSEUDO6HEADER_SIZE));
memcpy(packetData,(void *)ip6hdrSend, IPHEADER);
memcpy(packetData+IPHEADER_SIZE,(void *)udphdrSend,UDPHEADER_SIZE);
free(pseudo6hdr);
}
if ( isIp4)
{
clientsin.sin_family = AF_INET;
servTcpIpAddr = gethostbyname(destIp.c_str());
bcopy((char *)servTcpIpAddr->h_addr, (char *)&clientsin.sin_addr.s_addr, servTcpIpAddr->h_length);
clientsin.sin_port=htons(portNumber);
}
else
{
clientsin6.sin6_family = AF_INET6;
clientsin6.sin6_flowinfo = 0;
servTcpIpAddr = gethostbyname2(destIp.c_str(),AF_INET6);
bind(sockfd, ( struct sockaddr *) &clientsin6,sizeof(clientsin6));
clientsin6.sin6_port=htons(portNumber);
}
int retransStatus= NO_RESPONSE,icmpStatus = NO_ICMP;
for(int countRe =0;countRe< NUMBER_RETRANSMISSIONS && (retransStatus == NO_RESPONSE) && ( icmpStatus == NO_ICMP);countRe++)
{
const int on = 1;
int setsockStatus;
if ( isIp4) {
setsockStatus = setsockopt(sockfd, IPPROTO_IP, IP_HDRINCL, &on, sizeof (on));
if (setsockStatus < 0 )
{
cerr<< "Cannot set HDRINCL port";
delete[] packetData;
pthread_exit(NULL);
}
}
fcntl(sockicmpfd, F_SETFL, O_NONBLOCK);
fcntl(sockfd, F_SETFL, O_NONBLOCK);
int sendStatus;
if ( isIp4)
sendStatus = sendto(sockfd, packetData, iphdrSend->ip_len, 0, (struct sockaddr *)&clientsin, sizeof(clientsin)) ;
else
{
sendStatus = sendto(sockfd, packetData, ip6hdrSend->ip6_len, 0, (struct sockaddr *)&clientsin6, sizeof(clientsin6)) ;
}
if (sendStatus == -1 && errno != EINTR )
{
//perror(" error in sendto: ");
delete[] packetData;
pthread_exit(NULL);
}
setZeroMemory(recvUdpPacketData,0);
setZeroMemory(recvIcmpPacketData,0);
socklen_t fromlen = sizeof (recvStorage);
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(sockfd, &fds);
FD_SET(sockicmpfd,&fds);
tv.tv_sec = TIMEOUT+1;
tv.tv_usec = 0;
int selectStatus = select(sockfd+1, &fds, NULL, NULL, &tv);
int icmpSelectStatus = select(sockicmpfd+1, &fds, NULL, NULL, &tv);
int numbytesIcmpRecvd = recvfrom(sockicmpfd,recvIcmpPacketData, packetLen , 0,(struct sockaddr *)&recvStorage, &fromlen);
if ( numbytesIcmpRecvd > 0 )
{
icmpStatus = parseIcmpHeader(recvIcmpPacketData,numbytesIcmpRecvd,(void *)iphdrSend,version);
}
int numbytesUdpRecvd = recvfrom(sockfd, recvUdpPacketData, packetLen , 0,(struct sockaddr *)&recvStorage, &fromlen);
//WHERE
//http://linux.die.net/man/2/recvfrom
if ( numbytesUdpRecvd > 0 )
{
retransStatus = parseUdpHeader(recvUdpPacketData,numbytesUdpRecvd,(void *)iphdrSend,udphdrSend,version);
}
} //retransmission loop ends , lets check
//According to nmap guide:
if ( icmpStatus == NO_ICMP)
{
p.insert(pair<string,string>("UDP","open|filtered"));
int previousPort;
previousPort = portNumber;
result.insert(pair<int,map<string,string> >(previousPort,p));
map<int,map<string,string> >::iterator outerIter;
map<string,string>::iterator innerIter;
outerIter = result.find(previousPort);
cout<<"\nPort:"<<previousPort<<"\n";
for(innerIter = outerIter->second.begin(); innerIter!= outerIter->second.end(); innerIter++)
{
cout<<innerIter->first<<"--->"<<innerIter->second<<"\n";
}
result.erase(previousPort);
}
else
{
if ( icmpStatus == ICMP_CLOSE )
{
//cout << "Closed\n";
p.insert(pair<string,string>("UDP","Closed"));
}
else if ( icmpStatus == ICMP_UNREACHABLE || icmpStatus == ICMP_PROTO_UNREACHABLE)
{
//cout << "open|filtered\n";
p.insert(pair<string,string>("UDP","open|filtered"));
}
else if ( retransStatus == RESPONSE ) // for this case it will not come as we send no payload
{
//cout << "Udp port is open\n";
p.insert(pair<string,string>("UDP","open"));
}
int previousPort;
previousPort = portNumber;
result.insert(pair<int,map<string,string> >(previousPort,p));
map<int,map<string,string> >::iterator outerIter;
map<string,string>::iterator innerIter;
outerIter = result.find(previousPort);
cout<<"\nPort:"<<previousPort<<"\n";
for(innerIter = outerIter->second.begin(); innerIter!= outerIter->second.end(); innerIter++)
{
cout<<innerIter->first<<"--->"<<innerIter->second<<"\n";
}
result.erase(previousPort);
}
close(sockfd);
close(sockicmpfd);
pthread_mutex_lock(&lockPort);
result.insert(pair<int,map<string,string> >(portNumber,p));
portNumber= myOpt.getPort(portIndex);
portIndex++;
pthread_mutex_unlock(&lockPort);
}
delete[] packetData;
}
//---------------------------------------------------------------------------
void GSA_Parse( char *head)
{
// $GPGSA,A,3,03,19,27,23,13,16,15,11,07,,,,1.63,0.95,1.32*03
char *start, result[20];
short sv_cnt=0, i;
if(CheckSum(head, strlen(head)))
{
//Fix SV
memset(&g_gpsInfo.fixSV, 0, sizeof(g_gpsInfo.fixSV));
//Valid
start = strstr( head, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
if((*result == 'A') || (*result == 'M'))
{
// Fix Type
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
g_gpsInfo.FixType = atoi(result)-1;
if(g_gpsInfo.FixType > 0) // Fix
{
if(g_gpsInfo.FixService == 0)
g_gpsInfo.FixService = 1; //Assume SPS FixSerivce
}
else
{
g_gpsInfo.FixType = 0; // NoFix
g_gpsInfo.FixService = 0; // NoFix
}
}
else
{
g_gpsInfo.FixType = 0; // NoFix
g_gpsInfo.FixService = 0; // NoFix
}
for(i=0 ; i<12 ; i++)
{
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
FetchField( start, result);
if(strlen(result)>0)
g_gpsInfo.fixSV[sv_cnt++] = atoi(result);
}
//PDOP
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
g_gpsInfo.PDOP = (float)(atof(result));
//HDOP
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
g_gpsInfo.HDOP = (float)(atof(result));
//VDOP
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
g_gpsInfo.VDOP = (float)(atof(result));
}
}
//---------------------------------------------------------------------------
void VTG_Parse( char *head)
{
//$GPVTG,159.16,T,,M,0.013,N,0.023,K,A*34
char *start, result[20];
char FixService;
// check checksum
if(CheckSum(head, strlen(head)))
{
// Track
start = strstr( head, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
g_gpsInfo.Track = (float)(atof(result));
}
// ignore
start = strstr( start, ","); // T
if(start != NULL)
start = start +1;
else
return;
start = strstr( start, ","); // NULL
if(start != NULL)
start = start +1;
else
return;
start = strstr( start, ","); // M
if(start != NULL)
start = start +1;
else
return;
// Speed
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
g_gpsInfo.Speed = (float)(atof(result) * Knot2Kmhr);
}
// ignore
start = strstr( start, ","); // N
if(start != NULL)
start = start +1;
else
return;
start = strstr( start, ","); // 0.023
if(start != NULL)
start = start +1;
else
return;
start = strstr( start, ","); // K
if(start != NULL)
start = start +1;
else
return;
// Fix Service
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
FixService = *result;
if(FixService != 'N')
{
if(g_gpsInfo.FixType == 0)
g_gpsInfo.FixType = 1; //Assume 2D
switch(FixService)
{
case 'A':
{
g_gpsInfo.FixService = 1;
break;
}
case 'D':
{
g_gpsInfo.FixService = 2;
break;
}
case 'E':
{
g_gpsInfo.FixService = 6;
break;
}
}
}
else // NoFix
{
g_gpsInfo.FixType = 0; // NoFix
g_gpsInfo.FixService = 0; // NoFix
}
}
}
//*****************************************************************************
//
//! SendPacket() sends a data packet.
//!
//! \param pui8Data is the location of the data to be sent to the device.
//! \param ui8Size is the number of bytes to send from puData.
//! \param bAck is a boolean that is true if an ACK/NAK packet should be
//! received in response to this packet.
//!
//! This function sends a packet of data to the device.
//!
//! \returns The function returns zero to indicated success while any non-zero
//! value indicates a failure.
//
//*****************************************************************************
int32_t
SendPacket(uint8_t *pui8Data, uint8_t ui8Size, bool bAck)
{
uint8_t ui8CheckSum;
uint8_t ui8Ack;
ui8CheckSum = CheckSum(pui8Data, ui8Size);
//
// Make sure that we add the bytes for the size and checksum to the total.
//
ui8Size += 2;
//
// Send the Size in bytes.
//
if(UARTSendData(&ui8Size, 1))
{
return(-1);
}
//
// Send the CheckSum
//
if(UARTSendData(&ui8CheckSum, 1))
{
return(-1);
}
//
// Now send the remaining bytes out.
//
ui8Size -= 2;
//
// Send the Data
//
if(UARTSendData(pui8Data, ui8Size))
{
return(-1);
}
//
// Return immediately if no ACK/NAK is expected.
//
if(!bAck)
{
return(0);
}
//
// Wait for the acknowledge from the device.
//
do
{
if(UARTReceiveData(&ui8Ack, 1))
{
return(-1);
}
}
while(ui8Ack == 0);
if(ui8Ack != COMMAND_ACK)
{
return(-1);
}
return(0);
}
bool Header::Check() const
{
return CheckSum();
}
/*******************************************************
Main exe loop
*******************************************************/
Command::Dpt_Error RMWFlash::execute(String_List **output)
{
ENTER("Command::Dpt_Error RMWFlash::execute(String_List **output)");
Dpt_Error err;
bool more_Devs_Left = true;
DPT_TAG_T hbaTag;
Init_Engine(1);
*output = new String_List();
for (int dev_Index = 0; more_Devs_Left; dev_Index++)
{
hbaTag = Get_HBA_by_Index(dev_Index, &more_Devs_Left);
if (more_Devs_Left == 0)
{
break;
}
if (( hbaNum == -1 ) || ( hbaNum == dev_Index ))
{
unsigned long region_size = 8192;
switch (region) {
case 0: region_size = 2 * 1024L * 1024L; break;
case 1: region_size = 65536L; break;
case 2: region_size = 512L * 1024L; break;
case 3: region_size = 8192; break;
case 4: region_size = 8192; break;
}
char * original = new char[ region_size ];
if (original != NULL) {
// Send the command to the engine to set the region of flash memory to use.
engine->Reset();
engine->Insert((uLONG) region);
err = engine->Send(MSG_FLASH_SET_REGION, hbaTag);
if (err.Success())
{
// Send the command to the engine to read the flash memory.
engine->Reset();
engine->Insert((uLONG) 0);
engine->Insert((uLONG) region_size);
err = engine->Send(MSG_FLASH_READ, hbaTag);
// If the read succeeded then copy the data that was read from the flash
// memory into the caller's buffer.
if (err.Success())
{
engine->Extract(original, region_size);
# if (defined(DEBUG_RMWFlash))
{
int fd = creat ("before", 0777);
write (fd, original, (unsigned)region_size);
close (fd);
}
# endif
if (strcmp (data, "-") == 0)
{
char temp_Buf[ 256 ];
sprintf(temp_Buf, "%.*s\r\n", size, original + offset);
(**output).add_Item (temp_Buf);
continue;
}
engine->Reset();
engine->Insert((uLONG) region);
err = engine->Send(MSG_FLASH_SET_REGION, hbaTag);
if (err.Success())
{
/* Ideally we want the caller to specify the checksum handling, but for now ... */
switch (region) {
case 4:
if ((0x14 < offset) && (offset < 0x7F))
{
break;
}
*((unsigned long *)(&original[0x10])) += CheckSum (data, size) - CheckSum (original + offset, size);
case 0:
case 1:
case 2:
case 3:
break;
}
memcpy (original + offset, data, size);
# if (defined(DEBUG_RMWFlash))
{
int fd = creat ("after", 0777);
write (fd, original, (unsigned)region_size);
close (fd);
}
# endif
// Send the command to the engine to write the flash memory.
engine->Reset();
engine->Insert((void *)original, region_size);
err = engine->Send(MSG_FLASH_WR_NO_VERIFY, hbaTag);
if (err.Success())
{
// Send the command to the engine to stop writing flash memory.
engine->Reset();
err = engine->Send(MSG_FLASH_WRITE_DONE, hbaTag);
if (err.Success())
{
(**output).add_Item(EventStrings[STR_FLASH_COMPLETE]);
}
}
}
}
}
delete [] original;
if (err.Failure())
{
extern void Flush( String_List * );
char temp_Buf[ 256 ];
sprintf(temp_Buf,
EventStrings[STR_FLASH_ERR_MSG],
0L, (int)err, (char *)err);
(**output).add_Item (temp_Buf);
Flush (*output);
// Send the command to the engine to stop writing flash memory.
engine->Reset();
engine->Insert((uCHAR) 1);
err = engine->Send(MSG_FLASH_WRITE_DONE, hbaTag);
break;
}
} else {
err = Dpt_Error::DPT_ERR_NOT_ENOUGH_MEMORY;
break;
}
}
}
(**output).add_Item("\n");
return (err);
}
void GetState(u8 *buf)
{
u16 tmp16;
buf[0] = 0xAA;
buf[1] = 0x30;
buf[2] = 28;
tmp16 = (short)(angle.y*100);
buf[3] = BYTE1(tmp16);
buf[4] = BYTE0(tmp16);
tmp16 = (short)(angle.x*100);
buf[5] = BYTE1(tmp16);
buf[6] = BYTE0(tmp16);
tmp16 = (short)(angle.z*100);
buf[7] = BYTE1(tmp16);
buf[8] = BYTE0(tmp16);
tmp16 = (short)(mpu_info.gyro.x*Gyro2Degree);
buf[9] = BYTE1(tmp16);
buf[10] = BYTE0(tmp16);
tmp16 = (short)(mpu_info.gyro.y*Gyro2Degree);
buf[11] = BYTE1(tmp16);
buf[12] = BYTE0(tmp16);
tmp16 = (short)(mpu_info.gyro.z*Gyro2Degree);
buf[13] = BYTE1(tmp16);
buf[14] = BYTE0(tmp16);
tmp16 = (short)(rollRateDesired*10);
buf[15] = BYTE1(tmp16);
buf[16] = BYTE0(tmp16);
tmp16 = (short)(pitchRateDesired*10);
buf[17] = BYTE1(tmp16);
buf[18] = BYTE0(tmp16);
tmp16 = (short)(yawRateDesired*10);
buf[19] = BYTE1(tmp16);
buf[20] = BYTE0(tmp16);
tmp16 = rollOutput;
buf[21] = BYTE1(tmp16);
buf[22] = BYTE0(tmp16);
tmp16 = pitchOutput;
buf[23] = BYTE1(tmp16);
buf[24] = BYTE0(tmp16);
tmp16 = yawOutput;
buf[25] = BYTE1(tmp16);
buf[26] = BYTE0(tmp16);
tmp16 = Motor_Thr;
buf[27] = BYTE1(tmp16);
buf[28] = BYTE0(tmp16);
/*
buf[21] = MOTOR1/5;
buf[22] = MOTOR2/5;
buf[23] = MOTOR3/5;
buf[24] = MOTOR4/5;
buf[25] = BYTE3(freqMeas);
buf[26] = BYTE2(freqMeas);
buf[27] = BYTE1(freqMeas);
buf[28] = BYTE0(freqMeas);
*/
tmp16 = GetADCValue();
tmp16 = (tmp16*5*110/4096 +0.5);
buf[30] = tmp16-250;
buf[31] = CheckSum(buf,buf[2]+3);
}
void Pro_W2D_ErrorCmdHandle(Error_PacketsTypeDef Error_Type, uint8_t flag)
{
Pro_ErrorCmdTypeDef Pro_ErrorCmdStruct; //4.7 ??????
memcpy(&Pro_ErrorCmdStruct, UART_HandleStruct.Message_Buf, sizeof(Pro_ErrorCmdStruct));
if(flag == 1)
{
goto Print_O;
}
Pro_ErrorCmdStruct.Pro_HeadPart.Head[0] = 0xFF;
Pro_ErrorCmdStruct.Pro_HeadPart.Head[1] = 0xFF;
Pro_ErrorCmdStruct.Pro_HeadPart.Len = exchangeBytes(sizeof(Pro_ErrorCmdStruct) - 4);
Pro_ErrorCmdStruct.Pro_HeadPart.Cmd = Pro_D2W_ErrorPackage_Ack_Cmd;
Pro_ErrorCmdStruct.Error_Packets = Error_Type;
Pro_ErrorCmdStruct.Sum = CheckSum((uint8_t *)&Pro_ErrorCmdStruct, sizeof(Pro_ErrorCmdStruct));
Pro_UART_SendBuf((uint8_t *)&Pro_ErrorCmdStruct, sizeof(Pro_ErrorCmdStruct), 0);
#if(DEBUG==1)
Serial.print(F("Error :")); Serial.println(Error_Type, HEX);
Serial.print("[");Serial.print(SystemTimeCount,DEC);Serial.print("]");Serial.print(" MCU 4 :");
for(uint8_t i = 0; i < (sizeof(Pro_ErrorCmdStruct)); i++)
{
Serial.print(" "); Serial.print(*((uint8_t *)&Pro_ErrorCmdStruct + i),HEX);
}
Serial.println("");
#endif
return;
Print_O:
/*************************错误类型*****************************/
switch (Pro_ErrorCmdStruct.Error_Packets)
{
case Error_AckSum:
#if(DEBUG==1)
Serial.println(F("ACK : Error_AckSum OK"));
#endif
break;
case Error_Cmd:
#if(DEBUG==1)
Serial.println(F("ACK : Error_Cmd OK"));
#endif
break;
case Error_Other:
#if(DEBUG==1)
Serial.println(F("ACK : Error_Other OK"));
#endif
break;
default:
#if(DEBUG==1)
Serial.println(F("ACK : Error! "));
#endif
break;
}
}
int check_from_socket_data(unsigned char *buff , unsigned int length)
{
if((buff[0] == 0xA0) && (buff[length - 1] == CheckSum(buff,length - 1)))
return 0;
else return -1;
}
int SaveDataProc(int addr, double data)
{
int cmd,i,return_value;
char SciBuf[30]={0};
char str[30];
UNION32 u32data,u32data2;
return_value = 0;
cmd = CMD_READ_DATA;
i = get_code_information( addr,cmd,&code_inform);
if( i != 0 ) goto _SAVE_ERROR_INVALID_ADDR;
if( addr >= 800 ){
if( data < 1.22 ) goto _SAVE_ERROR_INVALID_DATA;
if( data > 1.24 ) goto _SAVE_ERROR_INVALID_DATA;
switch(addr)
{
case CODE_Data_Check: return_value = check_backup_data(); break;
case CODE_Data_Backup:
strncpy(str,"Wait for Data Backup",20);
load_sci_tx_mail_box(str); delay_msecs(20);
save_backup_data();
strncpy(str,"Wait for Error Check",20);
load_sci_tx_mail_box(str); delay_msecs(20);
break;
case CODE_Data_Load:
strncpy(str,"Wait for Data Load !",20);
load_sci_tx_mail_box(str); delay_msecs(20);
backup_data_load();
break;
case CODE_Data_Init:
strncpy(str,"Wait For Data Init !",20);
load_sci_tx_mail_box(str); delay_msecs(20);
init_eprom_data();
strncpy(str,"Data Init OK ! ",20);
load_sci_tx_mail_box(str); delay_msecs(20);
break;
case CODE_get_adc_offset: get_adc_offset(); break;
default:
goto _SAVE_ERROR_INVALID_ADDR;
}
return return_value;
}
if( code_inform.type == TYPE_INTEGER){
if( (code_inform.code_min.ints) > (int)data ){
goto _SAVE_ERROR_DATA_UNDER;
}
else if( (code_inform.code_max.ints) < (int)data){
goto _SAVE_ERROR_DATA_OVER;
}
else {
// u32data.word.word1 = 0;
u32data.word.word0 = (int)data;
read_eprom_data( addr, & u32data2);
if( u32data.word.word0 != u32data2.word.word0 ){
write_code_2_eeprom( addr, u32data);
read_eprom_data( addr, & u32data2);
if( u32data.word.word0 != u32data2.word.word0 ) goto _EEPROM_WRITE_ERROR;
code_inform.code_value.ints = (int)data;
cmd = CMD_WRITE_RAM;
get_code_information( addr,cmd, & code_inform);
load_sci_tx_mail_box("OK write success") ;
}
else{
load_sci_tx_mail_box("Equal Data write skipped");
}
return 0;
}
}
else { // code_inform->Type == TYPE_DOUBLE
if( ( code_inform.code_min.doubles) > data ){
goto _SAVE_ERROR_DATA_UNDER;
}
else if( ( code_inform.code_max.doubles) < data ){
goto _SAVE_ERROR_DATA_OVER;
}
else {
u32data.dword = data;
read_eprom_data( addr, & u32data2);
if( u32data.dword != u32data2.dword ){
write_code_2_eeprom( addr, u32data);
read_eprom_data( addr, & u32data2);
if( u32data.dword != u32data2.dword ) goto _EEPROM_WRITE_ERROR;
code_inform.code_value.doubles = data;
cmd = CMD_WRITE_RAM;
get_code_information( addr,cmd, & code_inform);
// CheckSum Save
u32data.dword = CheckSum();
EepromSaveFlag = 1;
write_code_2_eeprom(EPROM_ADDR_CHECKSUM, u32data);
EepromSaveFlag = 0;
load_sci_tx_mail_box("OK write success") ;
}
else{
load_sci_tx_mail_box("Equal Data write skipped");
}
return 0;
}
}
_SAVE_ERROR_INVALID_ADDR:
// strcpy(SciBuf, "ADDR");
strcpy(SciBuf,"Invalid Address" );
load_sci_tx_mail_box(SciBuf) ;
return -1;
_SAVE_ERROR_DATA_UNDER:
// strcpy(SciBuf, "UNDE");
strcpy(SciBuf,"Input data under" );
load_sci_tx_mail_box(SciBuf) ;
return -1;
_SAVE_ERROR_DATA_OVER:
// strcpy(SciBuf, "OVER");
strcpy(SciBuf,"Input data over" );
load_sci_tx_mail_box(SciBuf) ;
return -1;
_SAVE_ERROR_INVALID_DATA:
// strcpy(SciBuf, "DATA");
strcpy(SciBuf,"Invalid data " );
load_sci_tx_mail_box(SciBuf) ;
return -1;
_EEPROM_WRITE_ERROR:
// strcpy(SciBuf, "DATA");
strcpy(SciBuf,"Eeprom write error" );
load_sci_tx_mail_box(SciBuf) ;
return -1;
}
TEST_F(Platform, CheckSum) {
for (int i = 0; i < kTrialCount; i++)
EXPECT_EQ(checkSum_, CheckSum());
}
/* ReadSecMSG v0.2: 2005/02/10 */
char *ReadSecMSG(keystore *keys, char *buffer, char *cleartext,
int id, unsigned int buffer_size)
{
unsigned int msg_global = 0;
unsigned int msg_local = 0;
char *f_msg;
if(*buffer == ':')
{
buffer++;
}
else
{
merror(ENCFORMAT_ERROR, __local_name, keys->keyentries[id]->ip->ip);
return(NULL);
}
/* Decrypting message */
if(!OS_BF_Str(buffer, cleartext, keys->keyentries[id]->key,
buffer_size, OS_DECRYPT))
{
merror(ENCKEY_ERROR, __local_name, keys->keyentries[id]->ip->ip);
return(NULL);
}
/* Compressed */
else if(cleartext[0] == '!')
{
cleartext[buffer_size] = '\0';
cleartext++;
buffer_size--;
/* Removing padding */
while(*cleartext == '!')
{
cleartext++;
buffer_size--;
}
/* Uncompressing */
if(!os_zlib_uncompress(cleartext, buffer, buffer_size, OS_MAXSTR))
{
merror(UNCOMPRESS_ERR, __local_name);
return(NULL);
}
/* Checking checksum */
f_msg = CheckSum(buffer);
if(f_msg == NULL)
{
merror(ENCSUM_ERROR, __local_name, keys->keyentries[id]->ip->ip);
return(NULL);
}
/* Removing random */
f_msg+=5;
/* Checking count -- protecting against replay attacks */
msg_global = (unsigned int) atoi(f_msg);
f_msg+=10;
/* Checking for the right message format */
if(*f_msg != ':')
{
merror(ENCFORMAT_ERROR, __local_name,keys->keyentries[id]->ip->ip);
return(NULL);
}
f_msg++;
msg_local = (unsigned int) atoi(f_msg);
f_msg+=5;
/* Returning the message if we don't need to verify the counbter. */
if(!_s_verify_counter)
{
/* Updating currently counts */
keys->keyentries[id]->global = msg_global;
keys->keyentries[id]->local = msg_local;
if(rcv_count >= _s_recv_flush)
{
StoreCounter(keys, id, msg_global, msg_local);
rcv_count = 0;
}
rcv_count++;
return(f_msg);
}
if((msg_global > keys->keyentries[id]->global)||
((msg_global == keys->keyentries[id]->global) &&
(msg_local > keys->keyentries[id]->local)))
{
/* Updating currently counts */
keys->keyentries[id]->global = msg_global;
keys->keyentries[id]->local = msg_local;
if(rcv_count >= _s_recv_flush)
{
StoreCounter(keys, id, msg_global, msg_local);
rcv_count = 0;
}
rcv_count++;
return(f_msg);
}
/* Checking if it is a duplicated message */
if(msg_global == keys->keyentries[id]->global)
{
return(NULL);
}
/* Warn about duplicated messages */
merror("%s: WARN: Duplicate error: global: %u, local: %u, "
"saved global: %u, saved local:%u",
__local_name,
msg_global,
msg_local,
keys->keyentries[id]->global,
keys->keyentries[id]->local);
merror(ENCTIME_ERROR, __local_name, keys->keyentries[id]->name);
return(NULL);
}
/* Old format */
else if(cleartext[0] == ':')
{
unsigned int msg_count;
unsigned int msg_time;
/* Closing string */
cleartext[buffer_size] = '\0';
/* Checking checksum */
cleartext++;
f_msg = CheckSum(cleartext);
if(f_msg == NULL)
{
merror(ENCSUM_ERROR, __local_name, keys->keyentries[id]->ip->ip);
return(NULL);
}
/* Checking time -- protecting against replay attacks */
msg_time = (unsigned int) atoi(f_msg);
f_msg+=11;
msg_count = (unsigned int) atoi(f_msg);
f_msg+=5;
/* Returning the message if we don't need to verify the counbter. */
if(!_s_verify_counter)
{
/* Updating currently counts */
keys->keyentries[id]->global = msg_time;
keys->keyentries[id]->local = msg_local;
f_msg = strchr(f_msg, ':');
if(f_msg)
{
f_msg++;
return(f_msg);
}
else
{
merror(ENCFORMAT_ERROR, __local_name,keys->keyentries[id]->ip->ip);
return (NULL);
}
}
if((msg_time > keys->keyentries[id]->global) ||
((msg_time == keys->keyentries[id]->global)&&
(msg_count > keys->keyentries[id]->local)))
{
/* Updating currently time and count */
keys->keyentries[id]->global = msg_time;
keys->keyentries[id]->local = msg_count;
f_msg = strchr(f_msg, ':');
if(f_msg)
{
f_msg++;
return(f_msg);
}
else
{
merror(ENCFORMAT_ERROR, __local_name,keys->keyentries[id]->ip->ip);
return (NULL);
}
}
/* Checking if it is a duplicated message */
if((msg_count == keys->keyentries[id]->local) &&
(msg_time == keys->keyentries[id]->global))
{
return(NULL);
}
/* Warn about duplicated message */
merror("%s: WARN: Duplicate error: msg_count: %u, time: %u, "
"saved count: %u, saved_time:%u",
__local_name,
msg_count,
msg_time,
keys->keyentries[id]->local,
keys->keyentries[id]->global);
merror(ENCTIME_ERROR, __local_name, keys->keyentries[id]->name);
return(NULL);
}
merror(ENCFORMAT_ERROR, __local_name, keys->keyentries[id]->ip->ip);
return(NULL);
}
//*****************************************************************************
//
//! Receives a data packet.
//!
//! \param pui8Data is the location to store the data that is sent to the boot
//! loader.
//! \param pui32Size is the number of bytes returned in the pui8Data buffer
//! that was provided.
//!
//! This function receives a packet of data from specified transfer function.
//!
//! \return Returns zero to indicate success while any non-zero value indicates
//! a failure.
//
//*****************************************************************************
int
ReceivePacket(uint8_t *pui8Data, uint32_t *pui32Size)
{
uint32_t ui32Size, ui32CheckSum;
//
// Wait for non-zero data before getting the first byte that holds the
// size of the packet we are receiving.
//
ui32Size = 0;
while(ui32Size == 0)
{
ReceiveData((uint8_t *)&ui32Size, 1);
}
//
// Subtract off the size and checksum bytes.
//
ui32Size -= 2;
//
// Receive the checksum followed by the actual data.
//
ReceiveData((uint8_t *)&ui32CheckSum, 1);
//
// If there is room in the buffer then receive the requested data.
//
if(*pui32Size >= ui32Size)
{
//
// Receive the actual data in the packet.
//
ReceiveData(pui8Data, ui32Size);
//
// Send a no acknowledge if the checksum does not match, otherwise send
// an acknowledge to the packet later.
//
if(CheckSum(pui8Data, ui32Size) != (ui32CheckSum & 0xff))
{
//
// Indicate tha the packet was not received correctly.
//
NakPacket();
//
// Packet was not received, there is no valid data in the buffer.
//
return(-1);
}
}
else
{
//
// If the caller allocated a buffer that was too small for the received
// data packet, receive it but don't fill the buffer.
// Then inform the caller that the packet was not received correctly.
//
while(ui32Size--)
{
ReceiveData(pui8Data, 1);
}
//
// Packet was not received, there is no valid data in the buffer.
//
return(-1);
}
//
// Make sure to return the number of bytes received.
//
*pui32Size = ui32Size;
//
// Packet was received successfully.
//
return(0);
}
void Data_UNPackage(U8* data,U16* data_length)
{
U8 i=0;
U8 j=0;
U8 k;
U8 Length=0;
while(1)
{
if((Rx_Data_Count)<5) //数据量不足，退出
break;
/*寻找帧头*/
for(i=0;i<(Rx_Data_Count)-1;i++)
{
if((Rx_Data[i]==0xA5)&&(Rx_Data[i+1]==0x5A))
{
break;
}
}
/*去除废数据*/
Rx_Data_Count=(Rx_Data_Count)-i;
for(j=0;j<(Rx_Data_Count);j++)
{
Rx_Data[j]=Rx_Data[i+j];
}
/*获取包数据长度（总长度）*/
if((Rx_Data_Count)>2)
{
Length= Rx_Data[2];
}
Length=Length-3; //匹配
/*解包*/
if((Rx_Data_Count)>=(Length+3))
{
if(Rx_Data[3+Length]==CheckSum(Rx_Data,2+Length))
{
/*解包数据进队列*/
for(k=0;k<Length-1;k++)
{
data[k]=(Rx_Data[3+k]);
}
*data_length=Length-1;
}
/*丢包*/
for(i=0;i<((Rx_Data_Count)-1);i++)
{
Rx_Data[i]=Rx_Data[i+1];
}
}
}
}
DWORD WINAPI TcpSynScan(LPVOID pPar)
{
ThreadSyn cSynEntry;
SetBoolTrue cSBT(&bSynSending);
DWORD dwThreadId;
CreateThread(NULL,0,TcpReceiver,pPar,0,&dwThreadId);
pcap_if_t *pSelectDev;
ThreadParament * pThreadParament=(ThreadParament *)pPar;
pSelectDev=pThreadParament->SelectDev;
pcap_t *fp;
char error[256];
char bDestMac[6];
memset(bDestMac,0xff,6);
TcpPacket thePacket;
DWORD dwNetMask=pThreadParament->dwNetMAsk;
DWORD dwDefaultGatewayIp=pThreadParament->dwDefaultGateway;
ULONG bGatewayMac[2];
ULONG ulLen=6;
if(SendARP(htonl(dwDefaultGatewayIp),0,bGatewayMac,&ulLen)!=NO_ERROR)
{
return 0;
}
memcpy(thePacket.theIpPacket.theEthHead.bDestMac,bGatewayMac,6);
memcpy(thePacket.theIpPacket.theEthHead.bSourceMac,pThreadParament->HostMac,6);
thePacket.theIpPacket.theEthHead.usEthernetType=0x8;
thePacket.theIpPacket.theIpHead.ucVersionAndHeadLength=0x45;
thePacket.theIpPacket.theIpHead.ucTos=0;
thePacket.theIpPacket.theIpHead.usTotalLength=htons(48);;
thePacket.theIpPacket.theIpHead.usIdentification=1234;
thePacket.theIpPacket.theIpHead.usFlagsAndFragmentOffset=0;
thePacket.theIpPacket.theIpHead.ucTtl=119;
thePacket.theIpPacket.theIpHead.ucProtocol=6;//tcp
thePacket.theIpPacket.theIpHead.dwSourceAddr=htonl(pThreadParament->dwHostIP);
thePacket.theTcpHead.usSourcePort=htons(12345);
thePacket.theTcpHead.dwSeq=ntohl(198327);
thePacket.theTcpHead.dwAck=0;
thePacket.theTcpHead.ucLength=0x70;
thePacket.theTcpHead.ucFlag=2;
thePacket.theTcpHead.usWindow=0xFFFF; //16 位窗口大小
thePacket.theTcpHead.usCrc=0;//16 位校验和
thePacket.theTcpHead.usUrgent=0;//16 位紧急数据偏移量
thePacket.theTcpHead.unMssOpt=htonl(0x020405B4);
thePacket.theTcpHead.usNopOpt= 0x0101;
thePacket.theTcpHead.usSackOpt= 0x0204;
TcpFakeHeader theFakeHeader;
theFakeHeader.bZero=0;
theFakeHeader.bTcpLength=htons(28);
theFakeHeader.bProtocolType=6;
theFakeHeader.dwSourceAddr=htonl(pThreadParament->dwHostIP);
if((fp = pcap_open_live(pSelectDev->name, 65536, 1, 1000, error) ) == NULL)
{
return 0;
}
for(DWORD dwIP=pThreadParament->dwOriginalIP;dwIP<=pThreadParament->dwLastIP&&!bStop;dwIP++)
{
char strLog[256];
in_addr tmp;
tmp.S_un.S_addr=htonl(dwIP);
sprintf(strLog,"Scaning Host %s.",inet_ntoa(tmp));
pMainWindow->SendMessage(WM_UPDATA_LOG,(WPARAM)strLog);
if((dwIP&dwNetMask)==(dwDefaultGatewayIp&dwNetMask))
{
ulLen=6;
ULONG bHostMac[2];
if(SendARP(htonl(dwIP),0,bHostMac,&ulLen)!=NO_ERROR)
{
continue;
}
memcpy(thePacket.theIpPacket.theEthHead.bDestMac,bHostMac,6);
}
thePacket.theIpPacket.theIpHead.dwDestAddr=htonl(dwIP);
for(DWORD dwPort=pThreadParament->dwOriginalPort;dwPort<=pThreadParament->dwLastPort&&!bStop;dwPort++)
{
thePacket.theTcpHead.usDestPort=htons((WORD)dwPort);
thePacket.theIpPacket.theIpHead.usCrc=0;
thePacket.theIpPacket.theIpHead.usCrc=CheckSum((const char *)(&(thePacket.theIpPacket.theIpHead)),sizeof(IpHead));
thePacket.theTcpHead.usCrc=0;
theFakeHeader.dwDestAddr=htonl(dwIP);
thePacket.theTcpHead.usCrc=TcpCheckSum((char *)&(thePacket.theTcpHead),(char *)&(theFakeHeader),28);
if(pcap_sendpacket(fp,(u_char *)(&thePacket),sizeof(TcpPacket))!=0)
{
pMainWindow->MessageBox("Send Data Error!");
}
while(bPause)
{
Sleep(100);
}
}
}
return 0;
}
char *WriteSeq( char *outSpec, /* Output sequence spec */
SeqEntry *seq, /* Seq Entry data structure */
int format) /* Output format */
{
#define SIZE 255 /* String size */
char line[SIZE], header[SIZE], code[SIZE], testCode[SIZE];
char outFName[SIZE];
char *cPos, *pStrand;
int lineSize, blockSize;
int count, nOut;
Boolean twoFiles;
Boolean doText;
char errMsg[SIZE];
static char outFileName[SIZE]; /* Static because it's the return value */
FILE *outFile, *inFile;
/*
** Process available text for formats which allow freely formated text.
*/
switch ( format ) {
case PIR:
case GCG:
doText = (seq->text != NULL); break;
default:
doText = 0; break;
}
/*
** Set LineSize and BlockSize for variou formats
*/
switch ( seq->spec->format ) {
case GCG:
lineSize = 50; blockSize= 10; break;
case PIR:
case IBI:
lineSize = 60; blockSize = 10; break;
case IG:
case STRIDER:
default:
lineSize = blockSize= 80;
}
/*
** Extract the the code from OutSpec for multi-entry files. If OutSpec just
** has a filename then strip off the extension and use the filename as the code.
** Filenames are to the right of the equals.
** N.B. does not yet support pathnames.
**
** Outspec will look like either:
**
** code=filename.ext OR filename.ext
*/
if ( (cPos = strchr(outSpec,';')) ) *cPos = '\0'; /* Remove version */
strcpy(code,outSpec);
if ( (cPos = strchr(code,'=')) ) *cPos = '\0'; /* code=filename.ext */
if ( (cPos = strchr(code,'.')) ) *cPos = '\0'; /* filename.ext */
StrToUpper(code);
strcpy(outFName,outSpec);
if ( (cPos = strchr(outFName,'=')) ) strcpy(outFName, ++cPos);
/*
** Are we dealing with a new or old sequence file? Test for an existing
** file set "twoFiles" flag accordingly.
*/
if ( (inFile = fopen(outFName, "r")) ) {
if ( !(outFile = fopen(outFName, "w")) ) goto Error;
twoFiles = 1;
} else {
if ( !(outFile = fopen(outFName, "w")) ) goto Error;
twoFiles = 0;
}
/*
** Do what needs to be done to prepare the file for writing or over-writing
** the new sequence data.
**
** PIR - create header, read into filename until we find the entry to
** replace or reach EOF. Save existing text if there is no new text.
**
** IG - Save any lines beginning with ";" these are comment characters.
** Write out codeword. Generate circular/linear flag.
**
** STADEN - Translate symbols from IUPAC to STADEN, append end of sequence
** character, "@".
**
** GCG - Save all text at the top of the file up to but not including the
** line with ".." in it, unless we have new text. Convert "-"'s to
** "."'s. Recalculate CheckSum. Write a new GCG descriptor line.
**
** IBI - Rewrite the LOCUS line and ORIGIN line.
**
** STRIDER- Write out strider header line and sequnce title.
**
** RAWSEQ - Do nothing.
*/
switch ( format ) {
case PIR:
strcpy(header, ">");
switch (seq->type ) {
case PROTEIN: strcat(header,"P1;"); break;
case FRAGMENT: strcat(header,"F1;"); break;
case DNA:
seq->circular ? strcat(header,"DC;"):strcat(header,"DL;") ; break;
case RNA:
seq->circular ? strcat(header,"RC;"):strcat(header,"RL;") ; break;
case RRNA: strcat(header,"N1;"); break;
case TRNA: strcat(header,"N3;"); break;
default: strcat(header,"XX;"); break;
}
strcat(header, code);
/*
** Read in/out until the beginning of entry to be overwritten
** or End of File.
*/
if ( twoFiles ) {
testCode[0] = '\0';
while ( fgets(line, 255, inFile) ) {
if( line[0] == '>') {
strcpy(testCode, &line[4]);
if ( (cPos = strchr(testCode, '\n')) ) *cPos = '\0';
StrToUpper(StrCollapse(testCode));
if( strcmp(testCode,code) == 0 ) break;
}
fputs(line,outFile);
}
}
/*
** Write out the header and title lines.
*/
fprintf(outFile,"%s\n", header);
fprintf(outFile,"%s\n", seq->title);
break;
/****************************************************************/
case IG:
/*
** For IG format save any lines beginning with ";" these
** are comment characters
*/
if ( twoFiles && !doText ) {
while ( fgets(line, 255, inFile) ) {
if ( line[0] != ';' ) break;
fputs(line,outFile);
}
} else if ( doText ) {
if ( seq->text ) fputs(seq->text,outFile);
} else
fprintf(outFile, "; %s\n; %s\n", seq->title,seq->desc);
fprintf(outFile, "%s\n", code);
break;
/****************************************************************/
case STADEN:
if (seq->type >= DNA ) {
StrChange(seq->mem,'M','5');
StrChange(seq->mem,'K','6');
StrChange(seq->mem,'W','7');
StrChange(seq->mem,'S','8');
StrChange(seq->mem,'m','5');
StrChange(seq->mem,'k','6');
StrChange(seq->mem,'w','7');
StrChange(seq->mem,'s','8');
}
break;
/****************************************************************/
case GCG:
if ( twoFiles && !doText ) {
while ( fgets(line, 255, inFile) ) {
if ( StrIndex("..",line) ) break;
fputs(line,outFile);
}
} else if ( doText ) {
if ( seq->text ) fputs(seq->text,outFile);
} else
fprintf(outFile, " %s\n %s\n\n", seq->title,seq->desc);
StrChange(seq->mem,'-','.');
seq->checkSum = CheckSum(seq->mem);
fprintf(outFile, " %s Length: %ld %s Check: %d ..\n",
outFName, seq->length, GetTime(3), seq->checkSum);
fprintf(outFile," 1 ");
break;
/****************************************************************/
case IBI:
fprintf(outFile,
"LOCUS %s %ld BP UPDATED %s\n",
code, seq->length, GetTime(0));
if ( twoFiles && !doText ) {
while ( fgets(line, 255, inFile) ) {
if ( StrIndex("LOCUS",line) ) continue;
if ( StrIndex("ORIGIN",line) ) break;
fputs(line,outFile);
}
} else if ( doText && seq->text ) {
if ( (cPos = StrIndex("LOCUS ",seq->text)) )
strcpy(seq->text,strchr(cPos,'\n'));
if ( (cPos = StrIndex("\nORIGIN",seq->text)) )
*cPos = '\0';
fputs(seq->text,outFile);
}
fprintf(outFile, "ORIGIN %s\n", seq->title);
fprintf(outFile," 1 ");
break;
/****************************************************************/
case STRIDER:
fprintf(outFile,"; ### from DNA Strider %s\n",GetTime(3));
fprintf(outFile,"; %s sequence %s length %ld",
DecodeType(seq->type),seq->name,seq->length);
if ( seq->type <= PROTEIN )
fprintf(outFile," a.a. complete sequence\n; %s\n",seq->title);
else
fprintf(outFile," n.a. complete sequence\n; %s\n",seq->title);
break;
/****************************************************************/
case RAWSEQ:
default:
break;
}
/*
** Write the sequence in Block/Line size. "NOut" counts the number
** of symbols written in each line. Support GCG and IBI line numbering
** schemes.
*/
nOut = 0;
count = 1;
pStrand = seq->mem;
while( *pStrand ) {
fprintf(outFile, "%c", *pStrand);
nOut++;
count++;
if ( nOut == lineSize ) {
switch ( format ) {
case IBI:
case GCG:
fprintf(outFile,"\n%9d ",count);
break;
default:
fprintf(outFile, "\n");
}
nOut=0;
} else
if ( nOut%blockSize == 0 ) fprintf(outFile, " ");
pStrand++;
}
/*
** Depending on the format we have some finishing up to do.
**
** PIR - Add the End of sequence character. Read past the old sequence
** form the original file. If "DoText" is true, write out the new
** text comments, skip over the comments in the old file, if any.
** Finally, copy the balance of the old file into the new file.
**
** IBI - Add sequence terminator "//"
**
** IG - Append linear/circular flag at end of sequence.
**
** STRIDER - Add sequence terminator "//"
**
** STADEN - Add end of sequence character. Convert sequence symbols back
** to IUPAC.
**
** GCG - Convert "."'s back to "-"'s. Recalc CheckSum.
*/
switch ( format ) {
case PIR:
fprintf(outFile,"*\n");
/* Skip over sequence in the old file */
if ( twoFiles )
while ( fgets(line, 255, inFile) )
if( strchr(line,'*' ) ) break;
/* Skip over old comments, if any. */
if ( doText ) {
if ( seq->text ) fputs(seq->text,outFile);
if ( twoFiles )
while ( fgets(line, 255, inFile) )
if( *line == '>' ) {
fputs(line,outFile);
break;
}
}
/* Copy the remainder of the file */
if ( twoFiles ) {
while ( fgets(line, 255, inFile) )
fputs(line, outFile);
}
break;
/****************************************************************/
case IBI:
fprintf(outFile,"\n//");
break;
/****************************************************************/
case IG:
fprintf(outFile,"%c",seq->circular ? '2' : '1');
break;
/****************************************************************/
case STRIDER:
fprintf(outFile,"\n//");
break;
/****************************************************************/
case STADEN:
fprintf(outFile,"@");
if (seq->type >= DNA ) {
StrChange(seq->mem,'5','M');
StrChange(seq->mem,'6','K');
StrChange(seq->mem,'7','W');
StrChange(seq->mem,'8','S');
}
break;
/****************************************************************/
case GCG:
StrChange(seq->mem,'.','-');
seq->checkSum = CheckSum(seq->mem);
break;
}
/* fgetname(outFile, outFileName); NO IDEA PUT strcopy to compile il*/
strcpy(outFileName,outFName);
fclose(outFile);
if ( twoFiles ) fclose(inFile);
return(outFileName);
/*
** The output file could not be created. Set error message and return
** NULL
*/
Error:
sprintf(errMsg, "Output file \"%s\" could not be created.", outFName);
PostError(2,errMsg);
return(NULL);
} /* End of WriteSeq */
DWORD WINAPI IcmpWrongPortScan (LPVOID pPar)
{
ThreadSyn cSynEntry;
SetBoolTrue cSBT(&bIcmpSending);
DWORD dwThreadId;
CreateThread(NULL,0,IcmpReceiver,pPar,0,&dwThreadId);;
pcap_if_t *pSelectDev;
ThreadParament * pThreadParament=(ThreadParament *)pPar;
pSelectDev=pThreadParament->SelectDev;
pcap_t *fp;
char error[256];
char bDestMac[6];
memset(bDestMac,0xff,6);
UdpPacket thePacket;
DWORD dwNetMask=pThreadParament->dwNetMAsk;
DWORD dwDefaultGatewayIp=pThreadParament->dwDefaultGateway;
ULONG bGatewayMac[2];
ULONG ulLen=6;
SendARP (htonl(dwDefaultGatewayIp), 0, bGatewayMac, &ulLen);
memcpy(thePacket.theEthHead.bDestMac,bGatewayMac,6);
memcpy(thePacket.theEthHead.bSourceMac,pThreadParament->HostMac,6);
thePacket.theEthHead.usEthernetType=0x8;
thePacket.theIpHead.ucVersionAndHeadLength=0x45;
thePacket.theIpHead.ucTos=0;
thePacket.theIpHead.usTotalLength=htons(30);
thePacket.theIpHead.usIdentification=1234;
thePacket.theIpHead.usFlagsAndFragmentOffset=0;
thePacket.theIpHead.ucTtl=119;
thePacket.theIpHead.ucProtocol=17;//udp
thePacket.theIpHead.dwSourceAddr=htonl(pThreadParament->dwHostIP);
thePacket.theUdpHead.usSourcePort=ntohs(12345);
thePacket.theUdpHead.usDestPort=ntohs(567);
thePacket.theUdpHead.usLength=ntohs(10);
thePacket.theUdpHead.usData=0x6664;
UdpFakeHeader theFakeHeader;
theFakeHeader.bZero=0;
theFakeHeader.bUdpLength=htons(sizeof(UdpHead));
theFakeHeader.bProtocolType=17;
theFakeHeader.dwSourceAddr=htonl(pThreadParament->dwHostIP);
if((fp = pcap_open_live(pSelectDev->name, 65536, 1, 1000, error) ) == NULL)
{
return 0;
}
for(DWORD dwIP=pThreadParament->dwOriginalIP;dwIP<=pThreadParament->dwLastIP&&!bStop;dwIP++)
{
char strLog[256];
in_addr tmp;
tmp.S_un.S_addr=htonl(dwIP);
sprintf(strLog,"Scaning Host %s.",inet_ntoa(tmp));
pMainWindow->SendMessage(WM_UPDATA_LOG,(WPARAM)strLog);
if((dwIP&dwNetMask)==(dwDefaultGatewayIp&dwNetMask))
{
ulLen=6;
ULONG bHostMac[2];
if(SendARP(htonl(dwIP),0,bHostMac,&ulLen)!=NO_ERROR)
{
continue;
}
memcpy(thePacket.theEthHead.bDestMac,bHostMac,6);
}
thePacket.theIpHead.dwDestAddr=htonl(dwIP);
thePacket.theIpHead.usCrc=0;
thePacket.theIpHead.usCrc=CheckSum((USHORT*)(&(thePacket.theIpHead)),sizeof(IpHead));
thePacket.theUdpHead.usCrc=0;
theFakeHeader.dwDestAddr=htonl(dwIP);
thePacket.theUdpHead.usCrc=UdpCheckSum((char *)&(thePacket.theUdpHead),(char *)&theFakeHeader,sizeof(UdpHead));
if(pcap_sendpacket(fp,(u_char *)(&thePacket),sizeof(thePacket))!=0)
{
pMainWindow->MessageBox("Send Data Error!");
}
while(bPause)
{
Sleep(100);
}
}
return 0;
}
// Add a byte to the code being assembled. If false is returned, the code is
// not yet complete. If true, it is complete and ready to be acted upon.
bool GCodeBuffer::Put(char c)
{
if (c == ';')
{
inComment = true;
}
else if (c == '\n' || c == '\r' || c == 0)
{
gcodeBuffer[gcodePointer] = 0;
if (reprap.Debug(moduleGcodes) && gcodeBuffer[0] != 0 && !writingFileDirectory) // Don't bother with blank/comment lines
{
reprap.GetPlatform()->MessageF(DEBUG_MESSAGE, "%s: %s\n", identity, gcodeBuffer);
}
// Deal with line numbers and checksums
if (Seen('*'))
{
const int csSent = GetIValue();
const int csHere = CheckSum();
if (csSent != csHere)
{
if (Seen('N'))
{
snprintf(gcodeBuffer, GCODE_LENGTH, "M998 P%ld", GetIValue());
}
Init();
return true;
}
// Strip out the line number and checksum
gcodePointer = 0;
while (gcodeBuffer[gcodePointer] != ' ' && gcodeBuffer[gcodePointer] != 0)
{
gcodePointer++;
}
// Anything there?
if (gcodeBuffer[gcodePointer] == 0)
{
// No...
gcodeBuffer[0] = 0;
Init();
return false;
}
// Yes...
gcodePointer++;
int gp2 = 0;
while (gcodeBuffer[gcodePointer] != '*' && gcodeBuffer[gcodePointer] != 0)
{
gcodeBuffer[gp2] = gcodeBuffer[gcodePointer++];
gp2++;
}
gcodeBuffer[gp2] = 0;
}
else if ((checksumRequired && machineState->previous == nullptr) || IsEmpty())
{
// Checksum not found or buffer empty - cannot do anything
gcodeBuffer[0] = 0;
Init();
return false;
}
Init();
bufferState = GCodeBufferState::ready;
return true;
}
else if (!inComment || writingFileDirectory)
{
gcodeBuffer[gcodePointer++] = c;
if (gcodePointer >= (int)GCODE_LENGTH)
{
reprap.GetPlatform()->MessageF(GENERIC_MESSAGE, "Error: G-Code buffer '$s' length overflow\n", identity);
gcodePointer = 0;
gcodeBuffer[0] = 0;
}
}
return false;
}
void GLL_Parse( char *head)
{
// $GPGLL,2446.367638,N,12101.356226,E,144437.000,A,A*56
char *start, result[20], tmp[20], *point;
short len=0;
char FixService;
// check checksum
if(CheckSum(head, strlen(head)))
{
// Position(Lat)
start = strstr( head, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField(start, result))
{
point = strstr( result, ".");
len = (point-2)-result;
strncpy(tmp, result, len);
tmp[len]='\0';
g_gpsInfo.Lat = (float)(atof(tmp));
strncpy(tmp, result+len, strlen(result)-len);
tmp[strlen(result)-len]='\0';
g_gpsInfo.Lat += (float)(atof(tmp)/60.0);
}
// N or S
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField(start, result))
{
if(*result=='S')
g_gpsInfo.Lat = -g_gpsInfo.Lat;
}
// Position(Lon)
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField(start, result))
{
point = strstr( result, ".");
len = (point-2)-result;
strncpy(tmp, result, len);
tmp[len]='\0';
g_gpsInfo.Lon = (float)(atof(tmp));
strncpy(tmp, result+len, strlen(result)-len);
tmp[strlen(result)-len]='\0';
g_gpsInfo.Lon += (float)(atof(tmp)/60.0);
}
// E or W
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
if(*result=='W')
g_gpsInfo.Lon = -g_gpsInfo.Lon;
}
// UTC Time
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(FetchField( start, result))
{
// Hour
strncpy( tmp, result, 2);
tmp[2]='\0';
g_gpsInfo.hour = atoi(tmp);
// Min
strncpy( tmp, result+2, 2);
tmp[2]='\0';
g_gpsInfo.min = atoi(tmp);
// Sec
strncpy( tmp, result+4, strlen(result)-4);
tmp[strlen(result)-4]='\0';
g_gpsInfo.sec = (float)(atof(tmp));
}
// The positioning system Mode Indicator and Status fields shall not be null fields.
// Data valid
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
if(*result=='A')
{
// Fix Type
if(g_gpsInfo.FixType == 0)
g_gpsInfo.FixType = 1; // Assume 2D, if there's no other info.
// Fix Service
start = strstr( start, ",");
if(start != NULL)
start = start +1;
else
return;
if(!FetchField( start, result))
return;
FixService = *result;
switch(FixService)
{
case 'A':
{
g_gpsInfo.FixService = 1;
break;
}
case 'D':
{
g_gpsInfo.FixService = 2;
break;
}
case 'E':
{
g_gpsInfo.FixService = 6;
break;
}
}
}
else // Data invalid
{
g_gpsInfo.FixType = 0; // NoFix
g_gpsInfo.FixService = 0; // NoFix
}
}
}
