Ejemplo n.º 1
0
Archivo: rats.c Proyecto: nesl/sos-2x
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);
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
Archivo: radio.c Proyecto: nesl/sos-2x
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);
	}
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 5
0
Archivo: rats.c Proyecto: nesl/sos-2x
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;
}
Ejemplo n.º 6
0
Archivo: rats.c Proyecto: nesl/sos-2x
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;
}
Ejemplo n.º 7
0
Archivo: radio.c Proyecto: nesl/sos-2x
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();
}