//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;
}
