void error(err_t code) {
#if defined(UNIX) || defined(__CC65__)
printf("NanoVM error: %s\n", error_msg[code]);
exit(-1);
#else
uart_putc('E');
uart_putc('R');
uart_putc('R');
uart_putc(':');
uart_putc('A'+code);
uart_putc('\n');
for(;;) {
// reset watchdog here if in use
#ifdef ASURO
// yellow/red blinking status led
PORTD |= _BV(2);
PORTB |= _BV(0);
delay(MILLISEC(250));
PORTB &= ~_BV(0);
delay(MILLISEC(250));
#endif
}
#endif
}
/*
* radar_update_bmap
*
* updates the bnode map of the given `level' the root_node bnode in the bmap
* will also point to the gnode of level `gnode_level'+1 that is
* `rq'->quadg.gnode[_EL(gnode_level+1)].
*/
void
radar_update_bmap(struct radar_queue *rq, int level, int gnode_level)
{
map_gnode *gnode;
map_node *root_node;
map_rnode *rnode, rn;
int bm, rnode_pos, root_node_pos;
void *void_map;
if (level == me.cur_quadg.levels - 1)
return;
qspn_set_map_vars(level, 0, &root_node, &root_node_pos, 0);
void_map = me.ext_map;
gnode = rq->quadg.gnode[_EL(gnode_level + 1)];
bm = map_find_bnode(me.bnode_map[level], me.bmap_nodes[level],
root_node_pos);
if (bm == -1) {
bm = map_add_bnode(&me.bnode_map[level], &me.bmap_nodes[level],
root_node_pos, 0);
rnode_pos = -1;
} else
rnode_pos = rnode_find(&me.bnode_map[level][bm], &gnode->g);
if (rnode_pos == -1) {
setzero(&rn, sizeof(map_rnode));
rn.r_node = (int *) &gnode->g;
rnode_add(&me.bnode_map[level][bm], &rn);
rnode_pos = 0;
}
rnode = &me.bnode_map[level][bm].r_node[rnode_pos];
rnode->trtt = MILLISEC(rq->final_rtt);
}
/*
* final_radar_queue
*
* analyses the received ECHO_REPLY pkt and write the
* average rtt of each found node in the radar_queue.
*/
void
final_radar_queue(void)
{
struct radar_queue *rq;
int e;
struct timeval sum;
u_int f_rtt;
setzero(&sum, sizeof(struct timeval));
rq = radar_q;
list_for(rq) {
if (!rq->node)
continue;
/* Sum the rtt of all the received pongs */
for (e = 0; e < rq->pongs; e++)
timeradd(&rq->rtt[e], &sum, &sum);
/* Add penality rtt for each pong lost */
for (; e < MAX_RADAR_SCANS; e++)
timeradd(&rq->rtt[e - rq->pongs], &sum, &sum);
f_rtt = MILLISEC(sum) / MAX_RADAR_SCANS;
MILLISEC_TO_TV(f_rtt, rq->final_rtt);
}
my_echo_id = 0;
}
void error(err_t code) {
#if defined(UNIX) || defined(__CC65__)
printf("NanoVM error: %s\n", error_msg[code]);
exit(-1);
#else
uart_putc('E');
uart_putc('R');
uart_putc('R');
uart_putc(':');
uart_putc('A'+code);
uart_putc('\n');
for(;;) {
// reset watchdog here if in use
#ifdef ASURO
// yellow/red blinking status led
PORTD |= _BV(2);
PORTB |= _BV(0);
delay(MILLISEC(250));
PORTB &= ~_BV(0);
delay(MILLISEC(250));
#endif
#ifdef NIBOBEE
uint8_t cnt;
for (cnt=5; cnt; cnt--) {
PORTB &= 0xf0;
PORTB |= _BV(1);
_delay_ms(100);
PORTB &= 0xf0;
PORTB &= _BV(2);
_delay_ms(100);
}
PORTB &= 0xf0;
_delay_ms(1000);
PORTB |= 0x0f&code;
_delay_ms(1000);
#endif
}
#endif
}
/*
* radar_exec_reply
*
* It reads the received ECHO_REPLY pkt and updates the radar
* queue, storing the calculated rtt and the other infos relative to the sender
* node.
*/
int
radar_exec_reply(PACKET pkt)
{
struct timeval t;
struct radar_queue *rq;
u_int rtt_ms = 0;
int dev_pos;
gettimeofday(&t, 0);
/*
* Get the radar_queue struct relative to pkt.from
*/
rq = add_radar_q(pkt);
dev_pos = ifs_get_pos(me.cur_ifs, me.cur_ifs_n, pkt.dev);
if (dev_pos < 0)
debug(DBG_NORMAL, "The 0x%x ECHO_REPLY pkt was received by a non "
"existent interface", pkt.hdr.id);
if (me.cur_node->flags & MAP_HNODE) {
if (pkt.hdr.flags & HOOK_PKT) {
u_char scanning;
memcpy(&scanning, pkt.msg, sizeof(u_char));
/*
* If the pkt.from node has finished his scan, and we
* never received one of its ECHO_ME pkts, and we are
* still scanning, set the hook_retry.
*/
if (!scanning && !rq->pings &&
(radar_scan_mutex ||
radar_scans[dev_pos] <= MAX_RADAR_SCANS)) {
hook_retry = 1;
}
}
}
if (rq->pongs < radar_scans[dev_pos]) {
timersub(&t, &scan_start, &rq->rtt[(int) rq->pongs]);
/*
* Now we divide the rtt, because (t - scan_start) is the time
* the pkt used to reach B from A and to return to A from B
*/
rtt_ms = MILLISEC(rq->rtt[(int) rq->pongs]) / 2;
MILLISEC_TO_TV(rtt_ms, rq->rtt[(int) rq->pongs]);
rq->pongs++;
}
return 0;
}
int main(void)
/*****************************************************************************
* Input :
* Output :
* Function : The super loop.
******************************************************************************/
{
INT8U event;
INT8U counter_value;
disable_global_int();
init_systick();
init_gpio();
enable_global_int();
// Loop forever.
while(1)
{
// System part of the super loop.
// ------------------------------
while( !ticks );
// The following will be executed every 5mS
ticks--;
if( ! --alive_timer )
{
alive_timer = MILLISEC( 500 );
GPIO_PORTD_DATA_R ^= 0x40;
}
// Protected operating system mode
swt_ctrl();
// Application mode
task_rtc( TASK_RTC );
task_button( TASK_BUTTON );
task_set_time( TASK_SET_TIME );
task_lcd( TASK_LCD );
}
}
void native_nvmcomm_invoke(u08_t mref) {
if(mref == NATIVE_METHOD_SEND) {
uint8_t len = (uint8_t)stack_pop_int();
uint8_t *buf = (uint8_t *)stack_pop_addr();
uint8_t dest = (uint8_t)stack_pop_int();
DEBUGF_COMM("native send to "DBG8"\n", dest);
DEBUGF_COMM(""DBG8" bytes:", len);
for (uint8_t i=1; i<len+1; i++)
DEBUGF_COMM("["DBG8"] ", buf[i]);
DEBUGF_COMM("\n");
stack_push(nvmcomm_send(dest, NVMCOMM_CMD_APPMSG, buf+1, len)); // +1 to skip the first byte which indicates the type of the array
} else if(mref == NATIVE_METHOD_RECEIVE) {
nvm_int_t waitmsec=stack_pop_int();
DEBUGF_COMM("native nvmcomm.receive: waiting "DBG16" msec\n", waitmsec);
while (nvc3_appmsg_size==0 && waitmsec>0) {
delay(MILLISEC(1));
// Check if there's any packet coming in that we need to handle before processing the next VM instruction.
// Need to do this here because the VM's main loop is stopped.
// TODO: Reconsider this when we have a better design for receiving messages.
nvmcomm_poll();
waitmsec--;
}
if (nvc3_appmsg_size==0) {
// Nothing was received, return NULL;
stack_push(0);
DEBUGF_COMM("native nvmcomm.receive: timeout\n");
} else {
DEBUGF_COMM("native nvmcomm.receive: received "DBG8" bytes\n", nvc3_appmsg_size);
// TODO: copy the data to a Java array and return it.
heap_id_t array = array_new(nvc3_appmsg_size, T_BYTE);
for (uint8_t i=0; i<nvc3_appmsg_size; i++)
array_bastore(array, i, nvc3_appmsg_buf[i]);
stack_push(array | NVM_TYPE_HEAP);
nvc3_appmsg_size = 0; // Buffer available for next message
}
} else
error(ERROR_NATIVE_UNKNOWN_METHOD);
}
void uart_write_byte(u08_t byte) {
/* Wait for empty transmit buffer */
while(!(UCSRA & _BV(UDRE)));
// asuro needs echo cancellation, since the ir receiver "sees"
// the transmitter
#ifdef ASURO
// disable receiver
UCSRB &= ~(_BV(RXEN) | _BV(RXCIE));
#endif
// start transmission
UDR = byte;
#ifdef ASURO
// Wait for empty transmit buffer
while(!(UCSRA & _BV(UDRE)));
delay(MILLISEC(5));
// re-enable receiver
UCSRB |= _BV(RXEN) | _BV(RXCIE);
#endif
}
#include "systick.h"
#include "gpio.h"
#include "swtimers.h"
#include "task_rtc.h"
#include "task_button.h"
#include "task_lcd.h"
#include "task_set_time.h"
/***************************** Defines *******************************/
/***************************** Constants *******************************/
/***************************** Variables *******************************/
extern volatile INT16S ticks;
INT16S alive_timer = MILLISEC(500);
/***************************** Functions *******************************/
int main(void)
/*****************************************************************************
* Input :
* Output :
* Function : The super loop.
******************************************************************************/
{
INT8U event;
INT8U counter_value;
disable_global_int();
/*
* radar_update_map
*
* it updates the int_map and the ext_map if any bnodes are found.
* Note that the rnodes in the map are held in a different way. First of all the qspn
* is not applied to them (we already know how to reach them ;) and they have only
* one rnode... ME. So me.cur_node->r_node[x].r_node->r_node[0] == me.cur_node.
* Gotcha?
*/
void
radar_update_map(void)
{
struct qspn_buffer *qb;
struct radar_queue *rq;
ext_rnode_cache *erc;
map_gnode *gnode = 0;
map_node *node, *root_node;
map_rnode rnn, *new_root_rnode;
ext_rnode *e_rnode;
int i, diff, rnode_pos;
u_char rnode_added[MAX_LEVELS / 8], rnode_deleted[MAX_LEVELS / 8];
int level, external_node, total_levels, root_node_pos, node_update;
void *void_map;
const char *ntop;
char updated_rnodes, routes_update, devs_update;
updated_rnodes = routes_update = devs_update = 0;
setzero(rnode_added, sizeof(rnode_added));
setzero(rnode_deleted, sizeof(rnode_deleted));
/**
* Let's consider all our rnodes void, in this way we'll know what
* rnodes will remain void after the update.
*/
for (i = 0; i < me.cur_node->links; i++) {
node = (map_node *) me.cur_node->r_node[i].r_node;
node->flags |= MAP_VOID | MAP_UPDATE;
}
/**/ rq = radar_q;
list_for(rq) {
if (!rq->node)
continue;
if (!(me.cur_node->flags & MAP_HNODE) && (rq->flags & MAP_HNODE))
continue;
/*
* We need to know if it is a node which is not in the gnode
* where we are (external_rnode).
*/
if ((int) rq->node == RADQ_EXT_RNODE) {
external_node = 1;
total_levels = rq->quadg.levels;
} else {
external_node = 0;
total_levels = 1;
}
for (level = total_levels - 1; level >= 0; level--) {
qspn_set_map_vars(level, 0, &root_node, &root_node_pos, 0);
node_update = devs_update = 0;
if (!level) {
void_map = me.int_map;
node = rq->node;
} else {
/* Skip the levels where the ext_rnode belongs
* to our same gids */
if (!quadg_gids_cmp(rq->quadg, me.cur_quadg, level))
continue;
/* Update only the gnodes which belongs to
* our same gid of the upper level, because
* we don't keep the internal info of the
* extern gnodes. */
if ((level < rq->quadg.levels - 1) &&
quadg_gids_cmp(rq->quadg, me.cur_quadg, level + 1)) {
rq->quadg.gnode[_EL(level)] = 0;
continue;
}
/* Ehi, we are a bnode */
root_node->flags |= MAP_BNODE;
me.cur_node->flags |= MAP_BNODE;
void_map = me.ext_map;
gnode = rq->quadg.gnode[_EL(level)];
node = &gnode->g;
}
if (external_node && !level && me.cur_erc_counter) {
erc = e_rnode_find(me.cur_erc, &rq->quadg, 0);
if (!erc)
rnode_pos = -1;
else {
rnode_pos = erc->rnode_pos;
node = (map_node *) erc->e;
}
} else
rnode_pos = rnode_find(root_node, node);
if (rnode_pos == -1) { /* W00t, we've found a new rnode! */
node_update = 1;
rnode_pos = root_node->links;
ntop = inet_to_str(rq->quadg.ipstart[level]);
if (server_opt.dbg_lvl || !level)
loginfo
("Radar: New node found: %s, ext: %d, level: %d",
ntop, external_node, level);
if (external_node && !level) {
/*
* If this node we are processing is external, at level 0,
* in the root_node's rnodes we add a rnode which point
* to a ext_rnode struct.
*/
setzero(&rnn, sizeof(map_rnode));
e_rnode = xzalloc(sizeof(ext_rnode));
memcpy(&e_rnode->quadg, &rq->quadg,
sizeof(quadro_group));
e_rnode->node.flags =
MAP_BNODE | MAP_GNODE | MAP_RNODE | MAP_ERNODE;
rnn.r_node = (int *) e_rnode;
node = rq->node = &e_rnode->node;
new_root_rnode = &rnn;
/* Update the external_rnode_cache list */
e_rnode_add(&me.cur_erc, e_rnode, rnode_pos,
&me.cur_erc_counter);
} else {
/*We purge all the node's rnodes. */
rnode_destroy(node);
/*
* This node has only one rnode,
* and that is the root_node.
*/
setzero(&rnn, sizeof(map_rnode));
rnn.r_node = (int *) root_node;
rnode_add(node, &rnn);
/* It is a border node */
if (level)
node->flags |= MAP_BNODE | MAP_GNODE;
node->flags |= MAP_RNODE;
/*
* Fill the rnode to be added in the
* root_node.
*/
setzero(&rnn, sizeof(map_rnode));
rnn.r_node = (int *) node;
new_root_rnode = &rnn;
}
/*
* The new node is added in the root_node's
* rnodes.
*/
rnode_add(root_node, new_root_rnode);
/* Update the qspn_buffer */
if (!external_node || level) {
qb = xzalloc(sizeof(struct qspn_buffer));
qb->rnode = node;
qspn_b[level] = list_add(qspn_b[level], qb);
send_qspn_now[level] = 1;
}
/* If the new rnode wasn't present in the map,
* then it is also a new node in the map, so
* update the seeds counter too */
if (!level && !external_node && (node->flags & MAP_VOID)) {
gnode_inc_seeds(&me.cur_quadg, level);
qspn_inc_gcount(qspn_gnode_count, level + 1, 1);
}
SET_BIT(rnode_added, level);
} else {
/*
* Nah, We have the node in the map. Let's see if
* its rtt is changed
*/
if (!send_qspn_now[level] && node->links) {
diff = abs(root_node->r_node[rnode_pos].trtt -
MILLISEC(rq->final_rtt));
if (diff >= RTT_DELTA) {
node_update = 1;
send_qspn_now[level] = 1;
debug(DBG_NOISE, "node %s rtt changed, diff: %d",
inet_to_str(rq->ip), diff);
}
}
}
/* Restore the flags */
if (level)
gnode->flags &= ~GMAP_VOID;
node->flags &= ~MAP_VOID & ~MAP_UPDATE & ~QSPN_OLD;
/*
* Update the devices list of the rnode
*/
if (!level) {
devs_update = rnl_update_devs(&rlist, &rlist_counter,
node, rq->dev, rq->dev_n);
if (devs_update)
routes_update++;
}
/* Nothing is really changed */
if (!node_update)
continue;
/* Update the rtt */
root_node->r_node[rnode_pos].trtt = MILLISEC(rq->final_rtt);
/* Bnode map stuff */
if (external_node && level) {
/*
* All the root_node bnodes which are in the
* bmaps of level smaller than `level' points to
* the same gnode which is rq->quadg.gnode[_EL(level-1+1)].
* This is because the inferior levels cannot
* have knowledge about the bordering gnode
* which is in an upper level, but it's necessary that
* they know which who the root_node borders on,
* so the get_route algorithm can descend to
* the inferior levels and it will still know
* what is the border node which is linked
* to the target gnode.
*/
for (i = 0; i < level; i++)
radar_update_bmap(rq, i, level - 1);
send_qspn_now[level - 1] = 1;
}
if (node_update || devs_update)
node->flags |= MAP_UPDATE;
} /*for(level=0, ...) */
updated_rnodes++;
} /*list_for(rq) */
/* Burn the deads */
if (updated_rnodes < me.cur_node->links)
radar_remove_old_rnodes((char *) rnode_deleted);
/* <<keep your room tidy... order, ORDER>> */
if (!is_bufzero(rnode_added, sizeof(rnode_added)) ||
!is_bufzero(rnode_deleted, sizeof(rnode_deleted))) {
/***
* qsort the rnodes of me.cur_node and me.cur_quadg comparing
* their trtt */
rnode_trtt_order(me.cur_node);
for (i = 1; i < me.cur_quadg.levels; i++)
if (TEST_BIT(rnode_added, i) || TEST_BIT(rnode_deleted, i))
rnode_trtt_order(&me.cur_quadg.gnode[_EL(i)]->g);
/**/
/* adjust the rnode_pos variables in the ext_rnode_cache list */
erc_reorder_rnodepos(&me.cur_erc, &me.cur_erc_counter,
me.cur_node);
}
/* Give a refresh to the kernel */
if ((!is_bufzero(rnode_added, sizeof(rnode_added)) ||
routes_update) && !(me.cur_node->flags & MAP_HNODE))
rt_rnodes_update(1);
}