
/*-----------------------------------------------------------------------------------------------*/
/* asn1_io_buffer.cpp                                                                            */
/* copyright (c) innovaphone 2019                                                                */
/*                                                                                               */
/*-----------------------------------------------------------------------------------------------*/

#include <platform.h>
#include "common/ilib/asn1.h"
#include "../lib/asn1_io_buffer.h"

#define DBG_IN(x) //debug->printf x
#define DBG_OUT(x) //debug->printf x

/*-----------------------------------------------------------------------------------------------*/
/* asn1_in_buffer                                                                                */
/*-----------------------------------------------------------------------------------------------*/

asn1_in_buffer::asn1_in_buffer(byte * data, unsigned length)
{
    this->data = data;
    this->length = length;
    bit_count = 0;
    ptr = 0;
}

bool asn1_in_buffer::bit()
{
    if (!bit_count) {
        if (ptr >= length) {
            ptr++;
            return false;
        }
        byte b = data[ptr++];
        current = b;
        bit_count = 8;
    }
    bit_count--;
    current <<= 1;
    DBG_IN(("bit %x", current & 0x100 ? 1 : 0));
    return current & 0x100 ? true : false;
    return false;
}

dword asn1_in_buffer::bit_field(int len)
{
    dword r;

    current &= 0xff;
    current >>= (8 - bit_count);
    while (bit_count < len) {
        if (ptr >= length) {
            ptr++;
            return 0;
        }
        byte b = data[ptr++];
        current <<= 8;
        current |= b;
        bit_count += 8;
    }
    bit_count -= len;
    r = current >> bit_count;
    current <<= (8 - bit_count);
    DBG_IN(("bit_field(%u) %x", len, r));
    return r;
}

byte asn1_in_buffer::octet()
{
    if (bit_count) return (byte)bit_field(8);
    else {
        if (ptr >= length) {
            ptr++;
            return 0;
        }
        byte b = data[ptr++];
        DBG_IN(("o %02x", b));
        return b;
    }
}

void asn1_in_buffer::octet_field(byte * v, int len)
{
    if (length - ptr < (unsigned)len) {
        ptr = length + 1;
        return;
    }
    for (int i = 0; i < len; i++) v[i] = data[ptr++];
    DBG_IN(("octet_field(%u)", len));
}

void asn1_in_buffer::align()
{
    bit_count = 0;
    DBG_IN(("align"));
}

int asn1_in_buffer::left()
{
    return length - ptr;
}

void asn1_in_buffer::abort()
{
    ptr = length + 1;
}

int asn1_in_buffer::get_count()
{
    return ptr;
}

void asn1_in_buffer::set_count(int count)
{
    if (ptr < (unsigned)count) {
        ptr = count;
    }
}

void asn1_in_buffer::skip(int count)
{
    ptr += count;
}

void asn1_in_buffer::save()
{
    save_buffer.ptr = ptr;
    save_buffer.bit_count = bit_count;
    save_buffer.current = current;
}

void asn1_in_buffer::restore()
{
    ptr = save_buffer.ptr;
    bit_count = save_buffer.bit_count;
    current = save_buffer.current;
}

/*-----------------------------------------------------------------------------------------------*/
/* asn1_out_buffer                                                                               */
/*-----------------------------------------------------------------------------------------------*/

asn1_out_buffer::asn1_out_buffer(unsigned malloc_size)
{
    this->malloc_size = malloc_size;
    data = (byte *)malloc(malloc_size);
    malloced = malloc_size;
    bit_count = 0;
    current = 0;
    ptr = 0;
}

void asn1_out_buffer::fix_malloc()
{
    byte * s = data;
    malloced += malloc_size;
    data = (byte *)malloc(malloced);
    memcpy(data, s, ptr);
    free(s);
}

void asn1_out_buffer::bit(bool v)
{
    DBG_OUT(("bit %x", v ? 1 : 0));
    current <<= 1;
    current |= v ? 1 : 0;
    bit_count++;
    if (bit_count == 8) {
        byte b = (byte)current;
        if (ptr == malloced) fix_malloc();
        data[ptr++] = b;
        bit_count = 0;
    }
}

void asn1_out_buffer::bit_field(dword v, int len)
{
    DBG_OUT(("bit_field(%u) %x", len, v));
    current <<= len;
    current |= v;
    bit_count += len;
    if (bit_count >= 8) {
        byte b[4];
        int l = 0;

        current <<= 32 - bit_count;
        while (bit_count >= 8) {
            b[l++] = (byte)(current >> 24);
            bit_count -= 8;
            current <<= 8;
        }
        for (int i = 0; i < l; i++) {
            if (ptr == malloced) fix_malloc();
            data[ptr++] = b[i];
        }
        current >>= 32 - bit_count;
    }
}

void asn1_out_buffer::octet(byte v)
{
    DBG_OUT(("octet %02x", v));
    if (bit_count == 0) {
        if (ptr == malloced) fix_malloc();
        data[ptr++] = v;
    }
    else {
        bit_field(v, 8);
    }
}

void asn1_out_buffer::octet_field(byte * v, int len)
{
    DBG_OUT(("octet_field(%u)", len));
    for (int i = 0; i < len; i++) {
        if (ptr == malloced) fix_malloc();
        data[ptr++] = v[i];
    }
}

void asn1_out_buffer::align()
{
    DBG_OUT(("align"));
    if (bit_count != 0) {
        current <<= 8 - bit_count;
        byte b = (byte)current;
        if (ptr == malloced) fix_malloc();
        data[ptr++] = b;
        bit_count = 0;
    }
}

void * asn1_out_buffer::new_stream(void * stream)
{
    struct asn1_out_stream * s = (struct asn1_out_stream *)malloc(sizeof(struct asn1_out_stream));
    DBG_OUT(("new %x->%x", stream, s));
    s->data = data;
    s->ptr = ptr;
    s->malloced = malloced;
    if (stream) {
        data = ((struct asn1_out_stream *)stream)->data;
        ptr = ((struct asn1_out_stream *)stream)->ptr;
        malloced = ((struct asn1_out_stream *)stream)->malloced;
        free(stream);
    }
    else {
        data = (byte *)malloc(malloc_size);
        malloced = malloc_size;
        ptr = 0;
    }
    return s;
}

void asn1_out_buffer::append(void * stream)
{
    DBG_OUT(("append %x", stream));
    if (ptr + ((struct asn1_out_stream *)stream)->ptr > malloced) {
        for (malloced += malloc_size; ptr + ((struct asn1_out_stream *)stream)->ptr > malloced; malloced += malloc_size);
        byte * s = data;
        data = (byte *)malloc(malloced);
        memcpy(data, s, ptr);
        free(s);
    }
    memcpy(&data[ptr], ((struct asn1_out_stream *)stream)->data, ((struct asn1_out_stream *)stream)->ptr);
    ptr += ((struct asn1_out_stream *)stream)->ptr;
    free(((struct asn1_out_stream *)stream)->data);
    free(stream);
}

dword asn1_out_buffer::length()
{
    return ptr;
}
