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