static struct ip_meta get_ip_metadata(uint8_t const *curptr, uint32_t pktlen) {
struct ip_meta result = {0};
if (pktlen < IP_HLEN) {
result.ipm_hlen = -2;
return result;
}
//TODO missing IPv6 support:
if (CMASK(curptr, 0xF0, 4) != 4) {
result.ipm_hlen = -3;
return result;
}
// retrieve IP header size (bit 4..7)
int ip_header_size = CMASK(curptr, 0x0F, 0);
// this value is the word count, each word being 32 bits (4 bytes)
ip_header_size *= 4;
result.ipm_hlen = ip_header_size;
// skip to protocol field
curptr += 9;
result.ipm_protocol = CMASK(curptr, 0xFF, 0);
// skip to addresses
curptr += 3;
CPYIP(&result.ipm_sender, curptr);
curptr += 4;
CPYIP(&result.ipm_receiver, curptr);
return result;
}
static double terror(word_t qq, word_t ii, const int * angle_updates)
{
#define SSHIFT (sizeof(long) * 8 - IN_BITS)
long sqq = ((long) qq << SSHIFT) >> SSHIFT;
long sii = ((long) ii << SSHIFT) >> SSHIFT;
assert(CMASK(sqq) == qq);
assert(CMASK(sii) == ii);
assert((sqq < 0) == CTOP(qq));
assert((sii < 0) == CTOP(ii));
return error(qq, ii, angle_updates, expect(sqq, sii));
}
static int circle(bool print)
{
int angle_updates[ITERATIONS + 1];
build_angle_updates(angle_updates);
#define NUM (2048)
#define SNUM (NUM / 2 + 1)
double var = 0;
double mean = 0;
static double errors[NUM];
static complex spectrum[SNUM];
fftw_plan plan = fftw_plan_dft_r2c_1d(NUM, errors, spectrum, 0);
for (int i = 0; i != NUM; ++i) {
double angle = (i + drand48()) * (2 * M_PI / NUM);
assert (0 < angle && angle < 2 * M_PI);
word_t qq = CMASK((long) floor(cos(angle) * (1l << 32)));
word_t ii = CMASK((long) floor(sin(angle) * (1l << 32)));
double err = terror(qq, ii, angle_updates);
errors[i] = err;
var += err * err;
mean += err;
if (print)
printf("%5i % .2f\n", i, err);
}
mean /= NUM;
var = var / NUM - mean * mean;
fprintf(stderr, "Mean = %f, var = %f, std. dev. = %f\n",
mean, var, sqrt(var));
//fftw_execute_dft_r2c(plan, errors, spectrum);
fftw_execute(plan);
static enum_double power[SNUM];
for (int i = 1; i != SNUM; ++i)
power[i] = (enum_double) { i, cnorm(spectrum[i]) };
qsort(power, SNUM - 1, sizeof(enum_double), compare_enum_double);
fprintf(stderr, "Harmonic Amplitude Real Imaginary\n");
for (int i = 0; i != 10; ++i) {
int index = power[i].index;
fprintf(stderr, " %7i %8.4f%% % f % f\n",
index, sqrt(power[i].value) * (100.0 / NUM),
creal(spectrum[index]),
cimag(spectrum[index]));
}
fftw_destroy_plan(plan);
fftw_cleanup();
return 0;
}
static word_t phasedetect(word_t qq, word_t ii, const int * angle_updates)
{
bool positive = !CTOP(qq) ^ CTOP(ii);
word_t angle = (CTOP(ii) << (OUT_BITS - 1)) + 1;
if (CTOP(qq ^ ii))
angle += MASK(OUT_BITS - 1) - angle_updates[ITERATIONS];
if (CTOP(qq))
qq = CMASK(~qq);
if (CTOP(ii))
ii = CMASK(~ii);
for (int i = 0; i < ITERATIONS; ++i) {
word_t ii_shift = ii >> (i * 2);
word_t qq_trial = CMASK(qq + ii_shift);
word_t ii_trial = IMASK(ii - qq);
ii = IMASK(ii << 1);
if (!BIT(ii_trial, IN_BITS)) {
if (positive)
angle = AMASK(angle + angle_updates[i]);
else
angle = AMASK(angle - angle_updates[i]);
if (i != 0) {
qq = qq_trial;
ii = IMASK(ii_trial << 1);
}
else {
word_t qq3 = qq;
qq = CMASK(ii >> 1);
ii = qq3 << 1;
positive = !positive;
}
}
}
if (ROUNDOFF_BIT >= 0) {
if (BIT(angle, ROUNDOFF_BIT & 31))
angle += 1 << (ROUNDOFF_BIT & 31);
angle &= ~MASK(ROUNDOFF_BIT & 31);
}
angle &= ~MASK(TRUNCATE_BIT);
return angle;
}
static void build_main_table(void)
{
for (int i = 0; i != MAIN_NUM; ++i) {
double angle = 2 * M_PI / MAIN_NUM * (i + drand48());
long qq = floor(cos(angle) * (1l << 32));
long ii = floor(sin(angle) * (1l << 32));
expect_table[i] = expect(qq, ii);
cos_table[i] = CMASK(qq);
sin_table[i] = CMASK(ii);
}
for (int i = 0; i != smNUM; ++i) {
int j = i * (MAIN_NUM / smNUM) + rand() % (MAIN_NUM / smNUM);
smcos_table[i] = cos_table[j];
smsin_table[i] = sin_table[j];
smexpect_table[i] = expect_table[j];
}
}
static int handle_arp(uint8_t *packet,
uint8_t *curptr, uint32_t curlen, bool outgoing) {
// TODO add IPv6 support
if (curlen < ARP_LEN) {
return DROP_FRAME;
}
if (MASK(curptr, 0xFFFF, 0) == 1 /*Ethernet*/ &&
MASK(OFFSET(curptr, 2), 0xFFFF, 0) == 0x0800) {
curptr += 4;
if (CMASK(curptr, 0xFF, 0) != MAC_LEN ||
CMASK(OFFSET(curptr, 1), 0xFF, 0) != IP_LEN) {
return DROP_FRAME;
}
curptr += 2;
// choose operation
int op = MASK(curptr, 0xFFFF, 0);
curptr += 2;
uint8_t arp_target_mac[MAC_LEN], arp_source_mac[MAC_LEN],
arp_target_ip[IP_LEN], arp_source_ip[IP_LEN];
CPYMAC(arp_source_mac, curptr);
curptr += MAC_LEN;
CPYIP(arp_source_ip, curptr);
curptr += IP_LEN;
CPYMAC(arp_target_mac, curptr);
curptr += MAC_LEN;
CPYIP(arp_target_ip, curptr);
if (op == 1) {
//handle ARP request
if (EQIP(arp_target_ip, &ip_address) && !outgoing) {
send_arp_reply(packet, arp_source_mac, arp_source_ip);
return FRAME_TO_TAP;
} else if (outgoing) {
return FRAME_TO_TAP;
}
} else if (op == 2) {
//handle ARP reply
if (outgoing) {
return FRAME_TO_TAP;
} else {
return FRAME_TO_ALL;
}
}
}
return DROP_FRAME;
}
static const char *
format_for_ansi(int fg, int bg, int grad)
{
static char ansi[64];
int bold, plus, fgansi, bgansi;
int used;
if ((fg < 0 || fg >= fgcount) || (bg < 0 || bg >= bgcount) || (grad < 0 || grad >= LEVELS)) {
format_state_bg = -1;
format_state_fg = -1;
return (NULL);
}
#define ISBOLD(a) ((a) >= 8)
#define CMASK(a) ((a) & 7)
bold = ISBOLD(fg);
plus = ISBOLD(bg) ? 5 : 0;
fgansi = 30 + CMASK(fg);
bgansi = 40 + CMASK(bg);
strcpy(ansi, "\33[0;");
if (bold)
strcat(ansi, "1;");
if (plus)
strcat(ansi, "5;");
used = strlen(ansi);
snprintf(ansi + used, sizeof(ansi) - used, "%i;%im%c", fgansi, bgansi, gradbyte(grad));
#undef ISBOLD
#undef CMASK
format_state_bg = bg;
format_state_fg = fg;
return (ansi);
}
// Go around the outside of the square with max coords, and check for
// grossly inaccurate results (presumably due to overflows).
static int square(void)
{
int angle_updates[ITERATIONS + 1];
build_angle_updates(angle_updates);
#define SQSIZE (1<<24)
for (word_t i = -SQSIZE; i != SQSIZE; ++i) {
word_t small = CMASK((i << (IN_BITS - 1)) / SQSIZE);
word_t bigpos = (1ll << (IN_BITS - 1)) - 1;
word_t bigneg = CMASK(~bigpos);
check(small, bigpos, angle_updates);
check(bigpos, small, angle_updates);
check(small, bigneg, angle_updates);
check(bigneg, small, angle_updates);
}
return 0;
}
int pass_for_frame(void const *frame, uint32_t framelen, bool outgoing) {
int pass = DROP_FRAME;
if (framelen < ETH_HLEN) {
return pass;
}
// getting protocol type at byte 12
struct ip_meta pktipm;
uint16_t frametype = MASK(OFFSET(frame, ETHEROFF), 0xFFFF, 0);
uint8_t *curptr = OFFSET(frame, ETH_HLEN);
framelen -= ETH_HLEN;
//TODO handle more types of non-IP frames
switch (frametype) {
case 0x0800:
// regular IP, see below
pktipm = get_ip_metadata(curptr, framelen);
break;
case 0x0806:
// ARP
//pass = handle_arp(frame, curptr, framelen, outgoing);
if (outgoing) {
pass = FRAME_TO_TAP;
} else {
pass = FRAME_TO_ALL;
}
default:
return pass;
}
// handle IP
if (pktipm.ipm_hlen > 0) {
bool is_tcp = false;
curptr += pktipm.ipm_hlen;
framelen -= pktipm.ipm_hlen;
switch (pktipm.ipm_protocol) {
case 6:
is_tcp = true;
// fall through:
case 17:
{
struct conn_desc pktcd = get_conn_metadata(
curptr, framelen, is_tcp);
if (CMASK(&pktipm.ipm_receiver, 0xF0, 0) == 0xE0) {
/* multicast: */
if (outgoing) {
pass = FRAME_TO_ALL_AND_TAP;
} else {
pass = FRAME_TO_ALL;
}
} else if (EQIP(&pktipm.ipm_receiver, &ip_address)) {
/* own IP address, so the receiver is local */
/* Make sure packets are not re-sent:
* Either they come from outside,
* so we are the receiver, or they are loopback.
* Packets cannot be sent from the system
* to the outside world
* with another than the global system IP address. */
if (!outgoing ||
EQIP(&pktipm.ipm_sender, &ip_address)) {
pass = lookup_bus(
pktcd.cd_dest_port,
is_tcp);
}
} else {
/* for remote receivers */
if (outgoing &&
EQIP(&pktipm.ipm_sender, &ip_address)) {
// this is not for our realm -- send it out
int srcbus = lookup_bus(
pktcd.cd_source_port,
is_tcp);
if (srcbus >= 0) {
// source bus is valid, send frame
pass = FRAME_TO_TAP;
}
}
}
}
}
}
return pass;
}
static int dis_rdasr(uint32_t *pc, uint32_t inst)
{
char *asrs[32] = {
"rdy", "-", "rdccr", "rdasi",
"rdtick", "rdpc", "rdfprs", "-",
"-", "-", "-", "-",
"-", "-", "-", "-",
"-", "-", "-", "-",
"-", "-", "-", "-",
"-", "-", "-", "-",
"-", "-", "-", "-",
};
switch(RS1(inst)) {
case 0x01:
case 0x07:
case 0x08:
case 0x09:
case 0x0a:
case 0x0b:
case 0x0c:
case 0x0d:
case 0x0e:
ILLEGAL;
case 0x0:
case 0x2:
case 0x3:
case 0x4:
case 0x5:
case 0x6:
(void)printf("%p:\t%s\t%s\n", (void *)pc, asrs[RS1(inst)],
sregs[RD(inst)]);
return OK;
case 0xf: /* MEMBAR / STBAR */
if (RD(inst) == 0) {
if (IMM(inst)) {
(void)printf("%p\tmembar\t", (void *)pc);
if ((CMASK(inst) & 1))
(void)printf("#Lookaside ");
if ((CMASK(inst) & 2))
(void)printf("#MemIssue ");
if ((CMASK(inst) & 4))
(void)printf("#Sync ");
if ((MMASK(inst) & 1))
(void)printf("#LoadLoad ");
if ((MMASK(inst) & 2))
(void)printf("#StoreLoad ");
if ((MMASK(inst) & 4))
(void)printf("#LoadStore ");
if ((MMASK(inst) & 8))
(void)printf("#StoreStore");
(void)printf("\n");
} else {
(void)printf("%p:\tstbar\n", (void *)pc);
}
return OK;
} else {
ILLEGAL;
}
case 0x10:
case 0x11:
case 0x12:
case 0x13:
case 0x14:
case 0x15:
case 0x16:
case 0x17:
case 0x18:
case 0x19:
case 0x1a:
case 0x1b:
case 0x1c:
case 0x1d:
case 0x1e:
case 0x1f:
default:
(void)printf("%p:\trd\t%%asr%d, %s\n", (void *)pc, RS1(inst), sregs[RD(inst)]);
return OK;
}
}
