static inline __m128i byteswap32( __m128i v )
{
//rotate each 32 bit quantity by 16 bits
// 0xB1 = 10110001 = 2,3,0,1
v = _mm_shufflehi_epi16( _mm_shufflelo_epi16( v, 0xB1 ), 0xB1 );
return byteswap16( v );
}
static int dealias_ipid16_inseq(scamper_dealias_probe_t **probes,
int probec, uint16_t fudge, int bs)
{
uint16_t a, b, c;
int i;
/*
* do a preliminary check to see if the ipids could be in sequence with
* two samples.
*/
if(probec == 2)
{
/* if it is a strict sequence check, we don't actually know */
if(fudge == 0)
return 1;
a = probes[0]->replies[0]->ipid;
b = probes[1]->replies[0]->ipid;
if(bs != 0)
{
a = byteswap16(a);
b = byteswap16(b);
}
if(dealias_ipid16_inseq2(a, b, fudge) != 0)
return 1;
return 0;
}
for(i=0; i+2<probec; i++)
{
a = probes[i+0]->replies[0]->ipid;
b = probes[i+1]->replies[0]->ipid;
c = probes[i+2]->replies[0]->ipid;
if(bs != 0)
{
a = byteswap16(a);
b = byteswap16(b);
c = byteswap16(c);
}
if(dealias_ipid16_inseq3(a, b, c, fudge) == 0)
return 0;
}
return 1;
}
static int write_file_info(state *s)
{
BITMAPINFOHEADER b;
// Size of the this header information
b.biSize = byteswap32(HEADERINFOSIZE);
if (s->orientation == ORIENTATION_VERTICAL)
{
b.biWidth = byteswap32(s->image_width);
b.biHeight = byteswap32(s->image_height);
}
else
{
b.biWidth = byteswap32(s->image_height);
b.biHeight = byteswap32(s->image_width);
}
b.biPlanes = byteswap16(1);
b.biBitCount = byteswap16(24);
b.biCompression = BI_RGB;
// How much RGB data follows this header
b.biSizeImage = byteswap32((s->image_width * s->image_height) * 3);
b.biXPelsPerMeter = 0;
b.biYPelsPerMeter = 0;
// These values denote how many values are in the RGB color map
// that follows.
b.biClrUsed = 0;
b.biClrImportant = 0;
if (s->verbose)
print_status ("%s: Image will be %"PRIu32" x %"PRIu32" x %"PRIu32,
__progname,
byteswap32(b.biWidth),
byteswap32(b.biHeight),
byteswap16(b.biBitCount));
fwrite(&b,HEADERINFOSIZE,1,s->out_handle);
return FALSE;
}
static int dealias_ipid16_bo(scamper_dealias_probe_t **probes, int probec)
{
scamper_dealias_probe_t **s = NULL;
uint16_t a, b, c = 1, max_bs = 0, max_nobs = 0, u16;
int i, rc = 2;
if((s = memdup(probes, sizeof(scamper_dealias_probe_t *) * probec)) == NULL)
return -1;
array_qsort((void **)s, probec, (array_cmp_t)dealias_probe_def_cmp);
for(i=0; i<probec-1; i++)
{
if(s[i]->def != s[i+1]->def)
{
if(c >= 3)
{
if(max_nobs < max_bs)
rc = 0;
else if(max_nobs > max_bs)
rc = 1;
if(rc == 0)
goto done;
}
c = 1; max_nobs = 0; max_bs = 0;
}
else
{
a = s[i]->replies[0]->ipid; b = s[i+1]->replies[0]->ipid;
u16 = dealias_ipid16_diff(a, b);
if(u16 > max_nobs || max_nobs == 0)
max_nobs = u16;
u16 = dealias_ipid16_diff(byteswap16(a), byteswap16(b));
if(u16 > max_bs || max_bs == 0)
max_bs = u16;
c++;
}
}
done:
if(s != NULL) free(s);
return rc;
}
static int dealias_fudge_inseq(scamper_dealias_probe_t *pr_a,
scamper_dealias_probe_t *pr_b,
int bs, int fudge)
{
uint32_t a = pr_a->replies[0]->ipid;
uint32_t b = pr_b->replies[0]->ipid;
if(bs != 0)
{
a = byteswap16(a);
b = byteswap16(b);
}
if(a > b)
b += 0x10000;
if((int)(b - a) > fudge)
return 0;
return 1;
}
static inline uint16_t swaps(libtrace_t *libtrace, uint16_t num)
{
/* To deal with open_dead traces that might try and use this
* if we don't have any per trace data, assume host byte order
*/
if (!DATA(libtrace))
return num;
/* We can use the PCAP magic number to determine the byte order */
if (header_is_backwards_magic(&(DATA(libtrace)->header)))
return byteswap16(num);
return num;
}
/**
* @brief writes 16-bit value to memory at address
* @param mem memory to write to
* @param address location to write to
* @param value value to store in memory
* @return true if success, otherwise false
*/
bool write16Memory(Memory &mem, U32 address, const U16 &value)
{
bool retval;
// Check alignment
if ((address & 0x3) == 0) {
if (address < MEMORY_SIZE) {
U16 *memptr = (U16*)&mem.data[address];
*memptr = byteswap16(value);
retval = true;
} else {
printf("WRITE ERROR: Address 0x%08x >= 0x%08x\n",
address, MEMORY_SIZE);
retval = false;
}
} else {
printf("WRITE ERROR: Address 0x%08x is not 16-bit aligned\n", address);
retval = false;
}
return retval;
}
/**
* @brief reads 16-bit value memory at address and stores it in value
* @param mem memory to read from
* @param address location to read from
* @param value location to store value read
* @return true if success, otherwise false
*/
bool read16Memory(const Memory &mem, U32 address, U16 &value)
{
bool retval;
// Check alignment
if ((address & 0x1) == 0) {
if (address < MEMORY_SIZE) {
U16 *memptr = (U16*)&mem.data[address];
value = *memptr;
value = byteswap16(value);
retval = true;
} else {
printf("READ ERROR: Address 0x%08x >= 0x%08x\n",
address, MEMORY_SIZE);
retval = false;
}
} else {
printf("READ ERROR: Address 0x%08x is not 16-bit aligned\n", address);
retval = false;
}
return retval;
}
int scamper_dealias_radargun_fudge(scamper_dealias_t *dealias,
scamper_dealias_probedef_t *def,
scamper_dealias_probedef_t **defs, int *cnt,
int fudge)
{
scamper_dealias_radargun_t *rg = dealias->data;
scamper_dealias_probe_t *pr, *pr_a, *pr_b;
scamper_dealias_reply_t *re, *re_a, *re_b, *re_c;
dealias_resolv_t *dr = NULL;
dealias_resolv_t *drd;
uint32_t pid, x;
int i, j, k, bs, inseq, d = 0;
if(dealias->method != SCAMPER_DEALIAS_METHOD_RADARGUN)
goto err;
if((dr = malloc_zero(sizeof(dealias_resolv_t) * rg->probedefc)) == NULL)
goto err;
for(x=0; x<dealias->probec; x++)
{
pr = dealias->probes[x];
pid = pr->def->id;
/*
* if this probedef has already been determined to be useless for
* alias resolution, skip it
*/
if(dr[pid].probec < 0)
continue;
if(pr->replyc > 1)
{
if(dr[pid].probes != NULL)
free(dr[pid].probes);
dr[pid].probec = -1;
if(pr->def == def)
goto done;
continue;
}
/* total number of probes transmitted */
dr[pid].probet++;
if(pr->replyc == 0)
continue;
re = pr->replies[0];
/*
* with three replies, do some basic checks to see if we should
* continue considering this probedef.
*/
if(dr[pid].probec == 2)
{
pr_a = dr[pid].probes[0];
pr_b = dr[pid].probes[1];
re_a = pr_a->replies[0];
re_b = pr_b->replies[0];
if((re->ipid == pr->ipid && re_a->ipid == pr_a->ipid &&
re_b->ipid == pr_b->ipid) ||
(re->ipid == re_a->ipid && re->ipid == re_b->ipid))
{
free(dr[pid].probes);
dr[pid].probec = -1;
if(pr->def == def)
goto done;
continue;
}
}
if(array_insert((void ***)&dr[pid].probes,&dr[pid].probec,pr,NULL) != 0)
goto err;
}
/* figure out if we should byteswap the ipid sequence */
if(dr[def->id].probec < 3)
goto done;
re_a = dr[def->id].probes[0]->replies[0];
re_b = dr[def->id].probes[1]->replies[0];
re_c = dr[def->id].probes[2]->replies[0];
if(re_a->ipid < re_b->ipid)
i = re_b->ipid - re_a->ipid;
else
i = 0x10000 + re_b->ipid - re_a->ipid;
if(re_b->ipid < re_c->ipid)
i += re_c->ipid - re_b->ipid;
else
i += 0x10000 + re_c->ipid - re_b->ipid;
if(byteswap16(re_a->ipid) < byteswap16(re_b->ipid))
j = byteswap16(re_b->ipid) - byteswap16(re_a->ipid);
else
j = 0x10000 + byteswap16(re_b->ipid) - byteswap16(re_a->ipid);
if(byteswap16(re_b->ipid) < byteswap16(re_c->ipid))
j += byteswap16(re_c->ipid) - byteswap16(re_b->ipid);
else
j += 0x10000 + byteswap16(re_c->ipid) - byteswap16(re_b->ipid);
if(i < j)
bs = 0;
else
bs = 1;
/* for each probedef, consider if it could be an alias */
drd = &dr[def->id]; d = 0;
for(pid=0; pid<rg->probedefc; pid++)
{
if(&rg->probedefs[pid] == def || dr[pid].probec < 3)
continue;
j = 0; k = 0;
/* get the first ipid */
if(timeval_cmp(&drd->probes[j]->tx, &dr[pid].probes[k]->tx) < 0)
pr_a = drd->probes[j++];
else
pr_a = dr[pid].probes[k++];
for(;;)
{
if(timeval_cmp(&drd->probes[j]->tx, &dr[pid].probes[k]->tx) < 0)
pr_b = drd->probes[j++];
else
pr_b = dr[pid].probes[k++];
if((inseq = dealias_fudge_inseq(pr_a, pr_b, bs, fudge)) == 0)
break;
if(j == drd->probec || k == dr[pid].probec)
break;
}
/*
* if the pairs do not appear to have insequence IP-ID values, then
* abandon
*/
if(inseq == 0)
continue;
defs[d++] = &rg->probedefs[pid];
if(d == *cnt)
break;
}
done:
*cnt = d;
for(x=0; x<rg->probedefc; x++)
if(dr[x].probec > 0)
free(dr[x].probes);
return 0;
err:
if(dr != NULL)
{
for(x=0; x<rg->probedefc; x++)
if(dr[x].probec > 0)
free(dr[x].probes);
}
return -1;
}
int scamper_dealias_ipid(const scamper_dealias_probe_t **probes,
uint32_t probec, scamper_dealias_ipid_t *ipid)
{
const scamper_dealias_probe_t *p;
const scamper_dealias_reply_t *r;
uint32_t bs_mind = 0x30000;
uint32_t bs_maxd = 0;
uint32_t bs_sum = 0;
uint32_t mind = 0x30000;
uint32_t maxd = 0;
uint32_t sum = 0;
uint32_t diff;
uint32_t cur, prev;
uint32_t i;
int echo, cons;
ipid->type = SCAMPER_DEALIAS_IPID_UNKNOWN;
echo = 1;
cons = 1;
if(probec == 0 || probes[0] == NULL || probes[0]->replyc != 1)
return 0;
prev = probes[0]->replies[0]->ipid;
for(i=1; i<probec; i++)
{
if((p = probes[i]) == NULL)
return 0;
if(p->replyc != 1)
return 0;
if((r = p->replies[0]) == NULL)
return 0;
/* non byteswap case */
cur = r->ipid;
if(cur > prev)
diff = cur - prev;
else if(cur < prev)
diff = 0x10000 + cur - prev;
else
diff = 0;
if(diff < mind)
mind = diff;
if(diff > maxd)
maxd = diff;
sum += diff;
/* byteswap case */
cur = byteswap16(r->ipid);
prev = byteswap16(prev);
if(cur > prev)
diff = cur - prev;
else if(cur < prev)
diff = 0x10000 + cur - prev;
else
diff = 0;
if(diff < bs_mind)
bs_mind = diff;
if(diff > maxd)
bs_maxd = diff;
bs_sum += diff;
if(echo != 0 && p->ipid != r->ipid && p->ipid != byteswap16(r->ipid))
echo = 0;
else if(cons != 0 && probes[i-1]->replies[0]->ipid != r->ipid)
cons = 0;
prev = r->ipid;
}
if(cons == 0 && echo == 0)
{
/* figure out which byte ordering best explains the sequence */
if(sum < bs_sum)
{
ipid->mind = mind;
ipid->maxd = maxd;
}
else
{
ipid->mind = bs_mind;
ipid->maxd = bs_maxd;
}
ipid->type = SCAMPER_DEALIAS_IPID_INCR;
}
else if(cons != 0)
{
if(probes[0]->replies[0]->ipid == 0)
ipid->type = SCAMPER_DEALIAS_IPID_ZERO;
else
ipid->type = SCAMPER_DEALIAS_IPID_CONST;
}
else if(echo != 0)
{
ipid->type = SCAMPER_DEALIAS_IPID_ECHO;
}
return 0;
}
int savebmp(char *filename, unsigned char *buf, int w, int h,
enum BMPPIXELFORMAT f, int srcpitch, int srcbottomup)
{
int fd=-1, byteswritten, dstpitch, retcode=0;
int flags=O_RDWR|O_CREAT|O_TRUNC;
unsigned char *tempbuf=NULL; char *temp;
bmphdr bh; int mode;
#ifdef _WIN32
flags|=O_BINARY; mode=_S_IREAD|_S_IWRITE;
#else
mode=S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH;
#endif
if(!filename || !buf || w<1 || h<1 || f<0 || f>BMPPIXELFORMATS-1 || srcpitch<0)
_throw("bad argument to savebmp()");
if(srcpitch==0) srcpitch=w*ps[f];
if((temp=strrchr(filename, '.'))!=NULL)
{
if(!stricmp(temp, ".ppm"))
return saveppm(filename, buf, w, h, f, srcpitch, srcbottomup);
}
_unix(fd=open(filename, flags, mode));
dstpitch=((w*3)+3)&(~3);
bh.bfType=0x4d42;
bh.bfSize=BMPHDRSIZE+dstpitch*h;
bh.bfReserved1=0; bh.bfReserved2=0;
bh.bfOffBits=BMPHDRSIZE;
bh.biSize=40;
bh.biWidth=w; bh.biHeight=h;
bh.biPlanes=0; bh.biBitCount=24;
bh.biCompression=BI_RGB; bh.biSizeImage=0;
bh.biXPelsPerMeter=0; bh.biYPelsPerMeter=0;
bh.biClrUsed=0; bh.biClrImportant=0;
if(!littleendian())
{
bh.bfType=byteswap16(bh.bfType);
bh.bfSize=byteswap(bh.bfSize);
bh.bfOffBits=byteswap(bh.bfOffBits);
bh.biSize=byteswap(bh.biSize);
bh.biWidth=byteswap(bh.biWidth);
bh.biHeight=byteswap(bh.biHeight);
bh.biPlanes=byteswap16(bh.biPlanes);
bh.biBitCount=byteswap16(bh.biBitCount);
bh.biCompression=byteswap(bh.biCompression);
bh.biSizeImage=byteswap(bh.biSizeImage);
bh.biXPelsPerMeter=byteswap(bh.biXPelsPerMeter);
bh.biYPelsPerMeter=byteswap(bh.biYPelsPerMeter);
bh.biClrUsed=byteswap(bh.biClrUsed);
bh.biClrImportant=byteswap(bh.biClrImportant);
}
writeme(fd, &bh.bfType, sizeof(unsigned short));
writeme(fd, &bh.bfSize, sizeof(unsigned int));
writeme(fd, &bh.bfReserved1, sizeof(unsigned short));
writeme(fd, &bh.bfReserved2, sizeof(unsigned short));
writeme(fd, &bh.bfOffBits, sizeof(unsigned int));
writeme(fd, &bh.biSize, sizeof(unsigned int));
writeme(fd, &bh.biWidth, sizeof(int));
writeme(fd, &bh.biHeight, sizeof(int));
writeme(fd, &bh.biPlanes, sizeof(unsigned short));
writeme(fd, &bh.biBitCount, sizeof(unsigned short));
writeme(fd, &bh.biCompression, sizeof(unsigned int));
writeme(fd, &bh.biSizeImage, sizeof(unsigned int));
writeme(fd, &bh.biXPelsPerMeter, sizeof(int));
writeme(fd, &bh.biYPelsPerMeter, sizeof(int));
writeme(fd, &bh.biClrUsed, sizeof(unsigned int));
writeme(fd, &bh.biClrImportant, sizeof(unsigned int));
if((tempbuf=(unsigned char *)malloc(dstpitch*h))==NULL)
_throw("Memory allocation error");
pixelconvert(buf, f, srcpitch, tempbuf, BMP_BGR, dstpitch, w, h,
!srcbottomup);
if((byteswritten=write(fd, tempbuf, dstpitch*h))!=dstpitch*h)
_throw(strerror(errno));
finally:
if(tempbuf) free(tempbuf);
if(fd!=-1) close(fd);
return retcode;
}
int loadbmp(char *filename, unsigned char **buf, int *w, int *h,
enum BMPPIXELFORMAT f, int align, int dstbottomup)
{
int fd=-1, bytesread, srcpitch, srcbottomup=1, srcps, dstpitch,
retcode=0;
unsigned char *tempbuf=NULL;
bmphdr bh; int flags=O_RDONLY;
dstbottomup=dstbottomup? 1:0;
#ifdef _WIN32
flags|=O_BINARY;
#endif
if(!filename || !buf || !w || !h || f<0 || f>BMPPIXELFORMATS-1 || align<1)
_throw("invalid argument to loadbmp()");
if((align&(align-1))!=0)
_throw("Alignment must be a power of 2");
_unix(fd=open(filename, flags));
readme(fd, &bh.bfType, sizeof(unsigned short));
if(!littleendian()) bh.bfType=byteswap16(bh.bfType);
if(bh.bfType==0x3650)
{
_catch(loadppm(&fd, buf, w, h, f, align, dstbottomup, 0));
goto finally;
}
if(bh.bfType==0x3350)
{
_catch(loadppm(&fd, buf, w, h, f, align, dstbottomup, 1));
goto finally;
}
readme(fd, &bh.bfSize, sizeof(unsigned int));
readme(fd, &bh.bfReserved1, sizeof(unsigned short));
readme(fd, &bh.bfReserved2, sizeof(unsigned short));
readme(fd, &bh.bfOffBits, sizeof(unsigned int));
readme(fd, &bh.biSize, sizeof(unsigned int));
readme(fd, &bh.biWidth, sizeof(int));
readme(fd, &bh.biHeight, sizeof(int));
readme(fd, &bh.biPlanes, sizeof(unsigned short));
readme(fd, &bh.biBitCount, sizeof(unsigned short));
readme(fd, &bh.biCompression, sizeof(unsigned int));
readme(fd, &bh.biSizeImage, sizeof(unsigned int));
readme(fd, &bh.biXPelsPerMeter, sizeof(int));
readme(fd, &bh.biYPelsPerMeter, sizeof(int));
readme(fd, &bh.biClrUsed, sizeof(unsigned int));
readme(fd, &bh.biClrImportant, sizeof(unsigned int));
if(!littleendian())
{
bh.bfSize=byteswap(bh.bfSize);
bh.bfOffBits=byteswap(bh.bfOffBits);
bh.biSize=byteswap(bh.biSize);
bh.biWidth=byteswap(bh.biWidth);
bh.biHeight=byteswap(bh.biHeight);
bh.biPlanes=byteswap16(bh.biPlanes);
bh.biBitCount=byteswap16(bh.biBitCount);
bh.biCompression=byteswap(bh.biCompression);
bh.biSizeImage=byteswap(bh.biSizeImage);
bh.biXPelsPerMeter=byteswap(bh.biXPelsPerMeter);
bh.biYPelsPerMeter=byteswap(bh.biYPelsPerMeter);
bh.biClrUsed=byteswap(bh.biClrUsed);
bh.biClrImportant=byteswap(bh.biClrImportant);
}
if(bh.bfType!=0x4d42 || bh.bfOffBits<BMPHDRSIZE
|| bh.biWidth<1 || bh.biHeight==0)
_throw("Corrupt bitmap header");
if((bh.biBitCount!=24 && bh.biBitCount!=32) || bh.biCompression!=BI_RGB)
_throw("Only uncompessed RGB bitmaps are supported");
*w=bh.biWidth; *h=bh.biHeight; srcps=bh.biBitCount/8;
if(*h<0) {*h=-(*h); srcbottomup=0;}
srcpitch=(((*w)*srcps)+3)&(~3);
dstpitch=(((*w)*ps[f])+(align-1))&(~(align-1));
if(srcpitch*(*h)+bh.bfOffBits!=bh.bfSize) _throw("Corrupt bitmap header");
if((tempbuf=(unsigned char *)malloc(srcpitch*(*h)))==NULL
|| (*buf=(unsigned char *)malloc(dstpitch*(*h)))==NULL)
_throw("Memory allocation error");
if(lseek(fd, (long)bh.bfOffBits, SEEK_SET)!=(long)bh.bfOffBits)
_throw(strerror(errno));
_unix(bytesread=read(fd, tempbuf, srcpitch*(*h)));
if(bytesread!=srcpitch*(*h)) _throw("Read error");
pixelconvert(tempbuf, BMP_BGR, srcpitch, *buf, f, dstpitch, *w, *h,
srcbottomup!=dstbottomup);
finally:
if(tempbuf) free(tempbuf);
if(fd!=-1) close(fd);
return retcode;
}
static uint16_t _swap16(uint16_t x)
{
return byteswap16(x);
}