external_comm.c

无线通信的主要编程软件,是无线通信工作人员的必备工具,关天相关教程我会在后续传上.

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

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

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

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

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

/***************************************************************************
 * 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;
  // XXX PAL: Is there a chance that we don't write everything
  // and need to loop? Hope and pray, I guess...
  i = send(clifd, data, datalen, 0);
  EC_DEBUG(fprintf(stderr, "writeTossimEvent: waiting for ack...\n"));
  if (i >= 0) j = read(clifd, &ack, 1);
  EC_DEBUG(fprintf(stderr, "writeTossimEvent: ack received...\n"));
  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...
  }
  EC_DEBUG(fprintf(stderr, "writeTossimEvent: done\n"));
  return 0;
}

void buildTossimEvent(uint16_t moteID, uint16_t type, long long ftime, void *data,
                      unsigned char **msgp, int *lenp) {
  unsigned char *msg;
  int payload_size, total_size;
  
  // 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;
  case AM_VARIABLERESOLVERESPONSE:
    payload_size = sizeof(VariableResolveResponse);
    break;
  case AM_VARIABLEREQUESTRESPONSE:
    payload_size = sizeof(VariableRequestResponse);
    break;
  case AM_INTERRUPTEVENT:
    payload_size = sizeof(InterruptEvent);
    dbg(DBG_TEMP, "SIM: Sending InterruptEvent, payload is %i\n", (int)payload_size);
    break;
  case AM_LEDEVENT:
    payload_size = sizeof(LedEvent);
    break;
  default:
    EC_DEBUG(fprintf(stderr, "buildTossimEvent 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, "buildTossimEvent: msgType %d (0x%02x) moteID %d (0x%02x) payload size %d total size %d\n", type, type, moteID, moteID, payload_size, total_size));


  *msgp = msg;
  *lenp = total_size;
}

/* 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 total_size;
  int n;
  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));

  buildTossimEvent(moteID, type, ftime, data, &msg, &total_size);

  for (n = 0; n < MAX_CLIENT_CONNECTIONS; n++) {
    if (clients[n] != -1 && ((type & eventMask) != 0)) {
      if (writeTossimEvent(msg, total_size, clients[n]) < 0) {
	// Socket closed
	pthread_mutex_lock(&eventClientsLock);
	eventClients[n] = -1;
	pthread_mutex_unlock(&eventClientsLock);
      }
    }
  }
  EC_DEBUG(fprintf(stderr, "Sent.\n"));
  free(msg);
}