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

extern const char *__progname;

#include "pkt-util.h"

unsigned int get_uint32(const void *dp)
{
 return( (((const unsigned char *)dp)[0] * 0x01000000) |
	 (((const unsigned char *)dp)[1] * 0x00010000) |
	 (((const unsigned char *)dp)[2] * 0x00000100) |
	 (((const unsigned char *)dp)[3] * 0x00000001) );
}

void *put_uint32(void *dp, unsigned int v)
{
 ((unsigned char *)dp)[0] = (v >> 24) & 0xff;
 ((unsigned char *)dp)[1] = (v >> 16) & 0xff;
 ((unsigned char *)dp)[2] = (v >>  8) & 0xff;
 ((unsigned char *)dp)[3] = (v      ) & 0xff;
 return(((char *)dp)+4);
}

void *put_string(void *buf, const void *data, int len)
{
 if (len < 0) len = strlen(data);
 buf = put_uint32(buf,len);
 bcopy(data,buf,len);
 return(((char *)buf)+len);
}

/*
 * It is annoyingly difficult to work with numbers in base 256 in
 *  libgmp.  Suggestions for improvements are welcomed.
 */

void *put_mpint(void *buf, MP_INT *v)
{
 int szbits;
 int szbytes;
 MP_INT t;
 int i;

 if (mpz_cmp_ui(v,0) < 0) abort();
 szbits = mpz_sizeinbase(v,2);
 if (szbits == 0)
  { put_uint32(buf,0);
    return(((char *)buf)+4);
  }
 szbytes = (szbits >> 3) + 1;
 mpz_init(&t);
 mpz_set(&t,v);
 for (i=szbytes-1;i>=0;i--)
  { ((unsigned char *)buf)[4+i] = mpz_get_ui(&t) & 255;
    mpz_tdiv_q_2exp(&t,&t,8);
  }
 if ( (((unsigned char *)buf)[4] == 0) &&
      !(((unsigned char *)buf)[5] & 0x80) ) abort();
 put_uint32(buf,szbytes);
 return(((char *)buf)+4+szbytes);
}

int mpint_bytes(MP_INT *v)
{
 int n;

 n = mpz_sizeinbase(v,2);
 if (n == 0) return(4);
 return((n>>3)+5);
}

void pp_fail(BPP *b, const void *dataptr, const char *fmt, ...)
{
 int i;
 va_list ap;

 fprintf(stderr,"%s: ",__progname);
 va_start(ap,fmt);
 vfprintf(stderr,fmt,ap);
 va_end(ap);
 fprintf(stderr,"\n");
 for (i=0;i<b->iplen;i++)
  { if (&b->ipkt[i] == dataptr) fprintf(stderr," <<>>");
    fprintf(stderr," %02x",b->ipkt[i]);
  }
 fprintf(stderr,"\n");
 exit(1);
}

void data_to_mpint(MP_INT *mpi, const void *data, int len)
{
 int i;

 mpz_set_ui(mpi,0);
 for (i=0;i<len;i++)
  { mpz_mul_2exp(mpi,mpi,8);
    mpz_add_ui(mpi,mpi,((const unsigned char *)data)[i]);
  }
}

void mpint_to_data(MP_INT *v, void *data, int len)
{
 int i;

 for (i=len-1;i>=0;i--)
  { ((unsigned char *)data)[i] = mpz_get_ui(v) & 255;
    mpz_tdiv_q_2exp(v,v,8);
  }
}

void pktq_init(PKTQ *q)
{
 q->head = 0;
 q->tailp = &q->head;
}

void pktq_put(PKTQ *q, const void *data, int len)
{
 PKTQE *e;

 e = malloc(sizeof(PKTQE)+len);
 e->link = 0;
 *q->tailp = e;
 q->tailp = &e->link;
 e->len = len;
 bcopy(data,e+1,len);
}

int pktq_get(PKTQ *q, void *data, int *alenp, int maxlen)
{
 PKTQE *e;

 e = q->head;
 if (! e) return(0);
 if (e->len > maxlen) abort();
 bcopy(e+1,data,e->len);
 *alenp = e->len;
 if (! (q->head = e->link)) q->tailp = &q->head;
 free(e);
 return(1);
}

int comma_list(const void *s, int len, int (*fn)(const void *, int))
{
 int beg;
 int i;
 int v;
 int rv;

 rv = 0;
 beg = 0;
 i = 0;
 do
  { if ((i >= len) || (((const unsigned char *)s)[i] == ','))
     { v = (*fn)(((const char *)s)+beg,i-beg);
       if (v < 0) return(v);
       rv += v;
       beg = i + 1;
     }
    i ++;
  } while (i <= len);
 return(rv);
}

void badmsg(const char *state, const char *fmt, ...)
{
 va_list ap;

 fprintf(stderr,"%s: protocol error: ",__progname);
 va_start(ap,fmt);
 vfprintf(stderr,fmt,ap);
 va_end(ap);
 fprintf(stderr," message in state %s\n",state);
 exit(1);
}