void send_debug_packet(app_state_t *s, ts_packet_t *ts_packet_ptr, float est_error)
{
timesync_t * temp_ts_ptr = get_timesync_ptr(s, ROOT_NODE);
test_packet_t *test_packet_ptr = (test_packet_t *)sys_malloc(sizeof(test_packet_t));
if( temp_ts_ptr == NULL )
{
test_packet_ptr->a = 0;
test_packet_ptr->b = 0;
test_packet_ptr->sampling_period = 2;
}
else
{
test_packet_ptr->a = temp_ts_ptr->a;
test_packet_ptr->b = temp_ts_ptr->b;
test_packet_ptr->sampling_period = temp_ts_ptr->sampling_period;
}
test_packet_ptr->time[0] = ts_packet_ptr->time[0];
test_packet_ptr->time[1] = ts_packet_ptr->time[1];
test_packet_ptr->type = TEST_PACKET;
test_packet_ptr->node_id = ker_id();
test_packet_ptr->est_error = est_error;
post_uart(s->pid, s->pid, UART_CALC_DEBUG, sizeof(test_packet_t), test_packet_ptr, SOS_MSG_RELEASE, UART_ADDRESS);
send_buffer(s, ROOT_NODE);
send_buffer2(s, ROOT_NODE);
}
static int8_t loader_handler( void *state, Message *msg )
{
switch (msg->type){
case MSG_TIMER_TIMEOUT:
{ return handle_timeout( msg ); }
case MSG_VERSION_ADV:
{
msg_version_adv_t *pkt = (msg_version_adv_t *) msg->data;
pkt->version = version_ntoh(pkt->version);
if(pkt->version >= st.version_data->version) {
st.recent_neighbor = msg->saddr;
}
return handle_version_adv( msg );
}
case MSG_VERSION_DATA:
{
msg_version_data_t *pkt = (msg_version_data_t *) msg->data;
pkt->version = version_ntoh(pkt->version);
if(pkt->version >= st.version_data->version) {
st.recent_neighbor = msg->saddr;
}
return handle_version_data( msg );
}
case MSG_LOADER_LS_ON_NODE:
{
return handle_loader_ls_on_node( msg );
}
case MSG_FETCHER_DONE:
{
return handle_fetcher_done( msg );
}
case MSG_INIT:
{
#ifdef LOADER_NET_EXPERIMENT
handle_init();
if( ker_id() == 0 ) {
start_experiment(EXPERIMENT_SIZE);
}
#else
return handle_init();
#endif
}
default:
return -EINVAL;
}
return SOS_OK;
}
static void print_nodes()
{
int myj;
fprintf(stderr, "ker_id() : %d\n", ker_id());
fprintf(stderr, "radio_pkt_success_rate: %d\n",radio_pkt_success_rate);
fprintf(stderr, "totalNodes = %d\n", totalNodes);
for(myj=0;myj < totalNodes; myj++){
fprintf(stderr, "topo_array[%d].id = %d\n",myj,topo_array[myj].id);
fprintf(stderr, "topo_array[%d].sock = %d\n",myj,topo_array[myj].sock);
fprintf(stderr, "topo_array[%d].unit = %d\n",myj,topo_array[myj].unit);
fprintf(stderr, "topo_array[%d].x = %d\n",myj,topo_array[myj].x);
fprintf(stderr, "topo_array[%d].y = %d\n",myj,topo_array[myj].y);
fprintf(stderr, "topo_array[%d].z = %d\n",myj,topo_array[myj].z);
fprintf(stderr, "topo_array[%d].r2 = %d\n",myj,topo_array[myj].r2);
fprintf(stderr, "topo_array[%d].type = %u\n",myj,topo_array[myj].type);
}
}
static int8_t fetcher_handler(void *state, Message *msg)
{
switch (msg->type) {
case MSG_FETCHER_FRAGMENT:
{
fetcher_fragment_t *f;
f = (fetcher_fragment_t*)msg->data;
f->key = entohs( f->key );
f->frag_id = entohs(f->frag_id);
DEBUG_PID(KER_FETCHER_PID,"MSG_FETCHER_FRAGMENT:\n");
handle_overheard_fragment(msg);
if(fst == NULL) {
DEBUG_PID(KER_FETCHER_PID, "NO Request!!!\n");
return SOS_OK; //!< no request
}
//DEBUG_PID(KER_FETCHER_PID,"calling restart_request_timer()\n");
restart_request_timer();
fst->retx = 0;
//DEBUG_PID(KER_FETCHER_PID,"calling handle_data()\n");
return handle_data(msg);
}
case MSG_FETCHER_REQUEST:
{
fetcher_bitmap_t *bmap =
(fetcher_bitmap_t *) msg->data;
bmap->key = entohs( bmap->key );
//! received request from neighbors
DEBUG("handling request to %d from %d\n", msg->daddr, msg->saddr);
if(msg->daddr == ker_id()) {
return handle_request(msg);
}
if(fst == NULL) return SOS_OK; //!< no request
restart_request_timer();
fst->retx = 0;
return SOS_OK;
}
case MSG_TIMER_TIMEOUT:
{
MsgParam *params = (MsgParam*)(msg->data);
if(params->byte == FETCHER_REQUEST_TID) {
//DEBUG("request timeout\n");
if( no_mem_retry ) {
send_fetcher_done();
return SOS_OK;
}
handle_request_timeout();
} else if(params->byte == FETCHER_TRANSMIT_TID) {
//DEBUG("send fragment timeout\n");
if( send_state.num_msg_in_queue < FETCHER_MAX_MSG_IN_QUEUE ) {
send_fragment();
}
}
return SOS_OK;
}
case MSG_PKT_SENDDONE:
{
if( send_state.num_msg_in_queue > 0 ) {
send_state.num_msg_in_queue--;
}
return SOS_OK;
}
#ifdef SOS_HAS_EXFLASH
case MSG_EXFLASH_WRITEDONE:
{
ker_free(send_state.fragr);
send_state.fragr = NULL;
check_map_and_post();
return SOS_OK;
}
case MSG_EXFLASH_READDONE:
{
post_auto(KER_FETCHER_PID,
KER_FETCHER_PID,
MSG_FETCHER_FRAGMENT,
sizeof(fetcher_fragment_t),
send_state.frag,
SOS_MSG_RELEASE,
send_state.dest);
send_state.frag = NULL;
return SOS_OK;
}
#endif
case MSG_INIT:
{
send_state.map = NULL;
send_state.frag = NULL;
send_state.fragr = NULL;
send_state.num_msg_in_queue = 0;
ker_msg_change_rules(KER_FETCHER_PID, SOS_MSG_RULES_PROMISCUOUS);
ker_permanent_timer_init(&(send_state.timer), KER_FETCHER_PID, FETCHER_TRANSMIT_TID, TIMER_REPEAT);
ker_timer_init(KER_FETCHER_PID, FETCHER_REQUEST_TID, TIMER_ONE_SHOT);
return SOS_OK;
}
}
return -EINVAL;
}
uint8_t add_values(app_state_t *s, Message *msg)
{
timesync_t *ts_list_ptr;
ts_packet_t *ts_packet_ptr = (ts_packet_t *)msg->data;
float est_error = 0;
// Case 1: Empty list
if(s->ts_list == NULL)
{
return FALSE;
}
// Case 2: Entry found
ts_list_ptr = s->ts_list;
while(ts_list_ptr)
{
if(ts_list_ptr->node_id == msg->saddr)
{
uint8_t i;
DEBUG("RATS: Found entry for node %d\n", msg->saddr);
//first we have to move the old data one position downwards
for(i = 0; i< BUFFER_SIZE-1 ; i++)
{
ts_list_ptr->timestamps[i] = ts_list_ptr->timestamps[i+1];
ts_list_ptr->my_time[i] = ts_list_ptr->my_time[i+1];
}
//afterwards we write the new data in the last position
ts_list_ptr->timestamps[BUFFER_SIZE-1] = ts_packet_ptr->time[0];
ts_list_ptr->my_time[BUFFER_SIZE-1] = ts_packet_ptr->time[1];
//calculate error and new window size
if(ts_list_ptr->packet_count < BUFFER_SIZE) // learning state
{
ts_list_ptr->packet_count++;
DEBUG("RATS: Learning state: %d packets received\n", ts_list_ptr->packet_count);
}
else
{
getRegression(&ts_list_ptr->timestamps[0], &ts_list_ptr->my_time[0], BUFFER_SIZE,
ts_list_ptr->window_size, BUFFER_SIZE - ts_list_ptr->window_size, &ts_list_ptr->a, &ts_list_ptr->b);
DEBUG("RATS: est_error * SCALING_FACTOR = %f\n", est_error * SCALING_FACTOR);
DEBUG("RATS: LOWER_THRESHOLD * sync_precision = %f\n", LOWER_THRESHOLD * ts_list_ptr->sync_precision);
est_error = getError(&ts_list_ptr->timestamps[0], &ts_list_ptr->my_time[0], BUFFER_SIZE,
ts_list_ptr->window_size, BUFFER_SIZE - ts_list_ptr->window_size, &ts_list_ptr->a, &ts_list_ptr->b, ts_list_ptr->sampling_period, FALSE);
if( (est_error) < (LOWER_THRESHOLD * ts_list_ptr->sync_precision))
{
if( (ts_list_ptr->sampling_period * 2) <= MAX_SAMPLING_PERIOD)
ts_list_ptr->sampling_period *= 2;
else
ts_list_ptr->sampling_period = MAX_SAMPLING_PERIOD;
DEBUG("RATS: New period (doubled): %d\n", ts_list_ptr->sampling_period);
}
else if((est_error) > (HIGHER_THRESHOLD * ts_list_ptr->sync_precision))
{
if( (ts_list_ptr->sampling_period / 2) >= MIN_SAMPLING_PERIOD)
ts_list_ptr->sampling_period /= 2;
else
ts_list_ptr->sampling_period = MIN_SAMPLING_PERIOD;
DEBUG("RATS: New period (divided by 2): %d\n", ts_list_ptr->sampling_period);
}
else
{
DEBUG("RATS: Period remains constant: %d\n", ts_list_ptr->sampling_period);
}
// window size has to be in the limits of [2, BUFFER_SIZE]
uint8_t temp = (uint8_t)(TIME_CONSTANT/ts_list_ptr->sampling_period);
if((temp >= 2)&& (temp <= BUFFER_SIZE))
{
ts_list_ptr->window_size = temp;
}
else if (temp < 2)
ts_list_ptr->window_size = (uint8_t)2;
DEBUG("RATS: Current window size : %d\n", ts_list_ptr->window_size);
}
// send packet only if calculated period is less than the one used by the parent
// or if I am the node that has the minimum period or if transmitter has set
// his id equal to the id with the minimum period (used in transitive modes)
if((ts_list_ptr->sampling_period < ts_packet_ptr->transmission_period)
|| (ts_packet_ptr->min_period_node_id == ker_id())
|| (ts_packet_ptr->min_period_node_id == msg->saddr) )
{
DEBUG("RATS: new_period=min OR I am min_period_node => transmit new_period\n");
post_net(s->pid, s->pid, MSG_PERIOD_CHANGE, sizeof(uint16_t),
&ts_list_ptr->sampling_period, 0, msg->saddr);
}
//Update fields for panic packets
ts_list_ptr->panic_timer_retransmissions = PANIC_TIMER_RETRANSMISSIONS;
ts_list_ptr->panic_timer_counter = (ts_list_ptr->sampling_period / INITIAL_TRANSMISSION_PERIOD) + 4;
LED_DBG(LED_YELLOW_TOGGLE);
#ifdef UART_DEBUG
send_debug_packet(s, ts_packet_ptr, est_error);
#endif //UART_DEBUG
return TRUE;
}
if(ts_list_ptr->next != NULL)
ts_list_ptr = ts_list_ptr->next;
else
break;
}
// Case 3: Entry not found
return FALSE;
}
static int8_t module(void *state, Message *msg)
{
app_state_t *s = (app_state_t *) state;
MsgParam *p = (MsgParam*)(msg->data);
/**
* Switch to the correct message handler
*/
switch (msg->type){
case MSG_INIT:
{
DEBUG("RATS: node %d initializing\n", ker_id());
s->pid = msg->did;
s->ts_list = NULL;
s->ts_packet.type = NORMAL_PACKET;
//Notify neighbors that RATS is starting (in case node rebooted while it was
//synchronizing with another node
post_net(s->pid, s->pid, MSG_INVALIDATE_ENTRY, 0, NULL, 0, BCAST_ADDRESS);
return SOS_OK;
}
case MSG_RATS_CLIENT_START:
{
MsgParam *p = (MsgParam *)msg->data;
DEBUG("RATS: Received MSG_RATS_CLIENT_START for node %d\n", p->word);
uint8_t request_status = add_request(s, p->word, p->byte);
//If a new request was created, then send packet to parent
if(request_status != NO_REQUEST_CREATED)
{
DEBUG("RATS: Transmitting request to node %d\n", p->word);
LED_DBG(LED_RED_TOGGLE);
//If the current node is the parent of the target node, then the target node will
//reply by informing the parent, who will add the target to its list of children.
post_net(s->pid, s->pid, MSG_RATS_SERVER_START, 0, NULL, 0, p->word);
}
else
{
//Request already exists
DEBUG("RATS: Request already exists\n");
}
//If this was the first request that was created, we need to start the panic timer
if(request_status == CREATED_FIRST_REQUEST)
{
DEBUG("RATS: PANIC_TIMER started\n");
#ifdef USE_PANIC_PACKETS
sys_timer_start(PANIC_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
#endif //USE_PANIC_PACKETS
}
return SOS_OK;
}
case MSG_RATS_SERVER_START:
{
timesync_t * temp_ts_ptr = get_timesync_ptr(s, msg->saddr);
DEBUG("RATS: Received request from node %d\n", msg->saddr);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Starting timesync with node %d\n", msg->saddr);
LED_DBG(LED_RED_TOGGLE);
//If request is coming from node, with whom the node is not synchronizing, then
//synchronization is starting
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
s->ts_packet.transmission_period = INITIAL_TRANSMISSION_PERIOD;
s->ts_packet.min_period_node_id = msg->saddr;
s->transmit_timer_counter = 1; //s->ts_packet.transmission_period/INITIAL_TRANSMISSION_PERIOD;
s->validation_timer_counter = 5; //s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
sys_timer_start(TRANSMIT_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
sys_timer_start(VALIDATION_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
}
return SOS_OK;
}
case MSG_RATS_GET_TIME:
{
//If the module passed a NULL pointer or if the data size is wrong, then discard
if( (msg->data == NULL)
#ifndef PC_PLATFORM
|| (msg->len != sizeof(rats_t) )
#endif //PC_PLATFORM
)
{
DEBUG("RATS: Invalid data received in MSG_RATS_GET_TIME\n");
break;
}
rats_t * rats_ptr = (rats_t *)sys_msg_take_data(msg);
DEBUG("RATS: Received MSG_RATS_GET_TIME (mod_id=%d node=%d)\n", rats_ptr->mod_id, msg->saddr);
if(rats_ptr->source_node_id == ker_id())
{
timesync_t * temp_ts_ptr = get_timesync_ptr(s, rats_ptr->target_node_id);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Target node %d is not time synced\n", rats_ptr->target_node_id);
sys_free(rats_ptr);
break;
}
else
{
DEBUG("RATS: Calculating time for target node %d locally\n", rats_ptr->target_node_id);
if(temp_ts_ptr->packet_count < BUFFER_SIZE) // learning state
{
rats_ptr->time_at_target_node = 0;
rats_ptr->error = 0;
}
else
{
rats_ptr->time_at_target_node = convert_from_mine_to_parent_time(rats_ptr->time_at_source_node, rats_ptr->target_node_id);
rats_ptr->error = getError(&temp_ts_ptr->timestamps[0], &temp_ts_ptr->my_time[0], BUFFER_SIZE,
temp_ts_ptr->window_size, BUFFER_SIZE - temp_ts_ptr->window_size, &temp_ts_ptr->a, &temp_ts_ptr->b, temp_ts_ptr->sampling_period, FALSE);
}
}
}
else if (rats_ptr->target_node_id == ker_id())
{
timesync_t * temp_ts_ptr = get_timesync_ptr(s, rats_ptr->source_node_id);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Source node %d is not time synced\n", rats_ptr->source_node_id);
sys_free(rats_ptr);
break;
}
else
{
DEBUG("RATS: Calculating time for source node %d locally\n", rats_ptr->source_node_id);
if(temp_ts_ptr->packet_count < BUFFER_SIZE) // learning state
{
rats_ptr->time_at_target_node = 0;
rats_ptr->error = 0;
}
else
{
rats_ptr->time_at_target_node = convert_from_parent_to_my_time(rats_ptr->time_at_source_node, rats_ptr->source_node_id);
rats_ptr->error = getError(&temp_ts_ptr->timestamps[0], &temp_ts_ptr->my_time[0], BUFFER_SIZE,
temp_ts_ptr->window_size, BUFFER_SIZE - temp_ts_ptr->window_size, &temp_ts_ptr->a, &temp_ts_ptr->b, temp_ts_ptr->sampling_period, TRUE);
}
}
}
else
{
DEBUG("RATS: Invalid request (source = %d, target - %d)\n", rats_ptr->source_node_id, rats_ptr->target_node_id);
sys_free(rats_ptr);
break;
}
DEBUG("RATS: Sending reply to module %d\n", rats_ptr->mod_id);
post_long(rats_ptr->mod_id, s->pid, rats_ptr->msg_type, sizeof(rats_t), rats_ptr, SOS_MSG_RELEASE);
break;
}
case MSG_RATS_CLIENT_STOP:
{
MsgParam *p = (MsgParam *)msg->data;
uint16_t node_id = p->word;
//First we need to remove node from list of parents
/* Select node at head of list */
timesync_t * ts_list_ptr = s->ts_list;
timesync_t * ts_delete_list_ptr;
timesync_t * ts_previous_list_ptr = s->ts_list;
/* Loop until we've reached the end of the list */
while( ts_list_ptr != NULL )
{
if(ts_list_ptr->node_id == node_id)
{
if(--ts_list_ptr->ref_counter > 0)
return SOS_OK;
DEBUG("RATS: Removing node %d from list of parents. Sending MSG_RATS_SERVER_STOP.\n", node_id);
post_net(s->pid, s->pid, MSG_RATS_SERVER_STOP, 0, NULL, 0, node_id);
/* Found the item to be deleted,
re-link the list around it */
if( ts_list_ptr == s->ts_list )
/* We're deleting the head */
s->ts_list = ts_list_ptr->next;
else
ts_previous_list_ptr->next = ts_list_ptr->next;
ts_delete_list_ptr = ts_list_ptr;
ts_list_ptr = ts_list_ptr->next;
/* Free the node */
sys_free( ts_delete_list_ptr );
//If the parent list is empty, then we're stopping the panic timer
if(s->ts_list == NULL)
{
DEBUG("RATS: Parent list is empty. Stopping panic timer\n");
#ifdef USE_PANIC_PACKETS
sys_timer_stop(PANIC_TIMER);
#endif //USE_PANIC_PACKETS
}
return SOS_OK;
}
ts_previous_list_ptr = ts_list_ptr;
ts_list_ptr = ts_list_ptr->next;
}
DEBUG("RATS: Requested parent %d was not found\n", node_id);
break;
}
case MSG_RATS_SERVER_STOP:
{
DEBUG("RATS: Received MSG_RATS_SERVER_STOP from %d\n", msg->saddr);
//If node has minimum period, then go to validation protocol
if(msg->saddr == s->ts_packet.min_period_node_id)
{
DEBUG("RATS: Going to validation protocol\n");
s->validation_timer_counter = 1;
s->validation_timer_retransmissions = BROADCAST_VALIDATION_RETRANSMISSIONS;
s->validation_node_id = ker_id();
}
break;
}
case MSG_TIMER_TIMEOUT:
{
switch(p->byte)
{
case TRANSMIT_TIMER:
{
if( (--(s->transmit_timer_counter)) == 0)
{
DEBUG("RATS: Broadcasting MSG_TIMESTAMP packet\n");
LED_DBG(LED_GREEN_TOGGLE);
post_net(s->pid, s->pid, MSG_TIMESTAMP, sizeof(ts_packet_t), &s->ts_packet, 0, BCAST_ADDRESS);
#ifdef UART_DEBUG
post_uart(s->pid, s->pid, UART_TIMESTAMP, sizeof(ts_packet_t), &s->ts_packet, 0, BCAST_ADDRESS);
#endif //UART_DEBUG
s->transmit_timer_counter = (uint16_t)(s->ts_packet.transmission_period / MIN_SAMPLING_PERIOD);
}
break;
}
case VALIDATION_TIMER:
{
if( (--(s->validation_timer_counter)) == 0)
{
s->validation_timer_counter = 1;
//Send up to MSG_PERIOD_REQUEST packets (UNICAST_VALIDATION_RETRANSMISSIONS times) to node with minimum period.
//If the node doesn't respond until then, then broadcast BROADCAST_VALIDATION_RETRANSMISSIONS times
//After the transmitting BROADCAST_VALIDATION_RETRANSMISSIONS packets, use the minimum period that
//was sent during that interval
if( s->validation_timer_retransmissions > BROADCAST_VALIDATION_RETRANSMISSIONS )
{
--s->validation_timer_retransmissions;
DEBUG("RATS: Transmitting MSG_PERIOD_REQUEST (retries left = %d) to node %d\n", s->validation_timer_retransmissions, s->ts_packet.min_period_node_id);
post_net(s->pid, s->pid, MSG_PERIOD_REQUEST, 0, NULL, 0, s->ts_packet.min_period_node_id);
#ifdef UART_DEBUG
post_uart(s->pid, s->pid, UART_PERIOD_REQUEST, 0, NULL, 0, s->ts_packet.min_period_node_id);
#endif //UART_DEBUG
}
else if( s->validation_timer_retransmissions > 0)
{
--s->validation_timer_retransmissions;
DEBUG("RATS: Broadcasting MSG_PERIOD_REQUEST (retries left = %d)\n", s->validation_timer_retransmissions);
//Invalidate node with minimum period
s->validation_node_id = ker_id();
post_net(s->pid, s->pid, MSG_PERIOD_REQUEST, 0, NULL, 0, BCAST_ADDRESS);
#ifdef UART_DEBUG
post_uart(s->pid, s->pid, UART_PERIOD_REQUEST, 0, NULL, 0, BCAST_ADDRESS);
#endif //UART_DEBUG
}
else //s->validation_timer_retransmissions == 0
{
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
//Restart normal procedure only if there was a reply
if(ker_id() != s->validation_node_id)
{
DEBUG("RATS: Setting node %d as the one with min period (%d)\n",
s->validation_node_id, s->validation_period);
s->ts_packet.min_period_node_id = s->validation_node_id;
s->ts_packet.transmission_period = s->validation_period;
s->transmit_timer_counter = s->ts_packet.transmission_period/INITIAL_TRANSMISSION_PERIOD;
s->validation_timer_counter = s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
sys_timer_start(TRANSMIT_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
sys_timer_start(VALIDATION_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
}
else
{
DEBUG("RATS: Validation timer expired, without receiving any packets\n");
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
}
}
}
break;
}
case PANIC_TIMER:
{
//There is a fixed number of retransmissions. If the corresponding counter
//reaches zero, then the child is removed from the list
/* Select node at head of list */
timesync_t * ts_list_ptr = s->ts_list;
timesync_t * ts_delete_list_ptr;
timesync_t * ts_previous_list_ptr = s->ts_list;
/* Loop until we've reached the end of the list */
while( ts_list_ptr != NULL )
{
if(--ts_list_ptr->panic_timer_counter == 0)
{
if(ts_list_ptr->panic_timer_retransmissions > 0)
{
//Transmit the packet
--ts_list_ptr->panic_timer_retransmissions;
DEBUG("RATS: Sending panic packet to node %d (retries=%d)\n", ts_list_ptr->node_id, ts_list_ptr->panic_timer_retransmissions);
post_net(s->pid, s->pid, MSG_PANIC, 0, NULL, 0, ts_list_ptr->node_id);
//The retransmission period should be INITIAL_TRANSMISSION_PERIOD
ts_list_ptr->panic_timer_counter = 1;
}
else
{
DEBUG("RATS: Removing node %d from list of parents\n", ts_list_ptr->node_id);
/* Found the item to be deleted,
re-link the list around it */
if( ts_list_ptr == s->ts_list )
/* We're deleting the head */
s->ts_list = ts_list_ptr->next;
else
ts_previous_list_ptr->next = ts_list_ptr->next;
ts_delete_list_ptr = ts_list_ptr;
ts_list_ptr = ts_list_ptr->next;
/* Free the node */
sys_free( ts_delete_list_ptr );
continue;
}
}
ts_previous_list_ptr = ts_list_ptr;
ts_list_ptr = ts_list_ptr->next;
}
//If the parent list is empty, then we're stopping the panic timer
if(s->ts_list == NULL)
{
DEBUG("RATS: Parent list is empty. Stopping panic timer\n");
#ifdef USE_PANIC_PACKETS
sys_timer_stop(PANIC_TIMER);
#endif //USE_PANIC_PACKETS
}
break;
}
default:
break;
}
return SOS_OK;
}
case MSG_PERIOD_CHANGE:
{
uint16_t temp_transmission_period;
DEBUG("RATS: Received packet for period change from %d\n", msg->saddr);
LED_DBG(LED_YELLOW_TOGGLE);
if((msg->data == NULL) || (msg->len != sizeof(uint16_t)) )
{
DEBUG("RATS: Invalid parameters in MSG_PERIOD_CHANGE\n");
break;
}
temp_transmission_period = (* (uint16_t*)(msg->data));
//Change period if:
//a)received period is smaller than period in use
//b)node that sent period is the one that has the current smallest period
//c)I am currently using myself as the node with the smallest period (used in the beginning and in transitive modes)
if((temp_transmission_period < s->ts_packet.transmission_period)
|| (s->ts_packet.min_period_node_id == msg->saddr)
|| (s->ts_packet.min_period_node_id == ker_id()) )
{
DEBUG("RATS: Changing period (new_period=%d new_node=%d). Sending to UART\n", temp_transmission_period, msg->saddr);
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
#ifdef UART_DEBUG
period_packet_t * period_packet_ptr = sys_malloc(sizeof(period_packet_t));
period_packet_ptr->saddr = msg->saddr;
period_packet_ptr->old_period = s->ts_packet.transmission_period;
period_packet_ptr->new_period = temp_transmission_period;
post_uart(s->pid, s->pid, UART_PERIOD_CHANGE, sizeof(period_packet_t), period_packet_ptr, SOS_MSG_RELEASE, UART_ADDRESS);
#endif //UART_DEBUG
s->ts_packet.transmission_period = temp_transmission_period;
s->ts_packet.min_period_node_id = msg->saddr;
s->transmit_timer_counter = (uint16_t)(s->ts_packet.transmission_period / MIN_SAMPLING_PERIOD);
s->validation_timer_counter = s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
sys_timer_start(TRANSMIT_TIMER, INITIAL_TRANSMISSION_PERIOD*1024, TIMER_REPEAT);
sys_timer_start(VALIDATION_TIMER, INITIAL_TRANSMISSION_PERIOD*1024, TIMER_REPEAT);
}
return SOS_OK;
}
case MSG_TIMESTAMP:
{
ts_packet_t *ts_packet_ptr = (ts_packet_t *)msg->data;
DEBUG("RATS: MSG_TIMESTAMP with type = %d\n", ts_packet_ptr->type);
if(ts_packet_ptr->type == NORMAL_PACKET)
{
DEBUG("RATS: Receiving timestamp data from node %d\n", msg->saddr);
if( add_values(s, msg) == TRUE)
{
LED_DBG(LED_GREEN_TOGGLE);
DEBUG("RATS: Accessed internal structure\n");
}
else
{
DEBUG("RATS: Discarding MSG_TIMESTAMP from node %d\n", msg->saddr);
}
}
else // TEST_PACKET
{
if(ker_id() == ROOT_NODE)
{
DEBUG("RATS: Receiving test data from node %d. Sending to UART\n", msg->saddr);
#ifdef UART_DEBUG
ext_packet_t * ext_packet_ptr = (ext_packet_t *)msg->data;
debug_packet_t * debug_packet_ptr = (debug_packet_t *)sys_malloc(sizeof(debug_packet_t));
debug_packet_ptr->time[0] = ticks_to_msec_float(ext_packet_ptr->time[0]);
debug_packet_ptr->time[1] = ticks_to_msec_float(ext_packet_ptr->time[1]);
debug_packet_ptr->node_id = ker_id();
debug_packet_ptr->int_parent_time = ext_packet_ptr->time[1];
post_uart(s->pid, s->pid, UART_FORWARD_EXT, sizeof(debug_packet_t), debug_packet_ptr, SOS_MSG_RELEASE, UART_ADDRESS);
#endif //UART_DEBUG
}
else
{
DEBUG("RATS: Receiving test data from node %d. Sending to parent\n", msg->saddr);
#ifdef UART_DEBUG
ext_packet_t * ext_packet_ptr = (ext_packet_t *)msg->data;
uint32_t parent_time = convert_from_mine_to_parent_time(ext_packet_ptr->time[1], ROOT_NODE);
//Break if the parent is not found in the timestamping list
if(parent_time == 0)
{
break;
}
debug_packet_t * debug_packet_ptr = (debug_packet_t *)sys_malloc(sizeof(debug_packet_t));
debug_packet_ptr->time[0] = ticks_to_msec_float(ext_packet_ptr->time[0]);
debug_packet_ptr->time[1] = ticks_to_msec_float(parent_time);
debug_packet_ptr->int_parent_time = parent_time;
debug_packet_ptr->node_id = ker_id();
post_uart(s->pid, s->pid, UART_FORWARD_EXT, sizeof(debug_packet_t), debug_packet_ptr, SOS_MSG_RELEASE, UART_ADDRESS);
#endif //UART_DEBUG
}
}
break;
}
case MSG_PERIOD_REQUEST:
{
DEBUG("RATS: Received MSG_PERIOD_REQUEST packet from node %d\n", msg->saddr);
timesync_t * temp_ts_ptr = get_timesync_ptr(s, msg->saddr);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Discarding MSG_PERIOD_REQUEST\n");
break;
}
uint16_t *sampling_period = (uint16_t *)sys_malloc(sizeof(uint16_t));
if(sampling_period != NULL)
{
*sampling_period = temp_ts_ptr->sampling_period;
DEBUG("RATS: Sending MSG_PERIOD_REPLY packet (period=%d) to node %d\n", *sampling_period, msg->saddr);
post_net(s->pid, s->pid, MSG_PERIOD_REPLY, sizeof(uint16_t), sampling_period, SOS_MSG_RELEASE, msg->saddr);
}
break;
}
case MSG_PERIOD_REPLY:
{
uint16_t transmission_period;
DEBUG("RATS: Received MSG_PERIOD_REPLY packet from node %d\n", msg->saddr);
memcpy(&transmission_period, &msg->data[0], sizeof(transmission_period));
s->validation_timer_counter = s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
if((transmission_period < s->validation_period) || (s->validation_node_id == ker_id() ) )
{
DEBUG("RATS: Changing VALIDATION period (new_period=%d new_node=%d)\n", transmission_period, msg->saddr);
s->validation_period = transmission_period;
s->validation_node_id = msg->saddr;
}
break;
}
case MSG_PANIC:
{
//Transmit MSG_TIMESTAMP, restart timer, recalculate value for transmit_timer_counter
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
#ifdef UART_DEBUG
uint16_t *data = (uint16_t *)sys_malloc(sizeof(uint16_t));
*data = msg->saddr;
post_uart(s->pid, s->pid, UART_PANIC, sizeof(uint16_t), data, SOS_MSG_RELEASE, UART_ADDRESS);
#endif //UART_DEBUG
post_net(s->pid, s->pid, MSG_TIMESTAMP, sizeof(ts_packet_t), &s->ts_packet, 0, BCAST_ADDRESS);
s->transmit_timer_counter = (uint16_t)(s->ts_packet.transmission_period / MIN_SAMPLING_PERIOD);
s->validation_timer_counter = s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
sys_timer_start(TRANSMIT_TIMER, INITIAL_TRANSMISSION_PERIOD*1024, TIMER_REPEAT);
sys_timer_start(VALIDATION_TIMER, INITIAL_TRANSMISSION_PERIOD*1024, TIMER_REPEAT);
break;
}
case MSG_INVALIDATE_ENTRY:
{
DEBUG("RATS: Received invalidation message from node %d\n", msg->saddr);
timesync_t * temp_ts_ptr = get_timesync_ptr(s, msg->saddr);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Discarding MSG_INVALIDATE_ENTRY\n");
break;
}
DEBUG("RATS: Invalidation entry for node %d\n", msg->saddr);
temp_ts_ptr->packet_count = 0;
temp_ts_ptr->a = 0;
temp_ts_ptr->b = 0;
temp_ts_ptr->sampling_period = INITIAL_TRANSMISSION_PERIOD;
temp_ts_ptr->window_size = (uint8_t)BUFFER_SIZE;
temp_ts_ptr->panic_timer_counter = 5; //(s->ts_list->sampling_period / INITIAL_TRANSMISSION_PERIOD) + 4;
temp_ts_ptr->panic_timer_retransmissions = PANIC_TIMER_RETRANSMISSIONS;
memset(temp_ts_ptr->timestamps, 0, BUFFER_SIZE*sizeof(uint32_t));
memset(temp_ts_ptr->my_time, 0, BUFFER_SIZE*sizeof(uint32_t));
//Notify node to start procedure from beginning
post_net(s->pid, s->pid, MSG_RATS_SERVER_START, 0, NULL, 0, msg->saddr);
break;
}
case MSG_FINAL:
{
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
#ifdef USE_PANIC_PACKETS
sys_timer_stop(PANIC_TIMER);
#endif //USE_PANIC_PACKETS
return SOS_OK;
}
default:
return -EINVAL;
}
/**
* Return SOS_OK for those handlers that have successfully been handled.
*/
return SOS_OK;
}
void sim_radio_init()
{
//! initialize node id and location
FILE *fid;
char line_buf[LINE_BUF_SIZE]; // line buffer
int j = 0;
if((fid = fopen(topofile, "r")) == NULL){
char* sosrootdir;
char newtopofile[256];
sosrootdir = getenv("SOSROOT");
strcpy(newtopofile, sosrootdir);
printf("Unable to open %s\n", topofile);
strcat(newtopofile, "/platform/sim/topo.def\0");
if((fid = fopen(newtopofile, "r")) == NULL){
printf("Unable to open %s\n", newtopofile);
exit(1);
}
else
topofile = newtopofile;
}
printf("Using topology file %s\n", topofile);
// remove comments
do{
fgets(line_buf, LINE_BUF_SIZE, fid);
} while(line_buf[0] == '#');
if(sscanf(line_buf, "%d", &totalNodes) != 1){
fprintf(stderr, "no data in %s\n", topofile);
exit(1);
}
topo_array = (Topology*)malloc((totalNodes + 1) * sizeof(Topology));
if (topo_array == NULL){
fprintf(stderr, "not enough memory\n");
exit(1);
}
for(j = 0; j < totalNodes; j++){
do{
// remove comments
fgets(line_buf, LINE_BUF_SIZE, fid);
}while(line_buf[0] == '#');
if(sscanf(line_buf, "%d %d %d %d %d %u",
&topo_array[j].id,
&topo_array[j].unit,
&topo_array[j].x,
&topo_array[j].y,
&topo_array[j].z,
&topo_array[j].r2) != 6){
fprintf(stderr, "not enough definitions in %s: %s\n", topofile, line_buf);
exit(1);
}
//topo_array[j].id = j;
topo_array[j].type = TOPO_TYPE_OTHER;
topo_array[j].sock = -1;
}
#if 0
print_nodes();
exit(1);
#endif
if(fclose(fid) != 0){
perror("fclose");
exit(1);
}
// finding node id by finding available port
//for(j = 1; j <= totalNodes; j++)
{
int sock;
int myj = getj(ker_id());
struct sockaddr_in name;
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock < 0) {
perror("opening datagram socket");
exit(1);
}
/* Create name with wildcards. */
name.sin_family = AF_INET;
name.sin_addr.s_addr = INADDR_ANY;
// name.sin_port = htons(20000 + ker_id());
name.sin_port = htons( get_sin_port(ker_id()) );
if (bind(sock, (struct sockaddr *)&name, sizeof(name)) == 0) {
//node_address = j;
// successfully get an id
topo_array[myj].sock = sock;
topo_array[myj].type = TOPO_TYPE_SELF;
topo_self = topo_array[myj];
//! assign sos_info
node_loc.x = topo_self.x;
node_loc.y = topo_self.y;
node_loc.z = topo_self.z;
node_loc.unit = topo_self.unit;
}else{
fprintf(stderr, "Unable to allocate UDP for this node!\n");
fprintf(stderr, "Perhaps you are using the same node ID\n");
fprintf(stderr, "Use -n <node address> to specify different address\n\n");
exit(1);
}
}
{
uint16_t id;
node_loc_t self;
id = ker_id();
self = ker_loc();
if((id != topo_self.id) || (self.x != topo_self.x) ||
(self.y != topo_self.y) || (self.z != topo_self.z) ||
(self.unit != topo_self.unit)){
fprintf(stderr, "topo file and self settings do not agree.\n");
fprintf(stderr, "ker_id() = %d : topo_self.id = %d\n",
id, topo_self.id);
fprintf(stderr, "self.unit = %d : topo_self.unit = %d\n",
self.unit, topo_self.unit);
fprintf(stderr, "self.x = %d : topo_self.x = %d\n",
self.x, topo_self.x);
fprintf(stderr, "self.y = %d : topo_self.y = %d\n",
self.y, topo_self.y);
fprintf(stderr, "self.z = %d : topo_self.z = %d\n",
self.z, topo_self.z);
exit(1);
}
}
for(j = 0; j < totalNodes; j++){
uint32_t r2;
uint16_t id;
node_loc_t self, neighbor;
id = ker_id();
self = ker_loc();
if(topo_array[j].type == TOPO_TYPE_SELF) continue;
neighbor.unit = topo_array[j].unit;
neighbor.x = topo_array[j].x;
neighbor.y = topo_array[j].y;
neighbor.z = topo_array[j].z;
r2 = ker_loc_r2(&self,&neighbor);
if(r2 < 0){
fprintf(stderr, "units for neighbor do not agree.\n");
fprintf(stderr, "self.unit = %d : neighbor.unit = %d.\n",
self.unit, neighbor.unit);
exit(1);
}
DEBUG("neighbor %d r2 = %d, self r2 = %d\n", topo_array[j].id, r2, topo_self.r2);
if(r2 <= topo_self.r2){
topo_array[j].type = TOPO_TYPE_NEIGHBOR;
DEBUG("node %d is reachable\n", topo_array[j].id);
}
}
{
struct hostent *hostptr;
char theHost [MAXLENHOSTNAME];
unsigned long hostaddress;
/* find out who I am */
if ((gethostname(theHost, MAXLENHOSTNAME))<0) {
perror ("could not get hostname");
exit(1);
}
//DEBUG("-- found host name = %s\n", theHost);
if ((hostptr = gethostbyname (theHost)) == NULL) {
perror ("could not get host by name, use 127.0.0.1");
if(( hostptr = gethostbyname ("127.0.0.1") ) == NULL ) {
perror ("Cannot get host 127.0.0.1");
exit(1);
}
}
hostaddress = *((unsigned long *) hostptr->h_addr);
/* init the address structure */
sockaddr.sin_family = AF_INET;
sockaddr.sin_port = htons( get_sin_port(ker_id()) );
sockaddr.sin_addr.s_addr = hostaddress;
}
//! assign locations
//node_loc.x = (uint16_t)(topo_self.x);
//node_loc.y = (uint16_t)(topo_self.y);
//print_nodes();
}
