//READ_PINに変化があった場合の割り込みイベントハンドラ void signal(void){ printf("Interupt!\n"); int count = 0 ; int edgeNum=0; nowStatusSignal=NONE;//現状のリモコン送信モード int calcusec=0; bool flag=true; int currentPinStatus=1;//rigingエッジのため初期状態はHighであるとします //int currentPinStatus=digitalRead(READ_PIN); enum statusSignal beforeStatus = NONE; //時間情報の初期化 gettimeofday(¤tTime, NULL); gettimeofday(&beforeTime, NULL); beforeDeltaT=currentDeltaT=0; //printf("Start Signal\n"); while(flag){ pinStatus=digitalRead(READ_PIN); if(currentPinStatus!=pinStatus){//信号のエッジで条件分岐 //現在の時刻情報を取得 currentPinStatus=pinStatus; gettimeofday(¤tTime, NULL); currentDeltaT=deltaT(currentTime,beforeTime); flameDeltaT=deltaT(flameTime,currentTime); //現在の信号状態を取得 nowStatusSignal=detectStatus(nowStatusSignal); //現在の時間情報を保存 beforeDeltaT=deltaT(currentTime,beforeTime); struct timeval test= deltaTus(currentTime,beforeTime); printf("deltaus %ds %us beforeDeltaT %d \n",test.tv_sec,test.tv_usec,beforeDeltaT); gettimeofday(&beforeTime, NULL); if(nowStatusSignal==NONE){//信号を受信していない場合 }else if(nowStatusSignal==FLAME){//Flameに入っている場合 //if(beforeStatus==NONE){ //フレームに入ってその前のステータスがNONEの場合は信号開始 if(pinStatus==0){ printf("%d: pinStatus:Low Time:%d us\n",edgeNum,currentDeltaT); }else{ printf("%d: pinStatus:High Time:%d us\n",edgeNum,currentDeltaT); } //} }else if(nowStatusSignal==REPEAT){//Repeatに入っている場合 }else{ } beforeStatus=nowStatusSignal; }else{ gettimeofday(¤tTime, NULL); currentDeltaT=deltaT(currentTime,flameTime); if(currentDeltaT > 108*1000 ){//フレームが始まってからの累計時間が108ms以上の場合フレーム時間と判断 flag=false; nowStatusSignal=NONE; beforeDeltaT=currentDeltaT=flameDeltaT=0; }else{ } } } //printf("End Signal\n"); }
void ProgAlgXC3S::flow_program_legacy(BitFile &file) { byte data[2]; struct timeval tv[2]; gettimeofday(tv, NULL); jtag->shiftIR(&JSHUTDOWN); io->cycleTCK(tck_len); jtag->shiftIR(&CFG_IN); jtag->shiftDR((file.getData()),0,file.getLength()); io->cycleTCK(1); jtag->shiftIR(&JSTART); io->cycleTCK(2*tck_len); jtag->shiftIR(&BYPASS); data[0]=0x0; jtag->shiftDR(data,0,1); io->cycleTCK(1); // Print the timing summary if (io->getVerbose()) { gettimeofday(tv+1, NULL); printf("Done.\nProgramming time %.1f ms\n", (double)deltaT(tv, tv + 1)/1.0e3); } }
long update(long j){ double dt = car[j][0] - car[j - 1][0]; //printf("%lf\n", dt); double vcar = (car[j][1] - car[j-1][1]) / dt; double dmine = v*dt + 0.5*a*dt*dt + d; if (dmine > car[j][1]) { dmine = car[j][1]; d = car[j][1]; v = sqrt(v*v + 2*a*(car[j][1] - car[j-1][1])); t = car[j][0]; return 1; } else{ if (car[j][1] == D) { t = t + deltaT(); d = D; return 1; } return 1; } return 1; }
void Screen::displayDeltaFirst() { lcdSetCursor0_0(); printCharge(); deltaT(); lcdSetCursor0_1(); printChar_Time(); deltaV(); }
/* format a Julian Day to ISO format datetime Args: jd: time in JDTT tz: a integer as timezone, e.g. -8 for UTC-8, 2 for UTC2 fmt: like strftime but currently only support three format %y-%m-%d %H:%M:%S %y-%m-%d %H:%M %y-%m-%d ut: convert to UTC(adjust delta T) Return: time string in ISO format, e.g. 1984-01-01 23:59:59 */ size_t jdftime(char *isodt, double jd, char *fmt, double tz, int isut) { GregorianDate g; double deltat, utsec, secs, jdut; int isecs; /* char isodt[ISODTLEN]; */ g = jd2g(jd); deltat = isut ? deltaT(g.year, g.month) : 0; /* convert jd to seconds, then adjust deltat */ utsec = jd * 86400.0 + tz * 3600.0 - deltat; jdut = utsec / 86400.0; /* get time in seconds directly to minimize round error */ secs = fmod(utsec + 43200.0, 86400.0); if (strcmp(fmt, "%y-%m-%d %H:%M") == 0) { isecs = (int)(floor(0.5 + secs / 60.0) * 60.0); } else { isecs = (int)(secs); } if (86400 == secs) { jdut = floor(jdut) + 0.5; isecs = 0; } g = jd2g(jdut); /* use integer math hereafter */ int y, m, d, H, M, S, ms; y = g.year; m = g.month; d = floor(g.day); H = isecs / 3600; ms = isecs % 3600; M = ms / 60; S = ms % 60; if (strcmp(fmt, "%y-%m-%d") == 0) { sprintf(isodt, "%04d-%02d-%02d", y, m, d); } else if (strcmp(fmt, "%y-%m-%d %H:%M") == 0) { sprintf(isodt, "%04d-%02d-%02d %02d:%02d", y, m, d, H, M); } else { sprintf(isodt, "%04d-%02d-%02d %02d:%02d:%02d", y, m, d, H, M, S); } return strlen(isodt); }
void Screen::displayDeltaTextern() { lcdSetCursor0_0(); lcdPrint_P(PSTR("Text=")); if(settings.externT_ || settings.isDebug()) { lcdPrintTemperature(analogInputs.deltaLastT_, 9); } else { lcdPrint_P(PSTR("not used")); } lcdPrintSpaces(); lcdSetCursor0_1(); lcdPrint_P(PSTR("delta T= ")); deltaT(); }
bool ProgAlgSram::ProgramSram(BinaryFile &file, Sram_Options_t options) { struct timeval tv[2]; bool verbose=io->getVerbose(); gettimeofday(tv, NULL); // Switch to USER1 register, to access BSCAN.. jtag->shiftIR(&USER1,0); if(options==FULL||options==WRITE_ONLY) { printf("\nProgramming SRAM\n"); if(!Sram_Write(file.getData(), file.getLength(), verbose)) return false; } if(options==FULL||options==VERIFY_ONLY) { printf("\nVerifying SRAM\n"); if(!Sram_Verify(file.getData(), file.getLength(), verbose)) return false; } if (verbose) { gettimeofday(tv+1, NULL); printf("\nTotal SRAM execution time %.1f ms\n", (double)deltaT(tv, tv + 1)/1.0e3); } /* JPROGAM: Trigerr reconfiguration, not explained in ug332, but DS099 Figure 28: Boundary-Scan Configuration Flow Diagram (p.49) */ if(options==FULL) { jtag->shiftIR(&JPROGRAM); Sleep(1000);//just wait a bit to make sure everything is done.. } jtag->shiftIR(&BYPASS); return true; }
void ProgAlgXC3S::flow_array_program(BitFile &file) { struct timeval tv[2]; unsigned int i; gettimeofday(tv, NULL); for(i=0; i<file.getLength(); i= i+ array_transfer_len) { jtag->shiftIR(&ISC_PROGRAM); jtag->shiftDR(&(file.getData())[i/8],0,array_transfer_len); io->cycleTCK(1); if((i % (10000*array_transfer_len)) == 0) { fprintf(stdout,"."); fflush(stdout); } } gettimeofday(tv+1, NULL); // Print the timing summary if (io->getVerbose()) printf(" Done.\nProgramming time %.1f ms\n", (double)deltaT(tv, tv + 1)/1.0e3); }
void testdeltat() { // double d = -133.5; // int i; double jd; char strout[30]; jd = jdptime("2012-01-05 18:00", "%y-%m-%d %H:%M", 0, 0); //jd = jdptime("2012-01-05", "%y-%m-%d", 0, 0); printf("%f\n", jd); jdftime(strout, jd, "%y-%m-%d %H:%M", 0, 0); printf("jdftime output = %s\n", strout); int i,year; double deltat; year = -500; for (i = 0; i < 20; i++) { deltat = deltaT(year, 1); printf("%d = %.2f\n", year, deltat); year += 100; } return; }
/** Returns the pseudo-Nyquist frequency for a grid of observations. * * The pseudo-Nyquist frequency is defined as N/2T, where N is the number of * observations and T is the length of the time interval covered by the data. * * @param[in] times Times at which data were taken * * @return The pseudo-Nyquist frequency, in the inverse of whatever units * times is in. * * @pre @p times.size() ≥ 2 * @pre @p times contains at least two unique values * * @perform O(N) time, where N = @p times.size() * * @exception std::invalid_argument Thrown if @p times has at most one element. * @exception kpftimes::except::BadLightCurve Thrown if @p times has * at most one distinct value. * * @exceptsafe The function arguments are unchanged in the event * of an exception. * * @test Regular grid, length 1. Expected behavior: throws invalid_argument * @test Regular grid, length 2. Expected behavior: returns PNF = 1/(2*step) * @test Regular grid, length 100. Expected behavior: returns PNF = 1/(2*step) * * @todo Come up with test cases for an irregular grid. */ double pseudoNyquistFreq(const DoubleVec ×) { // Delegate input validation to deltaT return 0.5 * times.size() / deltaT(times); }
double traceroute_time_delta(struct traceroute *t) { return deltaT(&t->timesent, &t->timerecv); }
int main(int argc, char const *argv[]) { FILE * fin = fopen("input.in", "r"); FILE * fout = fopen("output.out", "w"); long T, N, A; //double D; long i, j, k; double out[250]; fscanf(fin, "%ld\n", &T); char ocase[40]; for (i = 0; i < T; ++i) { fscanf(fin, "%lf %ld %ld\n", &D, &N, &A); //printf("%lf %ld %ld\n", D, N, A); memset(car, 0, 2000*2*sizeof(double)); memset(acc, 0, 250*sizeof(double)); memset(out, 0, 250*sizeof(double)); for (j = 0; j < N; ++j) { fscanf(fin, "%lf %lf\n", &car[j][0], &car[j][1]); //printf("%lf %lf\n", car[j][0], car[j][1]); } for (j = 0; j < N; ++j) { if (car[j][1] >= D) { N = j + 1; break; } } for (j = 0; j < A - 1; ++j) { fscanf(fin, "%lf ", &acc[j]); //printf("%lf ", acc[j]); } fscanf(fin, "%lf ", &acc[j]); //printf("%lf\n", acc[j]); for (j = 0; j < A; ++j) { d = 0; t = 0; v = 0; a = acc[j]; out[j] = 0; double wait = 0; if (N == 1) { out[j] = deltaT(); continue; } else if (car[N - 1][1] > D) { car[N - 1][0] = (D - car[N - 2][1]) / (car[N - 1][1] - car[N - 2][1]) * (car[N - 1][0] - car[N - 2][0]) + car[N - 2][0]; car[N - 1][1] = D; } for (k = 0; k < N; ++k) { if(sqrt(car[k][1] * 2 / a) + wait < car[k][0]){ wait = car[k][0] - sqrt(car[k][1] * 2 / a); } } out[j] = wait + sqrt(D * 2 / a); } sprintf(ocase, "Case #%ld:", i + 1); //printf("%s\n", ocase); if (i != T - 1) { fprintf (fout, "%s\n", ocase); for (j = 0; j < A; ++j) { fprintf (fout, "%.7lf\n", out[j]); } } else{ fprintf (fout, "%s\n", ocase); for (j = 0; j < A - 1; ++j) { fprintf (fout, "%.7lf\n", out[j]); } fprintf (fout, "%.7lf", out[j]); } //fprintf (fout, "%s", ocase); } fclose(fin); fclose(fout); return 0; }
/* takes care of replies in the usrloc mode */ void handle_usrloc() { char *crlf; char ruri[11+12+20]; //FIXME: username length 20 should be dynamic if (regexec(&(regexps.proexp), rec, 0, 0, 0) == REG_NOERROR) { if (verbose > 2) { print_message_line(rec); printf("ignoring provisional response\n\n"); } if (inv_trans) { delays.retryAfter = timer_final; } else { delays.retryAfter = timer_t2; } cdata.dontsend = 1; } else { switch (usrlocstep) { case REG_REP: /* we have sent a register and look at the response now */ if (regexec(&(regexps.okexp), rec, 0, 0, 0) == REG_NOERROR) { if (verbose > 1) { printf ("\tOK\n"); } if (verbose > 2) { printf("\n%s\n", rec); } } else { fprintf(stderr, "received:\n%s\nerror: didn't " "received '200 OK' on register (see " "above). aborting\n", rec); log_message(req); exit_code(1, __PRETTY_FUNCTION__, "received non-2xx reply for REGISTER"); } if (invite == 0 && message == 0) { if (namebeg==nameend) { if (verbose>0) { printf("\nAll usrloc tests" " completed successful.\nreceived" " last message %.3f ms after first" " request (test duration).\n", deltaT(&(timers.firstsendt), &(timers.recvtime))); } if (delays.big_delay>0 && verbose>0) { printf("biggest delay between " "request and response was %.3f" " ms\n", delays.big_delay); } if (counters.retrans_r_c>0 && verbose>0) { printf("%i retransmission(s) received from server.\n", counters.retrans_r_c); } if (counters.retrans_s_c>0 && verbose>0) { printf("%i time(s) the timeout of " "%i ms exceeded and request was" " retransmitted.\n", counters.retrans_s_c, delays.retryAfter); if (counters.retrans_s_c > nagios_warn) { log_message(req); exit_code(4, __PRETTY_FUNCTION__, "#retransmissions above nagios warn level"); } } if (timing) { printf("%.3f ms\n", deltaT(&(timers.firstsendt), &(timers.recvtime))); } on_success(rec); } /* namebeg == nameend */ /* lets see if we deceid to remove a binding (case 6)*/ if ( ((float)rand()/RAND_MAX)*100 > rand_rem) { namebeg++; cseq_counter++; create_usern(usern, username, namebeg); create_msg(REQ_REG, req, NULL, usern, cseq_counter); } else { /* to prevent only removing of low user numbers new random number*/ cseq_counter++; create_usern(usern, username, ((float)rand()/RAND_MAX) * namebeg); create_msg(REQ_REM, req, NULL, usern, cseq_counter); usrlocstep=UNREG_REP; } } /* invite == 0 && message == 0 */ else if (invite == 1) { cseq_counter++; create_msg(REQ_INV, req, rep, usern, cseq_counter); inv_trans = 1; usrlocstep=INV_RECV; } else if (message == 1) { cseq_counter++; create_msg(REQ_MES, req, rep, usern, cseq_counter); inv_trans = 0; usrlocstep=MES_RECV; } break; case INV_RECV: /* see if we received our invite */ sprintf(ruri, "%s sip:%s", INV_STR, usern); if (!STRNCASECMP(rec, ruri, strlen(ruri))) { if (verbose > 1) { printf("\t\treceived invite\n"); } if (verbose > 2) { printf("\n%s\n", rec); } cpy_vias(rec, rep); cpy_rr(rec, rep, 0); swap_ptr(&req, &rep); usrlocstep=INV_OK_RECV; inv_trans = 0; } else { fprintf(stderr, "received:\n%s\nerror: did not " "received the INVITE that was sent " "(see above). aborting\n", rec); log_message(req); exit_code(1, __PRETTY_FUNCTION__, "did not received our own INVITE request"); } break; case INV_OK_RECV: /* did we received our ok ? */ if (STRNCASECMP(rec, INV_STR, INV_STR_LEN)==0) { if (verbose>0) { printf("ignoring INVITE retransmission\n"); } counters.retrans_r_c++; cdata.dontsend=1; return; } if (regexec(&(regexps.okexp), rec, 0, 0, 0) == REG_NOERROR) { if (verbose > 1) { printf("\t200 OK received\n"); } if (verbose > 2) { printf("\n%s\n", rec); } /* ACK was send already earlier generically */ usrlocstep=INV_ACK_RECV; cdata.dontsend=1; } else { fprintf(stderr, "received:\n%s\nerror: did not " "received the '200 OK' that was sent " "as the reply on the INVITE (see " "above). aborting\n", rec); log_message(req); exit_code(1, __PRETTY_FUNCTION__, "did not received our own 200 reply"); } break; case INV_ACK_RECV: /* did we received our ack */ if (STRNCASECMP(rec, SIP200_STR, SIP200_STR_LEN)==0) { if (verbose>0) { printf("ignoring 200 OK retransmission\n"); } counters.retrans_r_c++; cdata.dontsend=1; return; } sprintf(ruri, "%s sip:sipsak_conf@", ACK_STR); if (STRNCASECMP(rec, ruri, strlen(ruri))==0) { if (verbose > 1) { printf("\tACK received\n"); } if (verbose > 2) { printf("\n%s\n", rec); } if (verbose>0 && nameend>0) { printf("usrloc for %s%i completed " "successful\n", username, namebeg); } else if (verbose>0) { printf("usrloc for %s completed successful\n", username); } if (namebeg==nameend) { if (verbose>0) { printf("\nAll usrloc tests completed " "successful.\nreceived last message" " %.3f ms after first request (test" " duration).\n", deltaT(&(timers.firstsendt), &(timers.recvtime))); } if (delays.big_delay>0) { printf("biggest delay between " "request and response was %.3f" " ms\n", delays.big_delay); } if (counters.retrans_r_c>0) { printf("%i retransmission(s) received from server.\n", counters.retrans_r_c); } if (counters.retrans_s_c>0) { printf("%i time(s) the timeout of " "%i ms exceeded and request was" " retransmitted.\n", counters.retrans_s_c, delays.retryAfter); if (counters.retrans_s_c > nagios_warn) { log_message(req); exit_code(4, __PRETTY_FUNCTION__, "#retransmissions above nagios warn level"); } } on_success(rec); } /* namebeg == nameend */ if (usrloc == 1) { /* lets see if we deceid to remove a binding (case 6)*/ if (((float)rand()/RAND_MAX) * 100 > rand_rem) { namebeg++; cseq_counter++; create_usern(usern, username, namebeg); create_msg(REQ_REG, req, NULL, usern, cseq_counter); usrlocstep=REG_REP; } else { /* to prevent only removing of low user numbers new random number*/ cseq_counter++; create_usern(usern, username, ((float)rand()/RAND_MAX) * namebeg); create_msg(REQ_REM, req, NULL, usern, cseq_counter); usrlocstep=UNREG_REP; } } /* usrloc == 1 */ else { namebeg++; cseq_counter++; create_usern(usern, username, namebeg); create_msg(REQ_INV, req, rep, usern, cseq_counter); inv_trans = 1; usrlocstep=INV_RECV; } } /* STRNCASECMP */ else { fprintf(stderr, "received:\n%s\nerror: did not " "received the 'ACK' that was sent " "as the reply on the '200 OK' (see " "above). aborting\n", rec); log_message(req); exit_code(1, __PRETTY_FUNCTION__, "missing ACK that was send by myself"); } break; case MES_RECV: /* we sent the message and look if its forwarded to us */ sprintf(ruri, "%s sip:%s", MES_STR, usern); if (!STRNCASECMP(rec, ruri, strlen(ruri))) { if (verbose > 1) { crlf=STRCASESTR(rec, "\r\n\r\n"); crlf=crlf+4; printf(" received message\n '%s'\n", crlf); } if (verbose > 2) { printf("\n%s\n", rec); } cpy_vias(rec, rep); swap_ptr(&req, &rep); usrlocstep=MES_OK_RECV; } else { fprintf(stderr, "received:\n%s\nerror: did not " "received the 'MESSAGE' that was sent " "(see above). aborting\n", rec); log_message(req); exit_code(1, __PRETTY_FUNCTION__, "did not received my own MESSAGE request"); } break; case MES_OK_RECV: /* we sent our reply on the message and look if this is also forwarded to us */ if (STRNCASECMP(rec, MES_STR, MES_STR_LEN)==0) { if (verbose>0) { printf("ignoring MESSAGE retransmission\n"); } counters.retrans_r_c++; cdata.dontsend=1; return; } if (regexec(&(regexps.okexp), rec, 0, 0, 0) == REG_NOERROR) { if (verbose > 1) { printf(" reply received\n\n"); } else if (verbose>0 && nameend>0) { printf("usrloc for %s%i completed " "successful\n", username, namebeg); } else if (verbose>0) { printf("usrloc for %s completed successful\n", username); } if (namebeg==nameend) { if (verbose>0) { printf("\nAll usrloc tests completed " "successful.\nreceived last message" " %.3f ms after first request (test" " duration).\n", deltaT(&(timers.firstsendt), &(timers.recvtime))); } if (delays.big_delay>0) { printf("biggest delay between " "request and response was %.3f" " ms\n", delays.big_delay); } if (counters.retrans_r_c>0) { printf("%i retransmission(s) " "received from server.\n", counters.retrans_r_c); } if (counters.retrans_s_c>0) { printf("%i time(s) the timeout of " "%i ms exceeded and request was" " retransmitted.\n", counters.retrans_s_c, delays.retryAfter); if (counters.retrans_s_c > nagios_warn) { log_message(req); exit_code(4, __PRETTY_FUNCTION__, "#retransmissions above nagios warn level"); } } on_success(rec); } /* namebeg == nameend */ if (usrloc == 1) { /* lets see if we deceid to remove a binding (case 6)*/ if (((float)rand()/RAND_MAX) * 100 > rand_rem) { namebeg++; cseq_counter++; create_usern(usern, username, namebeg); create_msg(REQ_REG, req, NULL, usern, cseq_counter); usrlocstep=REG_REP; } else { /* to prevent only removing of low user numbers new random number*/ cseq_counter++; create_usern(usern, username, ((float)rand()/RAND_MAX) * namebeg); create_msg(REQ_REM, req, NULL, usern, cseq_counter); usrlocstep=UNREG_REP; } } /* usrloc == 1 */ else { namebeg++; cseq_counter++; create_usern(usern, username, namebeg); create_msg(REQ_MES, req, NULL, usern, cseq_counter); usrlocstep=MES_RECV; } } /* regexec */ else { if (verbose>0) { if (mes_body) { fprintf(stderr, "received:\n%s\nerror: did" " not received 200 for the " "MESSAGE (see above)\n", rec); } else { fprintf(stderr, "received:\n%s\nerror: did" " not received the '200 OK' " "that was sent as the reply on" " the MESSAGE (see above). " "aborting\n", rec); } } log_message(req); exit_code(1, __PRETTY_FUNCTION__, "received non-2xx reply for MESSAGE request"); } break; case UNREG_REP: if (STRNCASECMP(rec, MES_STR, MES_STR_LEN)==0) { if (verbose>0) { printf("ignoring MESSAGE retransmission\n"); } counters.retrans_r_c++; cdata.dontsend=1; return; } if (regexec(&(regexps.okexp), rec, 0, 0, 0) == REG_NOERROR) { if (verbose > 1) { printf(" OK\n\n"); } else if (verbose>0 && nameend>0) { printf("Binding removal for %s%i " "successful\n", username, namebeg); } else if (verbose>0) { printf("Binding removal for %s successful\n", username); } namebeg++; cseq_counter++; create_usern(usern, username, namebeg); create_msg(REQ_REG, req, NULL, usern, cseq_counter); usrlocstep=REG_REP; } else { fprintf(stderr, "received:\n%s\nerror: did not " "received the expected 200 on the " "remove bindings request for %s%i (see" " above). aborting\n", rec, username, namebeg); log_message(req); exit_code(1, __PRETTY_FUNCTION__, "received non-2xx reply for de-register request"); } break; default: fprintf(stderr, "error: unknown step in usrloc\n"); exit_code(2, __PRETTY_FUNCTION__, "unknown step in usrloc"); break; } /* switch */ } /* regexec proexp */ }
/* this is the main function with the loops and modes */ void shoot(char *buf, int buff_size) { struct timespec sleep_ms_s, sleep_rem; int ret, cseqtmp, rand_tmp; char buf2[BUFSIZE], buf3[BUFSIZE], lport_str[LPORT_STR_LEN]; /* delays.retryAfter = DEFAULT_TIMEOUT; */ if (transport == SIP_UDP_TRANSPORT) { delays.retryAfter = timer_t1; } else { delays.retryAfter = timer_final; } inv_trans = 0; cseq_counter = 1; usrlocstep = REG_REP; /* initalize local vars */ cdata.dontsend=cdata.dontrecv=counters.retrans_r_c=counters.retrans_s_c= 0; delays.big_delay=counters.send_counter=counters.run= 0; timers.delaytime.tv_sec = 0; timers.delaytime.tv_usec = 0; usern = NULL; /* initialize local arrays */ memset(buf2, 0, BUFSIZE); memset(buf3, 0, BUFSIZE); memset(lport_str, 0, LPORT_STR_LEN); cdata.csock = cdata.usock = -1; cdata.connected = 0; cdata.buf_tmp = NULL; cdata.buf_tmp_size = 0; memset(&(timers.sendtime), 0, sizeof(timers.sendtime)); memset(&(timers.recvtime), 0, sizeof(timers.recvtime)); memset(&(timers.firstsendt), 0, sizeof(timers.firstsendt)); memset(&(timers.starttime), 0, sizeof(timers.starttime)); memset(&(timers.delaytime), 0, sizeof(timers.delaytime)); req = buf; rep = buf2; rec = buf3; create_sockets(&cdata); if (sleep_ms != 0) { if (sleep_ms == -2) { rand_tmp = rand(); sleep_ms_s.tv_sec = rand_tmp / 1000; sleep_ms_s.tv_nsec = (rand_tmp % 1000) * 1000000; } else { sleep_ms_s.tv_sec = sleep_ms / 1000; sleep_ms_s.tv_nsec = (sleep_ms % 1000) * 1000000; } } if (replace_b == 1){ replace_string(req, "$dsthost$", domainname); replace_string(req, "$srchost$", fqdn); sprintf(lport_str, "%i", lport); replace_string(req, "$port$", lport_str); if (username) replace_string(req, "$user$", username); } if (replace_str) replace_strings(req, replace_str); /* set all regular expression to simplfy the result code indetification */ regcomp(&(regexps.replyexp), "^SIP/[0-9]\\.[0-9] [1-6][0-9][0-9]", REG_EXTENDED|REG_NOSUB|REG_ICASE); regcomp(&(regexps.proexp), "^SIP/[0-9]\\.[0-9] 1[0-9][0-9] ", REG_EXTENDED|REG_NOSUB|REG_ICASE); regcomp(&(regexps.okexp), "^SIP/[0-9]\\.[0-9] 2[0-9][0-9] ", REG_EXTENDED|REG_NOSUB|REG_ICASE); regcomp(&(regexps.redexp), "^SIP/[0-9]\\.[0-9] 3[0-9][0-9] ", REG_EXTENDED|REG_NOSUB|REG_ICASE); regcomp(&(regexps.authexp), "^SIP/[0-9]\\.[0-9] 40[17] ", REG_EXTENDED|REG_NOSUB|REG_ICASE); regcomp(&(regexps.errexp), "^SIP/[0-9]\\.[0-9] 4[0-9][0-9] ", REG_EXTENDED|REG_NOSUB|REG_ICASE); regcomp(&(regexps.tmhexp), "^SIP/[0-9]\\.[0-9] 483 ", REG_EXTENDED|REG_NOSUB|REG_ICASE); if (username) { if (nameend > 0) { usern = str_alloc(strlen(username) + 12); create_usern(usern, username, namebeg); } else { if (*(username + strlen(username) - 1) != '@') { usern = str_alloc(strlen(username) + 2); create_usern(usern, username, -1); } else { usern = username; } } } if (usrloc == 1||invite == 1||message == 1){ /* calculate the number of required steps and create initial mes */ if (usrloc == 1) { create_msg(REQ_REG, req, NULL, usern, cseq_counter); usrlocstep=REG_REP; } else if (invite == 1) { create_msg(REQ_INV, req, rep, usern, cseq_counter); inv_trans = 1; usrlocstep=INV_RECV; } else { create_msg(REQ_MES, req, rep, usern, cseq_counter); if (mes_body) usrlocstep=MES_OK_RECV; else usrlocstep=MES_RECV; } } else if (trace == 1){ /* for trace we need some spezial initis */ namebeg=0; create_msg(REQ_OPT, req, NULL, usern, cseq_counter); set_maxforw(req, namebeg); } else if (flood == 1){ if (nameend<=0) nameend=INT_MAX; namebeg=1; create_msg(REQ_FLOOD, req, NULL, usern, cseq_counter); } else if (randtrash == 1){ counters.randretrys=0; namebeg=1; create_msg(REQ_RAND, req, NULL, usern, cseq_counter); nameend=(int)strlen(req); if (trashchar == 1){ if (trashchar < nameend) nameend=trashchar; else fprintf(stderr, "warning: number of trashed chars to big. setting to " "request length\n"); } trash_random(req); } else { /* for none of the modes we also need some inits */ if (file_b == 0) { namebeg=1; create_msg(REQ_OPT, req, NULL, usern, cseq_counter); } else { if (STRNCASECMP(req, INV_STR, INV_STR_LEN) == 0) { inv_trans = 1; } if(via_ins == 1) add_via(req); } /* delays.retryAfter = delays.retryAfter / 10; */ if(maxforw!=-1) set_maxforw(req, maxforw); } cdata.connected = set_target(&(cdata.adr), address, rport, cdata.csock, cdata.connected); /* here we go until someone decides to exit */ while(1) { before_sending(); if (sleep_ms == -2) { rand_tmp = rand(); sleep_ms_s.tv_sec = rand_tmp / 1000; sleep_ms_s.tv_nsec = (rand_tmp % 1000) * 1000; } if (sleep_ms != 0) { dbg("sleeping for %li s + %li ns\n", sleep_ms_s.tv_sec, sleep_ms_s.tv_nsec); nanosleep(&sleep_ms_s, &sleep_rem); } send_message(req, &cdata, &counters, &timers); /* in flood we are only interested in sending so skip the rest */ if (flood == 0) { ret = recv_message(rec, BUFSIZE, inv_trans, &delays, &timers, &counters, &cdata, ®exps); if(ret > 0) { if (usrlocstep == INV_OK_RECV) { swap_ptr(&rep, &req); } /* send ACK for non-provisional reply on INVITE */ if ((STRNCASECMP(req, "INVITE", 6)==0) && (regexec(&(regexps.replyexp), rec, 0, 0, 0) == REG_NOERROR) && (regexec(&(regexps.proexp), rec, 0, 0, 0) == REG_NOMATCH)) { build_ack(req, rec, rep, ®exps); cdata.dontsend = 0; inv_trans = 0; /* lets fire the ACK to the server */ send_message(rep, &cdata, &counters, &timers); inv_trans = 1; } /* check for old CSeq => ignore retransmission */ cseqtmp = cseq(rec); if ((0 < cseqtmp) && (cseqtmp < cseq_counter)) { if (verbose>0) { printf("ignoring retransmission\n"); } counters.retrans_r_c++; cdata.dontsend = 1; continue; } else if (regexec(&(regexps.authexp), rec, 0, 0, 0) == REG_NOERROR) { if (!username && !auth_username) { if (timing > 0) { timing--; if (timing == 0) { if (counters.run == 0) { counters.run++; } printf("%.3f/%.3f/%.3f ms\n", delays.small_delay, delays.all_delay / counters.run, delays.big_delay); exit_code(0, __PRETTY_FUNCTION__, NULL); } counters.run++; new_transaction(req, rep); delays.retryAfter = timer_t1; continue; } fprintf(stderr, "%s\nerror: received 40[17] but cannot " "authentication without a username or auth username\n", rec); log_message(req); exit_code(2, __PRETTY_FUNCTION__, "missing username for authentication"); } /* prevents a strange error */ regcomp(&(regexps.authexp), "^SIP/[0-9]\\.[0-9] 40[17] ", REG_EXTENDED|REG_NOSUB|REG_ICASE); insert_auth(req, rec); if (verbose > 2) printf("\nreceived:\n%s\n", rec); new_transaction(req, rep); continue; } /* if auth...*/ /* lets see if received a redirect */ if (redirects == 1 && regexec(&(regexps.redexp), rec, 0, 0, 0) == REG_NOERROR) { handle_3xx(&(cdata.adr)); } /* if redircts... */ else if (trace == 1) { trace_reply(); } /* if trace ... */ else if (usrloc == 1||invite == 1||message == 1) { handle_usrloc(); } else if (randtrash == 1) { handle_randtrash(); } else { handle_default(); } /* redirect, auth, and modes */ } /* ret > 0 */ else if (ret == -1) { // we did not got anything back, send again /* no re-transmission on reliable transports */ if (transport != SIP_UDP_TRANSPORT) { cdata.dontsend = 1; } continue; } else if (ret == -2) { // we received non-matching ICMP cdata.dontsend = 1; continue; } else { if (usrloc == 1) { printf("failed\n"); } perror("socket error"); exit_code(3, __PRETTY_FUNCTION__, "internal socket error"); } } /* !flood */ else { if (counters.send_counter == 1) { memcpy(&(timers.firstsendt), &(timers.sendtime), sizeof(struct timeval)); } if (namebeg==nameend) { printf("flood end reached\n"); printf("it took %.3f ms seconds to send %i request.\n", deltaT(&(timers.firstsendt), &(timers.sendtime)), namebeg); printf("we sent %f requests per second.\n", (namebeg/(deltaT(&(timers.firstsendt), &(timers.sendtime)))*1000)); exit_code(0, __PRETTY_FUNCTION__, NULL); } namebeg++; cseq_counter++; create_msg(REQ_FLOOD, req, NULL, usern, cseq_counter); } } /* while 1 */ /* this should never happen any more... */ if (randtrash == 1) { exit_code(0, __PRETTY_FUNCTION__, NULL); } printf("** give up retransmissioning....\n"); if (counters.retrans_r_c>0 && (verbose > 1)) { printf("%i retransmissions received during test\n", counters.retrans_r_c); } if (counters.retrans_s_c>0 && (verbose > 1)) { printf("sent %i retransmissions during test\n", counters.retrans_s_c); } exit_code(3, __PRETTY_FUNCTION__, "got outside of endless messaging loop"); }
/* takes care of replies in the trace route mode */ void trace_reply() { char *contact; if (regexec(&(regexps.tmhexp), rec, 0, 0, 0) == REG_NOERROR) { /* we received 483 to many hops */ printf("%i: ", namebeg); if (verbose > 2) { printf("(%.3f ms)\n%s\n", deltaT(&(timers.sendtime), &(timers.recvtime)), rec); } else { warning_extract(rec); printf("(%.3f ms) ", deltaT(&(timers.sendtime), &(timers.recvtime))); print_message_line(rec); } namebeg++; cseq_counter++; create_msg(REQ_OPT, req, NULL, usern, cseq_counter); set_maxforw(req, namebeg); return; } else if (regexec(&(regexps.proexp), rec, 0, 0, 0) == REG_NOERROR) { /* we received a provisional response */ printf("%i: ", namebeg); if (verbose > 2) { printf("(%.3f ms)\n%s\n", deltaT(&(timers.sendtime), &(timers.recvtime)), rec); } else { warning_extract(rec); printf("(%.3f ms) ", deltaT(&(timers.sendtime), &(timers.recvtime))); print_message_line(rec); } delays.retryAfter = timer_t2; cdata.dontsend=1; return; } else { /* anything else then 483 or provisional will be treated as final */ printf("%i: ", namebeg); warning_extract(rec); printf("(%.3f ms) ", deltaT(&(timers.sendtime), &(timers.recvtime))); print_message_line(rec); if ((contact = STRCASESTR(rec, CONT_STR)) != NULL || (contact = STRCASESTR(rec, CONT_SHORT_STR)) != NULL) { if (*contact == '\n') { contact++; } printf("\t"); print_message_line(contact); } else { printf("\twithout Contact header\n"); } if (regexec(&(regexps.okexp), rec, 0, 0, 0) == REG_NOERROR) { on_success(rec); } else { log_message(req); exit_code(1, __PRETTY_FUNCTION__, "received final non-2xx reply"); } } }
/* takes care of replies in the default mode */ void handle_default() { /* in the normal send and reply case anything other then 1xx will be treated as final response*/ if (regexec(&(regexps.proexp), rec, 0, 0, 0) == REG_NOERROR) { if (verbose > 1) { printf("%s\n\n", rec); printf("** reply received "); if ((counters.send_counter == 1) || (STRNCASECMP(req, ACK_STR, ACK_STR_LEN) == 0)) { printf("after %.3f ms **\n", deltaT(&(timers.firstsendt), &(timers.recvtime))); } else { printf("%.3f ms after first send\n and " "%.3f ms after last send **\n", deltaT(&(timers.firstsendt), &(timers.recvtime)), deltaT(&(timers.sendtime), &(timers.recvtime))); } printf(" "); print_message_line(rec); printf(" provisional received; still" " waiting for a final response\n"); } if (inv_trans) { delays.retryAfter = timer_final; } else { delays.retryAfter = timer_t2; } cdata.dontsend = 1; return; } else { if (verbose > 1) { printf("%s\n\n", rec); printf("** reply received "); if ((counters.send_counter == 1) || (STRNCASECMP(req, ACK_STR, ACK_STR_LEN) == 0)){ printf("after %.3f ms **\n", deltaT(&(timers.firstsendt), &(timers.recvtime))); } else { printf("%.3f ms after first send\n and " "%.3f ms after last send **\n", deltaT(&(timers.firstsendt), &(timers.recvtime)), deltaT(&(timers.sendtime), &(timers.recvtime))); } printf(" "); print_message_line(rec); printf(" final received\n"); } else if (verbose>0) { printf("%s\n", rec); } if (timing > 0) { timing--; if (timing == 0) { if (counters.run == 0) { counters.run++; } printf("%.3f/%.3f/%.3f ms\n", delays.small_delay, delays.all_delay / counters.run, delays.big_delay); } else { counters.run++; new_transaction(req, rep); delays.retryAfter = timer_t1; } } if (timing == 0) { if (regexec(&(regexps.okexp), rec, 0, 0, 0) == REG_NOERROR) { on_success(rec); } else { log_message(req); exit_code(1, __PRETTY_FUNCTION__, "received final non-2xx reply"); } } } }
int main(int argc, char *argv[]) { char pa[MAXHOSTNAMELEN]; extern char *optarg; extern int optind; struct hostent *hp; struct sockaddr_in6 from, *to; int ch, i, on, probe, seq, tos, ttl; int socket_errno; icmp_sock = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6); socket_errno = errno; if (setuid(getuid())) { perror("traceroute6: setuid"); exit(-1); } on = 1; seq = tos = 0; to = (struct sockaddr_in6 *)&whereto; while ((ch = getopt(argc, argv, "dm:np:q:rs:t:w:vi:g:V")) != EOF) { switch(ch) { case 'd': options |= SO_DEBUG; break; case 'm': max_ttl = atoi(optarg); if (max_ttl <= 1) { Fprintf(stderr, "traceroute: max ttl must be >1.\n"); exit(1); } break; case 'n': nflag++; break; case 'p': port = atoi(optarg); if (port < 1) { Fprintf(stderr, "traceroute: port must be >0.\n"); exit(1); } break; case 'q': nprobes = atoi(optarg); if (nprobes < 1) { Fprintf(stderr, "traceroute: nprobes must be >0.\n"); exit(1); } break; case 'r': options |= SO_DONTROUTE; break; case 's': /* * set the ip source address of the outbound * probe (e.g., on a multi-homed host). */ source = optarg; break; case 'i': device = optarg; break; case 'g': Fprintf(stderr, "Sorry, rthdr is not yet supported\n"); break; case 'v': verbose++; break; case 'w': waittime = atoi(optarg); if (waittime <= 1) { Fprintf(stderr, "traceroute: wait must be >1 sec.\n"); exit(1); } break; case 'V': printf("traceroute6 utility, iputils-ss%s\n", SNAPSHOT); exit(0); default: usage(); } } argc -= optind; argv += optind; if (argc < 1) usage(); setlinebuf (stdout); (void) bzero((char *)&whereto, sizeof(whereto)); to->sin6_family = AF_INET6; to->sin6_port = htons(port); if (inet_pton(AF_INET6, *argv, &to->sin6_addr) > 0) { hostname = *argv; } else { hp = gethostbyname2(*argv, AF_INET6); if (hp) { memmove((caddr_t)&to->sin6_addr, hp->h_addr, sizeof(to->sin6_addr)); hostname = (char *)hp->h_name; } else { (void)fprintf(stderr, "traceroute: unknown host %s\n", *argv); exit(1); } } firsthop = *to; if (*++argv) { datalen = atoi(*argv); /* Message for rpm maintainers: have _shame_. If you want * to fix something send the patch to me for sanity checking. * "datalen" patch is a shit. */ if (datalen == 0) datalen = sizeof(struct pkt_format); else if (datalen < (int)sizeof(struct pkt_format) || datalen >= MAXPACKET) { Fprintf(stderr, "traceroute: packet size must be %d <= s < %d.\n", (int)sizeof(struct pkt_format), MAXPACKET); exit(1); } } ident = getpid(); sendbuff = malloc(datalen); if (sendbuff == NULL) { fprintf(stderr, "malloc failed\n"); exit(1); } if (icmp_sock < 0) { errno = socket_errno; perror("traceroute6: icmp socket"); exit(1); } #ifdef IPV6_RECVPKTINFO setsockopt(icmp_sock, SOL_IPV6, IPV6_RECVPKTINFO, &on, sizeof(on)); setsockopt(icmp_sock, SOL_IPV6, IPV6_2292PKTINFO, &on, sizeof(on)); #else setsockopt(icmp_sock, SOL_IPV6, IPV6_PKTINFO, &on, sizeof(on)); #endif if (options & SO_DEBUG) setsockopt(icmp_sock, SOL_SOCKET, SO_DEBUG, (char *)&on, sizeof(on)); if (options & SO_DONTROUTE) setsockopt(icmp_sock, SOL_SOCKET, SO_DONTROUTE, (char *)&on, sizeof(on)); on = 2; if (setsockopt(icmp_sock, SOL_RAW, IPV6_CHECKSUM, &on, sizeof(on)) < 0) { perror("setsockopt(RAW_CHECKSUM)"); exit(2); } if ((sndsock = socket(AF_INET6, SOCK_DGRAM, 0)) < 0) { perror("traceroute: UDP socket"); exit(5); } #ifdef SO_SNDBUF if (setsockopt(sndsock, SOL_SOCKET, SO_SNDBUF, (char *)&datalen, sizeof(datalen)) < 0) { perror("traceroute: SO_SNDBUF"); exit(6); } #endif /* SO_SNDBUF */ if (options & SO_DEBUG) (void) setsockopt(sndsock, SOL_SOCKET, SO_DEBUG, (char *)&on, sizeof(on)); if (options & SO_DONTROUTE) (void) setsockopt(sndsock, SOL_SOCKET, SO_DONTROUTE, (char *)&on, sizeof(on)); if (source == NULL) { socklen_t alen; int probe_fd = socket(AF_INET6, SOCK_DGRAM, 0); if (probe_fd < 0) { perror("socket"); exit(1); } if (device) { if (setsockopt(probe_fd, SOL_SOCKET, SO_BINDTODEVICE, device, strlen(device)+1) == -1) perror("WARNING: interface is ignored"); } firsthop.sin6_port = htons(1025); if (connect(probe_fd, (struct sockaddr*)&firsthop, sizeof(firsthop)) == -1) { perror("connect"); exit(1); } alen = sizeof(saddr); if (getsockname(probe_fd, (struct sockaddr*)&saddr, &alen) == -1) { perror("getsockname"); exit(1); } saddr.sin6_port = 0; close(probe_fd); } else { (void) bzero((char *)&saddr, sizeof(saddr)); saddr.sin6_family = AF_INET6; if (inet_pton(AF_INET6, source, &saddr.sin6_addr) <= 0) { Printf("traceroute: unknown addr %s\n", source); exit(1); } } if (bind(sndsock, (struct sockaddr *)&saddr, sizeof(saddr)) < 0) { perror ("traceroute: bind sending socket"); exit (1); } if (bind(icmp_sock, (struct sockaddr *)&saddr, sizeof(saddr)) < 0) { perror ("traceroute: bind icmp6 socket"); exit (1); } Fprintf(stderr, "traceroute to %s (%s)", hostname, inet_ntop(AF_INET6, &to->sin6_addr, pa, sizeof(pa))); Fprintf(stderr, " from %s", inet_ntop(AF_INET6, &saddr.sin6_addr, pa, sizeof(pa))); Fprintf(stderr, ", %d hops max, %d byte packets\n", max_ttl, datalen); (void) fflush(stderr); for (ttl = 1; ttl <= max_ttl; ++ttl) { struct in6_addr lastaddr = {{{0,}}}; int got_there = 0; int unreachable = 0; Printf("%2d ", ttl); for (probe = 0; probe < nprobes; ++probe) { int cc, reset_timer; struct timeval t1, t2; struct timezone tz; struct in6_addr to; gettimeofday(&t1, &tz); send_probe(++seq, ttl); reset_timer = 1; while ((cc = wait_for_reply(icmp_sock, &from, &to, reset_timer)) != 0) { gettimeofday(&t2, &tz); if ((i = packet_ok(packet, cc, &from, &to, seq, &t1))) { reset_timer = 1; if (memcmp(&from.sin6_addr, &lastaddr, sizeof(from.sin6_addr))) { print(packet, cc, &from); memcpy(&lastaddr, &from.sin6_addr, sizeof(lastaddr)); } Printf(" %g ms", deltaT(&t1, &t2)); switch(i - 1) { case ICMP6_DST_UNREACH_NOPORT: ++got_there; break; case ICMP6_DST_UNREACH_NOROUTE: ++unreachable; Printf(" !N"); break; case ICMP6_DST_UNREACH_ADDR: ++unreachable; Printf(" !H"); break; case ICMP6_DST_UNREACH_ADMIN: ++unreachable; Printf(" !S"); break; } break; } else reset_timer = 0; } if (cc <= 0) Printf(" *"); (void) fflush(stdout); } putchar('\n'); if (got_there || (unreachable > 0 && unreachable >= nprobes-1)) exit(0); } return 0; }
static void /* not inline */ send_probe(int seq, int ttl) { struct opacket *op = outpacket; struct ip *ip = &op->ip; struct udphdr *up = &op->udp; int i; struct timezone tz; #if defined (DMP_TRACEROUTE_1) int dataSize = datalen -12; /* Minus some parameters' length in struct opacket*/ #endif ip->ip_off = 0; ip->ip_hl = sizeof(*ip) >> 2; ip->ip_p = IPPROTO_UDP; #if defined (DMP_TRACEROUTE_1) ip->ip_len = dataSize; #else ip->ip_len = datalen; #endif ip->ip_ttl = ttl; ip->ip_v = IPVERSION; ip->ip_id = htons(ident+seq); up->source = htons(ident); up->dest = htons(port+seq); #if defined (DMP_TRACEROUTE_1) up->len = htons((u_short)(dataSize - sizeof(struct ip))); #else up->len = htons((u_short)(datalen - sizeof(struct ip))); #endif up->check = 0; op->seq = seq; op->ttl = ttl; (void) gettimeofday(&op->tv, &tz); #if defined (DMP_TRACEROUTE_1) i = sendto(sndsock, (char *)outpacket, dataSize, 0, &whereto, sizeof(struct sockaddr)); #else i = sendto(sndsock, (char *)outpacket, datalen, 0, &whereto, sizeof(struct sockaddr)); #endif #if defined (DMP_TRACEROUTE_1) if (i < 0 || i != dataSize) { #else if (i < 0 || i != datalen) { #endif if (i<0) perror("sendto"); #if defined (DMP_TRACEROUTE_1) printf("traceroute: wrote %s %d chars, ret=%d\n", hostname, dataSize, i); #else printf("traceroute: wrote %s %d chars, ret=%d\n", hostname, datalen, i); #endif (void) fflush(stdout); } } int #ifndef CONFIG_TRACEROUTE main(int argc, char *argv[]) #else traceroute_main(int argc, char *argv[]) #endif { extern char *optarg; extern int optind; struct hostent *hp; struct sockaddr_in from, *to; int ch, i, on, probe, seq, tos, ttl; int options = 0; /* socket options */ char *source = 0; int nprobes = 3; #if defined(AEI_VDSL_CUSTOMER_QWEST_Q2000) int ttl_set_flag = 0; #endif #if defined(AEI_VDSL_CUSTOMER_NCS) int failcount = 0; #endif #if defined(DMP_TRACEROUTE_1) unsigned int repTime; char strRepTime[16]; TraceRouteDataMsgBody traceRouteInfo; memset(&traceRouteInfo, 0 , sizeof(TraceRouteDataMsgBody)); TraceRouteDataMsgBody *pTraceRouteInfo = &traceRouteInfo; cmsMsg_init(EID_TRACEROUTE, &msgHandle); #endif on = 1; seq = tos = 0; to = (struct sockaddr_in *)&whereto; while ((ch = getopt(argc, argv, "dm:np:q:rs:t:w:v")) != EOF) switch(ch) { case 'd': #ifdef CONFIG_FEATURE_TRACEROUTE_SO_DEBUG options |= SO_DEBUG; #endif break; case 'm': max_ttl = atoi(optarg); #if defined(AEI_VDSL_CUSTOMER_QWEST_Q2000) ttl_set_flag = 1; #endif if (max_ttl <= 1) bb_error_msg_and_die("max ttl must be >1."); break; case 'n': nflag++; break; case 'p': port = atoi(optarg); if (port < 1) bb_error_msg_and_die("port must be >0."); break; case 'q': nprobes = atoi(optarg); if (nprobes < 1) bb_error_msg_and_die("nprobes must be >0."); break; case 'r': options |= SO_DONTROUTE; break; case 's': /* * set the ip source address of the outbound * probe (e.g., on a multi-homed host). */ source = optarg; break; case 't': tos = atoi(optarg); if (tos < 0 || tos > 255) bb_error_msg_and_die("tos must be 0 to 255."); break; case 'v': #ifdef CONFIG_FEATURE_TRACEROUTE_VERBOSE verbose++; #endif break; case 'w': waittime = atoi(optarg); if (waittime <= 1) bb_error_msg_and_die("wait must be >1 sec."); break; default: bb_show_usage(); } argc -= optind; argv += optind; if (argc < 1) bb_show_usage(); setlinebuf (stdout); memset(&whereto, 0, sizeof(struct sockaddr)); #if defined(DMP_TRACEROUTE_1) hp = gethostbyname(*argv); if (*++argv) { requesterId = atoi(*argv); } if (hp == NULL) { AEI_sendTraceRouteEventMessage(pTraceRouteInfo, Error_CannotResolveHostName); bb_herror_msg_and_die("%s", *--argv); } #else hp = xgethostbyname(*argv); #endif to->sin_family = hp->h_addrtype; memcpy(&to->sin_addr, hp->h_addr, hp->h_length); hostname = (char *)hp->h_name; if (*++argv) datalen = atoi(*argv); #if defined (DMP_TRACEROUTE_1) if (datalen == 0) { datalen = 40; /*Default data length*/ } #endif if (datalen < 0 || datalen >= MAXPACKET - sizeof(struct opacket)) bb_error_msg_and_die("packet size must be 0 <= s < %d.", MAXPACKET - sizeof(struct opacket)); datalen += sizeof(struct opacket); outpacket = (struct opacket *)xmalloc((unsigned)datalen); memset(outpacket, 0, datalen); outpacket->ip.ip_dst = to->sin_addr; outpacket->ip.ip_tos = tos; #if defined(DMP_TRACEROUTE_1) outpacket->ip.ip_tos = (outpacket->ip.ip_tos)<<2; #endif outpacket->ip.ip_v = IPVERSION; outpacket->ip.ip_id = 0; ident = (getpid() & 0xffff) | 0x8000; if ((sndsock = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0) bb_perror_msg_and_die(bb_msg_can_not_create_raw_socket); s = create_icmp_socket(); #ifdef CONFIG_FEATURE_TRACEROUTE_SO_DEBUG if (options & SO_DEBUG) (void) setsockopt(s, SOL_SOCKET, SO_DEBUG, (char *)&on, sizeof(on)); #endif if (options & SO_DONTROUTE) (void) setsockopt(s, SOL_SOCKET, SO_DONTROUTE, (char *)&on, sizeof(on)); #ifdef SO_SNDBUF if (setsockopt(sndsock, SOL_SOCKET, SO_SNDBUF, (char *)&datalen, sizeof(datalen)) < 0) bb_perror_msg_and_die("SO_SNDBUF"); #endif #ifdef IP_HDRINCL if (setsockopt(sndsock, IPPROTO_IP, IP_HDRINCL, (char *)&on, sizeof(on)) < 0) bb_perror_msg_and_die("IP_HDRINCL"); #endif #ifdef CONFIG_FEATURE_TRACEROUTE_SO_DEBUG if (options & SO_DEBUG) (void) setsockopt(sndsock, SOL_SOCKET, SO_DEBUG, (char *)&on, sizeof(on)); #endif if (options & SO_DONTROUTE) (void) setsockopt(sndsock, SOL_SOCKET, SO_DONTROUTE, (char *)&on, sizeof(on)); if (source) { memset(&from, 0, sizeof(struct sockaddr)); from.sin_family = AF_INET; from.sin_addr.s_addr = inet_addr(source); if (from.sin_addr.s_addr == -1) bb_error_msg_and_die("unknown host %s", source); outpacket->ip.ip_src = from.sin_addr; #ifndef IP_HDRINCL if (bind(sndsock, (struct sockaddr *)&from, sizeof(from)) < 0) bb_perror_msg_and_die("bind"); #endif } fprintf(stderr, "traceroute to %s (%s)", hostname, inet_ntoa(to->sin_addr)); if (source) fprintf(stderr, " from %s", source); fprintf(stderr, ", %d hops max, %d byte packets\n", max_ttl, datalen); for (ttl = 1; ttl <= max_ttl; ++ttl) { u_long lastaddr = 0; int got_there = 0; int unreachable = 0; #if defined(DMP_TRACEROUTE_1) pTraceRouteInfo->routeHopsNumberOfEntries++; #endif printf("%2d ", ttl); for (probe = 0; probe < nprobes; ++probe) { int cc, reset_timer; struct timeval t1, t2; struct timezone tz; struct ip *ip; (void) gettimeofday(&t1, &tz); send_probe(++seq, ttl); reset_timer = 1; while ((cc = wait_for_reply(s, &from, reset_timer)) != 0) { (void) gettimeofday(&t2, &tz); if ((i = packet_ok(packet, cc, &from, seq))) { reset_timer = 1; if (from.sin_addr.s_addr != lastaddr) { print(packet, cc, &from); lastaddr = from.sin_addr.s_addr; } printf(" %g ms", deltaT(&t1, &t2)); #if defined(DMP_TRACEROUTE_1) struct icmp *hopIcp; int hopHlen; struct ip *hopIp; hopIp = (struct ip *) packet; hopHlen = hopIp->ip_hl << 2; hopIcp = (struct icmp *)(packet + hopHlen); repTime = ((int)(t2.tv_sec - t1.tv_sec) * 1000 + (int)(t2.tv_usec - t1.tv_usec) / 1000); sprintf(strRepTime, "%d,", repTime); pTraceRouteInfo->responseTime+=repTime; pTraceRouteInfo->routeHops[ttl-1].hopErrorCode = hopIcp->icmp_code; sprintf(pTraceRouteInfo->routeHops[ttl-1].hopHost, "%s", inet_ntoa(from.sin_addr)); sprintf(pTraceRouteInfo->routeHops[ttl-1].hopHostAddress, "%s", inet_ntoa(from.sin_addr)); strcat(pTraceRouteInfo->routeHops[ttl-1].hopRTTimes, strRepTime); #endif switch(i - 1) { case ICMP_UNREACH_PORT: ip = (struct ip *)packet; if (ip->ip_ttl <= 1) printf(" !"); ++got_there; break; case ICMP_UNREACH_NET: ++unreachable; printf(" !N"); break; case ICMP_UNREACH_HOST: ++unreachable; printf(" !H"); break; case ICMP_UNREACH_PROTOCOL: ++got_there; printf(" !P"); break; case ICMP_UNREACH_NEEDFRAG: ++unreachable; printf(" !F"); break; case ICMP_UNREACH_SRCFAIL: ++unreachable; printf(" !S"); break; } break; } else reset_timer = 0; } if (cc == 0) { #if defined(AEI_VDSL_CUSTOMER_NCS) failcount++; #endif printf(" *"); } #if defined(AEI_VDSL_CUSTOMER_NCS) else failcount=0; #endif (void) fflush(stdout); } putchar('\n'); if (got_there || (unreachable && unreachable >= nprobes-1)) { #if defined(DMP_TRACEROUTE_1) if (got_there) { AEI_sendTraceRouteEventMessage(pTraceRouteInfo, Complete); } else if (unreachable && unreachable >= nprobes-1) { AEI_sendTraceRouteEventMessage(pTraceRouteInfo, Error_MaxHopCountExceeded); } #endif return 0; } #if defined(AEI_VDSL_CUSTOMER_NCS) #if defined(AEI_VDSL_CUSTOMER_QWEST_Q2000) if (failcount >= nprobes*2&&ttl_set_flag==0) #else if (failcount >= nprobes*2) #endif { #if defined(DMP_TRACEROUTE_1) AEI_sendTraceRouteEventMessage(pTraceRouteInfo, Error_MaxHopCountExceeded); #endif return 0; } #endif } #if defined(DMP_TRACEROUTE_1) AEI_sendTraceRouteEventMessage(pTraceRouteInfo, Error_MaxHopCountExceeded); #endif return 0; }
IK::Scalar IK::calcDeltaP(void) { /* Convert the end effector's transformation into an affine matrix: */ AffineTransformation effectorTransformation=jointTransformations[effectorJointIndex]; /* Extract origin and axes from the effector transformation: */ Point effectorOrigin=effectorJointIndex==numJoints?effectorTransformation.transform(leafOffset):effectorTransformation.getOrigin(); Vector effectorX=effectorTransformation.getDirection(0); Vector effectorY=effectorTransformation.getDirection(1); Vector effectorZ=effectorTransformation.getDirection(2); /********************************************************************* Calculate the goal difference vector and the current residual. *********************************************************************/ /* Calculate the goal difference vector: */ Scalar deltaTv[6]; for(int i=0;i<3;++i) deltaT(i,0)=deltaTv[i]=-effectorOrigin[i]; deltaT(3,0)=deltaTv[3]=-effectorX[1]*orientationWeights[0]; deltaT(4,0)=deltaTv[4]=-effectorY[2]*orientationWeights[1]; deltaT(5,0)=deltaTv[5]=-effectorZ[0]*orientationWeights[2]; /* Calculate the residual: */ Scalar residual=Scalar(0); for(int i=0;i<6;++i) residual+=Math::sqr(deltaTv[i]); residual=Math::sqrt(residual); /* Multiply the private copy of the goal difference vector with the orientation weights again: */ for(int i=0;i<3;++i) deltaTv[3+i]*=orientationWeights[i]; /********************************************************************* Calculate parameter increments for each joint by calculating each joint's Jacobian entries, and multiplying them with the parameter difference vector. *********************************************************************/ /* Calculate the Jacobian matrix column for each joint: */ for(int columnIndex=0;columnIndex<effectorJointIndex;++columnIndex) { /* We calculate the Jacobian matrix column implicitly as well: */ Scalar dAngle=Scalar(0); /* Calculate joint offset and axis vector in world coordinates: */ Point gOffset=jointTransformations[columnIndex].transform(joints[columnIndex].offset); Vector gAxis=jointTransformations[columnIndex].transform(joints[columnIndex].axis); /******************************************************************* Calculate translational Jacobian entries. *******************************************************************/ /* Calculate dOrigin/dAngle: */ Vector dOrigin=Geometry::cross(gAxis,effectorOrigin-gOffset); for(int i=0;i<3;++i) dAngle+=dOrigin[i]*deltaTv[i]; /******************************************************************* Calculate rotational Jacobian entries. Conceptually, the rotational Jacobian entries have to be multiplied with the orientation weights here, since the goal difference vector is multiplied with them. As a simple optimization, we multiply the goal difference vector with the weights twice. *******************************************************************/ /* Calculate d(X*goalY)/dAngle: */ dAngle+=(gAxis[2]*effectorX[0]-gAxis[0]*effectorX[2])*deltaTv[3]; /* Calculate d(Y*goalZ)/dAngle: */ dAngle+=(gAxis[0]*effectorY[1]-gAxis[1]*effectorY[0])*deltaTv[4]; /* Calculate d(Z*goalX)/dAngle: */ dAngle+=(gAxis[1]*effectorZ[2]-gAxis[2]*effectorZ[1])*deltaTv[5]; deltaP(columnIndex,0)=dAngle; } return residual; }
//-------------------------------------------------------------------------- // //-------------------------------------------------------------------------- void AlgWspect::run() { #ifdef TIMING_ALG_W_SPECT int quark_b, quark_e; int meson_b, meson_m, meson_e, meson_bmp, meson_mmp, meson_emp; int nucleon_b, nucleon_m, nucleon_e; int total_b = clock(); int total_e; #endif CgArg cg = *cg_arg_p; char *fname = "run()"; VRB.Func(d_class_name,fname); // printf("in AlgWspect::run \n"); WspectOutput * output = (WspectOutput *)common_arg->results; // Set the Lattice pointer //------------------------------------------------------------------------ Lattice& lat = AlgLattice(); int src_slice = d_arg_p->aots_start; int src_slice_step = d_arg_p->aots_step; int src_slice_end = src_slice + src_slice_step * d_arg_p->aots_num; VRB.Result(d_class_name,fname,"%d %d %d \n",src_slice,src_slice_step, src_slice_end); for ( ; src_slice < src_slice_end; src_slice += src_slice_step) { // Calculate quark propagator //---------------------------------------------------------------------- // Ping: certainly more work here to be done about the desired // combinations of non-degenerate quarks. // Presumably, more arguments will have to be passed in. // One way: for three flavors, [100] means use only q1 // to caculate spectrum. // Also some care needed to get the scope (CTOR and DTOR) // of each quark propagator right. // const WspectQuark & q2 = q; // const WspectQuark & q3 = q; // Xiaodong & Thomas: // Modified to calculate also extended mesons // q1 is the usual propagator(no source operator), which can be // used to pass propagation direction and src_slice infomation // to spectrum class // there is a problem here --> check ! VRB.Result(d_class_name,fname,"prop_dir = %d , src_slice = %d \n",d_arg_p->prop_dir, src_slice); WspectHyperRectangle hyperRect(d_arg_p->prop_dir, src_slice); #ifdef TIMING_ALG_W_SPECT quark_b = clock(); #endif VRB.Result(d_class_name,fname,"created quark q1 \n"); // create local quark propagator WspectQuark q1(lat, output->cg, output->pbp, output->mid_point, output->a0_p, d_arg_p[0], cg,hyperRect); VRB.Result(d_class_name,fname,"finished quark q1 \n"); #ifdef TIMING_ALG_W_SPECT quark_e = clock(); #endif //Note: for ExtendedMesons, do only zero momentum projection WspectMomenta mom(hyperRect, q1.SourceCenter2(), d_arg_p->num_mom - 1); // mom.dumpData(); // Calculate LOCAL meson CORRELATOR // added control by Thomas and Xiaodong //---------------------------------------------------------------------- { #ifdef TIMING_ALG_W_SPECT meson_b = clock(); #endif if(d_arg_p->normal_mesons_on) { WspectMesons mes(q1, q1, hyperRect, mom); #if 0 q1.dumpData("qprop.dat"); #endif #ifdef TIMING_ALG_W_SPECT meson_m = clock(); #endif //write data to files mes.print(output); } #ifdef TIMING_ALG_W_SPECT meson_e = clock(); #endif } //end of normal mesons // Calculate <\Delta J^5 \bar q1 \gamma^5 q1> with middle point sink // changed //---------------------------------------------------------------------- if (lat.Fclass() == F_CLASS_DWF && output->mid_point) { #ifdef TIMING_ALG_W_SPECT meson_bmp = clock(); #endif WspectMesons mes(q1.Data_SP1(), q1.Data_SP2(), hyperRect, mom); #ifdef TIMING_ALG_W_SPECT meson_mmp = clock(); #endif mes.print_mp(output->mid_point); #ifdef TIMING_ALG_W_SPECT meson_emp = clock(); #endif } // Calculate nucleon and delta's //---------------------------------------------------------------------- if (d_arg_p->baryons_on) { { #ifdef TIMING_ALG_W_SPECT nucleon_b = clock(); #endif WspectBaryon nuc(q1, q1, q1, hyperRect, WspectBaryon::NUCLEON_CONSTI, WspectBaryon::NUCLEON_DIRAC); #ifdef TIMING_ALG_W_SPECT nucleon_m = clock(); #endif nuc.print(output->nucleon, output->fold); #ifdef TIMING_ALG_W_SPECT nucleon_e = clock(); #endif } { WspectBaryon nucPrime(q1, q1, q1, hyperRect, WspectBaryon::NUCLEON_CONSTI, WspectBaryon::UnitUnit); nucPrime.print(output->nucleon_prime, output->fold); } { WspectBaryon deltaX(q1, q1, q1, hyperRect, WspectBaryon::DELTA_CONSTI, WspectBaryon::DELTAX_DIRAC); deltaX.print(output->delta_x, output->fold); } { WspectBaryon deltaY(q1, q1, q1, hyperRect, WspectBaryon::DELTA_CONSTI, WspectBaryon::DELTAY_DIRAC); deltaY.print(output->delta_y, output->fold); } { WspectBaryon deltaZ(q1, q1, q1, hyperRect, WspectBaryon::DELTA_CONSTI, WspectBaryon::DELTAZ_DIRAC); deltaZ.print(output->delta_z, output->fold); } { WspectBaryon deltaT(q1, q1, q1, hyperRect, WspectBaryon::DELTA_CONSTI, WspectBaryon::DELTAT_DIRAC); deltaT.print(output->delta_t, output->fold); } } //end if(baryons_on) // Increment the counter d_counter += d_count_step; } // end of for(sc_slice,..) #ifdef TIMING_ALG_W_SPECT total_e = clock(); printf("Total: %d = [%d - %d]\n", total_e - total_b, total_e, total_b); printf("Quark: %d = [%d - %d]\n", quark_e - quark_b, quark_e, quark_b); printf("Meson: \t%d = [%d - %d] = \n\tcalc %d = [%d - %d] + \n\tprint %d = [%d - %d]\n", meson_e - meson_b, meson_e, meson_b, meson_m - meson_b, meson_m, meson_b, meson_e - meson_m, meson_e, meson_m); printf("Nucleon:\t%d = [%d - %d] = \n\tcalc %d = [%d - %d] + \n\tprint %d = [%d - %d]\n", nucleon_e - nucleon_b, nucleon_e, nucleon_b, nucleon_m - nucleon_b, nucleon_m, nucleon_b, nucleon_e - nucleon_m, nucleon_e, nucleon_m); #endif VRB.FuncEnd(d_class_name,fname); }
void mprof::CompactnessAnalysis::build( const struct MprofRecordAlloc * in_record, std::vector<size_t>::const_iterator in_orderBegin, std::vector<size_t>::const_iterator in_orderEnd ) { std::list<size_t> retryList; size_t lastIndex = *in_orderBegin; for( std::vector<size_t>::const_iterator orderItr = in_orderBegin; orderItr != in_orderEnd; ++orderItr ) { if( addRecord( in_record, *orderItr ) ) { lastIndex = *orderItr; //keep retrying--FIXME: handle realloc better( damn you realloc ) bool progress; do { progress = false; for( std::list<size_t>::iterator retryItr = retryList.begin(); retryItr != retryList.end(); ) { if( addRecord( in_record, *retryItr ) ) { if( olderThan( in_record + *retryItr, in_record + lastIndex ) ) { std::cerr << "Info: mprof::CompactnessAnalysis::build: detected scheduler/timestamp inversion at index '" << *orderItr << "' of " << deltaT( in_record + *retryItr, in_record + lastIndex ) << " microseconds" << std::endl; } lastIndex = *retryItr; retryItr = retryList.erase( retryItr ); progress = true; } else { ++retryItr; } } } while( progress ); } else { if( in_record[ *orderItr ].header.mode == MPROF_MODE_REALLOC ) { //std::cerr << "Info: mprof::CompactnessAnalysis::build: realloc record at index '" << *orderItr << "' may have been paritally processed." << std::endl; } retryList.push_back( *orderItr ); } } for( std::list<size_t>::iterator retryItr = retryList.begin(); retryItr != retryList.end(); retryItr = retryList.erase( retryItr ) ) { size_t address, size; if( getAlloc( in_record + *retryItr, address, size ) ) { std::cerr << "Warning: mprof::CompactnessAnalysis::build: found double alloc at index '" << *retryItr << "' for address '" << std::hex << address << std::dec << "'" << std::endl; } if( getFree( in_record + *retryItr, address ) ) { std::cerr << "Warning: mprof::CompactnessAnalysis::build: found unmatched free at index '" << *retryItr << "' for address '" << std::hex << address << std::dec << "'" << std::endl; } } }
int main(int argc, char *argv[]) { int mib[4] = { CTL_NET, PF_INET6, IPPROTO_IPV6, IPV6CTL_DEFHLIM }; char hbuf[NI_MAXHOST], src0[NI_MAXHOST], *ep; int ch, i, on = 1, seq, rcvcmsglen, error, minlen; struct addrinfo hints, *res; static u_char *rcvcmsgbuf; u_long probe, hops, lport; struct hostent *hp; size_t size; uid_t uid; /* * Receive ICMP */ if ((rcvsock = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6)) < 0) { perror("socket(ICMPv6)"); exit(5); } /* revoke privs */ uid = getuid(); if (setresuid(uid, uid, uid) == -1) err(1, "setresuid"); size = sizeof(i); (void) sysctl(mib, sizeof(mib)/sizeof(mib[0]), &i, &size, NULL, 0); max_hops = i; /* specify to tell receiving interface */ if (setsockopt(rcvsock, IPPROTO_IPV6, IPV6_RECVPKTINFO, &on, sizeof(on)) < 0) err(1, "setsockopt(IPV6_RECVPKTINFO)"); /* specify to tell value of hoplimit field of received IP6 hdr */ if (setsockopt(rcvsock, IPPROTO_IPV6, IPV6_RECVHOPLIMIT, &on, sizeof(on)) < 0) err(1, "setsockopt(IPV6_RECVHOPLIMIT)"); seq = 0; while ((ch = getopt(argc, argv, "df:g:Ilm:np:q:rs:w:v")) != -1) switch (ch) { case 'd': options |= SO_DEBUG; break; case 'f': ep = NULL; errno = 0; first_hop = strtoul(optarg, &ep, 0); if (errno || !*optarg || *ep|| first_hop > 255) { fprintf(stderr, "traceroute6: invalid min hoplimit.\n"); exit(1); } break; case 'g': hp = getipnodebyname(optarg, AF_INET6, 0, &h_errno); if (hp == NULL) { fprintf(stderr, "traceroute6: unknown host %s\n", optarg); exit(1); } if (rth == NULL) { /* * XXX: We can't detect the number of * intermediate nodes yet. */ if ((rth = inet6_rth_init((void *)rtbuf, sizeof(rtbuf), IPV6_RTHDR_TYPE_0, 0)) == NULL) { fprintf(stderr, "inet6_rth_init failed.\n"); exit(1); } } if (inet6_rth_add((void *)rth, (struct in6_addr *)hp->h_addr)) { fprintf(stderr, "inet6_rth_add failed for %s\n", optarg); exit(1); } freehostent(hp); break; case 'I': useicmp++; ident = htons(getpid() & 0xffff); /* same as ping6 */ break; case 'l': lflag++; break; case 'm': ep = NULL; errno = 0; max_hops = strtoul(optarg, &ep, 0); if (errno || !*optarg || *ep || max_hops > 255) { fprintf(stderr, "traceroute6: invalid max hoplimit.\n"); exit(1); } break; case 'n': nflag++; break; case 'p': ep = NULL; errno = 0; lport = strtoul(optarg, &ep, 0); if (errno || !*optarg || *ep) { fprintf(stderr, "traceroute6: invalid port.\n"); exit(1); } if (lport == 0 || lport != (lport & 0xffff)) { fprintf(stderr, "traceroute6: port out of range.\n"); exit(1); } port = lport & 0xffff; break; case 'q': ep = NULL; errno = 0; nprobes = strtoul(optarg, &ep, 0); if (errno || !*optarg || *ep) { fprintf(stderr, "traceroute6: invalid nprobes.\n"); exit(1); } if (nprobes < 1) { fprintf(stderr, "traceroute6: nprobes must be >0.\n"); exit(1); } break; case 'r': options |= SO_DONTROUTE; break; case 's': /* * set the ip source address of the outbound * probe (e.g., on a multi-homed host). */ source = optarg; break; case 'v': verbose++; break; case 'w': ep = NULL; errno = 0; waittime = strtoul(optarg, &ep, 0); if (errno || !*optarg || *ep) { fprintf(stderr, "traceroute6: invalid wait time.\n"); exit(1); } if (waittime <= 1) { fprintf(stderr, "traceroute6: wait must be >1 sec.\n"); exit(1); } break; default: usage(); } argc -= optind; argv += optind; if (max_hops < first_hop) { fprintf(stderr, "traceroute6: max hoplimit must be larger than first hoplimit.\n"); exit(1); } if (argc < 1 || argc > 2) usage(); #if 1 setvbuf(stdout, NULL, _IOLBF, BUFSIZ); #else setlinebuf(stdout); #endif memset(&hints, 0, sizeof(hints)); hints.ai_family = PF_INET6; hints.ai_socktype = SOCK_RAW; hints.ai_protocol = IPPROTO_ICMPV6; hints.ai_flags = AI_CANONNAME; error = getaddrinfo(*argv, NULL, &hints, &res); if (error) { fprintf(stderr, "traceroute6: %s\n", gai_strerror(error)); exit(1); } if (res->ai_addrlen != sizeof(Dst)) { fprintf(stderr, "traceroute6: size of sockaddr mismatch\n"); exit(1); } memcpy(&Dst, res->ai_addr, res->ai_addrlen); hostname = res->ai_canonname ? strdup(res->ai_canonname) : *argv; if (!hostname) { fprintf(stderr, "traceroute6: not enough core\n"); exit(1); } if (res->ai_next) { if (getnameinfo(res->ai_addr, res->ai_addrlen, hbuf, sizeof(hbuf), NULL, 0, NI_NUMERICHOST) != 0) strlcpy(hbuf, "?", sizeof(hbuf)); fprintf(stderr, "traceroute6: Warning: %s has multiple " "addresses; using %s\n", hostname, hbuf); } if (*++argv) { ep = NULL; errno = 0; datalen = strtoul(*argv, &ep, 0); if (errno || !*argv || *ep) { fprintf(stderr, "traceroute6: invalid packet length.\n"); exit(1); } } if (useicmp) minlen = ICMP6ECHOLEN + sizeof(struct tv32); else minlen = sizeof(struct opacket); if (datalen < minlen) datalen = minlen; else if (datalen >= MAXPACKET) { fprintf(stderr, "traceroute6: packet size must be %d <= s < %ld.\n", minlen, (long)MAXPACKET); exit(1); } outpacket = (struct opacket *)malloc((unsigned)datalen); if (!outpacket) { perror("malloc"); exit(1); } (void) bzero((char *)outpacket, datalen); /* initialize msghdr for receiving packets */ rcviov[0].iov_base = (caddr_t)packet; rcviov[0].iov_len = sizeof(packet); rcvmhdr.msg_name = (caddr_t)&Rcv; rcvmhdr.msg_namelen = sizeof(Rcv); rcvmhdr.msg_iov = rcviov; rcvmhdr.msg_iovlen = 1; rcvcmsglen = CMSG_SPACE(sizeof(struct in6_pktinfo)) + CMSG_SPACE(sizeof(int)); if ((rcvcmsgbuf = malloc(rcvcmsglen)) == NULL) { fprintf(stderr, "traceroute6: malloc failed\n"); exit(1); } rcvmhdr.msg_control = (caddr_t) rcvcmsgbuf; rcvmhdr.msg_controllen = rcvcmsglen; if (options & SO_DEBUG) (void) setsockopt(rcvsock, SOL_SOCKET, SO_DEBUG, (char *)&on, sizeof(on)); if (options & SO_DONTROUTE) (void) setsockopt(rcvsock, SOL_SOCKET, SO_DONTROUTE, (char *)&on, sizeof(on)); /* * Send UDP or ICMP */ if (useicmp) { sndsock = rcvsock; } else { if ((sndsock = socket(AF_INET6, SOCK_DGRAM, 0)) < 0) { perror("socket(SOCK_DGRAM)"); exit(5); } } #ifdef SO_SNDBUF i = datalen; if (setsockopt(sndsock, SOL_SOCKET, SO_SNDBUF, (char *)&i, sizeof(i)) < 0) { perror("setsockopt(SO_SNDBUF)"); exit(6); } #endif /* SO_SNDBUF */ if (options & SO_DEBUG) (void) setsockopt(sndsock, SOL_SOCKET, SO_DEBUG, (char *)&on, sizeof(on)); if (options & SO_DONTROUTE) (void) setsockopt(sndsock, SOL_SOCKET, SO_DONTROUTE, (char *)&on, sizeof(on)); if (rth) {/* XXX: there is no library to finalize the header... */ rth->ip6r_len = rth->ip6r_segleft * 2; if (setsockopt(sndsock, IPPROTO_IPV6, IPV6_RTHDR, (void *)rth, (rth->ip6r_len + 1) << 3)) { fprintf(stderr, "setsockopt(IPV6_RTHDR): %s\n", strerror(errno)); exit(1); } } /* * Source selection */ bzero(&Src, sizeof(Src)); if (source) { struct addrinfo hints, *res; int error; memset(&hints, 0, sizeof(hints)); hints.ai_family = AF_INET6; hints.ai_socktype = SOCK_DGRAM; /*dummy*/ hints.ai_flags = AI_NUMERICHOST; error = getaddrinfo(source, "0", &hints, &res); if (error) { printf("traceroute6: %s: %s\n", source, gai_strerror(error)); exit(1); } if (res->ai_addrlen > sizeof(Src)) { printf("traceroute6: %s: %s\n", source, gai_strerror(error)); exit(1); } memcpy(&Src, res->ai_addr, res->ai_addrlen); freeaddrinfo(res); } else { struct sockaddr_in6 Nxt; int dummy; socklen_t len; Nxt = Dst; Nxt.sin6_port = htons(DUMMY_PORT); if (cmsg != NULL) bcopy(inet6_rthdr_getaddr(cmsg, 1), &Nxt.sin6_addr, sizeof(Nxt.sin6_addr)); if ((dummy = socket(AF_INET6, SOCK_DGRAM, 0)) < 0) { perror("socket"); exit(1); } if (connect(dummy, (struct sockaddr *)&Nxt, Nxt.sin6_len) < 0) { perror("connect"); exit(1); } len = sizeof(Src); if (getsockname(dummy, (struct sockaddr *)&Src, &len) < 0) { perror("getsockname"); exit(1); } if (getnameinfo((struct sockaddr *)&Src, Src.sin6_len, src0, sizeof(src0), NULL, 0, NI_NUMERICHOST)) { fprintf(stderr, "getnameinfo failed for source\n"); exit(1); } source = src0; close(dummy); } Src.sin6_port = htons(0); if (bind(sndsock, (struct sockaddr *)&Src, Src.sin6_len) < 0) { perror("bind"); exit(1); } { socklen_t len; len = sizeof(Src); if (getsockname(sndsock, (struct sockaddr *)&Src, &len) < 0) { perror("getsockname"); exit(1); } srcport = ntohs(Src.sin6_port); } /* * Message to users */ if (getnameinfo((struct sockaddr *)&Dst, Dst.sin6_len, hbuf, sizeof(hbuf), NULL, 0, NI_NUMERICHOST)) strlcpy(hbuf, "(invalid)", sizeof(hbuf)); fprintf(stderr, "traceroute6"); fprintf(stderr, " to %s (%s)", hostname, hbuf); if (source) fprintf(stderr, " from %s", source); fprintf(stderr, ", %lu hops max, %lu byte packets\n", max_hops, datalen); (void) fflush(stderr); if (first_hop > 1) printf("Skipping %lu intermediate hops\n", first_hop - 1); /* * Main loop */ for (hops = first_hop; hops <= max_hops; ++hops) { struct in6_addr lastaddr; int got_there = 0; int unreachable = 0; printf("%2lu ", hops); bzero(&lastaddr, sizeof(lastaddr)); for (probe = 0; probe < nprobes; ++probe) { int cc; struct timeval t1, t2; (void) gettimeofday(&t1, NULL); send_probe(++seq, hops); while ((cc = wait_for_reply(rcvsock, &rcvmhdr))) { (void) gettimeofday(&t2, NULL); if ((i = packet_ok(&rcvmhdr, cc, seq))) { if (!IN6_ARE_ADDR_EQUAL(&Rcv.sin6_addr, &lastaddr)) { print(&rcvmhdr, cc); lastaddr = Rcv.sin6_addr; } printf(" %g ms", deltaT(&t1, &t2)); switch (i - 1) { case ICMP6_DST_UNREACH_NOROUTE: ++unreachable; printf(" !N"); break; case ICMP6_DST_UNREACH_ADMIN: ++unreachable; printf(" !P"); break; case ICMP6_DST_UNREACH_NOTNEIGHBOR: ++unreachable; printf(" !S"); break; case ICMP6_DST_UNREACH_ADDR: ++unreachable; printf(" !A"); break; case ICMP6_DST_UNREACH_NOPORT: if (rcvhlim >= 0 && rcvhlim <= 1) printf(" !"); ++got_there; break; } break; } } if (cc == 0) printf(" *"); (void) fflush(stdout); } putchar('\n'); if (got_there || (unreachable > 0 && unreachable >= ((nprobes + 1) / 2))) { exit(0); } } exit(0); }
traceroute_main(int argc, char *argv[]) #endif { extern char *optarg; extern int optind; struct hostent *hp; struct sockaddr_in from, *to; int ch, i, on, probe, seq, tos, ttl; int options = 0; /* socket options */ char *source = 0; int nprobes = 3; on = 1; seq = tos = 0; to = (struct sockaddr_in *)&whereto; while ((ch = getopt(argc, argv, "dm:np:q:rs:t:w:v")) != EOF) switch(ch) { case 'd': #ifdef CONFIG_FEATURE_TRACEROUTE_SO_DEBUG options |= SO_DEBUG; #endif break; case 'm': max_ttl = atoi(optarg); if (max_ttl <= 1) bb_error_msg_and_die("max ttl must be >1."); break; case 'n': nflag++; break; case 'p': port = atoi(optarg); if (port < 1) bb_error_msg_and_die("port must be >0."); break; case 'q': nprobes = atoi(optarg); if (nprobes < 1) bb_error_msg_and_die("nprobes must be >0."); break; case 'r': options |= SO_DONTROUTE; break; case 's': /* * set the ip source address of the outbound * probe (e.g., on a multi-homed host). */ source = optarg; break; case 't': tos = atoi(optarg); if (tos < 0 || tos > 255) bb_error_msg_and_die("tos must be 0 to 255."); break; case 'v': #ifdef CONFIG_FEATURE_TRACEROUTE_VERBOSE verbose++; #endif break; case 'w': waittime = atoi(optarg); if (waittime <= 1) bb_error_msg_and_die("wait must be >1 sec."); break; default: bb_show_usage(); } argc -= optind; argv += optind; if (argc < 1) bb_show_usage(); setlinebuf (stdout); memset(&whereto, 0, sizeof(struct sockaddr)); hp = xgethostbyname(*argv); to->sin_family = hp->h_addrtype; memcpy(&to->sin_addr, hp->h_addr, hp->h_length); hostname = (char *)hp->h_name; if (*++argv) datalen = atoi(*argv); if (datalen < 0 || datalen >= MAXPACKET - sizeof(struct opacket)) bb_error_msg_and_die("packet size must be 0 <= s < %d.", MAXPACKET - sizeof(struct opacket)); datalen += sizeof(struct opacket); outpacket = (struct opacket *)xmalloc((unsigned)datalen); memset(outpacket, 0, datalen); outpacket->ip.ip_dst = to->sin_addr; outpacket->ip.ip_tos = tos; outpacket->ip.ip_v = IPVERSION; outpacket->ip.ip_id = 0; ident = (getpid() & 0xffff) | 0x8000; if ((sndsock = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0) bb_perror_msg_and_die(bb_msg_can_not_create_raw_socket); s = create_icmp_socket(); #ifdef CONFIG_FEATURE_TRACEROUTE_SO_DEBUG if (options & SO_DEBUG) (void) setsockopt(s, SOL_SOCKET, SO_DEBUG, (char *)&on, sizeof(on)); #endif if (options & SO_DONTROUTE) (void) setsockopt(s, SOL_SOCKET, SO_DONTROUTE, (char *)&on, sizeof(on)); #ifdef SO_SNDBUF if (setsockopt(sndsock, SOL_SOCKET, SO_SNDBUF, (char *)&datalen, sizeof(datalen)) < 0) bb_perror_msg_and_die("SO_SNDBUF"); #endif #ifdef IP_HDRINCL if (setsockopt(sndsock, IPPROTO_IP, IP_HDRINCL, (char *)&on, sizeof(on)) < 0) bb_perror_msg_and_die("IP_HDRINCL"); #endif #ifdef CONFIG_FEATURE_TRACEROUTE_SO_DEBUG if (options & SO_DEBUG) (void) setsockopt(sndsock, SOL_SOCKET, SO_DEBUG, (char *)&on, sizeof(on)); #endif if (options & SO_DONTROUTE) (void) setsockopt(sndsock, SOL_SOCKET, SO_DONTROUTE, (char *)&on, sizeof(on)); if (source) { memset(&from, 0, sizeof(struct sockaddr)); from.sin_family = AF_INET; from.sin_addr.s_addr = inet_addr(source); if (from.sin_addr.s_addr == -1) bb_error_msg_and_die("unknown host %s", source); outpacket->ip.ip_src = from.sin_addr; #ifndef IP_HDRINCL if (bind(sndsock, (struct sockaddr *)&from, sizeof(from)) < 0) bb_perror_msg_and_die("bind"); #endif } fprintf(stderr, "traceroute to %s (%s)", hostname, inet_ntoa(to->sin_addr)); if (source) fprintf(stderr, " from %s", source); fprintf(stderr, ", %d hops max, %d byte packets\n", max_ttl, datalen); for (ttl = 1; ttl <= max_ttl; ++ttl) { u_long lastaddr = 0; int got_there = 0; int unreachable = 0; printf("%2d ", ttl); for (probe = 0; probe < nprobes; ++probe) { int cc, reset_timer; struct timeval t1, t2; struct timezone tz; struct ip *ip; (void) gettimeofday(&t1, &tz); send_probe(++seq, ttl); reset_timer = 1; while ((cc = wait_for_reply(s, &from, reset_timer)) != 0) { (void) gettimeofday(&t2, &tz); if ((i = packet_ok(packet, cc, &from, seq))) { reset_timer = 1; if (from.sin_addr.s_addr != lastaddr) { print(packet, cc, &from); lastaddr = from.sin_addr.s_addr; } printf(" %g ms", deltaT(&t1, &t2)); switch(i - 1) { case ICMP_UNREACH_PORT: ip = (struct ip *)packet; if (ip->ip_ttl <= 1) printf(" !"); ++got_there; break; case ICMP_UNREACH_NET: ++unreachable; printf(" !N"); break; case ICMP_UNREACH_HOST: ++unreachable; printf(" !H"); break; case ICMP_UNREACH_PROTOCOL: ++got_there; printf(" !P"); break; case ICMP_UNREACH_NEEDFRAG: ++unreachable; printf(" !F"); break; case ICMP_UNREACH_SRCFAIL: ++unreachable; printf(" !S"); break; } break; } else reset_timer = 0; } if (cc == 0) printf(" *"); (void) fflush(stdout); } putchar('\n'); if (got_there || unreachable >= nprobes-1) return 0; } return 0; }
int recv_message(char *buf, int size, int inv_trans, struct sipsak_delay *sd, struct sipsak_sr_time *srt, struct sipsak_counter *count, struct sipsak_con_data *cd, struct sipsak_regexp *reg) { int ret = 0; int sock = 0; double tmp_delay; #ifdef HAVE_INET_NTOP struct sockaddr_in peer_adr; socklen_t psize = sizeof(peer_adr); #endif #ifdef RAW_SUPPORT struct sockaddr_in faddr; struct ip *r_ip_hdr, *s_ip_hdr; struct icmp *icmp_hdr; struct udphdr *udp_hdr; size_t r_ip_len, s_ip_len, icmp_len; int srcport, dstport; unsigned int flen; #endif if (cd->buf_tmp) { buf = cd->buf_tmp; size = size - cd->buf_tmp_size; } sock = check_for_message(buf, size, cd, srt, count, sd); if (sock <= 1) { return -1; } #ifdef RAW_SUPPORT if (sock != rawsock) { #else else { #endif check_socket_error(sock, size); ret = recvfrom(sock, buf, size, 0, NULL, 0); } #ifdef RAW_SUPPORT else { /* lets check if the ICMP message matches with our sent packet */ flen = sizeof(faddr); memset(&faddr, 0, sizeof(struct sockaddr)); ret = recvfrom(rawsock, buf, size, 0, (struct sockaddr *)&faddr, &flen); if (ret == -1) { perror("error while trying to read from icmp raw socket"); exit_code(2); } r_ip_hdr = (struct ip *) buf; r_ip_len = r_ip_hdr->ip_hl << 2; icmp_hdr = (struct icmp *) (buf + r_ip_len); icmp_len = ret - r_ip_len; if (icmp_len < 8) { if (verbose > 1) printf(": ignoring (ICMP header length below 8 bytes)\n"); return -2; } else if (icmp_len < 36) { if (verbose > 1) printf(": ignoring (ICMP message too short to contain IP and UDP header)\n"); return -2; } s_ip_hdr = (struct ip *) ((char *)icmp_hdr + 8); s_ip_len = s_ip_hdr->ip_hl << 2; if (s_ip_hdr->ip_p == IPPROTO_UDP) { udp_hdr = (struct udphdr *) ((char *)s_ip_hdr + s_ip_len); srcport = ntohs(udp_hdr->uh_sport); dstport = ntohs(udp_hdr->uh_dport); #ifdef DEBUG printf("\nlport: %i, rport: %i\n", lport, rport); #endif if ((srcport == lport) && (dstport == rport)) { printf(" (type: %u, code: %u)", icmp_hdr->icmp_type, icmp_hdr->icmp_code); #ifdef HAVE_INET_NTOP if (inet_ntop(AF_INET, &faddr.sin_addr, &source_dot[0], INET_ADDRSTRLEN) != NULL) printf(": from %s\n", source_dot); else printf("\n"); #else printf("\n"); #endif // HAVE_INET_NTOP exit_code(3); } else { if (verbose > 2) printf(": ignoring (ICMP message does not match used ports)\n"); return -2; } } else { if (verbose > 1) printf(": ignoring (ICMP data is not a UDP packet)\n"); return -2; } } #endif // RAW_SUPPORT if (ret > 0) { *(buf+ ret) = '\0'; if (transport != SIP_UDP_TRANSPORT) { if (verbose > 0) printf("\nchecking message for completness...\n"); if (complete_mes(buf, ret) == 1) { cd->buf_tmp = NULL; ret += cd->buf_tmp_size; cd->buf_tmp_size = 0; } else { if (cd->buf_tmp) { cd->buf_tmp += ret; cd->buf_tmp_size += ret; } else { cd->buf_tmp = buf + ret; cd->buf_tmp_size = ret; } cd->dontsend = 1; ret = -1; } } /* store the biggest delay if one occured */ if (srt->delaytime.tv_sec != 0) { tmp_delay = deltaT(&(srt->delaytime), &(srt->recvtime)); if (tmp_delay > sd->big_delay) sd->big_delay = tmp_delay; if ((sd->small_delay == 0) || (tmp_delay < sd->small_delay)) sd->small_delay = tmp_delay; srt->delaytime.tv_sec = 0; srt->delaytime.tv_usec = 0; } if (timing > 0) { tmp_delay = deltaT(&(srt->sendtime), &(srt->recvtime)); if (tmp_delay > sd->big_delay) sd->big_delay = tmp_delay; if ((sd->small_delay == 0) || (tmp_delay < sd->small_delay)) sd->small_delay = tmp_delay; sd->all_delay += tmp_delay; } #ifdef HAVE_INET_NTOP if ((verbose > 2) && (getpeername(sock, (struct sockaddr *)&peer_adr, &psize) == 0) && (inet_ntop(peer_adr.sin_family, &peer_adr.sin_addr, &source_dot[0], INET_ADDRSTRLEN) != NULL)) { printf("\nreceived from: %s:%s:%i\n", transport_str, source_dot, ntohs(peer_adr.sin_port)); } else if (verbose > 1 && trace == 0 && usrloc == 0) printf(":\n"); #else if (trace == 0 && usrloc == 0) printf(":\n"); #endif // HAVE_INET_NTOP if (!inv_trans && ret > 0 && (regexec(&(reg->proexp), buf, 0, 0, 0) != REG_NOERROR)) { sd->retryAfter = SIP_T1; } } else { check_socket_error(sock, size); printf("nothing received, select returned error\n"); exit_code(2); } return ret; }
int check_for_message(char *recv, int size, struct sipsak_con_data *cd, struct sipsak_sr_time *srt, struct sipsak_counter *count, struct sipsak_delay *sd) { fd_set fd; struct timezone tz; struct timeval tv; double senddiff; int ret = 0; if (cd->dontrecv == 0) { /* set the timeout and wait for a response */ tv.tv_sec = sd->retryAfter/1000; tv.tv_usec = (sd->retryAfter % 1000) * 1000; FD_ZERO(&fd); if (cd->usock != -1) FD_SET(cd->usock, &fd); if (cd->csock != -1) FD_SET(cd->csock, &fd); #ifdef RAW_SUPPORT if (rawsock != -1) FD_SET(rawsock, &fd); #endif ret = select(FD_SETSIZE, &fd, NULL, NULL, &tv); (void)gettimeofday(&(srt->recvtime), &tz); } else { cd->dontrecv = 0; } /* store the time of our first send */ if (count->send_counter==1) { memcpy(&(srt->firstsendt), &(srt->sendtime), sizeof(struct timeval)); } if (sd->retryAfter == SIP_T1) { memcpy(&(srt->starttime), &(srt->sendtime), sizeof(struct timeval)); } if (ret == 0) { /* lets see if we at least received an icmp error */ if (cd->csock == -1) check_socket_error(cd->usock, size); else check_socket_error(cd->csock, size); /* printout that we did not received anything */ if (trace == 1) { printf("%i: timeout after %i ms\n", namebeg, sd->retryAfter); } else if (usrloc == 1||invite == 1||message == 1) { printf("timeout after %i ms\n", sd->retryAfter); } else if (verbose>0) printf("** timeout after %i ms**\n", sd->retryAfter); if (randtrash == 1) { printf("did not get a response on this request:\n%s\n", req); if (cseq_counter < nameend) { if (count->randretrys == 2) { printf("sended the following message three " "times without getting a response:\n%s\n" "give up further retransmissions...\n", req); exit_code(3); } else { printf("resending it without additional " "random changes...\n\n"); (count->randretrys)++; } } } senddiff = deltaT(&(srt->starttime), &(srt->recvtime)); if (senddiff > inv_final) { if (timing == 0) { if (verbose>0) printf("*** giving up, no final response after %.3f ms\n", senddiff); exit_code(3); } else { timing--; count->run++; sd->all_delay += senddiff; sd->big_delay = senddiff; new_transaction(req); sd->retryAfter = SIP_T1; if (timing == 0) { printf("%.3f/%.3f/%.3f ms\n", sd->small_delay, sd->all_delay / count->run, sd->big_delay); exit_code(3); } } } else { /* set retry time according to RFC3261 */ if ((inv_trans) || (sd->retryAfter *2 < SIP_T2)) { sd->retryAfter = sd->retryAfter * 2; } else { sd->retryAfter = SIP_T2; } } (count->retrans_s_c)++; if (srt->delaytime.tv_sec == 0) memcpy(&(srt->delaytime), &(srt->sendtime), sizeof(struct timeval)); /* if we did not exit until here lets try another send */ return -1; } else if ( ret == -1 ) { perror("select error"); exit_code(2); } else if (((cd->usock != -1) && FD_ISSET(cd->usock, &fd)) || ((cd->csock != -1) && FD_ISSET(cd->csock, &fd))) { if ((cd->usock != -1) && FD_ISSET(cd->usock, &fd)) ret = cd->usock; else if ((cd->csock != -1) && FD_ISSET(cd->csock, &fd)) ret = cd->csock; else { printf("unable to determine the socket which received something\n"); exit_code(2); } /* no timeout, no error ... something has happened :-) */ if (trace == 0 && usrloc ==0 && invite == 0 && message == 0 && randtrash == 0 && (verbose > 1)) printf ("\nmessage received"); } #ifdef RAW_SUPPORT else if ((rawsock != -1) && FD_ISSET(rawsock, &fd)) { if (verbose > 1) printf("\nreceived ICMP message"); ret = rawsock; } #endif else { printf("\nselect returned succesfuly, nothing received\n"); return -1; } return ret; }
void ReliefGeneration::CompressHeightField(std::vector<double>& heightField, int resX, int resY) { double bbScale = 2; double alpha = bbScale * 300.0; double threthold = bbScale * 0.05; int vertNum = (resX + 1) * (resY + 1); std::vector<MagicMath::Vector3> DeltaVector(vertNum); for (int xid = 0; xid < resX; xid++) { for (int yid = 0; yid < resY; yid++) { int index = xid * (resY + 1) + yid; MagicMath::Vector3 deltaT(0, 0, 0); deltaT[0] = heightField.at(index + resY + 1) - heightField.at(index); deltaT[1] = heightField.at(index + 1) - heightField.at(index); double deltaMag = deltaT.Normalise(); if (deltaMag > threthold) { deltaMag = 0; } else { deltaMag = log(1 + alpha * deltaMag) / alpha; } DeltaVector.at(index) = deltaT * deltaMag; } } std::vector<double> LaplaceField(vertNum); for (int xid = 1; xid < resX; xid++) { for (int yid = 1; yid < resY; yid++) { int index = xid * (resY + 1) + yid; LaplaceField.at(index) = DeltaVector.at(index)[0] - DeltaVector.at(index - resY - 1)[0] + DeltaVector.at(index)[1] - DeltaVector.at(index - 1)[1]; } } DebugLog << "Relief: Construct Matrix" << std::endl; std::vector< Eigen::Triplet<double> > tripletList; Eigen::VectorXd b(vertNum, 1); for (int xid = 0; xid < resX + 1; xid++) { for (int yid = 0; yid < resY + 1; yid++) { int index = xid * (resY + 1) + yid; if (xid == 0 || xid == resX || yid == 0 || yid == resY) { tripletList.push_back( Eigen::Triplet<double>(index, index, 1) ); b(index) = 0; } else { tripletList.push_back( Eigen::Triplet<double>(index, index, -4.0) ); tripletList.push_back( Eigen::Triplet<double>(index, index + 1, 1.0) ); tripletList.push_back( Eigen::Triplet<double>(index, index - 1, 1.0) ); tripletList.push_back( Eigen::Triplet<double>(index, index + resY + 1, 1.0) ); tripletList.push_back( Eigen::Triplet<double>(index, index - resY - 1, 1.0) ); b(index) = LaplaceField.at(index); } } } DebugLog << "Relief: Solve Matrix" << std::endl; Eigen::SparseMatrix<double, Eigen::ColMajor> matA(vertNum,vertNum); matA.setFromTriplets(tripletList.begin(), tripletList.end()); Eigen::SparseLU<Eigen::SparseMatrix<double, Eigen::ColMajor> > solver; solver.compute(matA); if(solver.info()!= Eigen::Success) { DebugLog << "Relief: SuperLU Failed" << std::endl; } Eigen::VectorXd res = solver.solve(b); //Copy results for (int i = 0; i < vertNum; i++) { heightField.at(i) = res(i); } }
// this is just a workbench. most of the stuff here will go into the Frontend class. int main(int argc, char **argv) { ros::init(argc, argv, "okvis_node_synchronous"); google::InitGoogleLogging(argv[0]); FLAGS_stderrthreshold = 0; // INFO: 0, WARNING: 1, ERROR: 2, FATAL: 3 FLAGS_colorlogtostderr = 1; if (argc != 3 && argc != 4) { LOG(ERROR) << "Usage: ./" << argv[0] << " configuration-yaml-file bag-to-read-from [skip-first-seconds]"; return -1; } okvis::Duration deltaT(0.0); if (argc == 4) { deltaT = okvis::Duration(atof(argv[3])); } // set up the node ros::NodeHandle nh("okvis_node"); // publisher okvis::Publisher publisher(nh); // read configuration file std::string configFilename(argv[1]); okvis::RosParametersReader vio_parameters_reader(configFilename); okvis::VioParameters parameters; vio_parameters_reader.getParameters(parameters); okvis::ThreadedKFVio okvis_estimator(parameters); okvis_estimator.setFullStateCallback(std::bind(&okvis::Publisher::publishFullStateAsCallback,&publisher,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4)); okvis_estimator.setLandmarksCallback(std::bind(&okvis::Publisher::publishLandmarksAsCallback,&publisher,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); okvis_estimator.setStateCallback(std::bind(&okvis::Publisher::publishStateAsCallback,&publisher,std::placeholders::_1,std::placeholders::_2)); okvis_estimator.setBlocking(true); publisher.setParameters(parameters); // pass the specified publishing stuff // extract the folder path std::string bagname(argv[2]); size_t pos = bagname.find_last_of("/"); std::string path; if (pos == std::string::npos) path = "."; else path = bagname.substr(0, pos); const unsigned int numCameras = parameters.nCameraSystem.numCameras(); // setup files to be written publisher.setCsvFile(path + "/okvis_estimator_output.csv"); publisher.setLandmarksCsvFile(path + "/okvis_estimator_landmarks.csv"); okvis_estimator.setImuCsvFile(path + "/imu0_data.csv"); for (size_t i = 0; i < numCameras; ++i) { std::stringstream num; num << i; okvis_estimator.setTracksCsvFile(i, path + "/cam" + num.str() + "_tracks.csv"); } // open the bag rosbag::Bag bag(argv[2], rosbag::bagmode::Read); // views on topics. the slash is needs to be correct, it's ridiculous... std::string imu_topic("/imu0"); rosbag::View view_imu( bag, rosbag::TopicQuery(imu_topic)); if (view_imu.size() == 0) { LOG(ERROR) << "no imu topic"; return -1; } rosbag::View::iterator view_imu_iterator = view_imu.begin(); LOG(INFO) << "No. IMU messages: " << view_imu.size(); std::vector<std::shared_ptr<rosbag::View> > view_cams_ptr; std::vector<rosbag::View::iterator> view_cam_iterators; std::vector<okvis::Time> times; okvis::Time latest(0); for(size_t i=0; i<numCameras;++i) { std::string camera_topic("/cam"+std::to_string(i)+"/image_raw"); std::shared_ptr<rosbag::View> view_ptr( new rosbag::View( bag, rosbag::TopicQuery(camera_topic))); if (view_ptr->size() == 0) { LOG(ERROR) << "no camera topic"; return 1; } view_cams_ptr.push_back(view_ptr); view_cam_iterators.push_back(view_ptr->begin()); sensor_msgs::ImageConstPtr msg1 = view_cam_iterators[i] ->instantiate<sensor_msgs::Image>(); times.push_back( okvis::Time(msg1->header.stamp.sec, msg1->header.stamp.nsec)); if (times.back() > latest) latest = times.back(); LOG(INFO) << "No. cam " << i << " messages: " << view_cams_ptr.back()->size(); } for(size_t i=0; i<numCameras;++i) { if ((latest - times[i]).toSec() > 0.01) view_cam_iterators[i]++; } int counter = 0; okvis::Time start(0.0); while (ros::ok()) { ros::spinOnce(); okvis_estimator.display(); // check if at the end if (view_imu_iterator == view_imu.end()){ std::cout << std::endl << "Finished. Press any key to exit." << std::endl << std::flush; char k = 0; while(k==0 && ros::ok()){ k = cv::waitKey(1); ros::spinOnce(); } return 0; } for (size_t i = 0; i < numCameras; ++i) { if (view_cam_iterators[i] == view_cams_ptr[i]->end()) { std::cout << std::endl << "Finished. Press any key to exit." << std::endl << std::flush; char k = 0; while(k==0 && ros::ok()){ k = cv::waitKey(1); ros::spinOnce(); } return 0; } } // add images okvis::Time t; for(size_t i=0; i<numCameras;++i) { sensor_msgs::ImageConstPtr msg1 = view_cam_iterators[i] ->instantiate<sensor_msgs::Image>(); cv::Mat filtered(msg1->height, msg1->width, CV_8UC1); memcpy(filtered.data, &msg1->data[0], msg1->height * msg1->width); t = okvis::Time(msg1->header.stamp.sec, msg1->header.stamp.nsec); if (start == okvis::Time(0.0)) { start = t; } // get all IMU measurements till then okvis::Time t_imu=start; do { sensor_msgs::ImuConstPtr msg = view_imu_iterator ->instantiate<sensor_msgs::Imu>(); Eigen::Vector3d gyr(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); Eigen::Vector3d acc(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); t_imu = okvis::Time(msg->header.stamp.sec, msg->header.stamp.nsec); // add the IMU measurement for (blocking) processing if (t_imu - start > deltaT) okvis_estimator.addImuMeasurement(t_imu, acc, gyr); view_imu_iterator++; } while (view_imu_iterator != view_imu.end() && t_imu <= t); // add the image to the frontend for (blocking) processing if (t - start > deltaT) okvis_estimator.addImage(t, i, filtered); view_cam_iterators[i]++; } ++counter; // display progress if (counter % 20 == 0) { std::cout << "\rProgress: " << int(double(counter) / double(view_cams_ptr.back()->size()) * 100) << "% " ; } } std::cout << std::endl; return 0; }