quagga.c

wifi 无线网络路由协议OLSR linux下C代码

  unsigned char *buf = NULL;
  ssize_t ret = 0, bsize = 0;
  uint16_t length = 0, l = 0;
  int sockstate;

  *len = 0;

  sockstate = fcntl (zebra.sock, F_GETFL, 0);
  fcntl (zebra.sock, F_SETFL, sockstate|O_NONBLOCK);

  do { 
    if (*len == bsize) {
      bsize += BUFSIZE;
      buf = my_realloc (buf, bsize, "Zebra try_read");
    }
    ret = read (zebra.sock, buf + l, bsize - l);
    if (!ret) { // nothing more to read, packet is broken, discard!
      free (buf);
      return NULL;
    }
    if (ret < 0 && errno != EAGAIN) { // oops - we got disconnected
      olsr_printf (1, "(QUAGGA) Disconnected from zebra\n");
      zebra.status &= ~STATUS_CONNECTED;
    }

    *len += ret;
    while ((*len - l) > length) {
      l += length;
      memcpy (&length, buf + l, 2);
      length = ntohs (length);
    }
    if (((*len) - l) == length) break; // GOT FULL PACKAGE!!
    if (*len < l) {
      fcntl (zebra.sock, F_SETFL, sockstate);
      continue;
    }
  } while (1);

  fcntl (zebra.sock, F_SETFL, sockstate);
  return buf;
}


/* Parse a packet recived from zebra */
int zebra_parse_packet (unsigned char *packet, ssize_t maxlen) {

  uint16_t command;
  int skip;

  /* Array of functions */
  int (*foo[ZEBRA_MESSAGE_MAX]) (unsigned char *, size_t) = {
    parse_interface_add,
    parse_interface_delete,
    parse_interface_address_add,
    parse_interface_address_delete,
    parse_interface_up,
    parse_interface_down,
    ipv4_route_add,
    ipv4_route_delete,
    parse_ipv6_route_add
  };

  uint16_t length;
  int ret;

#ifdef MY_DEBUG
  puts ("DEBUG: zebra_parse_packet");
#endif

  memcpy (&length, packet, 2);
  length = ntohs (length);
  
  if (maxlen < length) {
    olsr_printf (1, "(QUAGGA) maxlen = %lu, packet_length = %d\n", (unsigned long)maxlen, length);
    olsr_exit ("(QUAGGA) programmer is an idiot", EXIT_FAILURE);
  }

#ifdef ZEBRA_HEADER_MARKER
  if (packet[2] == 255) { // found header marker!!
    //packet[3] == ZSERV_VERSION: FIXME: HANDLE THIS!
    memcpy (command, packet + 4, sizeof command); // two bytes command now!
    command-- = ntohs (command); 
    skip = 6;
  }
#else
  command = packet[2] - 1;
  skip = 3;
#endif

  if (command < ZEBRA_MESSAGE_MAX && foo[command]) { 
    if (!(ret = foo[command] (packet + skip, length - skip))) 
      return length;
    else olsr_printf (1, "(QUAGGA) Parse error: %d\n", ret);
  }
  else
    olsr_printf (1, "(QUAGGA) Unknown packet type: %d\n", packet[2]);

  olsr_printf (1, "(Quagga) RECIVED PACKET FROM ZEBRA THAT I CAN'T PARSE");

  return length;
}


static int parse_interface_add (unsigned char *opt __attribute__((unused)), size_t len __attribute__((unused))) {
  //todo
  return 0;
}


static int parse_interface_delete (unsigned char *opt __attribute__((unused)), size_t len __attribute__((unused))) {
  //todo
  return 0;
}


static int parse_interface_address_add (unsigned char *opt __attribute__((unused)), size_t len __attribute__((unused))) {
  
  //todo
  return 0;
}

static int parse_interface_up (unsigned char *opt __attribute__((unused)), size_t len __attribute__((unused))) {
  
  //todo
  return 0;
}

static int parse_interface_down (unsigned char *opt __attribute__((unused)), size_t len __attribute__((unused))) {
  
  //todo
  return 0;
}


static int parse_interface_address_delete (unsigned char *opt __attribute__((unused)), size_t  len __attribute__((unused))) {
  //todo
  return 0;
}


/* Parse an ipv4-route-packet recived from zebra
 */
static int parse_ipv4_route (unsigned char *opt, size_t len, struct ipv4_route *r) {
  int c;

  if (len < 4) return -1;
  
  r->type = *opt++;
  r->flags = *opt++;
  r->message = *opt++;
  r->prefixlen = *opt++;
  len -= 4;
  r->prefix = 0;
  
  if ((int)len < r->prefixlen/8 + (r->prefixlen % 8 ? 1 : 0)) return -1;
  
  memcpy (&r->prefix, opt, r->prefixlen/8 + (r->prefixlen % 8 ? 1 : 0));
  opt += r->prefixlen/8 + (r->prefixlen % 8 ? 1 : 0);
  
  if (r->message & ZAPI_MESSAGE_NEXTHOP) {
    if (len < 1) return -1;
    r->nh_count = *opt++;
    len--;
    if (len < (sizeof (uint32_t) + 1) * r->nh_count) return -1;
    r->nexthops = olsr_malloc ((sizeof r->nexthops->type +
				sizeof r->nexthops->payload) * r->nh_count,
			       "quagga: parse_ipv4_route_add");
    for (c = 0; c < r->nh_count; c++) {
      r->nexthops[c].type = *opt++;
      memcpy (&r->nexthops[c].payload.v4, opt, sizeof (uint32_t));
      opt += sizeof (uint32_t);
      len -= sizeof (uint32_t) + 1;
    }
  }

  if (r->message & ZAPI_MESSAGE_IFINDEX) {
    if (len < 1) return -1;
    r->ind_num = *opt++;
    if (len < sizeof (uint32_t) * r->ind_num) return -1;
    r->index = olsr_malloc (sizeof (uint32_t) * r->ind_num,
			    "quagga: parse_ipv4_route_add");
    memcpy (r->index, opt, r->ind_num * sizeof (uint32_t));
    opt += sizeof (uint32_t) * r->ind_num;
    len -= sizeof (uint32_t) * r->ind_num;
  }

  if (r->message & ZAPI_MESSAGE_DISTANCE) {
    if (len < 1) return -1;
    r->distance = *opt++;
    len--;
  }

  if (r->message & ZAPI_MESSAGE_METRIC) {
    if (len < sizeof (uint32_t)) return -1;
    memcpy (&r->metric, opt, sizeof (uint32_t));
  }

  return 0;
}


static int ipv4_route_add (unsigned char *opt, size_t len) {

  struct ipv4_route r;
  int f;
    
  f = parse_ipv4_route (opt, len, &r);
  if (f < 0) return f;

  return add_hna4_route (r);
}

static int ipv4_route_delete (unsigned char *opt, size_t len) {
  struct ipv4_route r;
  int f;
  
  f = parse_ipv4_route (opt, len, &r);
  if (f < 0) return f;

  return delete_hna4_route (r);
  
}

static int parse_ipv6_route_add (unsigned char *opt __attribute__((unused)), size_t len __attribute__((unused))) {
  //todo
  return 0;
}


/* start redistribution FROM zebra */
int zebra_redistribute (unsigned char type) {

  if (type > ZEBRA_ROUTE_MAX) return -1;
  zebra.redistribute[type - 1] = 1;

  return zebra_send_command (ZEBRA_REDISTRIBUTE_ADD, &type, 1);
  
  
}  


/* end redistribution FROM zebra */
int zebra_disable_redistribute (unsigned char type) {
  
  if (type > ZEBRA_ROUTE_MAX) return -1;
  zebra.redistribute[type - 1] = 0;

  return zebra_send_command (ZEBRA_REDISTRIBUTE_DELETE, &type, 1);

}
  
static uint32_t prefixlentomask (uint8_t prefix) {
  uint32_t mask = 0;

  if (prefix) {
    mask = 0xffffffff<<(32-prefix);
    mask = ntohl(mask);
  }

  return mask;
}

int add_hna4_route (struct ipv4_route r) {
  union olsr_ip_addr net, mask;
  
#ifdef MY_DEBUG
  dump_ipv4_route(r, "add_hna4_route");
#endif

  mask.v4 = prefixlentomask(r.prefixlen);
  net.v4 = r.prefix;

  add_local_hna4_entry(&net, &mask);
  free_ipv4_route(r);
  return 0;
}

int delete_hna4_route (struct ipv4_route r) {

  union olsr_ip_addr net, mask;

#ifdef MY_DEBUG
  dump_ipv4_route(r, "delete_hna4_route");
#endif

  mask.v4 = prefixlentomask(r.prefixlen);
  net.v4 = r.prefix;

  remove_local_hna4_entry(&net, &mask) ? 0 : -1;
  free_ipv4_route(r);
  return 0;

}

static void free_ipv4_route (struct ipv4_route r) {

  if(r.message&ZAPI_MESSAGE_IFINDEX && r.ind_num) free(r.index);
  if(r.message&ZAPI_MESSAGE_NEXTHOP && r.nh_count) free(r.nexthops);

}

/*
static uint8_t masktoprefixlen (uint32_t mask) {
  
  uint8_t prefixlen = 0;

  mask = htonl (mask);

  if (mask) while (mask << ++prefixlen && prefixlen < 32);

  return prefixlen;
  
}
*/

int zebra_add_olsr_v4_route (struct rt_entry *r) {
  
  struct ipv4_route route;
  int retval;
  
  route.type = ZEBRA_ROUTE_OLSR; // OLSR
  route.message = ZAPI_MESSAGE_METRIC;
  route.flags = zebra.flags;
  route.prefixlen =(r->rt_dst.prefix_len);
  route.prefix = r->rt_dst.prefix.v4;
  if ((r->rt_best->rtp_nexthop.gateway.v4 == r->rt_dst.prefix.v4 && 
       route.prefixlen == 32)) {
    route.message |= ZAPI_MESSAGE_IFINDEX | ZAPI_MESSAGE_NEXTHOP;
    route.ind_num = 1;
    route.index = olsr_malloc (sizeof *route.index, 
			       "zebra_add_olsr_v4_route");
    *route.index = htonl(r->rt_best->rtp_nexthop.iif_index);
    route.nexthops = olsr_malloc (sizeof route.nexthops->type +
				  sizeof route.nexthops->payload,
				  "zebra_add_olsr_v4_route");
    route.nh_count = 1;
    route.nexthops->type = 0;
  }
  else {
    route.message |= ZAPI_MESSAGE_NEXTHOP;
    route.nh_count = 1;
    route.nexthops = olsr_malloc (route.nh_count * 
				  (sizeof route.nexthops->type + 
				   sizeof route.nexthops->payload), 
				   "zebra_add_olsr_v4_route");
    route.nexthops->type = ZEBRA_NEXTHOP_IPV4;
    route.nexthops->payload.v4 = r->rt_best->rtp_nexthop.gateway.v4;
  }

  route.metric = r->rt_best->rtp_metric.hops;
  route.metric = htonl(route.metric);

  if (zebra.distance) {
    route.message |= ZAPI_MESSAGE_DISTANCE;
    route.distance = zebra.distance;
  }

  retval = zebra_add_v4_route(route);
  free_ipv4_route (route);
  return retval;
}

int zebra_del_olsr_v4_route (struct rt_entry *r) {
  
  struct ipv4_route route;
  int retval;
  route.type = ZEBRA_ROUTE_OLSR; // OLSR
  route.message = ZAPI_MESSAGE_METRIC;
  route.flags = zebra.flags;
  route.prefixlen = r->rt_dst.prefix_len;
  route.prefix = r->rt_dst.prefix.v4;
  if ((r->rt_best->rtp_nexthop.gateway.v4 == r->rt_dst.prefix.v4 && 
       route.prefixlen == 32)){
    route.message |= ZAPI_MESSAGE_IFINDEX;
    route.ind_num = 1;
    route.index = olsr_malloc (sizeof *route.index, 
			       "zebra_add_olsr_v4_route");
    *route.index = htonl (r->rt_best->rtp_nexthop.iif_index);
    route.nexthops = olsr_malloc (sizeof route.nexthops->type +
				  sizeof route.nexthops->payload,
				  "zebra_add_olsr_v4_route");
    route.nh_count = 1;
    route.nexthops->type = 0;
  }
  else {
    route.message |= ZAPI_MESSAGE_NEXTHOP;
    route.nh_count = 1;
    route.nexthops = olsr_malloc (route.nh_count * 
				  (sizeof route.nexthops->type +
				   sizeof route.nexthops->payload), 
				  "zebra_add_olsr_v4_route");
    route.nexthops->type = ZEBRA_NEXTHOP_IPV4;
    route.nexthops->payload.v4 = r->rt_best->rtp_nexthop.gateway.v4;
  }
  route.metric = r->rt_best->rtp_metric.hops;
  route.metric = htonl (route.metric);
  
  if (zebra.distance) {
    route.message |= ZAPI_MESSAGE_DISTANCE;
    route.distance = zebra.distance;
  }

  retval = zebra_delete_v4_route(route);

  free_ipv4_route (route);
  return retval;
}

void zebra_olsr_distance (unsigned char dist) {
  zebra.distance = dist;
}

void zebra_olsr_localpref (void) {
  zebra.flags &= ZEBRA_FLAG_SELECTED;
}

void zebra_export_routes (unsigned char t) {
  if (t)
    zebra.options |= OPTION_EXPORT;
  else
    zebra.options &= ~OPTION_EXPORT;
}