external_comm.c

nesC写的heed算法

	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;
      }
    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);
      }
  }
}

// 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;

  default:
    dbg_clear(DBG_SIM, "SIM: Unrecognizable command type received from TinyViz %i\n", msg->msgType);
    break;
  }

  event_cleanup(event);
}

// 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) {
  GuiMsg* msg; 
  unsigned char *header;
  char* payLoad = NULL;
  int curlen = 0;
  int rval;
  unsigned char ack;

  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);
      }
    }
  }

  processCommand(msg, payLoad);

  do {
    rval = write(clifd, &ack, 1);
    if (rval < 0) {
      dbg(DBG_SIM, "SIM: Closing client socket %d.\n", clifd);
      goto done;
    }
  } while (rval != 1);

done:
  return 0;
}

/***************************************************************************
 * Command read thread
 ***************************************************************************/

void *commandReadThreadFunc(void *arg) {
  int i, found = 0;
  fd_set readset, exceptset;
  int highest;
  int numclients;
  struct timeval tv;

  dbg_clear(DBG_SIM, "SIM: commandReadThread running.\n");

  while (1) {
    // Wait for clients
    pthread_mutex_lock(&commandClientsLock);
    found = 0;
    while (!found) {
      FD_ZERO(&readset);
      FD_ZERO(&exceptset);
      highest = -1;
      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);
	  found = 1;
	  numclients++;
	}
      }
      if (!found) {
	EC_DEBUG(fprintf(stderr, "SIM: commandReadThread: Waiting for clients\n"));
	pthread_cond_wait(&commandClientsCond, &commandClientsLock);  
      }
    }
    pthread_mutex_unlock(&commandClientsLock);

    EC_DEBUG(fprintf(stderr, "SIM: commandReadThread: Doing select, %d clients, highest %d\n", numclients, highest));
    // Need timeout so that we pick up new clients
    tv.tv_sec = 5;
    tv.tv_usec = 0;
    if (select(highest+1, &readset, NULL, &exceptset, &tv) < 0) {
      dbg_clear(DBG_SIM, "SIM: commandReadThreadFunc: error in select(): %s\n", strerror(errno));
    }
    EC_DEBUG(fprintf(stderr, "SIM: commandReadThread: Returned from select\n"));

    pthread_mutex_lock(&commandClientsLock);
    // Read from clients and check for errors
    for (i = 0; i < MAX_CLIENT_CONNECTIONS; i++) {
      EC_DEBUG(fprintf(stderr, "SIM: commandClients[i] %d except %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]) < 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;
      }
    }
    pthread_mutex_unlock(&commandClientsLock);
  }
  return 0;
}

/***************************************************************************
 * Writing events
 ***************************************************************************/

// Write an event to the given client socket and wait for an ACK.
// Returns 0 if successful, -1 if the client connection was closed
int writeTossimEvent(void *data, int datalen, int clifd) {
  unsigned char ack;
  int i, j;

  /* Debugging only */
  /* fprintf(stderr,"WRITING: ");
   * for (i = 0; i < datalen; i++) {
   *   fprintf(stderr,"%2x ", ((unsigned char *)data)[i]);
   * }
   * fprintf(stderr,"\n");
   */

  EC_DEBUG(fprintf(stderr,"writeTossimEvent: fd %d datalen %d (0x%2x)\n", clifd, datalen, datalen));
  j = 0;
  i = send(clifd, data, datalen, 0);
  if (i >= 0) j = read(clifd, &ack, 1);
  if ((i < 0)  || (j < 0)) {
    EC_DEBUG(fprintf(stderr, "writeTossimEvent: Socket closed: %s\n", strerror(errno)));
    close(clifd);
    return -1;
    // XXX MDW: If -gui, should really wait for a new connection?
    // That's painful if we have multiple clients...
  }
  return 0;
}

/* Send a TOSSIM event to all clients connected to the event port.
 * Note that this requires waiting for an ACK from each client in turn.
 */
void sendTossimEvent(uint16_t moteID, uint16_t type, long long ftime, void *data) {
  unsigned char *msg;
  int n;
  int payload_size, total_size;
  int numclients = 0;
  int clients[MAX_CLIENT_CONNECTIONS];

  if (!socketsInitialized) return;

  pthread_mutex_lock(&eventClientsLock);
  while (numclients == 0) {
    for (n = 0; n < MAX_CLIENT_CONNECTIONS; n++) {
      clients[n] = -1;
      if (eventClients[n] != -1) {
	clients[n] = eventClients[n];
	numclients++;
      }
    }
    // If no clients and '-gui', wait for a connection
    if (numclients == 0 && GUI_enabled) {
      EC_DEBUG(fprintf(stderr, "sendTossimEvent waiting for connection\n"));
      pthread_cond_wait(&eventClientsCond, &eventClientsLock);
      EC_DEBUG(fprintf(stderr, "sendTossimEvent woke up\n"));
    } else if (numclients == 0) {
      // No clients, but don't wait around for them
      pthread_mutex_unlock(&eventClientsLock);
      return;
    }
  }
  pthread_mutex_unlock(&eventClientsLock);

  EC_DEBUG(fprintf(stderr, "sendTossimEvent: msgType %d (0x%02x) moteID %d (0x%02x)\n", type, type, moteID, moteID));

  // Determine payload size
  
  switch (type) {
    case AM_DEBUGMSGEVENT: 
      payload_size = sizeof(DebugMsgEvent);
      break;
    case AM_RADIOMSGSENTEVENT:
      payload_size = sizeof(RadioMsgSentEvent);
      break;
    case AM_UARTMSGSENTEVENT:
      payload_size = sizeof(RadioMsgSentEvent);
      break;
    case AM_ADCDATAREADYEVENT:
      payload_size = sizeof(ADCDataReadyEvent);
      break;
    case AM_TOSSIMINITEVENT:
      payload_size = sizeof(TossimInitEvent);
      break;
    default:
      EC_DEBUG(fprintf(stderr, "sendTossimEvent for invalid type: %d", type));
      return;
  }

  total_size = GUI_MSG_HEADER_LENGTH + payload_size;
  msg = (unsigned char *)malloc(total_size);

  *(unsigned short *)(&msg[0]) = htons(type);
  *(unsigned short *)(&msg[2]) = htons(moteID);
  *(long long *)(&msg[4]) = htonll(ftime);
  *(unsigned short *)(&msg[12]) = htons(payload_size);
  memcpy(((unsigned char *)msg)+GUI_MSG_HEADER_LENGTH, data, payload_size);

  EC_DEBUG(fprintf(stderr, "sendTossimEvent: msgType %d (0x%02x) moteID %d (0x%02x) payload size %d total size %d\n", type, type, moteID, moteID, payload_size, total_size));

  for (n = 0; n < MAX_CLIENT_CONNECTIONS; n++) {
    if (clients[n] != -1) {
      if (writeTossimEvent(msg, total_size, clients[n]) < 0) {
	// Socket closed
	pthread_mutex_lock(&eventClientsLock);
	eventClients[n] = -1;
	pthread_mutex_unlock(&eventClientsLock);
      }
    }
  }

  free(msg);
}