bool listen_for_message(const unsigned char **packet, int fd, int *length)
{
struct pollfd pl;
int len;
pl.fd = fd;
pl.events = POLLIN;
if (poll(&pl, 1, MSG_TIMEOUT))
{
// read that packet;
*packet = read_sf_packet(fd, &len);
// we got a packet but it's not the kind we were expecting...
if ( (*packet)[2] != 23)
{
free(*packet);
*packet = NULL;
return listen_for_message(packet, fd, length);
}
else
{
//print_received_message(*packet, fd, len);
*length = len;
return TRUE;
}
}
else
{
*length = -1;
return FALSE;
}
//return packet;
}
void check_clients(fd_set *fds)
{
struct client_list **c;
for (c = &clients; *c; )
{
int next = 1;
if (FD_ISSET((*c)->fd, fds))
{
int len;
const void *packet = read_sf_packet((*c)->fd, &len);
if (packet)
{
forward_packet(packet, len);
free((void *)packet);
}
else
{
rem_client(c);
next = 0;
}
}
if (next)
c = &(*c)->next;
}
}
int main(int argc, char **argv)
{
int fd;
if (argc != 3) {
fprintf(stderr,
"Usage: %s <host> <port> - dump packets from a serial forwarder\n",
argv[0]);
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);
}
for (;;) {
int len, i;
const unsigned char *packet = read_sf_packet(fd, &len);
if (!packet)
exit(0);
for (i = 0; i < len; i++)
printf("%02x ", packet[i]);
putchar('\n');
fflush(stdout);
free((void *)packet);
}
}
telospacket *read_telos_packet(int fd)
{
int length;
telospacket *packet;
int i;
const unsigned char *rawpkt;
rawpkt = read_sf_packet(fd, &length);
dbg(TSRC,"read_sf_packet returned (rawpkt=%X, length=%i).\n",(int)rawpkt, length);
if (rawpkt == NULL)
{
dbg(TSRC,"rawpkt is NULL\n");
return NULL;
}
if (length < 5)
{
dbg(TSRC,"too short.\n");
free((void*)rawpkt);
return NULL;
}
packet = malloc(sizeof(telospacket));
//get header information
packet->length = rawpkt[2];
packet->dsn = rawpkt[0];
packet->type = rawpkt[4];
packet->group = rawpkt[3];
packet->data = malloc(packet->length);
if (!packet->data)
{
printf("Could not allocate memory!\n");
free(packet);
}
memcpy(packet->data, rawpkt+5, packet->length);
free((void*)rawpkt);
return packet;
}
int serial_receive(int *node, int *op, int *result, uint8_t *data, int len)
{
uint8_t *buf;
tmsg_t *msg;
unsigned char *packet;
int packet_len;
packet = (src_type == SRC_DEVICE) ?
read_serial_packet(src_dev, &packet_len) :
read_sf_packet(src_fd, &packet_len);
if (!packet)
return -1;
DEBUG_PACKET(packet);
/* skip the header */
buf = packet + 1 + SPACKET_SIZE;
packet_len -= 1 + SPACKET_SIZE;
if (packet_len > MFSMSG_SIZE)
{
fprintf(stderr, "packet too big: %d\n", len);
packet_len = MFSMSG_SIZE;
}
msg = new_tmsg(buf, MFSMSG_SIZE);
if (node)
*node = mfsmsg_node_get(msg);
if (op)
*op = mfsmsg_op_get(msg);
if (result)
*result = mfsmsg_result_get(msg);
if (len > packet_len)
len = packet_len;
get_data(msg, data, len);
free(packet);
return len;
}
/* 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;
uint16_T *y = ssGetOutputPortSignal(S,0);
real_T *t = ssGetOutputPortSignal(S,1);
/* Read packet from SerialForwarder server using functions linked in sfsource.o */
const unsigned char *packet = read_sf_packet(fd, &len);
int_T width = ssGetOutputPortWidth(S,0);
if (len == (width*2 + HEADER_LEN)){
for (i=HEADER_LEN; i < (width*2); i=i+2) {
*y++ = packet[i] << 8 | packet[i+1];
}
trigger = (trigger == 1 ? 0 : 1);
*t = trigger;
}
}
int main(int argc, char **argv)
{
int fd,i;
if (argc != 3) {
fprintf(stderr, "Usage: %s <host> <port> - print received packets\n", argv[0]);
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);
}
for (;;) {
int len, i;
const unsigned char *packet = read_sf_packet(fd, &len);
char* myPacket = (char*)malloc(len);
memcpy(myPacket, packet, len);
free((void*)packet);
if (!packet)
exit(0);
else {
tmsg_t* serialMsg = new_tmsg(myPacket, len);
void* payload = (void*)myPacket + (spacket_data_offsetbits(0) / 8);
tmsg_t* dataMsg = new_tmsg(payload, len - SPACKET_SIZE);
void* data = payload + (dissemination_message_data_offsetbits(0) / 8);
for (i = 0; i < len; i++)
printf("%02x ", packet[i]);
putchar('\n');
fflush(stdout);
free((void *)myPacket);
}
}
}
void mote_read(void *buf)
{
TOS_write_t *tos = (TOS_write_t *) buf;
for (;;)
{
int len, i;
struct pollfd pl;
pl.fd = tos->fd;
pl.events = POLLIN;
//if (poll(&pl, 1, 3000))
if (poll(&pl, 1, 1500))
{
printf("about to read: \n");
}
else
{
printf("timed out \n");
}
const unsigned char *packet = read_sf_packet(tos->fd, &len);
if (!packet)
exit(0);
for (i = 0; i < len; i++)
{
if (i == 5 || i== 9)
printf("| ");
printf("%02x ", packet[i]);
}
putchar('\n');
fflush(stdout);
free((void *)packet);
}
}
int
main(int argc, char **argv) {
int i;
uint8_t *packet;
char *prog_name;
int len;
int c, bail;
input_src_t input_src;
struct termios newtio;
tcflag_t baudflag;
int cnt;
uint8_t *buf;
serial_src = NULL;
sf_src = 0;
input_src = INPUT_RAW;
bail = 0;
prog_name = basename(argv[0]);
while ((c = getopt_long(argc, argv, "Dvr", longopts, NULL)) != EOF) {
switch (c) {
case 1:
bail = 1;
input_src = INPUT_SF;
break;
case 2:
bail = 1;
input_src = INPUT_SERIAL;
break;
case 'D':
debug++;
break;
case 'v':
verbose++;
break;
case 'r':
raw++;
input_src = INPUT_RAW;
break;
default:
usage(prog_name);
}
if (bail)
break;
}
argc -= optind;
argv += optind;
if (argc != 2) {
usage(prog_name);
exit(2);
}
if (verbose) {
fprintf(stderr, VERSION);
switch (input_src) {
case INPUT_SERIAL:
fprintf(stderr, "opening: serial@%s:%d\n", argv[0], platform_baud_rate(argv[1]));
break;
case INPUT_SF:
fprintf(stderr, "opening: sf@%s:%d\n", argv[0], atoi(argv[1]));
break;
case INPUT_RAW:
fprintf(stderr, "opening: raw@%s:%d\n", argv[0], platform_baud_rate(argv[1]));
break;
}
}
if (input_src == INPUT_RAW) {
baudflag = parse_baudrate(platform_baud_rate(argv[1]));
if (!baudflag) {
fprintf(stderr, "couldn't figure out the baud rate\n");
exit(2);
}
raw_fd = open(argv[0], O_RDWR | O_NOCTTY);
if (raw_fd < 0) {
fprintf(stderr, "*** Couldn't open serial port at %s:%s\n", argv[0], argv[1]);
perror("error: ");
exit(2);
}
/* Serial port setting */
memset(&newtio, 0, sizeof(newtio));
newtio.c_cflag = CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR | IGNBRK;
cfsetispeed(&newtio, baudflag);
cfsetospeed(&newtio, baudflag);
/* Raw output_file */
newtio.c_oflag = 0;
if (tcflush(raw_fd, TCIFLUSH) >= 0 && tcsetattr(raw_fd, TCSANOW, &newtio) >= 0) {
buf = malloc(256);
while(1) {
cnt = read(raw_fd, buf, 1);
if (cnt == 0)
continue;
fprintf(stderr, "%02x ", buf[0]);
}
} else
close(raw_fd);
exit(0);
}
switch(input_src) {
case INPUT_SERIAL:
serial_src = open_serial_source(argv[0], platform_baud_rate(argv[1]), 0, stderr_msg);
if (!serial_src) {
fprintf(stderr, "*** Couldn't open serial port at %s:%s\n", argv[0], argv[1]);
perror("error: ");
exit(1);
}
break;
case INPUT_SF:
sf_src = open_sf_source(argv[0], atoi(argv[1]));
if (sf_src < 0) {
fprintf(stderr, "*** Couldn't open serial forwarder at %s:%s\n", argv[0], argv[1]);
perror("error: ");
exit(1);
}
break;
default:
fprintf(stderr, "shouldn't be here\n");
exit(1);
}
for(;;) {
switch(input_src) {
case INPUT_SERIAL:
packet = read_serial_packet(serial_src, &len);
break;
case INPUT_SF:
packet = read_sf_packet(sf_src, &len);
break;
default:
fprintf(stderr, "shouldn't be here\n");
exit(1);
}
if (!packet) {
if (verbose)
fprintf(stderr, "*** end of stream, terminating\n");
exit(0);
}
for (i = 0; i < len; i++)
fprintf(stderr, "%02x ", packet[i]);
fprintf(stderr, "\n");
free((void *)packet);
}
}
