ti/runtime/netapi/test/router.c

   1 #include "trie.h"
   2 #include "net_test.h"
   3 #include "router.h"
   4
   5
   6
   7
   8 Trie * route_init(void)
   9 {
  10     Trie *Pt = trie_new();
  11     return Pt;
  12 }
  13
  14
  15
  16 void route_add(Trie * Pt, unsigned long * Pdest_ipBE, void * Pour_route)
  17 {
  18
  19    trie_insert(Pt,(char *)Pdest_ipBE, sizeof(int), Pour_route);
  20 }
  21
  22 //route the packet
  23 //  lookup next hop in route trie
  24 int route_pkt(Trie *Pt, void * Ppkt, IP_netTestHead_T *Phead, unsigned char * Pbuf, int * Plen, netTestSA_t **Prs, int *PoutPort)
  25 {
  26     OUR_ROUTE_T *Pr;
  27     int ret;
  28     Pr = trie_lookup(Pt, (char *) &Phead->dst, 4);
  29     if (!Pr ) return -1;  //can't route
  30     if (Pr->sec_ptr)
  31     {
  32         if(!Prs) return -3;
  33         //tunnel i/f
  34         ret=process_tunnel(Pt, Ppkt, Phead, Pbuf, Plen, Pr, PoutPort);
  35         *Prs = Pr->sec_ptr;
  36         return ret;
  37     }
  38     if (Prs) *Prs=NULL;
  39     //simple route
  40     //copy new mac
  41     memcpy(Pbuf,Pr->out_mac,14);
  42     Pbuf[14+8]-=1;  //ttl--
  43     //todo do check ttl!
  44     Pbuf[14+10]=0; Pbuf[14+11]=0;  //zap [outer] header checksum
  45     if(PoutPort) *PoutPort=Pr->out_port;
  46     return 1;
  47 }
  48
  49 int process_tunnel(Trie *Pt, void *Ppkt, IP_netTestHead_T * Phead,
  50                    unsigned char * Pbuf, int* Plen, OUR_ROUTE_T * Proute,
  51                    int * PoutPort)
  52 {
  53 unsigned char *sb = Pbuf;
  54 unsigned char *eb= &Pbuf[*Plen];
  55 int newlen=*Plen;
  56 int seq;
  57 int pl_len;
  58 int new_pl_len;
  59 int pad_len;
  60 //unsigned char IV[20];
  61 netTestSA_t * p_sec=Proute->sec_ptr;
  62 unsigned char *pc;
  63 int nb;
  64 int i;
  65 int ret;
  66
  67 /* move sb to new start */
  68 sb = sb - p_sec->iv_len-8-20;  //make room for esp header and new outer ip
  69 newlen +=  (Pbuf-sb);
  70
  71 //adjust IP_Header to make outer:
  72 Phead->src=p_sec->src;
  73 Phead->dst=p_sec->dst;
  74
  75 //next_proto
  76 pc = (unsigned char *)&Phead->w3;
  77 pc[1]=50;
  78
  79 //payload len
  80 pc= (unsigned char*)&Phead->w1;
  81 pl_len=  ((pc[2]<<8) | (pc[3]));
  82
  83 //only for non-zero bl
  84 if (p_sec->bl)
  85 {
  86   nb = pl_len/ (p_sec->bl);       //128bits for AES
  87   pad_len =   pl_len - nb*p_sec->bl;
  88   if (p_sec->bl==16) //AES based
  89   {
  90     switch(pad_len)
  91     {
  92     case(15):
  93       pad_len=15;
  94       break;
  95     default:
  96       pad_len = p_sec->bl-pad_len-2;
  97     }
  98   }
  99   else //assume 8 byte block (DES)
 100   {
 101     switch(pad_len)
 102     {
 103     case(7):
 104       pad_len=7;
 105       break;
 106     default:
 107       pad_len = p_sec->bl-pad_len-2;
 108     }
 109   }
 110 }
 111 else
 112 {
 113    pad_len=0;
 114 }
 115
 116 new_pl_len = pl_len  +20+ 8 + p_sec->iv_len + pad_len + p_sec->auth_tag_size +2;
 117 pc[2] =  (new_pl_len&0xff00)>>8;
 118 pc[3] =  (new_pl_len&0xff);
 119 memcpy(&sb[14],Phead,20);  //copy outer ip header into buffer
 120
 121 //build esp header
 122 memcpy(&sb[14+20],(unsigned char *)&p_sec->spi,4);
 123 seq=BE(p_sec->seq++); //TODO: multicore safe ;  CHECK to make sure that SA does this, if so remove this code
 124 memcpy(&sb[14+24],(unsigned char *)&seq,4);
 125
 126 //IV: don't need:   Sa does it.
 127 //memcpy(&sb[14+28],&IV[0],p_sec->iv_len);
 128
 129 //Padding, trailer, room for tag
 130 for(i=1;i<=pad_len;i++) *eb++ = i;
 131 *eb++=pad_len; //pad len
 132 *eb++=4;  //next proto= ipinip
 133 newlen += pad_len+2+p_sec->auth_tag_size;
 134 Cppi_setData (Cppi_DescType_HOST, (Cppi_Desc *) Ppkt, sb,newlen);
 135 Pktlib_setPacketLen(Ppkt,newlen);
 136
 137 //now route based on outer ip
 138 ret=route_pkt(Pt, Ppkt, Phead, sb, &newlen,NULL,PoutPort);
 139 if (ret<0) return ret;
 140 *Plen=newlen;
 141 return 1;
 142 }
 143 //#define TEST
 144 #ifdef TEST
 145 #define NR 4
 146 OUR_ROUTE_T routes[]=
 147 {
 148 {1,{0x00,0x01,0x2,0x03,0x4,0x2, 0x00,0x01,0x02,0x03,0x14,0x02,0x00,0x80},0},
 149 {2,{0x00,0x01,0x2,0x03,0x4,0x3, 0x00,0x01,0x02,0x03,0x24,0x02,0x00,0x80},0},
 150 {3,{0x00,0x01,0x2,0x03,0x4,0x4, 0x00,0x01,0x02,0x03,0x34,0x02,0x00,0x80},0},
 151 {4,{0x00,0x01,0x2,0x03,0x4,0x5, 0x00,0x01,0x02,0x03,0x44,0x02,0x00,0x80},0}};
 152
 153 unsigned int ip[]={BE(0x0a000010),BE(0xa0000110),BE(0x0a000210),BE(0x0a000310)};
 154 Trie *rt;
 155 char tp[1500];
 156 IP_netTestHead_T th={0x00,0x00,0x00,0x01020304,0x1002000a};
 157 main()
 158 {
 159 int i;
 160 int l;
 161 int out_port;
 162 rt = route_init();
 163 for (i=0;i<NR;i++)
 164 {
 165   route_add(rt,&ip[i],&routes[i]);
 166 }
 167
 168 memcpy(&tp[14],&th,20);
 169 l=1500;
 170 route_pkt(rt, 0, &th, &tp[0],&l,NULL,&out_port);
 171 }
 172 #endif