#include <gmp.h>
#include <stdarg.h>
#include <stdlib.h>
#include <strings.h>

#include "str.h"
#include "pkt-util.h"

#define PP__INLINE
#include "pp.h"

typedef struct backout BACKOUT;

struct backout {
  BACKOUT *link;
  void *blk;
  } ;

static void parse_it(const char *data, int datalen, void (*f0)(const void *, const char *), void (*f1)(const void *, const char *, int), void (*f2)(const void *, const char *, int, int), va_list ap)
{
 __label__ ret;

 int key;
 const unsigned char *ipp;
 const unsigned char *ipend;
 int store;
 BACKOUT *backouts;

 static void add_backout(void *blk)
  { BACKOUT *b;
    b = malloc(sizeof(BACKOUT));
    b->blk = blk;
    b->link = backouts;
    backouts = b;
  }

 static void *backout_alloc(int nb)
  { void *blk;
    blk = malloc(nb);
    if (! blk) return(0);
    add_backout(blk);
    return(blk);
  }

 static void walk_backouts(int freethem)
  { BACKOUT *b;
    while (backouts)
     { b = backouts;
       backouts = b->link;
       if (freethem) free(b->blk);
       free(b);
     }
  }

 static void fail0(const void *at, const char *fmt)
  { walk_backouts(1);
    (*f0)(at,fmt);
    abort();
  }

 static void fail1(const void *at, const char *fmt, int arg)
  { walk_backouts(1);
    (*f1)(at,fmt,arg);
    abort();
  }

 static void fail2(const void *at, const char *fmt, int arg1, int arg2)
  { walk_backouts(1);
    (*f2)(at,fmt,arg1,arg2);
    abort();
  }

 static void need(unsigned int n)
  { if (ipend-ipp < n)
     { walk_backouts(1);
       (*f2)(ipp,"packet too short (%d left, need %u)",(int)(ipend-ipp),n);
       goto ret;
     }
  }

 ipp = data;
 ipend = ipp + datalen;
 store = 1;
 backouts = 0;
 while (1)
  { key = va_arg(ap,int);
    switch (key)
     { case PP__SKIP:
	  store = 0;
	  continue;
	  break;
       case PP__BLOB:
	   { int i;
	     void *vp;
	     i = va_arg(ap,int);
	     vp = va_arg(ap,void *);
	     need(i);
	     if (store) bcopy(ipp,vp,i);
	     ipp += i;
	   }
	  break;
       case PP__STRING:
	   { int len;
	     STR *sp;
	     sp = va_arg(ap,STR *);
	     need(4);
	     len = get_uint32(ipp);
	     ipp += 4;
	     need(len);
	     if (store)
	      { sp->len = len;
		sp->data = backout_alloc(sp->len);
		bcopy(ipp,sp->data,sp->len);
	      }
	     ipp += sp->len;
	   }
	  break;
       case PP__STRCALL:
	   { ROSTR s;
	     void (*fn)(ROSTR, void *);
	     void *arg;
	     fn = va_arg(ap,__typeof__(fn));
	     arg = va_arg(ap,void *);
	     need(4);
	     s.len = get_uint32(ipp);
	     ipp += 4;
	     need(s.len);
	     s.data = (const void *) ipp;
	     ipp += s.len;
	     if (store) (*fn)(s,arg);
	   }
	  break;
       case PP__BOOL:
	   { int *ip;
	     ip = va_arg(ap,int *);
	     need(1);
	     switch (*ipp)
	      { case 0: if (store) *ip = 0; break;
		case 1: if (store) *ip = 1; break;
		default: (*f1)(ipp,"invalid boolean 0x%02x",*ipp); break;
	      }
	     ipp ++;
	   }
	  break;
       case PP__UINT32:
	   { unsigned int *uip;
	     uip = va_arg(ap,unsigned int *);
	     need(4);
	     if (store) *uip = get_uint32(ipp);
	     ipp += 4;
	   }
	  break;
       case PP__MPINT:
	   { int bytes;
	     MP_INT *mip;
	     mip = va_arg(ap,MP_INT *);
	     need(4);
	     bytes = get_uint32(ipp);
	     ipp += 4;
	     need(bytes);
	     if (store)
	      { mpz_set_ui(mip,0);
		for (;bytes>0;bytes--)
		 { mpz_mul_2exp(mip,mip,8);
		   mpz_add_ui(mip,mip,*ipp++);
		 }
	      }
	     else
	      { ipp += bytes;
	      }
	   }
	  break;
       case PP__ENDSHERE:
	  if (ipp != ipend) (*f0)(ipp,"junk at end of packet");
	  walk_backouts(0);
	  return;
	  break;
       case PP__REST:
	   { const void **bp;
	     int *lenp;
	     bp = va_arg(ap,const void **);
	     lenp = va_arg(ap,int *);
	     if (store)
	      { *bp = ipp;
		*lenp = ipend - ipp;
	      }
	   }
	  walk_backouts(0);
	  return;
	  break;
       case PP__BACKOUT:
	  add_backout(va_arg(ap,void *));
	  break;
       default:
	  abort();
	  break;
     }
    store = 1;
  }
ret:;
 walk_backouts(0);
}

/* Annoyingly, we cannot share the bulk of the code between parse_packet
   and parse_data without going through gyrations with the failure routine.
   Fortunately, the failure routine is called from only a few places, with
   only three different call type signatures. */

void parse_packet(BPP *bpp, void (*fail)(BPP *, const void *, const char *, ...), ...)
{
 va_list ap;

 static void f0(const void *ipp, const char *s)
  { (*fail)(bpp,ipp,s);
  }

 static void f1(const void *ipp, const char *s, int i)
  { (*fail)(bpp,ipp,s,i);
  }

 static void f2(const void *ipp, const char *s, int i1, int i2)
  { (*fail)(bpp,ipp,s,i1,i2);
  }

 va_start(ap,fail);
 parse_it(&bpp->ipkt[0],bpp->iplen,f0,f1,f2,ap);
 va_end(ap);
}

void parse_data(const void *data, int datalen, void (*fail)(const void *, const char *, ...), ...)
{
 va_list ap;

 static void f0(const void *ipp, const char *s)
  { (*fail)(ipp,s);
  }

 static void f1(const void *ipp, const char *s, int i)
  { (*fail)(ipp,s,i);
  }

 static void f2(const void *ipp, const char *s, int i1, int i2)
  { (*fail)(ipp,s,i1,i2);
  }

 va_start(ap,fail);
 parse_it(data,datalen,f0,f1,f2,ap);
 va_end(ap);
}