void printTransport(transport *input){
int i;
dbg("transport", "Transport Packet - SrcPort: %hhu, DestPort: %hhu, Type: ", input->srcPort, input->destPort);
switch(input->type){
case TRANSPORT_SYN:
dbg_clear("transport", "SYN ");
break;
case TRANSPORT_ACK:
dbg_clear("transport", "ACK ");
break;
case TRANSPORT_FIN:
dbg_clear("transport", "FIN ");
break;
case TRANSPORT_DATA:
dbg_clear("transport", "DATA ");
break;
default:
dbg_clear("transport", "UNKNOWN");
}
dbg_clear("transport", "Window: %lu, Seq: %lu, Length: %lu, Payload: ", input->window, input->seq, input->length, input->payload);
for(i=0; i<input->length; i++){
dbg_clear("transport", "%hhu, ", input->payload[i]);
}
dbg_clear("transport", "\n");
}
void send_route_to_random_pair() {
short from,to;
Coordinates *coords_from,*coords_to;
dbg_clear(DBG_USR3,"II:Choosing a random pair to route\n");
if (select_random_routing_pair(&from,&to,&coords_from,&coords_to) == SUCCESS) {
dbg_clear(DBG_USR3,"II:Selected pair: from %d to %d\n",from,to);
dbg_clear(DBG_USR3,"II:Coordinates for dest mote %d: ",to);
coordinates_print(DBG_USR3,coords_to);
//Send route command
send_route_to_command(from,to,coords_to);
} else {
dbg_clear(DBG_USR3,"II:Failed to select pair, skipping slot\n");
}
scheduleInterrupt(INT_EVENT_SEND_MSG, tos_state.tos_time + get_next_route_interval());
}
int acceptConnection(int servfd) {
struct sockaddr_in cli_addr;
int clilen = sizeof(cli_addr);
int clifd;
EC_DEBUG(dbg_clear(DBG_SIM, "SIM: Waiting for connection on socket %d\n", servfd));
clifd = accept(servfd, (struct sockaddr*)&cli_addr, &clilen);
if (clifd < 0) {
EC_DEBUG(dbg_clear(DBG_SIM, "SIM: Could not accept socket: %s\n", strerror(errno)));
// MDW: Maybe want to return -1 and keep going
exit(-1);
}
EC_DEBUG(dbg_clear(DBG_SIM, "SIM: Accepted client socket: fd %d\n", clifd));
return clifd;
}
void *eventAcceptThreadFunc(void *arg) {
int clifd;
fd_set acceptset;
dbg_clear(DBG_SIM, "SIM: eventAcceptThread running.\n");
while (1) {
FD_ZERO(&acceptset);
FD_SET(eventServerSocket, &acceptset);
EC_DEBUG(fprintf(stderr, "SIM: eventAcceptThread: calling select\n"));
if (select(eventServerSocket + 1, &acceptset, NULL, NULL, NULL) < 0) {
EC_DEBUG(fprintf(stderr, "SIM: eventAcceptThreadFunc: error in select(): %s\n", strerror(errno)));
}
EC_DEBUG(fprintf(stderr, "SIM: eventAcceptThread: select returned\n"));
if (FD_ISSET(eventServerSocket, &acceptset)) {
EC_DEBUG(fprintf(stderr, "SIM: eventAcceptThread: Checking for event connection\n"));
clifd = acceptConnection(eventServerSocket);
EC_DEBUG(fprintf(stderr, "SIM: eventAcceptThread: Got event connection %d\n", clifd));
pthread_mutex_lock(&eventClientsLock);
addClient(eventClients, clifd);
sendInitEvent(clifd);
pthread_cond_broadcast(&eventClientsCond);
pthread_mutex_unlock(&eventClientsLock);
}
}
return 0;
}
//choose a mote that is alive and is not a beacon
void periodic_kill_motes() {
short victim;
if (select_mote_to_kill(&victim)) {
dbg_clear(DBG_USR3,"II: Killing mote %d at %llu\n",victim,tos_state.tos_time);
nido_stop_mote(victim);
scheduleInterrupt(INT_EVENT_KILL_MOTE, tos_state.tos_time + get_next_kill_interval());
}
//else stop: no one else to kill
}
void waitForGuiConnection() {
int numclients = 0;
int n;
dbg_clear(DBG_SIM, "SIM: Waiting for connection from GUI...\n");
pthread_mutex_lock(&eventClientsLock);
while (numclients == 0) {
for (n = 0; n < MAX_CLIENT_CONNECTIONS; n++) {
if (eventClients[n] != -1) {
dbg_clear(DBG_SIM, "SIM: Got client connection fd %d\n", eventClients[n]);
numclients++;
}
}
if (numclients == 0) {
pthread_cond_wait(&eventClientsCond, &eventClientsLock);
}
}
pthread_mutex_unlock(&eventClientsLock);
}
int createServerSocket(short port) {
struct sockaddr_in sock;
int sfd;
int rval = -1;
long enable = 1;
memset(&sock, 0, sizeof(sock));
sock.sin_family = AF_INET;
sock.sin_port = htons(port);
sock.sin_addr.s_addr = htonl(localAddr);
sfd = socket(AF_INET, SOCK_STREAM, 0);
if (sfd < 0) {
dbg_clear(DBG_SIM|DBG_ERROR, "SIM: Could not create server socket: %s\n", strerror(errno));
exit(-1);
}
setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, (char *)&enable, sizeof(int));
while(rval < 0) {
rval = bind(sfd, (struct sockaddr*)&sock, sizeof(sock));
if (rval < 0) {
dbg_clear(DBG_SIM|DBG_ERROR, "SIM: Could not bind server socket to port %d: %s\n", port, strerror(errno));
dbg_clear(DBG_SIM|DBG_ERROR, "SIM: Perhaps another copy of TOSSIM is already running?\n");
dbg_clear(DBG_SIM|DBG_ERROR, "SIM: Will retry in 10 seconds.\n");
sleep(10);
}
}
if (listen(sfd, 1) < 0) {
dbg_clear(DBG_SIM|DBG_ERROR, "SIM: Could not listen on server socket: %s\n", strerror(errno));
exit(-1);
}
dbg_clear(DBG_SIM, "SIM: Created server socket listening on port %d.\n", port);
return sfd;
}
/*This function is called whenever an interrupt event is
*triggered. This allows us to dispatch different handlers
*based on id. Rescheduling can be done by calling
*scheduleInterrupt(id,time)
*/
void internalInterruptHandler(uint32_t id) {
dbg_clear(DBG_USR3,"II:InternalInterruptHandler %llu!\n",tos_state.tos_time);
switch(id) {
case INT_EVENT_FIRST:
schedule_first_events();
break;
case INT_EVENT_SEND_MSG:
send_route_to_random_pair();
break;
case INT_EVENT_KILL_MOTE:
periodic_kill_motes();
break;
default:
break;
}
}
void initializeSockets() {
int i;
dbg_clear(DBG_SIM, "SIM: Initializing sockets\n");
pthread_mutex_init(&(tos_state.pause_lock), NULL);
pthread_cond_init(&(tos_state.pause_cond), NULL);
pthread_cond_init(&(tos_state.pause_ack_cond), NULL);
for (i = 0; i < MAX_CLIENT_CONNECTIONS; i++) {
commandClients[i] = -1;
eventClients[i] = -1;
batchState[i] = 0;
}
eventMask = 0xffff;
commandServerSocket = createServerSocket(COMMAND_PORT);
eventServerSocket = createServerSocket(EVENT_PORT);
pthread_mutex_init(&eventClientsLock, NULL);
pthread_cond_init(&eventClientsCond, NULL);
pthread_create(&eventAcceptThread, NULL, eventAcceptThreadFunc, NULL);
pthread_create(&commandReadThread, NULL, commandReadThreadFunc, NULL);
socketsInitialized = 1;
}
void schedule_first_events() {
dbg_clear(DBG_USR3,"II:schedule_first_events, at %llu\n",tos_state.tos_time);
scheduleInterrupt(INT_EVENT_SEND_MSG, (long long)MSGS_INITIAL_WAIT*4000000);
scheduleInterrupt(INT_EVENT_KILL_MOTE, (long long)KILL_INITIAL_WAIT*4000000);
}
void internalInterruptInit() {
dbg_clear(DBG_USR3,"II:InternalInterruptInit! %llu\n",tos_state.tos_time);
scheduleInterrupt(INT_EVENT_FIRST, tos_state.tos_time);
}
void *commandReadThreadFunc(void *arg) {
int i;
fd_set readset, exceptset;
int highest;
int numclients;
dbg_clear(DBG_SIM, "SIM: commandReadThread running.\n");
while (1) {
// Build up the fd_set
FD_ZERO(&readset);
FD_ZERO(&exceptset);
FD_SET(commandServerSocket, &readset);
FD_SET(commandServerSocket, &exceptset);
highest = commandServerSocket;
numclients = 0;
for (i = 0; i < MAX_CLIENT_CONNECTIONS; i++) {
if (commandClients[i] != -1) {
if (commandClients[i] > highest) highest = commandClients[i];
EC_DEBUG(fprintf(stderr, "SIM: commandReadThread: Adding fd %d to select set\n",
commandClients[i]));
FD_SET(commandClients[i], &readset);
FD_SET(commandClients[i], &exceptset);
numclients++;
}
}
EC_DEBUG(fprintf(stderr, "SIM: commandReadThread: Doing select, %d clients, highest %d\n",
numclients, highest));
if (select(highest+1, &readset, NULL, &exceptset, 0) < 0) {
dbg_clear(DBG_SIM, "SIM: commandReadThreadFunc: error in select(): %s\n", strerror(errno));
}
EC_DEBUG(fprintf(stderr, "SIM: commandReadThread: Returned from select\n"));
// Read from clients and check for errors
for (i = 0; i < MAX_CLIENT_CONNECTIONS; i++) {
/*EC_DEBUG(fprintf(stderr, "SIM: commandClients[i] %d excepta %d read %d\n",
commandClients[i],
((commandClients[i] != -1)?
FD_ISSET(commandClients[i], &exceptset) : -1),
((commandClients[i] != -1)?
FD_ISSET(commandClients[i], &readset) : -1)));*/
if (commandClients[i] != -1 && FD_ISSET(commandClients[i], &readset)) {
if (readTossimCommand(commandClients[i], i) < 0) {
close(commandClients[i]);
commandClients[i] = -1;
}
}
if (commandClients[i] != -1 && FD_ISSET(commandClients[i], &exceptset)) {
// Assume we need to close this one
close(commandClients[i]);
commandClients[i] = -1;
}
}
// Check for new clients
if (FD_ISSET(commandServerSocket, &readset)) {
int clifd;
EC_DEBUG(fprintf(stderr, "SIM: commandReadThread: accepting command connection\n"));
clifd = acceptConnection(commandServerSocket);
EC_DEBUG(fprintf(stderr, "SIM: commandReadThread: Got command connection %d\n", clifd));
addClient(commandClients, clifd);
}
}
return 0;
}
// Read in a command from the given client socket and process it.
// Returns 0 if successful, -1 if the client connection was closed
int readTossimCommand(int clifd, int clidx) {
GuiMsg* msg;
unsigned char *header;
char* payLoad = NULL;
int curlen = 0;
int rval;
unsigned char ack;
int reply;
unsigned char* replyMsg = 0;
int replyLen = 0;
dbg_clear(DBG_SIM, "SIM: Reading command from client fd %d\n", clifd);
header = (unsigned char *)malloc(GUI_MSG_HEADER_LENGTH);
msg = (GuiMsg*)malloc(sizeof(GuiMsg));
// read in header of GuiMsg
curlen = 0;
while (curlen < GUI_MSG_HEADER_LENGTH) {
dbg_clear(DBG_SIM, "SIM: Reading in GuiMsg header of size %d with length %d\n", GUI_MSG_HEADER_LENGTH, curlen);
rval = read(clifd, header + curlen, GUI_MSG_HEADER_LENGTH - curlen);
if (rval <= 0) {
dbg_clear(DBG_SIM, "SIM: Closing client socket %d.\n", clifd);
free(msg);
close(clifd);
goto done;
} else {
curlen += rval;
}
}
// fill in values into allocated GuiMsg
msg->msgType = ntohs(*(unsigned short *)&header[0]);
msg->moteID = ntohs(*(unsigned short *)&header[2]);
msg->time = ntohll(*(long long *)&header[4]);
msg->payLoadLen = ntohs(*(unsigned short *)&header[12]);
dbg_clear(DBG_SIM, "SIM: Command type %d mote %d time 0x%lx payloadlen %d\n", msg->msgType, msg->moteID, msg->time, msg->payLoadLen);
if (msg->time < tos_state.tos_time) {
msg->time = tos_state.tos_time;
}
// read in payload
if (msg->payLoadLen > 0) {
payLoad = (char*)malloc(msg->payLoadLen);
curlen = 0;
while (curlen < msg->payLoadLen) {
dbg(DBG_SIM, "SIM: Reading in GuiMsg payload of size %d with length %d\n", msg->payLoadLen, curlen);
rval = read(clifd, payLoad + curlen, msg->payLoadLen - curlen);
if (rval <= 0) {
dbg(DBG_SIM, "SIM: Closing client socket %d.\n", clifd);
free(msg);
free(payLoad);
goto done;
} else {
curlen += rval;
dbg(DBG_SIM, "SIM: Read from command port, total: %d, need %d\n", curlen, msg->payLoadLen - curlen);
}
}
}
if (msg->moteID < tos_state.num_nodes) {
reply = processCommand(clifd, clidx, msg, payLoad, &replyMsg, &replyLen);
}
else {
dbg(DBG_SIM|DBG_ERROR, "SIM: Received command for invalid mote: %i\n", (int)msg->moteID);
}
// if we're in a batch, we don't send an ack for each command
if (batchState[clidx] != 0) {
if (reply) {
dbg(DBG_SIM|DBG_ERROR, "SIM: unexpected command response in batch!!\n");
}
return 0;
}
do {
rval = write(clifd, &ack, 1);
if (rval < 0) {
dbg(DBG_SIM, "SIM: Closing client socket %d.\n", clifd);
goto done;
}
} while (rval != 1);
if (reply) {
dbg(DBG_SIM, "SIM: Sending %d byte reply.\n", replyLen);
writeTossimEvent(replyMsg, replyLen, clifd);
free(replyMsg);
}
done:
return 0;
}
// Process commands that are posted to the event queue
void event_command_in_handle(event_t* event,
struct TOS_state* state) {
incoming_command_data_t* cmdData = (incoming_command_data_t*)event->data;
GuiMsg* msg = cmdData->msg;
dbg_clear(DBG_SIM, "SIM: Handling incoming command type %d for mote %d\n", msg->msgType, msg->moteID);
switch (msg->msgType) {
case AM_TURNONMOTECOMMAND:
dbg_clear(DBG_SIM, "SIM: Turning on mote %d\n", msg->moteID);
nido_start_mote(msg->moteID);
break;
case AM_TURNOFFMOTECOMMAND:
dbg_clear(DBG_SIM, "SIM: Turning off mote %d\n", msg->moteID);
nido_stop_mote(msg->moteID);
break;
case AM_RADIOMSGSENDCOMMAND:
{
RadioMsgSendCommand *rmsg = (RadioMsgSendCommand*)cmdData->payLoad;
TOS_MsgPtr buffer;
dbg_clear(DBG_SIM, "SIM: Enqueueing radio message for mote %d (payloadlen %d)\n", msg->moteID, msg->payLoadLen);
if (external_comm_buffers_[msg->moteID] == NULL)
external_comm_buffers_[msg->moteID] = &external_comm_msgs_[msg->moteID];
buffer = external_comm_buffers_[msg->moteID];
memcpy(buffer, &(rmsg->message), msg->payLoadLen);
buffer->group = TOS_AM_GROUP;
external_comm_buffers_[msg->moteID] = NIDO_received_radio(buffer);
}
break;
case AM_UARTMSGSENDCOMMAND:
{
UARTMsgSendCommand *umsg = (UARTMsgSendCommand*)cmdData->payLoad;
TOS_MsgPtr buffer;
int len = (msg->payLoadLen > sizeof(TOS_Msg))? sizeof(TOS_Msg):msg->payLoadLen;
dbg_clear(DBG_SIM, "SIM: Enqueueing UART message for mote %d (payloadlen %d)\n", msg->moteID, msg->payLoadLen);
if (external_comm_buffers_[msg->moteID] == NULL)
external_comm_buffers_[msg->moteID] = &external_comm_msgs_[msg->moteID];
buffer = external_comm_buffers_[msg->moteID];
memcpy(buffer, &(umsg->message), len);
buffer->group = TOS_AM_GROUP;
external_comm_buffers_[msg->moteID] = NIDO_received_uart(buffer);
}
break;
case AM_INTERRUPTCOMMAND:
{
InterruptEvent interruptEvent;
InterruptCommand* pcmd = (InterruptCommand*)cmdData->payLoad;
interruptEvent.id = pcmd->id;
dbg_clear(DBG_TEMP, "\nSIM: Interrupt command, id: %i.\n\n", pcmd->id);
sendTossimEvent(TOS_BCAST_ADDR, AM_INTERRUPTEVENT,
tos_state.tos_time, &interruptEvent);
break;
}
default:
dbg_clear(DBG_SIM, "SIM: Unrecognizable command type received from TinyViz %i\n", msg->msgType);
break;
}
event_cleanup(event);
}
// Actually process a command. Most command types are turned into
// events on the event queue, but others are processed instantaneously.
// if there's a response event, fill in reply_msg with the event and return 1
int processCommand(int clifd, int clidx, GuiMsg *msg, char *payLoad,
unsigned char** replyMsg, int* replyLen) {
int ret = 0;
switch (msg->msgType) {
case AM_SETLINKPROBCOMMAND:
{
SetLinkProbCommand *linkmsg = (SetLinkProbCommand*)payLoad;
double prob = ((double)linkmsg->scaledProb)/10000;
set_link_prob_value(msg->moteID, linkmsg->moteReceiver, prob);
break;
}
case AM_SETADCPORTVALUECOMMAND:
{
SetADCPortValueCommand *adcmsg = (SetADCPortValueCommand*)payLoad;
set_adc_value(msg->moteID, adcmsg->port, adcmsg->value);
break;
}
case AM_SETRATECOMMAND:
{
SetRateCommand *ratemsg = (SetRateCommand*)payLoad;
set_sim_rate(ratemsg->rate);
break;
}
case AM_VARIABLERESOLVECOMMAND:
{
VariableResolveResponse varResult;
VariableResolveCommand *rmsg = (VariableResolveCommand*)payLoad;
/*
* Note that the following will need to be changed on
* non-32bit systems.
*/
if (__nesc_nido_resolve(msg->moteID, (char*)rmsg->name,
(uintptr_t*)&varResult.addr,
(size_t*)&varResult.length) != 0)
{
varResult.addr = 0;
varResult.length = -1;
}
dbg_clear(DBG_SIM, "SIM: Resolving variable %s for mote %d: 0x%x %d\n",
rmsg->name, msg->moteID, varResult.addr, varResult.length);
buildTossimEvent(TOS_BCAST_ADDR, AM_VARIABLERESOLVERESPONSE,
tos_state.tos_time, &varResult, replyMsg, replyLen);
ret = 1;
break;
}
case AM_VARIABLEREQUESTCOMMAND:
{
VariableRequestResponse varResult;
VariableRequestCommand *rmsg = (VariableRequestCommand*)payLoad;
uint8_t* ptr = (uint8_t*)rmsg->addr;
varResult.length = rmsg->length;
if (varResult.length == 0)
varResult.length = 256; // special case
memcpy(varResult.value, ptr, varResult.length);
buildTossimEvent(TOS_BCAST_ADDR, AM_VARIABLEREQUESTRESPONSE,
tos_state.tos_time, &varResult, replyMsg, replyLen);
ret = 1;
break;
}
case AM_GETMOTECOUNTCOMMAND:
{
int i;
GetMoteCountResponse countResponse;
countResponse.totalMotes = tos_state.num_nodes;
bzero(&countResponse.bitmask, sizeof(countResponse.bitmask));
for (i = 0; i < TOSNODES; i++) {
countResponse.bitmask[i/8] |= (1 << (7 - (i % 8)));
}
buildTossimEvent(TOS_BCAST_ADDR, AM_GETMOTECOUNTRESPONSE,
tos_state.tos_time, &countResponse, replyMsg, replyLen);
ret = 1;
break;
}
case AM_SETDBGCOMMAND:
{
SetDBGCommand* cmd = (SetDBGCommand*)payLoad;
dbg_set(cmd->dbg);
break;
}
case AM_SETEVENTMASKCOMMAND:
{
SetEventMaskCommand* setMaskCommand = (SetEventMaskCommand*)payLoad;
eventMask = setMaskCommand->mask;
break;
}
case AM_BEGINBATCHCOMMAND:
{
if (batchState[clidx] != 0) {
dbg(DBG_SIM|DBG_ERROR, "SIM: duplicate begin batch");
}
dbg(DBG_SIM, "SIM: begin batch");
batchState[clidx] = 1;
break;
}
case AM_ENDBATCHCOMMAND:
{
if (batchState[clidx] == 0) {
dbg(DBG_SIM|DBG_ERROR, "SIM: end batch without begin");
}
dbg(DBG_SIM, "SIM: end batch");
batchState[clidx] = 0;
break;
}
default:
{
// For all other commands, place on the event queue.
// See event_command_in_handle for these
event_t* event = (event_t*)malloc(sizeof(event_t));
event_command_in_create(event, msg, payLoad);
dbg(DBG_SIM, "SIM: Enqueuing command event 0x%lx\n", (unsigned long)event);
TOS_queue_insert_event(event);
}
}
return ret;
}