simple routing protocol

routing.c

//gcc 5.4.0

#include  <stdio.h>
#include <stdint.h>
#define MAXRT 64
#define NO_TRY
uint64_t me_id = 0xFFEEDDCC;
struct rtable {
  uint64_t id;
  uint64_t mask;
  void* udata;
  int64_t (*snd)(void*,size_t,void*);
  int64_t (*rcv)(void*,size_t,void*);
};
struct rtpackethdr {
  uint64_t dest;
  uint64_t src;
  uint16_t cksum;
  uint16_t hops;
  uint16_t size;
};
struct rtable nodes[MAXRT];
int cmpid(uint64_t id, uint64_t mask) {
  char aa[24]; char bb[24]; char* a = aa; char *b = bb;
  sprintf(a, "%016llx", id);
  sprintf(b, "%016llx", mask);
  while(*a && *b) {
   if (*a == *b || *b == '0') { a++; b++; continue; }
   return 0;
  }
  return -1;
}
int rtable_find_path(uint64_t dest_id) {
  uint64_t i;
  for ( i = 0; i < MAXRT; i++ ) {
    if ( nodes[i].id == dest_id ) return i;
  }
  for ( i = 0; i < MAXRT; i++ ) if (nodes[i].id && cmpid(dest_id, nodes[i].mask)) return i;
#ifndef NO_TRY
  for ( i = 0; i < MAXRT; i++ ) if ( dest_id & nodes[i].mask ) return i;
#endif
  return -1;
}
int rtable_add_entry(uint64_t id, uint64_t mask, int64_t (*snd)(void*,size_t,void*), int64_t (*rcv)(void*,size_t,void*),void* udata) {
  uint64_t i;
  for ( i = 0; i < MAXRT; i++ ) {
    if ( !nodes[i].id ) {
      nodes[i].id = id; nodes[i].mask = mask; nodes[i].snd = snd; nodes[i].rcv = rcv, nodes[i].udata = udata;
      return i;
    }
  }
  return -1;
}
int64_t dummy_snd(void* a, size_t b, void* c) {
  printf("Send to %016llx: packet:\n", *(uint64_t*)c);
  struct rtpackethdr* aa = a;
  printf("dest=%016llx src=%016llx size=%d\n", aa->dest, aa->src, aa->size);
  return b;
}
uint8_t pkt_buf[2048];
int64_t net_sendto(int via, uint64_t id, uint64_t src, void *data, size_t size){
  int nxhop = via != -1 ? via : rtable_find_path(id);
  if ( nxhop == -1 ) return nxhop;
  if ( size > 1536 ) return -1;
  struct rtpackethdr pkt = { id, src, 0, 1, data ? size : 0 };
  memcpy(pkt_buf, &pkt, sizeof(pkt));
  if(data) memcpy(pkt_buf + sizeof(pkt), data, size);
  return nodes[nxhop].snd(pkt_buf, sizeof(pkt) + size, nodes[nxhop].udata) - sizeof(pkt);
}
uint64_t net_saddr(char *s) {
  return strtoull(s, NULL, 16);
}
int64_t _x;
#define test(n) printf("%s = %lld, %llx \n", #n, _x=n, _x)
int main(void)
{
  uint64_t a = 0xFE0EFEFE00000000;
  uint64_t b = 0xCE00000000000000;
  uint64_t c = 0xCE10000000000000;
  test(rtable_add_entry(a, a, dummy_snd, 0, &a));
  test(rtable_add_entry(b, b, dummy_snd, 0, &b));
  test(rtable_find_path(a));
  test(rtable_find_path(b));
  test(rtable_find_path(c));
  test(net_sendto(-1, a, me_id, 0, 0));
  test(net_sendto(-1, b, me_id, 0, 0));
  test(net_sendto(-1, c, me_id, 0, 0));
}