/* This file is in the public domain. */

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

extern const char *__progname;

#include "errf.h"
#include "panic.h"

#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 fput_uint32(FILE *f, unsigned int v)
{
 putc((v>>24)&0xff,f);
 putc((v>>16)&0xff,f);
 putc((v>> 8)&0xff,f);
 putc( v     &0xff,f);
}

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);
}

void fput_string(FILE *f, const void *data, int len)
{
 if (len < 0) len = strlen(data);
 fput_uint32(f,len);
 fwrite(data,1,len,f);
}

/*
 * 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) panic("negative");
 szbits = mpz_sizeinbase(v,2);
 if (szbits == 0) return(put_uint32(buf,0));
 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) ) panic("bad leading 0");
 put_uint32(buf,szbytes);
 return(((char *)buf)+4+szbytes);
}

void *put_bool(void *buf, int v)
{
 *(unsigned char *)buf = v ? 1 : 0;
 return(((char *)buf)+1);
}

int mpint_bytes(MP_INT *v)
{
 int n;

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

void pp_vfail(BPP *b, const void *dataptr, const char *fmt, va_list ap)
{
 int i;
 FILE *f;

 f = logmsg_fopen(LM_ERR|LM_PEER);
 vfprintf(f,fmt,ap);
 fprintf(f,"\n");
 for (i=0;i<b->iplen;i++)
  { if (&b->ipkt[i] == dataptr) fprintf(f," <<>>");
    fprintf(f," %02x",b->ipkt[i]);
  }
 fclose(f);
 exit(1);
}

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

 va_start(ap,fmt);
 pp_vfail(b,dataptr,fmt,ap);
 va_end(ap);
}

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) panic("overflow");
 bcopy(e+1,data,e->len);
 *alenp = e->len;
 if (! (q->head = e->link)) q->tailp = &q->head;
 free(e);
 return(1);
}

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

 f = logmsg_fopen(LM_WARN|LM_PEER);
 fprintf(f,"%s: protocol error: ",__progname);
 va_start(ap,fmt);
 vfprintf(f,fmt,ap);
 va_end(ap);
 fprintf(f," message in state %s",state);
 fclose(f);
 exit(1);
}

void *put_block(void *dp, const void *data, int len)
{
 if (len > 0) bcopy(data,dp,len);
 return(len+(char *)dp);
}