Exemple #1
0
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);
}
Exemple #12
0
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;
}
Exemple #13
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;
}
Exemple #14
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);
}
Exemple #15
0
// 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;
}