int serial_send(int node, int op, const uint8_t *data, int len)
{
uint8_t buf[1 + SPACKET_SIZE + MFSMSG_SIZE];
memset(buf, 0, sizeof buf);
tmsg_t *spacket_header, *msg;
buf[0] = 0;
spacket_header = new_tmsg(buf + 1, SPACKET_SIZE);
spacket_header_dest_set(spacket_header, 0xffff);
spacket_header_length_set(spacket_header, MFSMSG_SIZE);
spacket_header_type_set(spacket_header, MFSMSG_AM_TYPE);
msg = new_tmsg(buf + 1 + SPACKET_SIZE, MFSMSG_SIZE);
mfsmsg_node_set(msg, node);
mfsmsg_op_set(msg, op);
if (len > MFS_DATA_SIZE)
len = MFS_DATA_SIZE;
set_data(msg, data, len);
DEBUG_PACKET(buf);
return (src_type == SRC_DEVICE) ?
write_serial_packet(src_dev, buf, sizeof buf) :
write_sf_packet(src_fd, buf, sizeof buf);
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int len, i;
char packet[100];
char input_value[8];
const real_T *BI = (const real_T *) ssGetInputPortSignal(S,0);
const real_T *T1 = (const real_T *) ssGetInputPortSignal(S,1);
const real_T *T2 = (const real_T *) ssGetInputPortSignal(S,2);
const real_T *T3 = (const real_T *) ssGetInputPortSignal(S,3);
const real_T *T4 = (const real_T *) ssGetInputPortSignal(S,4);
const real_T *T5 = (const real_T *) ssGetInputPortSignal(S,5);
const real_T *T6 = (const real_T *) ssGetInputPortSignal(S,6);
const real_T *T7 = (const real_T *) ssGetInputPortSignal(S,7);
const real_T *INIT = (const real_T *) ssGetInputPortSignal(S,8);
const real_T *OK = (const real_T *) ssGetInputPortSignal(S,9);
packet[0] = 0x00;
packet[1] = 0xff;
packet[2] = 0xff;
packet[3] = 0xff;
packet[4] = 0xff;
packet[5] = (char) sizeof(char)*8; /* payload length */
packet[6] = 0x00;
packet[7] = 0x06;
if( INIT[0] == 0 ) {
input_value[0] = (char) BI[0];
input_value[1] = (char) T1[0];
input_value[2] = (char) T2[0];
input_value[3] = (char) T3[0];
input_value[4] = (char) T4[0];
input_value[5] = (char) T5[0];
input_value[6] = (char) T6[0];
input_value[7] = (char) T7[0];
} else {
input_value[0] = (char) 0xff;
input_value[1] = (char) 0xff;
input_value[2] = (char) 0xff;
input_value[3] = (char) 0xff;
input_value[4] = (char) 0xff;
input_value[5] = (char) 0xff;
input_value[6] = (char) 0xff;
input_value[7] = (char) 0xff;
}
memcpy(packet + 8, input_value, packet[5]);
if(OK[0]==1) {
write_sf_packet(fd, (void *) packet, 16);
}
}
void dispatch_packet(const void *packet, int len)
{
struct client_list **c;
for (c = &clients; *c; )
if (write_sf_packet((*c)->fd, packet, len) >= 0)
c = &(*c)->next;
else
rem_client(c);
}
int write_telos_packet(int fd, const telospacket *packet)
/* Effects: writes len byte packet to serial forwarder on file descriptor
fd
Returns: 0 if packet successfully written, -1 otherwise
*/
{
unsigned char l;
uint8_t *buffer;
int result;
int i;
if (packet->length > 28)
{
dbg(TSRC,"packet too long! (%i bytes)\n",packet->length);
return -1;
}
buffer = malloc(packet->length + 5);
buffer[0] = 0xff;
buffer[1] = 0xff;
buffer[2] = packet->type;
//buffer[3] = telos_dsn;
//telos_dsn++;
buffer[3] = packet->group;
buffer[4] = packet->length;
/* buffer[6] = 0;
buffer[7] = 0;
buffer[8] = packet->type;
buffer[9] = packet->group;*/
memcpy(buffer+5, packet->data, packet->length);
l = packet->length + 5;
printf("Sending...");
for (i=0; i < l; i++)
{
printf("%X ",buffer[i]);
}
printf("\n");
pthread_mutex_lock(&sfwritemutex);
result = write_sf_packet(fd, (void*)buffer, packet->length+5);
pthread_mutex_unlock(&sfwritemutex);
free((void*)buffer);
return result;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
// This is where the Magic Happens
///////////////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char **argv)
{
///////////////////////////////////////////////////////////////////////////////////////////////
// Communication Variables
///////////////////////////////////////////////////////////////////////////////////////////////
int fd;
int mote_addr;
TOS_write_t tos_write;
struct pollfd pl;
///////////////////////////////////////////////////////////////////////////////////////////////
// Collection Variables
///////////////////////////////////////////////////////////////////////////////////////////////
gen_msg_t msg;
int state = STATE_START_TRAVERSAL;
int seq_num = 0;
int num_retrys = 0;
int total_msgs;
uint32_t num_bundles = 0;
uint32_t curr_nundle_num = 0;
FILE *Fp;
///////////////////////////////////////////////////////////////////////////////////////////////
// Thread Variables
///////////////////////////////////////////////////////////////////////////////////////////////
pthread_t *read_thread;
pthread_t *write_thread;
pthread_attr_t pthread_custom_attr;
pthread_attr_init(&pthread_custom_attr);
read_thread = (pthread_t *) malloc(sizeof(pthread_t));
write_thread = (pthread_t *) malloc(sizeof(pthread_t));
if (argc != 4)
{
fprintf(stderr, "Usage: %s <host> <port> <mote_addr> - dump packets from a serial forwarder %d \n", argv[0], argc);
exit(2);
}
fd = open_sf_source(argv[1], atoi(argv[2]));
if (fd < 0)
{
fprintf(stderr, "Couldn't open serial forwarder at %s:%s\n", argv[1], argv[2]);
exit(1);
}
Fp = fopen("data.txt", "w+");
mote_addr = atoi(argv[3]);
msg.addr = mote_addr;
msg.group = 0x7d;
msg.length = sizeof(uint16_t)+sizeof(uint8_t);
msg.src_addr = SRC_ADDR;
pl.fd = fd;
pl.events = POLLIN;
while (1337)
{
void *data;
int w_ret;
int r;
msg.seq_num = seq_num++;
msg.type = AM_RADIO_HI_POWER;
data = get_packet(&msg);
w_ret = write_sf_packet(fd, data, TOS_MSG_SIZE);
r = rand() % 300;
usleep((256 + r)*1024);
}
/*
while (1337)
{
int w_ret;
void *data;
int len;
int i;
const unsigned char *packet;
switch (state) {
case STATE_START_TRAVERSAL:
printf ("Sending BEGIN_TRAVERSAL Message \n");
msg.seq_num = seq_num;
msg.type = AM_BEGIN_TRAVERSAL_MSG;
data = get_packet(&msg);
w_ret = write_sf_packet(fd, data, TOS_MSG_SIZE);
if ( listen_for_message(&packet, fd, &len) )
{
// we got a response, but did we succeed?
if (packet[MSG_STATUS_OFFSET] != 0x00)
{
seq_num++;
num_retrys = 0;
printf ("BeginTraversal succeeded \n");
memcpy(&num_bundles, &packet[9], sizeof(uint32_t));
printf ("There should be %d bundles \n\n" , num_bundles);
fprintf(Fp, "There should be %d bundles \n\n", num_bundles);
state = STATE_GET_BUNDLE;
}
else
{
if (num_retrys > MAX_RETRYS)
{
printf ("BeginTraversal FAILED.. EXITING \n");
exit(0);
}
num_retrys++;
seq_num++;
printf ("BeginTraversal FAILED.. trying again \n\n");
}
free (packet);
}
free(data);
break;
case STATE_GET_BUNDLE:
printf ("Sending GET_BUNDLE_MSG Message \n");
msg.seq_num = seq_num;
msg.type = AM_GET_BUNDLE_MSG;
data = get_packet(&msg);
w_ret = write_sf_packet(fd, data, TOS_MSG_SIZE);
if ( listen_for_message(&packet, fd, &len) )
{
// we got a response, but did we succeed?
if (packet[MSG_STATUS_OFFSET] == 0x01)
{
uint16_t turtle_num;
uint16_t bundle_num;
num_retrys = 0;
seq_num++;
curr_nundle_num++;
state = STATE_GET_NEXT_CHUNK;
printf ("GetBundle succeeded \n");
print_received_message(packet, len);
memcpy(&turtle_num, &packet[9], sizeof(uint16_t) );
memcpy(&bundle_num, &packet[11], sizeof(uint16_t) );
fprintf(Fp, "Turtle: %d Bundle: %d\n", turtle_num, bundle_num);
}
else if (packet[MSG_STATUS_OFFSET] == 0x02) // we got the bundle, but it's not valid
{
num_retrys = 0;
seq_num++;
state = STATE_G0_NEXT_BUNDLE;
printf ("GetBundle succeeded but the bundle is invalid \n");
}
else // BAD
{
if (num_retrys > MAX_RETRYS)
{
printf ("GetBundle FAILED.. EXITING \n");
exit(0);
}
state = STATE_GET_BUNDLE;
num_retrys++;
printf ("ERROR: GetBundle failed... trying again \n");
}
free(packet);
}
free (data);
break;
case STATE_GET_NEXT_CHUNK:
printf ("Sending GET_NEXT_CHUNK Message \n");
msg.seq_num = seq_num;
msg.type = AM_GET_NEXT_CHUNK;
data = get_packet(&msg);
w_ret = write_sf_packet(fd, data, TOS_MSG_SIZE);
if ( listen_for_message(&packet, fd, &len) )
{
if (packet[MSG_STATUS_OFFSET] == 0x01)
{
num_retrys = 0;
seq_num++;
state = STATE_GET_NEXT_CHUNK;
printf ("GetNextChunk succeeded \n");
print_data_message(packet, len, Fp);
//fprintf(Fp, "%s\n", packet);
}
else if (packet[MSG_STATUS_OFFSET] == 0x02) // we got the bundle, but it's not valid
{
num_retrys = 0;
seq_num++;
state = STATE_G0_NEXT_BUNDLE;
printf ("We reached the end of this stream \n");
fprintf(Fp, "\n");
if (curr_nundle_num == num_bundles)
{
printf ("We got all the bundles. Let's end the collection session. \n");
state = STATE_END_COLLECTION;
}
}
else // BAD
{
if (num_retrys > MAX_RETRYS)
{
printf ("GetNextChunk FAILED.. EXITING \n");
exit(0);
}
num_retrys++;
printf ("ERROR: GetNextChunk failed... trying again \n");
state = STATE_GET_NEXT_CHUNK;
}
free(packet);
}
free (data);
break;
case STATE_G0_NEXT_BUNDLE:
printf ("Sending G0_NEXT_BUNDLE Message \n");
msg.seq_num = seq_num;
msg.type = AM_GO_NEXT_MSG;
data = get_packet(&msg);
w_ret = write_sf_packet(fd, data, TOS_MSG_SIZE);
if ( listen_for_message(&packet, fd, &len) )
{
if (packet[MSG_STATUS_OFFSET] == 0x01)
{
num_retrys = 0;
seq_num++;
state = STATE_GET_BUNDLE;
printf ("GoNext succeeded \n");
}
else // BAD
{
if (num_retrys > MAX_RETRYS)
{
printf ("GoNext FAILED.. Ending Collection Session \n");
state = STATE_END_COLLECTION;
exit(0);
}
seq_num++;
num_retrys++;
printf ("ERROR: GetNextChunk failed... trying again \n");
state = STATE_G0_NEXT_BUNDLE;
}
free(packet);
}
free (data);
break;
case STATE_END_COLLECTION:
printf ("Sending G0_END_COLLECTION Message \n");
msg.seq_num = seq_num;
msg.type = AM_END_DATA_COLLECTION_SESSION;
data = get_packet(&msg);
w_ret = write_sf_packet(fd, data, TOS_MSG_SIZE);
if ( listen_for_message(&packet, fd, &len) )
{
if (packet[MSG_STATUS_OFFSET] == 0x01)
{
num_retrys = 0;
printf ("EndCollectionSession succeeded. exiting... \n");
fclose(Fp);
exit(0);
}
else
{
if (num_retrys > MAX_RETRYS)
{
printf ("EndCollectionSession FAILED.. EXITING \n");
exit(0);
}
num_retrys++;
printf ("ERROR: EndCollectionSession failed... trying again \n");
state = AM_END_DATA_COLLECTION_SESSION;
}
free(packet);
}
free (data);
break;
default:
printf("ERROR: %d is not a known state\n", state);
exit(0);
break;
}
if (w_ret == -1)
{
printf("error writing to socket... exiting\n");
exit (0);
}
}
*/
return 0;
}
void* mote_write(void *buf)
{
TOS_write_t *tos = (TOS_write_t *) buf;
uint8_t seq_num = 0;
gen_msg_t msg;
char *my_string;
int nbytes = 100;
int bytes_read;
msg.addr = tos->mote_addr;
msg.group = 0x7d;
msg.length = sizeof(uint16_t)+sizeof(uint8_t);
msg.src_addr = SRC_ADDR;
printf("What would you like to do: \n");
printf("bd: Bundle Delete \n");
printf("gb: Get Bundle \n");
printf("gc: Get Next Chunk \n");
printf("bt: Begin Traversal \n");
printf("gn: Go Next \n");
printf("ec: End Collection Session\n");
while (1)
{
int w_ret = 0;
void *data;
my_string = (char *) malloc (nbytes + 1);
bytes_read = getline (&my_string, &nbytes, stdin);
if (my_string[0] == 's' && my_string[1] == 'n')
{
seq_num++;
printf ("Sequence Number incremented: %d\n",seq_num);
}
if (my_string[0] == 'b' && my_string[1] == 'd')
{
msg.seq_num = seq_num;
msg.type = AM_DELETE_BUNDLE_MSG;
data = get_packet(&msg);
w_ret = write_sf_packet(tos->fd, data, TOS_MSG_SIZE);
free(data);
}
if (my_string[0] == 'g' && my_string[1] == 'b')
{
msg.seq_num = seq_num;
msg.type = AM_GET_BUNDLE_MSG;
data = get_packet(&msg);
w_ret = write_sf_packet(tos->fd, data, TOS_MSG_SIZE);
free(data);
}
if (my_string[0] == 'g' && my_string[1] == 'c')
{
msg.seq_num = seq_num;
msg.type = AM_GET_NEXT_CHUNK;
data = get_packet(&msg);
w_ret = write_sf_packet(tos->fd, data, TOS_MSG_SIZE);
free(data);
}
if (my_string[0] == 'b' && my_string[1] == 't')
{
msg.seq_num = seq_num;
msg.type = AM_BEGIN_TRAVERSAL_MSG;
data = get_packet(&msg);
w_ret = write_sf_packet(tos->fd, data, TOS_MSG_SIZE);
free(data);
}
if (my_string[0] == 'g' && my_string[1] == 'n')
{
msg.seq_num = seq_num;
msg.type = AM_GO_NEXT_MSG;
data = get_packet(&msg);
w_ret = write_sf_packet(tos->fd, data, TOS_MSG_SIZE);
free(data);
}
if (my_string[0] == 'e' && my_string[1] == 'c')
{
msg.seq_num = seq_num;
msg.type = AM_END_DATA_COLLECTION_SESSION;
data = get_packet(&msg);
w_ret = write_sf_packet(tos->fd, data, TOS_MSG_SIZE);
free(data);
}
if (w_ret == -1)
{
printf("error writing to socket... exiting\n");
exit (0);
}
}
return NULL;
}
