#include "trie.h"
#include "net_test.h"


#if 0
#define BE(x) ( (((x)&0xff000000)>>24) | (((x)&0xff0000)>>8) | (((x)&0xff00)<<8) | (((x)&0xff)<<24)  )

typedef struct iphead_t
{
  unsigned long w1;
  unsigned long w2;
  unsigned long w3;
  unsigned long src;
  unsigned long dst;
} IP_HEAD_T;

#define ROUTE_SEC_T netTestSA_t

typedef struct our_route_t
{
   int out_port;
   unsigned char out_mac[14];
   ROUTE_SEC_T * sec_ptr;
} OUR_ROUTE_T;

#endif

Trie * route_init(void)

{

Trie *Pt = trie_new();

return Pt;

}

 

void route_add(Trie * Pt, unsigned int * Pdest_ipBE, void * Pour_route)
{

   trie_insert(Pt,(char *)Pdest_ipBE, sizeof(int), Pour_route); 
}

//route the packet 
//  lookup next hop in route trie
int route_pkt(Trie *Pt, void * Ppkt, IP_HEAD_T *Phead, unsigned char * Pbuf, int * Plen, ROUTE_SEC_T **Prs, int *PoutPort)
{
   OUR_ROUTE_T *Pr;
   int ret;
   Pr = trie_lookup(Pt, (char *) &Phead->dst, 4);
   if (!Pr ) return -1;  //can't route
   if (Pr->sec_ptr) 
   {
      if(!Prs) return -3;
     //tunnel i/f
      ret=process_tunnel(Pt, Ppkt, Phead, Pbuf, Plen, Pr,*PoutPort);             
      *Prs = Pr->sec_ptr;
      return ret;
   }
   if (Prs) *Prs=NULL;
   //simple route
   //copy new mac
    memcpy(Pbuf,Pr->out_mac,14);
    Pbuf[14+8]-=1;  //ttl--
    //todo do check ttl!
    Pbuf[14+10]=0; Pbuf[14+11]=0;  //zap [outer] header checksum
   if(PoutPort) *PoutPort=Pr->out_port;
   return 1;
}

int process_tunnel(Trie *Pt, void *Ppkt, IP_HEAD_T * Phead, 
                   unsigned char * Pbuf, int* Plen, OUR_ROUTE_T * Proute,
                   int * PoutPort)
{
unsigned char *sb = Pbuf;
unsigned char *eb= &Pbuf[*Plen]; 
int newlen=*Plen;
int seq;
int pl_len;
int new_pl_len;
int pad_len;
//unsigned char IV[20];
ROUTE_SEC_T * p_sec=Proute->sec_ptr;
unsigned char *pc;
int nb;
int i;
int ret;

/* move sb to new start */
sb = sb - p_sec->iv_len-8-20;  //make room for esp header and new outer ip
newlen +=  (Pbuf-sb);

//adjust IP_Header to make outer:
Phead->src=p_sec->src;
Phead->dst=p_sec->dst;

//next_proto
pc = (unsigned char *)&Phead->w3;
pc[1]=50;

//payload len
pc= (unsigned char*)&Phead->w1;
pl_len=  ((pc[2]<<8) | (pc[3]));  

//pad length
nb = pl_len/p_sec->bl;       //128 for AES
pad_len =   pl_len - nb*p_sec->bl;
switch(pad_len)
{
  case(15):
   pad_len=15;
   break;
  default:
    pad_len = p_sec->bl-pad_len-2;
}

new_pl_len = pl_len + +20+ 8 + p_sec->iv_len + pad_len + p_sec->auth_tag_size +2;
pc[2] =  (new_pl_len&0xff00)>>8;
pc[3] =  (new_pl_len&0xff);
memcpy(&sb[14],Phead,20);  //copy outer ip header into buffer 

//build esp header
memcpy(&sb[14+20],(unsigned char *)&p_sec->spi,4);
seq=BE(p_sec->seq++); //TODO: multicore safe ;  CHECK to make sure that SA does this, if so remove this code
memcpy(&sb[14+24],(unsigned char *)&seq,4);

//IV: don't need:   Sa does it.
//memcpy(&sb[14+28],&IV[0],p_sec->iv_len);

//Padding, trailer, room for tag
for(i=1;i<=pad_len;i++) *eb++ = i;
*eb++=pad_len; //pad len
*eb++=4;  //next proto= ipinip
newlen += pad_len+2+p_sec->auth_tag_size;
Cppi_setData (Cppi_DescType_HOST, (Cppi_Desc *) Ppkt, sb,newlen);
Pktlib_setPacketLen(Ppkt,newlen);

//now route based on outer ip
ret=route_pkt(Pt, Ppkt, Phead, sb, &newlen,NULL,PoutPort);
if (ret<0) return ret;
*Plen=newlen;
return 1;
} 
//#define TEST
#ifdef TEST
#define NR 4
OUR_ROUTE_T routes[]=
{
{1,{0x00,0x01,0x2,0x03,0x4,0x2, 0x00,0x01,0x02,0x03,0x14,0x02,0x00,0x80},0},
{2,{0x00,0x01,0x2,0x03,0x4,0x3, 0x00,0x01,0x02,0x03,0x24,0x02,0x00,0x80},0},
{3,{0x00,0x01,0x2,0x03,0x4,0x4, 0x00,0x01,0x02,0x03,0x34,0x02,0x00,0x80},0},
{4,{0x00,0x01,0x2,0x03,0x4,0x5, 0x00,0x01,0x02,0x03,0x44,0x02,0x00,0x80},0}};

unsigned int ip[]={BE(0x0a000010),BE(0xa0000110),BE(0x0a000210),BE(0x0a000310)};
Trie *rt;
char tp[1500];
IP_HEAD_T th={0x00,0x00,0x00,0x01020304,0x1002000a};
main()
{
int i;
int l;
int out_port;
rt = route_init();
for (i=0;i<NR;i++)
{  
  route_add(rt,&ip[i],&routes[i]);
}

memcpy(&tp[14],&th,20);
l=1500;
route_pkt(rt, 0, &th, &tp[0],&l,NULL,&out_port);
}
#endif