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