
/* 
 * tlv.c  -  do Type Length Value parsing of an BER encodeded ASN.1 stream
 *
 * Copyright (c) 1993 Nordic SR-NET 
 * All Rights reserved 
 * 
 * Written by
 *	Geir Pedersen, Univ of Oslo
 *	Geir.Pedersen@usit.uio.no
 * 
*/

/*

   These routines will work on a socket where it is assumed to be data
   available for reading. If the socket is non-blocking control may return
   before a complete PDU has been read, indicating so. If a PDU is available, 
   it will be returned.

void tlv_open ( int fd );
void tlv_close ( int fd );
unsigned char *tlv_parse ( int fd );

*/

#include <sys/types.h>
#include <sys/ioctl.h>
#include "SR.h"
#include <sr-general.h>
#include <sr-util.h>


#define PDUBUFSIZE	5000


typedef enum parseState { ps_idle, ps_readingType,
			     ps_readingLength, ps_readingValue } parseState; 

typedef struct tlv_level {
   int			len;
}tlv_level;

typedef struct dynamic_buffer {
   int			allocated;
   int			ff;
   unsigned char	*buf;
   int			increment;
} dynamic_buffer;

typedef struct tlv_buffer {
   int			error;
   parseState		state;
   int			fd;
   int			nxtparse; /* next byte to be parsed from read */
   int			stacklevel; /* index into stack */
   int			left;	/* number of bytes left to read on this stacklevel */
   Boolean		constructed; /* if TLV currently parsed is constructed */
   Boolean		indefiniteLength;
   Boolean		PDUready;
   dynamic_buffer 	*read;
   struct tlv_buffer 	*next;
} tlv_buffer;

tlv_buffer	*buffers	= (tlv_buffer *) NULL;


dynamic_buffer *alloc_dynbuf ( int size )
{
   dynamic_buffer *b	= (dynamic_buffer *)malloc ( sizeof (dynamic_buffer) );

   b->buf 		= (unsigned char *)malloc ( size );
   b->allocated 	= size;
   b->ff 		= 0;
   b->increment		= size;

   return b;
}

int left_dynbuf ( dynamic_buffer *b )
{
   return b->allocated - b->ff;
}

unsigned char *ff_dynbuf ( dynamic_buffer *b )
{
   return b->buf + b->ff;
}

unsigned char *initial_dynbuf ( dynamic_buffer *b )
{
   return b->buf;
}

int size_dynbuf ( dynamic_buffer *b )
{
   return b->allocated;
}

void consume_dynbuf ( dynamic_buffer *b, int size )
{
   if ( b->ff + size > b->allocated )
   {
      LOG ( facTLV, llevExceptions, "consume_dynbuf: user wrote over dynbuf edge" );
      b->ff = b->allocated;
   }
   else
      b->ff += size;
}

unsigned char *index_dynbuf ( dynamic_buffer *b, int index )
{
   return b->buf + index;
}

void freeinitial_dynbuf ( dynamic_buffer *b, int size )
{
   if ( size >= b->allocated )
      b->ff = 0;
   else
   {
      bcopy ( b->buf + size, b->buf, b->allocated - size );
      b->ff -= size;
   }
}

dynamic_buffer *realloc_dynbuf ( dynamic_buffer *b, int size )
{
   if ( b && b->allocated < size )
   {
      b->buf = (unsigned char *) realloc ( (char *) b->buf, size );
      b->allocated = size;
   }

   return b;
}

void free_dynbuf ( dynamic_buffer *b )
{
   if ( !b )
      return;
   free ( b->buf );
   free ( b );
}




void tlv_open ( int fd )
{
   tlv_buffer	*b	= (tlv_buffer *) malloc ( sizeof ( tlv_buffer ) );

   b->error = 0;
   b->state = ps_idle;
   b->fd = fd;
   b->read = alloc_dynbuf ( PDUBUFSIZE );
   b->stacklevel = 0;
   b->PDUready = False;
   b->next = buffers;
   buffers = b;
}

void tlv_close ( int fd )
{
   tlv_buffer	*b, *bp;

   for ( b = buffers, bp = (tlv_buffer *) NULL;
	 b && b->fd != fd;
	 bp = b, b = b->next );
   bp->next = b;

   free ( b->read );
   free ( b );
}

int tlv_error ( int fd )
{
   tlv_buffer	*b;

   for ( b = buffers; b && b->fd != fd; b = b->next );

   if ( b )
      return b->error;
   else
      return 0;
}


unsigned char *tlv_parse ( int fd, int *PDUlen )
{
   tlv_buffer	*b;
   long		nread;

   for ( b = buffers; b && b->fd != fd; b = b->next );

   if ( b->PDUready )
   {
      /* A PDU was returned last time this function was called - free space */
      freeinitial_dynbuf ( b->read, b->nxtparse );
      b->nxtparse = 0;
      b->stacklevel = 0;
   }

   /* read whatever can be read
      - if FIONREAD is not supported, this will have to be rewritten */
   if ( ioctl ( fd, FIONREAD, &nread ) == -1 )
   {
      LOG ( facTLV, llevExceptions, "ioctl() failed: %s", strerror(errno) );
      b->error = errno;
      return 0;
   }
   if ( nread > left_dynbuf ( b->read ) )
      b->read = realloc_dynbuf ( b->read, nread + PDUBUFSIZE );
   if ( (nread = read ( fd, ff_dynbuf ( b->read ), nread )) == -1 )
   {
      LOG ( facTLV, llevExceptions, "read(2) failed: %s", strerror(errno) );
      b->error = errno;
      return 0;
   }
   consume_dynbuf ( b->read, nread );

   LOG ( facTLV, llevDebug, "Read %d bytes", nread );

   while ( 1 )
   {
      switch ( b->state )
      {
       case ps_idle:
	 b->state = ps_readingType;
	 b->nxtparse = 0;
	 break;

       case ps_readingType:
         {
	    unsigned char	*cp;
	    Boolean		multibyte	= False;
	    Boolean		popped		= False;
	    
	    for ( cp = index_dynbuf ( b->read, b->nxtparse ); cp < ff_dynbuf ( b->read ); cp++ )
	       if ( !multibyte )
	       {
		  if ( *cp & 0x20 )
		     b->constructed = True;
		  if ( (*cp & 0x1f) == 0x1f )
		     multibyte = True;
		  else if ( *cp == 0 && b->stacklevel )
		  {
		     /* could be an End-of-contents mark */
		     if ( ff_dynbuf ( b->read ) < cp + 2 )
			return 0;	/* not enough data to parse */
		     else
			if ( *(cp+1) == 0 )
			{
			   cp++;
			   b->stacklevel--;
			   b->state = ps_readingType;
			   popped = True;
			   if ( b->stacklevel == 0 )
			      b->PDUready = True;
			   LOG ( facTLV, llevDebug, "POP" );
			   break;
			}
		  }
		  else
		     break;
	       }
	       else
	       {
		  if ( !(*cp & 0x80) )
		     break;	/* last byte */
	       }

	    if ( cp == ff_dynbuf ( b->read ) )
	       /* ran out of data to parse */
	       return 0;

	    b->nxtparse = cp - initial_dynbuf ( b->read ) + 1;
	    if ( !popped )
	       b->state = ps_readingLength;
	 }	       
	 break;

       case ps_readingLength:
         {
	    unsigned char	*cp	= index_dynbuf ( b->read, b->nxtparse );
	    unsigned long	l; /* presumed to be 32 bits wide */
	    unsigned int	nobytes;

	    b->indefiniteLength = False;

	    switch ( *cp & 0x80 )
	    {
	     case 0x00:
	       /* short form */
	       l = *cp & 0x7f;
	       break;

	     default:
	       /* long form */
	       if ( (nobytes = *cp & 0x7f) == 0 )
	       {
		  /* indefinite length */
		  l = 0;
		  b->indefiniteLength = True;
		  LOG ( facTLV, llevDebug, "PUSH" );
	       }
	       else
	       {
		  if ( b->nxtparse + 1 + nobytes
		       >= ff_dynbuf ( b->read ) - initial_dynbuf ( b->read ) )
		     /* ran out of data to parse */
		     return 0;
		  if ( nobytes > 4 )
		  {
		     /* can't handle this large L-field */
		     LOG ( facTLV, llevExceptions, "Can't handle this large Length-field, %d octets",
			   nobytes );
		     b->error = -1;
		     return 0;
		  }
		  
		  for ( cp++, l = 0; nobytes; nobytes--, cp++ )
		     l = (l << 8) + *cp; /* #### () was missing. --hbf */
	       }
	    }

	    b->nxtparse = cp - initial_dynbuf ( b->read ) + 1;
	    b->state = ps_readingValue;
	    b->left = l;
	    LOG ( facTLV, llevDebug, "Length %d", l );
	 }
	 break;

       case ps_readingValue:
	 if ( b->indefiniteLength )
	 {
	    b->stacklevel++;
	    b->state = ps_readingType;
	    break;
	 }

	 if ( b->left <= ff_dynbuf ( b->read ) - initial_dynbuf ( b->read ) )
	 {
	    b->nxtparse += b->left;
	    b->left = 0;
	    b->state = ps_readingType;
	    if ( b->stacklevel == 0 )
	       b->PDUready = True;
	    LOG ( facTLV, llevDebug, "Done reading value" );
	 }
	 else
	    /* ran out of data to parse */
	    return 0;
	 break;
      }

      if ( b->PDUready )
      {
	 /* have parsed a PDU - return it */
	 LOG ( facTLV, llevDebug, "PDU ready" );
	 *PDUlen = b->nxtparse;
	 return initial_dynbuf ( b->read );
      }
   }
}

