/**
* Program new rp and backtrack wsnet scheduler
**/
static void worldsens_backtrack_rq(uint64_t period){
union _worldsens_pkt pkt;
/* sync update */
ws_nsync = ws_csync + period;
ws_nsyncseq++;
ws_rp_updated = 1;
ws_synched = 0;
ws_backtrack = 1;
WSNET_S_DBG_EXC("WSNET2:: --> BACKTRACK (seq: %"PRId64")(rp seq: %"PRId64", period: %"PRId64", rp:%"PRId64")\n", ws_seq - 1, ws_nsyncseq, period, ws_nsync);
/* forge and send packet */
pkt.bktrk.type = WORLDSENS_S_BACKTRACK;
pkt.bktrk.seq = ws_seq++;
pkt.bktrk.rp_next = ws_nsyncseq;
pkt.bktrk.rp_duration = ws_nsync - ws_csync;
worldsens_packet_display(&pkt);
worldsens_packet_hton(&pkt);
wsens_srv_msend((char *) &pkt, sizeof(struct _worldsens_s_backtrack));
return;
}
static void worldsens_syncrelease(uint64_t clock) {
if (ws_connected == 0) {
return;
} else {
union _worldsens_pkt pkt;
/* sync updates */
ws_nsync = clock;
ws_nsyncseq++;
ws_rp_updated = 1;
ws_synched = 0;
/* forge and send packet */
pkt.sync_release.type = WORLDSENS_S_SYNC_RELEASE;
pkt.sync_release.seq = ws_seq++;
pkt.sync_release.rp_next = ws_nsyncseq;
pkt.sync_release.rp_duration = ws_nsync - ws_csync;
worldsens_packet_display(&pkt);
worldsens_packet_hton(&pkt);
wsens_srv_msend((char *) &pkt, sizeof(struct _worldsens_s_sync_release));
WSNET_S_DBG_DBG("WSNET2:: --> RELEASE (seq: %"PRId64")\n", ws_seq - 1);
WSNET_S_DBG_DBG("WSNET2:: --> RP (seq: %"PRId64") (rp seq: %"PRId64", period: %"PRId64", rp:%"PRId64")\n", ws_seq - 1, ws_nsyncseq, ws_nsync - ws_csync, ws_nsync);
}
}
/**
* TX data to wsnet
**/
int wsnet2_tx(char data, double freq, int mod, double txdB, uint64_t delay, int radio_id) {
char msg[WORLDSENS_MAX_PKTLENGTH];
union _worldsens_pkt pkt;
int len;
/* put doubles into uint64_t variables for swap */
// uint64_t *pfreq = (uint64_t *) &freq;
// uint64_t *ptxdB = (uint64_t *) &txdB;
/* format */
pkt.byte_tx.type = WORLDSENS_C_BYTE_TX;
pkt.byte_tx.node_id = wsens.id;
pkt.byte_tx.period = MACHINE_TIME_GET_NANO() - wsens.l_rp;
pkt.byte_tx.data = data;
// pkt.byte_tx.freq = *pfreq;
memcpy(&pkt.byte_tx.freq, &freq, sizeof(freq));
pkt.byte_tx.antenna_id = wsens.radio[radio_id].antenna_id;
pkt.byte_tx.wsnet_mod_id = wsnet_mod_id_map[mod];
pkt.byte_tx.wsim_mod_id = mod;
// pkt.byte_tx.power_dbm = *ptxdB;
memcpy(&pkt.byte_tx.power_dbm, &txdB, sizeof(txdB));
pkt.byte_tx.duration = delay;
worldsens_packet_dump(&pkt);
worldsens_packet_hton(&pkt);
/* send */
if (send(wsens.u_fd, (char *) (&pkt), sizeof(struct _worldsens_c_byte_tx), 0) < 0) {
perror("(send)");
goto error;
}
//WSNET2_DBG("libwsnet2:wsnet2_tx: machine time=%"PRIu64"\n", MACHINE_TIME_GET_NANO());
//WSNET2_DBG("libwsnet2:wsnet2_tx: wsens last rp=%"PRIu64"\n", wsens.l_rp);
WSNET2_TX("libwsnet2:wsnet2_tx: packet 0x%02x sent\n", data);
/* wait either for backtrack or rp reminder */
wsens.state = WORLDSENS_CLT_STATE_TXING;
while (wsens.state != WORLDSENS_CLT_STATE_IDLE) {
/* receive */
if ((len = recv(wsens.m_fd, msg, WORLDSENS_MAX_PKTLENGTH, 0)) <= 0) {
perror("(recv)");
goto error;
}
/* parse */
if (wsnet2_parse(msg))
return -1;
}
return 0;
error:
WSNET2_ERROR("libwsnet2:wsnet2_tx: error during tx\n");
wsnet2_finalize();
return -1;
}
void worldsens_tx_kill(nodeid_t id) {
union _worldsens_pkt pkt;
pkt.kill.type = WORLDSENS_S_KILL;
pkt.kill.seq = ws_seq++;
pkt.kill.node_id = id;
worldsens_packet_display(&pkt);
worldsens_packet_hton(&pkt);
wsens_srv_msend((char *) &pkt, sizeof(struct _worldsens_s_kill));
ws_connected--;
}
static void worldsens_quit(void) {
if (ws_connected == 0) {
return;
} else {
union _worldsens_pkt pkt;
pkt.killsim.type = WORLDSENS_S_KILLSIM;
pkt.killsim.seq = ws_seq++;
worldsens_packet_hton(&pkt);
worldsens_packet_display(&pkt);
wsens_srv_msend((char *) &pkt, sizeof(struct _worldsens_s_killsim));
ws_connected = 0;
}
}
static void worldsens_syncreminder(void) {
union _worldsens_pkt pkt;
/* forge and send packet */
pkt.sync_reminder.type = WORLDSENS_S_SYNC_REMINDER;
pkt.sync_reminder.seq = ws_seq++;
pkt.sync_reminder.rp_next = ws_nsyncseq;
worldsens_packet_display(&pkt);
worldsens_packet_hton(&pkt);
wsens_srv_msend((char *) &pkt, sizeof(struct _worldsens_s_sync_release));
WSNET_S_DBG_DBG("WSNET2:: --> RP REMINDER (seq: %"PRId64") (rp seq: %"PRId64")\n", ws_seq - 1, ws_nsyncseq);
}
/**
* Called when libwsnet is sync on rp. Wait for a new rp.
**/
static int wsnet2_sync(void) {
char msg[WORLDSENS_MAX_PKTLENGTH];
union _worldsens_pkt pkt;
int len;
/* format */
pkt.sync_ack.type = WORLDSENS_C_SYNC_ACK;
pkt.sync_ack.node_id = wsens.id;
pkt.sync_ack.rp_id = wsens.rpseq;
worldsens_packet_dump(&pkt);
worldsens_packet_hton(&pkt);
/* send */
if (send(wsens.u_fd, (char *) (&pkt), sizeof(struct _worldsens_c_sync_ack), 0) < 0) {
perror("(send)");
goto error;
}
WSNET2_DBG("libwsnet2:wsnet2_sync: synched on rp %d\n", wsens.rpseq);
/* wait for new rp */
wsens.state = WORLDSENS_CLT_STATE_PENDING;
while (wsens.state != WORLDSENS_CLT_STATE_IDLE &&
wsens.state != WORLDSENS_CLT_STATE_KILLED) {
/* receive */
if ((len = recv(wsens.m_fd, msg, WORLDSENS_MAX_PKTLENGTH, 0)) <= 0) {
perror("(recvfrom)");
goto error;
}
/* parse */
if (wsnet2_parse(msg))
return -1;
}
return 0;
error:
WSNET2_ERROR("libwsnet2:wsnet2_sync: Error during synchronization\n");
wsnet2_finalize();
return -1;
}
/**
* TX a measure request to wsnet
**/
int wsnet2_tx_measure_req(int measure_pos_id) {
char msg[WORLDSENS_MAX_PKTLENGTH];
union _worldsens_pkt pkt;
int len;
/* format */
pkt.measure_req.type = WORLDSENS_C_MEASURE_REQ;
pkt.measure_req.node_id = wsens.id;
pkt.measure_req.measure_id = wsens.measure[measure_pos_id].id;
pkt.measure_req.period = MACHINE_TIME_GET_NANO() - wsens.l_rp;
worldsens_packet_dump(&pkt);
worldsens_packet_hton(&pkt);
/* send */
if (send(wsens.u_fd, (char *) (&pkt), sizeof(struct _worldsens_c_byte_tx), 0) < 0) {
perror("(send)");
goto error;
}
/* wait for new rp */
wsens.state = WORLDSENS_CLT_STATE_TXING;
while (wsens.state != WORLDSENS_CLT_STATE_IDLE) {
/* receive */
if ((len = recv(wsens.m_fd, msg, WORLDSENS_MAX_PKTLENGTH, 0)) <= 0) {
perror("(recv)");
goto error;
}
/* parse */
if (wsnet2_parse(msg))
return -1;
}
return 0;
error:
WSNET2_ERROR("libwsnet2:wsnet2_tx_measure_req: Error during tx measure req\n");
wsnet2_finalize();
return -1;
}
/**
* Sends registering request to wsnet, and wait for response
**/
int wsnet2_subscribe(void) {
union _worldsens_pkt pkt;
char msg[WORLDSENS_MAX_PKTLENGTH];
int len;
/* format */
pkt.cnx_req.type = WORLDSENS_C_CONNECT_REQ;
pkt.cnx_req.node_id = wsens.id;
worldsens_packet_dump(&pkt);
worldsens_packet_hton(&pkt);
/* send */
if (send(wsens.u_fd, (char *) (&pkt), sizeof(struct _worldsens_c_connect_req), 0) < 0) {
perror("(send)");
goto error;
}
WSNET2_CNCT("libwsnet2:wsnet2_subscribe: Attempting to connect with id %d...\n", wsens.id);
/* wait for server response */
wsens.state = WORLDSENS_CLT_STATE_CONNECTING;
while (wsens.state != WORLDSENS_CLT_STATE_IDLE) {
/* receive */
if ((len = recv(wsens.u_fd, msg, WORLDSENS_MAX_PKTLENGTH, 0)) < 0) {
perror("(recv)");
goto error;
}
if (wsnet2_parse(msg))
return -1;
}
WSNET2_CNCT("libwsnet2:wsnet2_subscribe: Connection to server successfull \n");
return 0;
error:
WSNET2_ERROR("libwsnet2:wsnet2_subscribe: Error when receiving subscribe response\n");
wsnet2_finalize();
return -1;
}
int wsnet2_unsubscribe(void) {
union _worldsens_pkt pkt;
/* format */
pkt.disconnect.type = WORLDSENS_C_DISCONNECT;
pkt.disconnect.node_id = wsens.id;
worldsens_packet_dump(&pkt);
worldsens_packet_hton(&pkt);
/* send */
if (send(wsens.u_fd, (char *) (&pkt), sizeof(struct _worldsens_c_disconnect), 0) < 0) {
perror("(send)");
worldsens_packet_dump(&pkt);
goto error;
}
WSNET2_CNCT("libwsnet2:wsnet2_unsubscribe: Disconnected id %d\n", wsens.id);
return 0;
error:
WSNET2_ERROR("libwsnet2:wsnet2_unsubscribe: Error when sending unsubscribe request\n");
return -1;
}
/**
* Function to be callback for measure sending
**/
static int worldsens_callback_tx_measure(call_t *c, void *arg) {
struct _worldsens_c_measure_req *msg = (struct _worldsens_c_measure_req *) arg;
ws_rdv_t *next_rdv = worldsens_rdv_see_next();
event_t *next_event = scheduler_see_next();
double value;
if(next_event->priority == PRIORITY_CALLBACK && (uint8_t)(next_event->u.nodeid) == worldsens_get_wsnet_node_id(msg->node_id)) {
WSNET_S_DBG_DBG("WSNET2:: --> TX_MEASURE: same measure found in fifo, skip sending (ip:%d)\n", c->node);
return 0;
}
if(next_rdv->clock == get_time()) { /* send measure only, no rdv */
/* read measure */
READ_MEASURE(c, msg->measure_id, &value);
/* forge msg */
union _worldsens_pkt pkt_rsp;
pkt_rsp.measure_rsp.type = WORLDSENS_S_MEASURE_RSP;
pkt_rsp.measure_rsp.seq = ws_seq++;
pkt_rsp.measure_rsp.node_id = msg->node_id;
pkt_rsp.measure_rsp.measure_id = msg->measure_id;
pkt_rsp.measure_rsp.measure_val = value;
/* send data */
worldsens_packet_display(&pkt_rsp);
worldsens_packet_hton(&pkt_rsp);
if(wsens_srv_msend((char *) &pkt_rsp, sizeof(struct _worldsens_s_measure_rsp))){
return -1;
}
}
else { /* send measure and rdv */
/* read measure */
READ_MEASURE(c, msg->measure_id, &value);
/* update next rp */
ws_nsync = next_event->clock;
ws_nsyncseq++;
ws_synched = 0;
ws_rp_updated = 1;
/* forge msg */
union _worldsens_pkt pkt_sr_rsp;
pkt_sr_rsp.measure_sr_rsp.type = WORLDSENS_S_MEASURE_SR_RSP;
pkt_sr_rsp.measure_sr_rsp.seq = ws_seq++;
pkt_sr_rsp.measure_sr_rsp.node_id = msg->node_id;
pkt_sr_rsp.measure_sr_rsp.measure_id = msg->measure_id;
pkt_sr_rsp.measure_sr_rsp.measure_val = value;
pkt_sr_rsp.measure_sr_rsp.rp_next = ws_nsyncseq;
pkt_sr_rsp.measure_sr_rsp.rp_duration = ws_nsync - ws_csync;
/* send data */
worldsens_packet_display(&pkt_sr_rsp);
worldsens_packet_hton(&pkt_sr_rsp);
if(wsens_srv_msend((char *) &pkt_sr_rsp, sizeof(struct _worldsens_s_measure_sr_rsp))){
return -1;
}
}
return 0;
}
/**
* Retransmit data to nodes
**/
int worldsens_nodes_rx(call_t *c, packet_t *packet){
ws_rdv_t *next_rdv = worldsens_rdv_see_next();
if(next_rdv->clock == get_time()){ /* just send data (no rdv)*/
union _worldsens_pkt pkt_rx;
/* compute sinr */
double sinr = packet->rxdBm - mW2dBm(*(packet->noise_mW));
/* put doubles into uint64_t variables for swap */
uint64_t *prxdBm = (uint64_t *) &(packet->rxdBm);
uint64_t *pworldsens_freq = (uint64_t *) &(packet->worldsens_freq);
uint64_t *psinr = (uint64_t *) &(sinr);
/* forge msg */
pkt_rx.byte_rx.type = WORLDSENS_S_BYTE_RX;
pkt_rx.byte_rx.seq = ws_seq++;
pkt_rx.byte_rx.antenna_id = packet->antenna;
pkt_rx.byte_rx.wsim_mod_id = packet->worldsens_mod;
pkt_rx.byte_rx.freq = *pworldsens_freq;
pkt_rx.byte_rx.data = *(packet->data);
pkt_rx.byte_rx.node_id = worldsens_get_wsim_node_id(c->node);
pkt_rx.byte_rx.power_dbm = *prxdBm;
pkt_rx.byte_rx.sinr = *psinr;
/* send data */
worldsens_packet_display(&pkt_rx);
worldsens_packet_hton (&pkt_rx);
if(wsens_srv_msend((char *) &pkt_rx, sizeof(struct _worldsens_s_byte_rx))){
return -1;
}
WSNET_S_DBG_DBG("WSNET2:: --> RX (dest ip:%d, data:0x%02x, freq:%ghz, wsim modul:%d, power:%gdbm)\n", worldsens_get_wsim_node_id(c->node), (*(packet->data)) & 0xff, packet->worldsens_freq, packet->worldsens_mod, packet->rxdBm);
packet_dealloc(packet);
}
else { /* program new rdv and send data */
union _worldsens_pkt pkt_sr_rx;
/* compute sinr */
double sinr = packet->rxdBm - mW2dBm(*(packet->noise_mW));
/* put doubles into uint64_t variables for swap */
uint64_t *prxdBm = (uint64_t *) &(packet->rxdBm);
uint64_t *pworldsens_freq = (uint64_t *) &(packet->worldsens_freq);
uint64_t *psinr = (uint64_t *) &(sinr);
/* update next rdv */
ws_nsync = next_rdv->clock;
ws_nsyncseq++;
ws_synched = 0;
ws_rp_updated = 1;
/* forge msg */
pkt_sr_rx.byte_sr_rx.type = WORLDSENS_S_BYTE_SR_RX;
pkt_sr_rx.byte_sr_rx.seq = ws_seq++;
pkt_sr_rx.byte_sr_rx.rp_next = ws_nsyncseq;
pkt_sr_rx.byte_sr_rx.rp_duration = ws_nsync - ws_csync;
pkt_sr_rx.byte_sr_rx.antenna_id = packet->antenna;
pkt_sr_rx.byte_sr_rx.wsim_mod_id = packet->worldsens_mod;
pkt_sr_rx.byte_sr_rx.freq = *pworldsens_freq;
pkt_sr_rx.byte_sr_rx.data = *(packet->data);
pkt_sr_rx.byte_sr_rx.node_id = worldsens_get_wsim_node_id(c->node);
pkt_sr_rx.byte_sr_rx.power_dbm = *prxdBm;
pkt_sr_rx.byte_sr_rx.sinr = *psinr;
/* send data */
worldsens_packet_display(&pkt_sr_rx);
worldsens_packet_hton (&pkt_sr_rx);
if(wsens_srv_msend((char *) &pkt_sr_rx, sizeof(struct _worldsens_s_byte_sr_rx))){
return -1;
}
WSNET_S_DBG_DBG("WSNET2:: --> RX (dest ip:%d, data:0x%02x, freq:%ghz, wsim modul:%d, power:%gdbm)\n", worldsens_get_wsim_node_id(c->node), (*(packet->data)) & 0xff, packet->worldsens_freq, packet->worldsens_mod, packet->rxdBm);
WSNET_S_DBG_DBG("WSNET2:: --> RP (seq: %"PRId64") (rp seq: %"PRId64", period: %"PRId64", rp:%"PRId64")\n", ws_seq - 1, ws_nsyncseq, ws_nsync - ws_csync, ws_nsync);
packet_dealloc(packet);
}
return 0;
}
/**
* Forge response to a node connect request
*/
static void worldsens_rx_connect_req(struct _worldsens_c_connect_req *pkt, struct sockaddr_in *addr) {
node_t *node = get_node_by_id(worldsens_register_node_infos(pkt->node_id));
bundle_t *bundle = get_bundle_by_id(node->bundle);
entity_t *entity;
union _worldsens_pkt pkt0;
int offset = 0;
int i;
int nb_mod = 0;
if (node->worldsens != NODE_DISCONNECTED) {
//PRINT_WORLDSENS("wsnet:worldsens_rx_connect_req (%"PRId64"): node %d not disconnected!\n", get_time(), node->id);
return;
}
/* forge response*/
pkt0.cnx_rsp_ok.type = WORLDSENS_S_CONNECT_RSP_OK;
pkt0.cnx_rsp_ok.seq = ws_seq;
pkt0.cnx_rsp_ok.rp_next = ws_nsyncseq;
pkt0.cnx_rsp_ok.rp_duration = ws_nsync - ws_csync;
pkt0.cnx_rsp_ok.n_antenna_id = bundle->antenna.size;
pkt0.cnx_rsp_ok.n_measure_id = measures.size;
/* pkt->cnx_rsp_ok.names_and_ids format: */
/*|***************antennas***************|**************modulations*************|***************measures***************|*/
/*|ant id1|ant name1|ant id2|ant name2|..|mod id1|mod name1|mod id2|mod name2|..|mea id1|mea name1|mea id2|mea name2|..|*/
/* *********************************************************************************************************************/
/* forge list of available antennas */
for (i = 0; i < bundle->antenna.size; i++) {
entity = get_entity_by_id(bundle->antenna.elts[i]);
/* PRINT_WORLDSENS("wsnet: worldsens_rx_connect_req (%"PRId64"): antenna '%s'
(id %d) available\n", get_time(), entity->name, entity->id); */
*((uint32_t *) (pkt0.cnx_rsp_ok.names_and_ids + offset)) = entity->id;
offset += sizeof(uint32_t);
strcpy(pkt0.cnx_rsp_ok.names_and_ids + offset, entity->name);
offset += strlen(entity->name) + 1;
}
/* forge list of available modulations */
das_init_traverse(modulation_entities);
while ((entity = (entity_t *) das_traverse(modulation_entities)) != NULL) {
/*PRINT_WORLDSENS("wsnet: worldsens_rx_connect_req (%"PRId64"): modulation '%s'
(id %d) available\n", get_time(), entity->library.name, entity->id); */
*((uint32_t *) (pkt0.cnx_rsp_ok.names_and_ids + offset)) = entity->id;
offset += sizeof(uint32_t);
strcpy(pkt0.cnx_rsp_ok.names_and_ids + offset, entity->library.name);
offset += strlen(entity->library.name) + 1;
nb_mod++;
if ((nb_mod == 1) || (strcmp(entity->library.name, "modulation_none") == 0)) {
ws_default_mod = entity->id;
}
}
pkt0.cnx_rsp_ok.n_modulation_id = nb_mod;
/* forge list of available measure */
for (i = 0; i < measures.size; i++) {
measure_t *measure = get_measure_by_id(i);
/* PRINT_WORLDSENS("wsnet: worldsens_rx_connect_req (%"PRId64"): measure '%s'
(id %d) available\n", get_time(), measure->name, measure->id); */
*((uint32_t *) (pkt0.cnx_rsp_ok.names_and_ids + offset)) = measure->id;
offset += sizeof(uint32_t);
strcpy(pkt0.cnx_rsp_ok.names_and_ids + offset, measure->name);
offset += strlen(measure->name) + 1;
}
/* give birth to node */
ws_connected++;
node->worldsens = NODE_CONNECTED;
node_birth(node->id);
WSNET_S_DBG_DBG("WSNET2:: <-- CONNECT(%d/%d) (ip: %d)\n", ws_connected, ws_count, pkt->node_id);
WSNET_S_DBG_DBG("WSNET2:: --> RP (seq: %"PRId64") (rp seq: %"PRId64", period: %"PRId64", rp:%"PRId64")\n", ws_seq, ws_nsyncseq, ws_nsync - ws_csync, ws_nsync);
/* send response */
worldsens_packet_display(&pkt0);
worldsens_packet_hton(&pkt0);
wsens_srv_send(addr, (char *) &pkt0, sizeof(struct _worldsens_s_connect_rsp_ok));
}