sfsource.c 3.32 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <unistd.h>
#include <errno.h>
#include <stdlib.h>
#include <string.h>

#include "sfsource.h"

uint32_t platform;

int saferead(int fd, void *buffer, int count)
{
  int actual = 0;

  while (count > 0)
    {
      int n = read(fd, buffer, count);

      if (n == -1 && errno == EINTR)
	continue;
      if (n == -1)
	return -1;
      if (n == 0)
	return actual;

      count -= n;
      actual += n;
      buffer += n;
    }
  return actual;
}

int safewrite(int fd, const void *buffer, int count)
{
  int actual = 0;

  while (count > 0)
    {
      int n = write(fd, buffer, count);

      if (n == -1 && errno == EINTR)
	continue;
      if (n == -1)
	return -1;

      count -= n;
      actual += n;
      buffer += n;
    }
  return actual;
}

int open_sf_source(const char *host, int port)
/* Returns: file descriptor for serial forwarder at host:port
 */
{
  int fd = socket(AF_INET, SOCK_STREAM, 0);
  struct hostent *entry;
  struct sockaddr_in addr;

  if (fd < 0)
    return fd;

  entry = gethostbyname(host);
  if (!entry)
    {
      close(fd);
      return -1;
    }      

  addr.sin_family = entry->h_addrtype;
  memcpy(&addr.sin_addr, entry->h_addr, entry->h_length);
  addr.sin_port = htons(port);
  if (connect(fd, (struct sockaddr *)&addr, sizeof addr) < 0)
    {
      close(fd);
      return -1;
    }

  if (init_sf_source(fd) < 0)
    {
      close(fd);
      return -1;
    }

  return fd;
}

extern uint32_t platform; 

int init_sf_source(int fd)
/* Effects: Checks that fd is following the serial forwarder protocol
     Sends 'platform' for protocol version '!', and sets 'platform' to
     the received platform value.
   Modifies: platform
   Returns: 0 if it is, -1 otherwise
 */
{
  char check[2], us[2];
  int version;
  unsigned char nonce[4];
  /* Indicate version and check if serial forwarder on the other end */
  us[0] = 'T'; us[1] = '!';
  if (safewrite(fd, us, 2) != 2 ||
      saferead(fd, check, 2) != 2 ||
      check[0] != 'T' || check[1] < ' ')
    return -1;

  version = check[1];
  if (us[1] < version)
    version = us[1];

  switch (version)
    {
    case ' ': break;
    case '!': 
      nonce[0] = platform;
      nonce[1] = platform >>  8;
      nonce[2] = platform >> 16;
      nonce[3] = platform >> 24;
      if (safewrite(fd, nonce, 4) != 4)
	return -1;
      if (saferead(fd, nonce, 4) != 4)
	return -1;
      platform = nonce[0] | nonce[1] << 8 | nonce[2] << 16 | nonce[3] << 24;
      break;
    }

  return 0;
}

void *read_sf_packet(int fd, int *len)
/* Effects: reads packet from serial forwarder on file descriptor fd
   Returns: the packet read (in newly allocated memory), and *len is
     set to the packet length, or NULL for failure
*/
{
  unsigned char l;
  void *packet;

  if (saferead(fd, &l, 1) != 1)
    return NULL;

  packet = malloc(l);
  if (!packet)
    return NULL;

  if (saferead(fd, packet, l) != l)
    {
      free(packet);
      return NULL;
    }
  *len = l;
  
  return packet;
}

int write_sf_packet(int fd, const void *packet, int len)
/* Effects: writes len byte packet to serial forwarder on file descriptor
     fd
   Returns: 0 if packet successfully written, -1 otherwise
*/
{
  unsigned char l = len;

  if (safewrite(fd, &l, 1) != 1 ||
      safewrite(fd, packet, l) != l)
    return -1;

  return 0;
}