/*
 * Copyright (c) 2017 Lev Walkin <vlm@lionet.info>.
 * All rights reserved.
 * Redistribution and modifications are permitted subject to BSD license.
 */
#include <asn_system.h>
#include <asn_internal.h>

#include <oer_support.h>

/*
 * Fetch the length determinant (X.696 08/2015, #8.6) into *len_r.
 * RETURN VALUES:
 *       0:     More data expected than bufptr contains.
 *      -1:     Fatal error deciphering length.
 *      >0:     Number of bytes used from bufptr.
 */
ssize_t
oer_fetch_length(const void *bufptr, size_t size, size_t *len_r) {
    uint8_t first_byte;
    uint8_t len_len;    /* Length of the length determinant */
    const uint8_t *b;
    const uint8_t *bend;
    size_t len;

    if(size == 0) {
        *len_r = 0;
        return 0;
    }

    first_byte = *(const uint8_t *)bufptr;
    if((first_byte & 0x80) == 0) {   /* Short form */
        *len_r = first_byte; /* 0..127 */
        return 1;
    }

    len_len = (first_byte & 0x7f);
    if((1 + len_len) > size) {
        *len_r = 0;
        return 0;
    }

    b = (const uint8_t *)bufptr + 1;
    bend = b + len_len;

    for(; b < bend && *b == 0; b++) {
        /* Skip the leading 0-bytes */
    }

    if((bend - b) > (ssize_t)sizeof(size_t)) {
        /* Length is not representable by the native size_t type */
        *len_r = 0;
        return -1;
    }

    for(len = 0; b < bend; b++) {
        len = (len << 8) + *b;
    }

    if(len > RSIZE_MAX) { /* A bit of C11 validation */
        *len_r = 0;
        return -1;
    }

    *len_r = len;
    assert(len_len + 1 == bend - (const uint8_t *)bufptr);
    return len_len + 1;
}


/*
 * Serialize OER length. Returns the number of bytes serialized
 * or -1 if a given callback returned with negative result.
 */
ssize_t
oer_serialize_length(size_t length, asn_app_consume_bytes_f *cb,
                     void *app_key) {
    uint8_t scratch[1 + sizeof(length)];
    uint8_t *sp = scratch;
    int littleEndian = 1;   /* Run-time detection */
    const uint8_t *pstart;
    const uint8_t *pend;
    const uint8_t *p;
    int add;

    if(length <= 127) {
        uint8_t b = length;
        if(cb(&b, 1, app_key) < 0) {
            return -1;
        }
        return 1;
    }

    if(*(char *)&littleEndian) {
        pstart = (const uint8_t *)&length + sizeof(length) - 1;
        pend = (const uint8_t *)&length;
        add = -1;
    } else {
        pstart = (const uint8_t *)&length;
        pend = pstart + sizeof(length);
        add = 1;
    }

    for(p = pstart; p != pend; p += add) {
        /* Skip leading zeros. */
        if(*p) break;
    }

    for(sp = scratch + 1; ; p += add) {
        *sp++ = *p;
        if(p == pend) break;
    }
    assert((sp - scratch) - 1 <= 0x7f);
    scratch[0] = 0x80 + ((sp - scratch) - 1);

    if(cb(scratch, sp - scratch, app_key) < 0) {
        return -1;
    }

    return sp - scratch;
}