serialsource.c

tinyos-2.x.rar

	continue;
      if (n < 0)
	{
	  message(src, msg_unix_error);
	  actual = -1;
	  break;
	}

      count -= n;
      actual += n;
      buffer += n;
    }
  if (fcntl(src->fd, F_SETFL, O_NONBLOCK) < 0)
    {
      message(src, msg_unix_error);
      /* We're in trouble, but there's no obvious fix. */
    }
  return actual;
#else // LOSE32
    int actual = 0;
    int n;
    const unsigned char * b = buffer;

    while (count > 0) {
      if (!WriteFile(src->hComm, b, count, &n, NULL)) {
	      message(src, msg_unix_error);
          actual = -1;
          break;
      }

      count  -= n;
      actual += n;
      b      += n;
    }

    return actual;
#endif
}

static void push_protocol_packet(serial_source src,
				 uint8_t type, uint8_t *packet, uint8_t len)
{
  /* I'm assuming short queues */
  struct packet_list *entry = malloc(sizeof *entry), **last;

  if (!entry)
    {
      message(src, msg_no_memory);
      free(packet);
      return;
    }

  entry->packet = packet;
  entry->len = len;
  entry->next = NULL;

  last = &src->recv.queue[type];
  while (*last)
    last = &(*last)->next;
  *last = entry;
}

static struct packet_list *pop_protocol_packet(serial_source src, uint8_t type)
{
  struct packet_list *entry = src->recv.queue[type];

  if (entry)
    src->recv.queue[type] = entry->next;

  return entry;
}

static bool packet_available(serial_source src, uint8_t type)
{
  return src->recv.queue[type] != NULL;
}

int serial_source_empty(serial_source src)
/* Returns: true if serial source does not contain any pending data, i.e.,
     if the result is true and there is no data available on the source's
     file descriptor, then read_serial_packet will:
       - return NULL if the source is non-blocking
       - block if it is blocking

    (Note: the presence of this calls allows the serial_source to do some
    internal buffering)
*/
{
  return src->recv.bufpos >= src->recv.bufused &&
    !packet_available(src, P_PACKET_NO_ACK);
}

/* Slow implementation of crc function */
static uint16_t crc_byte(uint16_t crc, uint8_t b)
{
  uint8_t i;
  
  crc = crc ^ b << 8;
  i = 8;
  do
    if (crc & 0x8000)
      crc = crc << 1 ^ 0x1021;
    else
      crc = crc << 1;
  while (--i);

  return crc;
}

static uint16_t crc_packet(uint8_t *data, int len)
{
  uint16_t crc = 0;

  while (len-- > 0)
    crc = crc_byte(crc, *data++);

  return crc;
}

static int read_byte(serial_source src, int non_blocking)
/* Returns: next byte (>= 0), or -1 if no data available and non-blocking is true.
*/
{
  if (src->recv.bufpos >= src->recv.bufused)
    {
      for (;;)
	{
	  int n = serial_read(src, non_blocking, src->recv.buffer, sizeof src->recv.buffer);

	  if (n == 0) /* Can't occur because of serial_read bug workaround */
	    {
	      message(src, msg_closed);
	      return -1;
	    }
	  if (n > 0)
	    {
#ifdef DEBUG
	      dump("raw", src->recv.buffer, n);
#endif
	      src->recv.bufpos = 0;
	      src->recv.bufused = n;
	      break;
	    }
#ifndef LOSE32
	  if (errno == EAGAIN)
	    return -1;
	  if (errno != EINTR)
	    message(src, msg_unix_error);
#endif
    }
    }
  //printf("in %02x\n", src->recv.buffer[src->recv.bufpos]);
  return src->recv.buffer[src->recv.bufpos++];
}

static void process_packet(serial_source src, uint8_t *packet, int len);
static int write_framed_packet(serial_source src,
			       uint8_t packet_type, uint8_t first_byte,
			       const uint8_t *packet, int count);

static void read_and_process(serial_source src, int non_blocking)
/* Effects: reads and processes up to one packet.
*/
{
  uint8_t *packet = src->recv.packet;

  for (;;)
    {
      int byte = read_byte(src, non_blocking);

      if (byte < 0)
	return;

      if (!src->recv.in_sync)
	{
	  if (byte == SYNC_BYTE)
	    {
	      src->recv.in_sync = TRUE;
	      message(src, msg_sync);
	      src->recv.count = 0;
	      src->recv.escaped = FALSE;
	    }
	  continue;
	}
      if (src->recv.count >= MTU)
	{
	  message(src, msg_too_long);
	  src->recv.in_sync = FALSE;
	  continue;
	}
      if (src->recv.escaped)
	{
	  if (byte == SYNC_BYTE)
	    {
	      /* sync byte following escape is an error, resync */
	      message(src, msg_bad_sync);
	      src->recv.in_sync = FALSE;
	      continue;
	    }
	  byte ^= 0x20;
	  src->recv.escaped = FALSE;
	}
      else if (byte == ESCAPE_BYTE)
	{
	  src->recv.escaped = TRUE;
	  continue;
	}
      else if (byte == SYNC_BYTE)
	{
	  int count = src->recv.count;
	  uint8_t *received;
	  uint16_t read_crc, computed_crc;

	  src->recv.count = 0; /* ready for next packet */

	  if (count < 4)
	    /* frames that are too small are ignored */
	    continue;

	  received = malloc(count - 2);
	  if (!received)
	    {
	      message(src, msg_no_memory);
	      continue;
	    }
	  memcpy(received, packet, count - 2);

	  read_crc = packet[count - 2] | packet[count - 1] << 8;
	  computed_crc = crc_packet(received, count - 2);

#ifdef DEBUG
	  dump("received", packet, count);
	  printf("  crc %x comp %x\n", read_crc, computed_crc);
#endif
	  if (read_crc == computed_crc) 
	    {
	      process_packet(src, received, count - 2);
	      return; /* give rest of world chance to do something */
	    }
	  else
	    {
	      message(src, msg_bad_crc);
	      free(received);
	      /* We don't lose sync here. If we did, garbage on the line
		 at startup will cause loss of the first packet. */
	      continue;
	    }
	}
      packet[src->recv.count++] = byte;
    }
}

static void process_packet(serial_source src, uint8_t *packet, int len)
{
  int packet_type = packet[0], offset = 1;

  if (packet_type == P_PACKET_ACK)
    {
      /* send ack */
      write_framed_packet(src, P_ACK, packet[1], NULL, 0);
      /* And merge with un-acked packets */
      packet_type = P_PACKET_NO_ACK;
      offset = 2;
    }
  /* packet must remain a valid pointer to pass to free. So we move the
     data rather than pass an internal pointer */
  memmove(packet, packet + offset, len - offset);
  push_protocol_packet(src, packet_type, packet, len - offset);
}

void *read_serial_packet(serial_source src, int *len)
/* Effects: Read the serial source src. If a packet is available, return it.
     If in blocking mode and no packet is available, wait for one.
   Returns: the packet read (in newly allocated memory), with *len is
     set to the packet length, or NULL if no packet is yet available and
     the serial source is in non-blocking mode
*/
{
  read_and_process(src, TRUE);
  for (;;)
    {
      struct packet_list *entry;

      entry = pop_protocol_packet(src, P_PACKET_NO_ACK);
      if (entry)
	{
	  uint8_t *packet = entry->packet;

	  *len = entry->len;
	  free(entry);

	  return packet;
	}
      if (src->non_blocking && serial_source_empty(src))
	return NULL;
      source_wait(src, NULL);
      read_and_process(src, src->non_blocking);
    }
}

/* The escaper does the sync bytes+escape-like encoding+crc of packets */

static void escape_add(serial_source src, uint8_t b)
{
  src->send.escaped[src->send.escapeptr++] = b;
}

static int init_escaper(serial_source src, int count)
{
  src->send.escaped = malloc(count * 2 + 2);
  if (!src->send.escaped)
    {
      message(src, msg_no_memory);
      return -1;
    }
  src->send.escapeptr = 0;
  src->send.crc = 0;

  escape_add(src, SYNC_BYTE);

  return 0;
}

static void terminate_escaper(serial_source src)
{
  escape_add(src, SYNC_BYTE);
}

static void escape_byte(serial_source src, uint8_t b)
{
  src->send.crc = crc_byte(src->send.crc, b);
  if (b == SYNC_BYTE || b == ESCAPE_BYTE)
    {
      escape_add(src, ESCAPE_BYTE);
      escape_add(src, b ^ 0x20);
    }
  else
    escape_add(src, b);
}

static void free_escaper(serial_source src)
{
  free(src->send.escaped);
}

// Write a packet of type 'packetType', first byte 'firstByte'
// and bytes 2..'count'+1 in 'packet'
static int write_framed_packet(serial_source src,
			       uint8_t packet_type, uint8_t first_byte,
			       const uint8_t *packet, int count)
{
  int i, crc;

#ifdef DEBUG
  printf("writing %02x %02x", packet_type, first_byte);
  dump("", packet, count);
#endif

  if (init_escaper(src, count + 4) < 0)
    return -1;
	
  escape_byte(src, packet_type);
  escape_byte(src, first_byte);
  for (i = 0; i < count; i++)
    escape_byte(src, packet[i]);

  crc = src->send.crc;
  escape_byte(src, crc & 0xff);
  escape_byte(src, crc >> 8);
  
  terminate_escaper(src);

#ifdef DEBUG
  dump("encoded", src->send.escaped, src->send.escapeptr);
#endif

  if (source_write(src, src->send.escaped, src->send.escapeptr) < 0)
    {
      free_escaper(src);
      return -1;
    }
  free_escaper(src);
  return 0;
}

static void add_timeval(struct timeval *tv, long us)
/* Specialised for this app */
{
  tv->tv_sec += us / 1000000;
  tv->tv_usec += us % 1000000;
  if (tv->tv_usec > 1000000)
    {
      tv->tv_usec -= 1000000;
      tv->tv_sec++;
    }
}

int write_serial_packet(serial_source src, const void *packet, int len)
/* Effects: writes len byte packet to serial source src
   Returns: 0 if packet successfully written, 1 if successfully written
     but not acknowledged, -1 otherwise
*/
{
  struct timeval deadline;

  src->send.seqno++;
  if (write_framed_packet(src, P_PACKET_ACK, src->send.seqno, packet, len) < 0)
    return -1;

  // FIXME: the WIN32 implementation of source_wait()
  //        disregards the deadline parameter anyway
#ifndef LOSE32
  gettimeofday(&deadline, NULL);
  add_timeval(&deadline, ACK_TIMEOUT);
#endif

  for (;;) 
    {
      struct packet_list *entry;
      
      read_and_process(src, TRUE);
      entry = pop_protocol_packet(src, P_ACK);
      if (entry)
	{
	  uint8_t acked = entry->packet[0];

	  free(entry->packet);
	  free(entry);
	  if (acked == src->send.seqno)
	    return 0;
	}
      else if (source_wait(src, &deadline) < 0)
	return 1;
    }
}

/* This somewhat convoluted code allows us to use a common baudrate table
   with the Java code. This could be improved if we generated the Java
   code from a common table.
*/

struct pargs {
  char *name;
  int rate;
};

static void padd(struct pargs *args, const char *name, int baudrate)
{
  if (!strcmp(args->name, name))
    args->rate = baudrate;
}

static void init(void) { }

int platform_baud_rate(char *platform_name)
/* Returns: The baud rate of the specified platform, or -1 for unknown
     platforms
*/
{
  /* The Java code looks like Platform.add(Platform.x, "name", baudrate); 
     Fake up some C stuff which will make that work right. */
  struct pargs args;
  struct {
    void (*add)(struct pargs *args, const char *name, int baudrate);
    struct pargs *x;
  } Platform = { padd, &args };
  static struct {
    struct {
      int packet;
    } tinyos;
  } net;

  if (isdigit(platform_name[0]))
    return atoi(platform_name);

  args.name = platform_name;
  args.rate = -1;

#define class
#define BaudRate
#define static
#define void
#define throws ;
#define Exception
#define package
#include "../../java/net/tinyos/packet/BaudRate.java"

  return args.rate;
}