/*
 *  asn1per.c
 *
 *  Copyright (C) 2010  Altobridge, Inc
 *  Written by: Xiaohua Chen <xiaohua.chen@altobridge.com>
 *
 *  Specs: ITU-T X.691
 */

#include <string.h>
#include "./include/asn1per.h"

const unsigned char NB[256] = {0,1,1,
									2,2,
                                    3,3,3,3,
                                    4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
                                    6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
                                    7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
                                    7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
                                    8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,
                                    8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,
                                    8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,
                                    8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8
                                    };

const unsigned char MB[9] = {0x00,0x01,0x03,0x07,0x0f,0x1f,0x3f,0x7f,0xff};

static int pe_bit(unsigned char *pdst, bool value, unsigned char *offset_bit)
{
	int offset = 0;
	pdst[offset]|= (value << (7- (*offset_bit)));
	(*offset_bit)++;
	if((*offset_bit) == 8){offset++;*offset_bit=0;}
	return offset;
}

static int pd_bit(unsigned char *psrc, bool *value, unsigned char *offset_bit)
{
	int offset = 0;
	*value = (psrc[offset] >> (7-(*offset_bit))) & 0x01;
	(*offset_bit)++;
	if((*offset_bit) == 8){offset++;*offset_bit=0;}
	return offset;
}

/**************************************************************
 *
 * Length determinant
 *
 **************************************************************/
int per_decode_Length(unsigned int *plen, unsigned char *psrc)
{
	int offset = 0, m = 0;

	if((psrc[offset] & 0xc0) == 0xc0){
		m = psrc[offset] & 0x3F;
		*plen = m * 16384;
		offset++;
	}
	else if((psrc[offset] & 0x80) == 0x80){
		*plen = ((psrc[offset] & 0x3F) << 8)+psrc[offset+1];
		offset+=2;
	}
	else{
		*plen = psrc[offset];
		offset++;
	}
	return offset;
}

int per_encode_Length(unsigned char *pdst, int len)
{
	int offset = 0;
	
	if(len > 0x7F){
		pdst[offset++] = 0x80|((len >> 8) & 0x7F);
		pdst[offset++] = len & 0xff;
	}
	else{
		pdst[offset++] = len;
	}
	return offset;
}

/**************************************************************
 *
 * Open type fields
 *
 **************************************************************/

int per_decode_OpenType(ASN1OpenType *popen, unsigned char *psrc)
{
	int offset = 0;

	offset = per_decode_Length(&popen->len, psrc);
	popen->pdata = psrc+offset;
	offset+=popen->len;

	return offset;
}

int per_encode_OpenType(unsigned char *pdst, ASN1OpenType *popen)
{
	int offset = 0;
	offset = per_encode_Length(pdst, popen->len);
	memcpy(pdst+offset, popen->pdata, popen->len);
	offset+=popen->len;

	return offset;
}

/**************************************************************
 *
 * The Boolean type
 *
 **************************************************************/
 
int per_encode_Boolean(unsigned char *pdst, bool value, unsigned char *offset_bit)
{
	int offset = 0;
       
	value &= 0x01;
	pdst[offset]|= (value << (7- (*offset_bit)));
	(*offset_bit)++;
	if((*offset_bit) == 8){offset++;*offset_bit=0;}
	return offset;
}

int per_decode_Boolean(unsigned char *psrc, bool *value, unsigned char *offset_bit)
{
	int offset = 0;

	*value = (psrc[offset] >> (7-(*offset_bit))) & 0x01;
	(*offset_bit)++;
	if((*offset_bit) == 8){offset++;*offset_bit=0;}
	return offset;
}

/**************************************************************
 *
 * Constrained Interger (Note: it only handles integers that fit inside a 32 bit integer)
 *
 **************************************************************/

int per_encode_Constrained_Integer(unsigned char *pdst, unsigned int value, 
									unsigned int lower, unsigned int upper, unsigned char *offset_bit)
{
	int offset = 0, nbs = 0, m = 0, i, j;
    unsigned char bl[16];
	unsigned int r, tmp = 0;
	
	r = upper - lower + 1;

    if((upper-lower) == 4294967295UL)
		r = 4294967295UL;

	   //if r=0 then return;
	if(r == 1){
		nbs = 1;
		offset+=pe_bit(pdst+offset, 0, offset_bit);
	}
	else if (r < 256)
	{	
		nbs = NB[r];
		m = MB[nbs];

        j = 0;
        tmp = (value - lower) & m;
        for(i = nbs-1; i >=0; i--){
			bl[j++] = (tmp >> i) & 0x01;  
        }
        for (i = 0; i < nbs; i++){
			offset+=pe_bit(pdst+offset, bl[i], offset_bit);
        }
	}
	else if (r == 256)
	{
		nbs = 8;
        if((*offset_bit) > 0) {offset++; *offset_bit = 0;}
        tmp = (value - lower) & 0xff;
        pdst[offset++] = tmp;           
	}
	else if (r<=65536)
	{
		nbs = 16;
        if((*offset_bit) > 0) {offset++; *offset_bit = 0;}
        tmp = (value - lower) & 0xffff;

        pdst[offset++] = (tmp >> 8) & 0xff;
        pdst[offset++] = tmp & 0xff;             
	}
    else {
		int on;
        bool b0, b1;

        tmp = value - lower;
        if(tmp < 0x100) {on = 1; b0 = 0; b1 = 0;}
        else if(tmp < 0x10000) {on = 2; b0 = 0; b1 = 1;}
        else if(tmp < 0x1000000) {on = 3; b0 = 1; b1 = 0;}
        else {on = 4; b0 = 1; b1 = 1;}

        offset+=pe_bit(pdst+offset, b0, offset_bit);
        offset+=pe_bit(pdst+offset, b1, offset_bit);
        if((*offset_bit) > 0) {offset++; *offset_bit = 0;}

        if(on == 1) {pdst[offset] = tmp & 0xff; offset++;}
        else if(on == 2) {pdst[offset] = (tmp >> 8) & 0xff; pdst[offset+1] = tmp & 0xff;offset+=2;}
        else if(on == 3) {pdst[offset] = (tmp >> 16) & 0xff; pdst[offset+1] = (tmp >> 8) & 0xff; pdst[offset+2] = tmp & 0xff;offset+=3;}
        else {pdst[offset] = (tmp >> 24) & 0xff;pdst[offset+1] = (tmp >> 16) & 0xff; pdst[offset+2] = (tmp >> 8) & 0xff; pdst[offset+3] = tmp & 0xff;offset+=4;}
	}
	
	return offset;
}

int per_decode_Constrained_Integer(unsigned char *psrc, unsigned int *value, 
									unsigned int lower, unsigned int upper, unsigned char *offset_bit)
{
	int offset = 0, nbs = 0, m = 0, i, j;
    unsigned char bl[16];
	unsigned int r, tmp = 0;
	
	r = upper - lower + 1;

	if((upper-lower) == 4294967295UL)
           r = 4294967295UL;
	
	if(r==1){
		nbs = 1;
		offset+=pd_bit(psrc+offset, &bl[0], offset_bit);
		*value = bl[0];
	}
	else if (r < 256)
	{	
		nbs = NB[r];
		m = MB[nbs];
		
		j = 0;
        for (i = 0; i < nbs; i++){
			offset+=pd_bit(psrc+offset, &bl[i], offset_bit);
        }
		for(i = nbs-1; i >=0; i--){
			tmp += (bl[j++] << i);
        }
		*value = tmp + lower;
	}
	else if (r == 256)
	{
		nbs = 8;
        if((*offset_bit) > 0) {offset++; *offset_bit = 0;}
	    tmp = psrc[offset++];
		*value = tmp + lower;   
	}
	else if (r<=65536)
	{
		nbs = 16;
        if((*offset_bit) > 0) {offset++; *offset_bit = 0;}
        tmp = (psrc[offset] << 8) + psrc[offset+1];
		*value = tmp + lower;
		offset+=2;
	}
	else {
        int on;
        bool b0, b1;

        offset+=pd_bit(psrc+offset, &b0, offset_bit);
        offset+=pd_bit(psrc+offset, &b1, offset_bit);
        if((*offset_bit) > 0) {offset++; *offset_bit = 0;}

		if((b0 == 0)&&(b1 == 0)){on = 1;}
		else if((b0 == 0)&&(b1 == 1)) {on = 2;}
		else if((b0 == 1)&&(b1 == 0)) {on = 3;}
		else {on = 4;}
		
        if(on == 1) {tmp = psrc[offset]; offset++;}
        else if(on == 2) { tmp = (psrc[offset] << 8) + psrc[offset+1]; offset+=2;}
        else if(on == 3) { tmp = (psrc[offset] << 16) + (psrc[offset+1] << 8)+psrc[offset+2];offset+=3;}
        else {tmp = (psrc[offset] << 24) + (psrc[offset+1] << 16) + (psrc[offset+2] << 8) + psrc[offset+3];offset+=4;}
	    *value = tmp + lower;
    }
	return offset;
}

/**************************************************************
 *
 * BIT STRING
 *
 **************************************************************/

int per_encode_BitString(unsigned char *pdst, unsigned int numbits, unsigned char *pdata)
{
	int offset = 0, octlen = 0;

	octlen = numbits >> 3;
	if(numbits % 8)
		octlen++;
	memcpy(pdst, pdata, octlen);
	offset+=octlen;
	return offset;
}

int per_decode_BitString(unsigned char *psrc, unsigned int numbits, unsigned char *pdata)
{
	int offset = 0, octlen = 0;
	octlen = numbits >> 3;
	if(numbits % 8)
        octlen++;
	memcpy(pdata, psrc, octlen);
	offset+=octlen;
	return offset;
}

int per_encode_BitStr16(unsigned char *pdst, unsigned char *value, unsigned int lbits, unsigned int ubits, unsigned char *offset_bit)
{
       int offset = 0, i, j;
       unsigned char b1[16];
       unsigned int tmp;
       
       if((ubits == lbits) && (ubits <=16)){
            j = 0;
            if(ubits > 8)
                tmp = (value[0] << 8)+value[1];
            else
                tmp = value[0];
            for(i = ubits-1; i >=0; i--){
                b1[j++] = (tmp >> i) & 0x01;  
            }
            for (i = 0; i < ubits; i++){
                offset+=pe_bit(pdst+offset, b1[i], offset_bit);
            }
       }
       return offset;
}

int per_decode_BitStr16(unsigned char *psrc, unsigned char *value, unsigned int lbits, unsigned int ubits, unsigned char *offset_bit)
{
	int offset = 0, i, j;
	unsigned char b1[16];
	unsigned int tmp = 0;
       
	if((ubits == lbits) && (ubits <=16)){
		j = 0;
		for (i = 0; i < ubits; i++){
			offset+=pd_bit(psrc+offset, &b1[i], offset_bit);
		}
		for(i = ubits-1; i >=0; i--){
			tmp += (b1[j++] << i);
		}
		if(ubits > 8){value[0] = (tmp >> 8) & 0xff; value[1] = tmp & 0xff;}
            else {value[0] = tmp & 0xff;}
	}
	return offset;
}

/**************************************************************
 *
 * OCTET STRING
 *
 **************************************************************/

int per_encode_OctetString(unsigned char *pdst, unsigned int numocts, unsigned char *pdata)
{
	int offset = 0;

	memcpy(pdst, pdata, numocts);
	offset+=numocts;
	return offset;
}

int per_decode_OctetString(unsigned char *psrc, unsigned int numocts, unsigned char *pdata)
{
	int offset = 0;

	memcpy(pdata, psrc, numocts);
	offset+=numocts;
	return offset;
}

/**************************************************************
 *
 * Normally Small Non-Negative Whole Number
 *
 **************************************************************/

int per_encode_SmallNonNegWholeNumber(unsigned char *pdst, unsigned int value, unsigned char *offset_bit)
{
       int offset = 0, i, j;
       bool sn = 0;
       unsigned char b1[8];
       
       if(value > 63)
            sn = 1;

       offset+=pe_bit(pdst+offset, sn, offset_bit);
       if(!sn){
            j = 0;
            for(i = 5; i >=0; i--){
                b1[j++] = (value >> i) & 0x01;  
            }
            for (i = 0; i < 6; i++){
                offset+=pe_bit(pdst+offset, b1[i], offset_bit);
            }
       }
       else{
			if((*offset_bit) > 0) offset++;
			if(value > 0x7f){pdst[offset++] = 0x80|((value << 8) & 0x7f); pdst[offset++] = value & 0xff;}
			else {pdst[offset++] = value & 0xff;}
       }
       return offset;
}

int per_decode_SmallNonNegWholeNumber(unsigned char *psrc, unsigned int *value, unsigned char *offset_bit)
{
       int offset = 0, i, j;
       bool sn = 0;
       unsigned char b1[8];
	unsigned int tmp = 0;
	
       offset+=pd_bit(psrc+offset, &sn, offset_bit);
	
       if(!sn){
            j = 0;
            
            for (i = 0; i < 6; i++){
                offset+=pd_bit(psrc+offset, &b1[i], offset_bit);
            }
	        for(i = 5; i >=0; i--){
                tmp += (b1[j++] << i);  
            }
	        *value = tmp;
       }
       else{
           	if((*offset_bit) > 0) offset++;
	    	if((psrc[offset] & 0x80) == 0x80){
				*value = ((psrc[offset] & 0x3F) << 8)+psrc[offset+1];
				offset+=2;
			}
			else{
				*value = psrc[offset];
				offset++;
			}
       }
       return offset;
}

