fd433b4b11815cb52bcd76c8c6d2da6448da8a8f
[keystone-rtos/netapi.git] / ti / runtime / netapi / test / router.c
1 #include "trie.h"
2 #include "net_test.h"
3 #include "router.h"
8 Trie * route_init(void)
9 {
10     Trie *Pt = trie_new();
11     return Pt;
12 }
14  
16 void route_add(Trie * Pt, unsigned long * Pdest_ipBE, void * Pour_route)
17 {
19    trie_insert(Pt,(char *)Pdest_ipBE, sizeof(int), Pour_route); 
20 }
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 }
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;
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);
71 //adjust IP_Header to make outer:
72 Phead->src=p_sec->src;
73 Phead->dst=p_sec->dst;
75 //next_proto
76 pc = (unsigned char *)&Phead->w3;
77 pc[1]=50;
79 //payload len
80 pc= (unsigned char*)&Phead->w1;
81 pl_len=  ((pc[2]<<8) | (pc[3]));  
83 //pad length
84 nb = pl_len/p_sec->bl;       //128 for AES
85 pad_len =   pl_len - nb*p_sec->bl;
86 switch(pad_len)
87 {
88   case(15):
89    pad_len=15;
90    break;
91   default:
92     pad_len = p_sec->bl-pad_len-2;
93 }
95 new_pl_len = pl_len + +20+ 8 + p_sec->iv_len + pad_len + p_sec->auth_tag_size +2;
96 pc[2] =  (new_pl_len&0xff00)>>8;
97 pc[3] =  (new_pl_len&0xff);
98 memcpy(&sb[14],Phead,20);  //copy outer ip header into buffer 
100 //build esp header
101 memcpy(&sb[14+20],(unsigned char *)&p_sec->spi,4);
102 seq=BE(p_sec->seq++); //TODO: multicore safe ;  CHECK to make sure that SA does this, if so remove this code
103 memcpy(&sb[14+24],(unsigned char *)&seq,4);
105 //IV: don't need:   Sa does it.
106 //memcpy(&sb[14+28],&IV[0],p_sec->iv_len);
108 //Padding, trailer, room for tag
109 for(i=1;i<=pad_len;i++) *eb++ = i;
110 *eb++=pad_len; //pad len
111 *eb++=4;  //next proto= ipinip
112 newlen += pad_len+2+p_sec->auth_tag_size;
113 Cppi_setData (Cppi_DescType_HOST, (Cppi_Desc *) Ppkt, sb,newlen);
114 Pktlib_setPacketLen(Ppkt,newlen);
116 //now route based on outer ip
117 ret=route_pkt(Pt, Ppkt, Phead, sb, &newlen,NULL,PoutPort);
118 if (ret<0) return ret;
119 *Plen=newlen;
120 return 1;
121
122 //#define TEST
123 #ifdef TEST
124 #define NR 4
125 OUR_ROUTE_T routes[]=
127 {1,{0x00,0x01,0x2,0x03,0x4,0x2, 0x00,0x01,0x02,0x03,0x14,0x02,0x00,0x80},0},
128 {2,{0x00,0x01,0x2,0x03,0x4,0x3, 0x00,0x01,0x02,0x03,0x24,0x02,0x00,0x80},0},
129 {3,{0x00,0x01,0x2,0x03,0x4,0x4, 0x00,0x01,0x02,0x03,0x34,0x02,0x00,0x80},0},
130 {4,{0x00,0x01,0x2,0x03,0x4,0x5, 0x00,0x01,0x02,0x03,0x44,0x02,0x00,0x80},0}};
132 unsigned int ip[]={BE(0x0a000010),BE(0xa0000110),BE(0x0a000210),BE(0x0a000310)};
133 Trie *rt;
134 char tp[1500];
135 IP_netTestHead_T th={0x00,0x00,0x00,0x01020304,0x1002000a};
136 main()
138 int i;
139 int l;
140 int out_port;
141 rt = route_init();
142 for (i=0;i<NR;i++)
143 {  
144   route_add(rt,&ip[i],&routes[i]);
147 memcpy(&tp[14],&th,20);
148 l=1500;
149 route_pkt(rt, 0, &th, &tp[0],&l,NULL,&out_port);
151 #endif