tlv.cpp


#include <cstdio>
#include <cstdarg>
#include <stack>
#include <functional>
#include <algorithm>
#include <cassert>
#include <tlv.hpp>

namespace LibtlvUtil
{
    std::vector<uint8_t> unhexify( std::string_view hexInput, bool throw_ex )
    {
        if( hexInput.size() % 2 != 0 )
        {
            if( throw_ex )
                throw std::invalid_argument( "Hex Decode: input string must have even number of chars!" );
            return std::vector<uint8_t>();
        }

        auto hexChar2Bin = [&](char c)
        {
           if( c >= '0' && c <= '9' )
              return static_cast<uint8_t>(c - '0');
           else if ( c >= 'A' && c <= 'F' )
              return static_cast<uint8_t>(c - 'A' + 10 );
           else if ( c >= 'a' && c <= 'f' )
              return static_cast<uint8_t>(c - 'a' + 10 );

           throw std::invalid_argument( std::string( "Hex Decode: invalid input char '" ) + c + "'" );
        };

        std::vector<uint8_t> data;
        try
        {
            data.reserve(hexInput.size()/2 );
            for( size_t i = 0; i < hexInput.size(); i+=2 )
            {
               uint8_t byte  = hexChar2Bin( hexInput.at( i ) ) << 4;
                       byte += hexChar2Bin( hexInput.at( i+1 ) );
               data.push_back( byte );
            }
        }
        catch( std::exception &e )
        {
            data.clear();
            if( throw_ex )
                throw; // rethrow
        }

        return data;
    }

    std::string hexify( const std::vector<uint8_t> &data, bool lower_case )
    {
        const char* characters = lower_case ? "0123456789abcdef" : "0123456789ABCDEF";
        std::string hexString;
        hexString.reserve( data.size()*2 );

        for ( const auto &byte : data )
        {
          hexString += characters[byte >> 4];
          hexString += characters[byte & 0x0F];
        }

        return hexString;
    }

    // most significant bit
    static uint32_t msb( uint32_t value )
    {
        return value >> ( 8 * ( sizeof(value) - __builtin_clz( value ) / 8 - 1 ) );
    }
}

using namespace LibtlvUtil;

/*
 * TlvTag
 */

Tlv::Tag Tlv::Tag::build( Tlv::Tag::Class cls, bool constructed, uint32_t tag_number )
{
    // See X.690 BER tar encoding
    static const uint8_t next_byte = 0x80;
    static const uint8_t first_byte_bits = 0x1F;
    static const uint8_t paytload_bits = 0x7F;
    static const uint8_t constructed_tag = 0x20;
    static const uint8_t primitive_tag = 0x00;
    Tlv::Tag ret; // invalid tag
    if( tag_number <= max_tag_number ) // must fit in 4 bytes
    {
        if ( tag_number < 31 )
        {
            // Short form
            ret._value = (uint32_t)cls | ( constructed ? constructed_tag : primitive_tag ) | tag_number;
        } else {
            // Long form
            // First byte
            ret._value = (uint32_t)cls | ( constructed ? constructed_tag : primitive_tag ) | first_byte_bits;
            // Next bytes
            size_t effective_bits = sizeof( tag_number ) * 8 - __builtin_clz( tag_number );
            for( int i = ( effective_bits / 7 ) + ( ( effective_bits % 7 ) ? 1 : 0 ); i > 0; i-- )
            {
                ret._value <<= 8;
                ret._value |= next_byte | ( ( tag_number >> ( 7 * ( i - 1 ) ) ) & paytload_bits );
            }
            ret._value ^= next_byte; // Unset last byte "next byte" indicator
        }
    }
    return ret;
}

Tlv::Tag Tlv::Tag::build( UniversalTagType type, bool constructed )
{
    return build( Class::Universal, constructed, (uint32_t)type );
}

Tlv::Tag::Tag( const char* null_terminated_tag ) :
    _value( 0 )
{
    for( unsigned i = 0; null_terminated_tag[i] != 0; ++i )
    {
        if( i >= sizeof(uint32_t) )
        {
            _value = 0;
            return;
        }
        const uint8_t byte = static_cast<uint8_t>(null_terminated_tag[i]);
        _value <<= 8;
        _value |= byte;
    }
}

size_t Tlv::Tag::size() const
{
    if ( _value == empty_tag_value )
    {
        return 0;
    }
    else
    {
        return 4 - __builtin_clz( _value ) / 8;
    }
}

Tlv::Tag::Class Tlv::Tag::tag_class() const
{
    return (Class)( msb( _value ) & 0xC0 );
}

bool Tlv::Tag::constructed() const
{
    return empty() || msb( _value ) & 0x20;
}

uint32_t Tlv::Tag::tag_number() const
{
    int i = ( sizeof(_value) - __builtin_clz( _value ) / 8 );
    uint32_t tag = ( i > 1 ) ? 0 : ( _value & 0x1F );
    for( i--; i > 0; i-- )
    {
        tag <<= 7;
        tag |= ( 0x7F & ( _value >> ( ( i - 1 ) * 8 ) ) );
    }
    return tag;
}

std::string Tlv::Tag::to_hex_string() const
{
    const char* characters = "0123456789ABCDEF";
    int most_significant_byte = ( sizeof( _value ) - __builtin_clz( _value ) / 8 ) - 1;
    std::string hex_string;
    hex_string.reserve( (most_significant_byte + 1) * 2 );

    for( int i = most_significant_byte; i >= 0; i-- )
    {
        uint8_t byte = ( _value >> ( i * 8 ) ) & 0xFF;
        hex_string += characters[byte >> 4];
        hex_string += characters[byte & 0x0F];
    }

    return hex_string;
}

/*
 * Tlv::Data
 */
struct Tlv::Data
{
    Tag tag;
    Data* parent;
    // Leaf
    Value value;
    // Branch
    ChildContainer children;

    Data() :
        parent( nullptr )
    {}

    Data( const Data &rhs ) = delete;
    Data operator=( const Data &rhs ) = delete;

    ~Data()
    {
        // make sure that parent ptr of children is unset, when parent is destroyed
        for( auto& child : children )
        {
            child.data_->parent = nullptr;
        }
    }
    bool operator==( const Data &rhs ) const
    {
        return tag == rhs.tag && parent == rhs.parent && value == rhs.value && children == rhs.children;
    }
    bool operator!=( const Data &rhs ) const
    {
        return !operator==( rhs );
    }
    bool empty()
    {
        return tag.empty() && value.empty() && children.empty();
    }
};

/*
 * Error
 */
Tlv::Status::Status( const Code code, const size_t length ) :
    code_( code ),
    length_( length )
{}

Tlv::Status::Status( Code code, const size_t length, const char *fmt, ... ) :
    code_( code ),
    length_( length )
{
    va_list vl, vl2;
    va_start( vl, fmt );
    va_copy( vl2, vl );
    unsigned len = vsnprintf( nullptr, 0, fmt, vl ) + 1;
    va_end( vl );
    if ( len > 0 )
    {
        message_.resize( len );
        vsnprintf( (char*)message_.c_str(), len, fmt, vl2 );
    }
    va_end( vl2 );
}

/*
 * Parser
 */

class Tlv::Parser
{
    static constexpr const uint8_t multi_octet_tag_mask_ = 0x1F;
    static constexpr const uint8_t more_octet_mask_ = 0x80;

    const uint8_t* _pos;
    const uint8_t* _end;
    const uint8_t* _tree_start;

    inline bool next_byte( uint8_t &byte )
    {
        if ( _pos < _end )
        {
            byte = *_pos;
            _pos++;
            return true;
        } else {
            return false;
        }
    }

    void skip_zero_bytes()
    {
        while( _pos < _end && *_pos == 0 ) _pos++;
    }

public:

    struct ShallowNode  // for detected nodes during parser run
    {
        Tlv::Tag tag;
        const uint8_t* begin;
        const uint8_t* end;

        ShallowNode() :
            tag(),
            begin( nullptr ),
            end( nullptr )
        {}
    };

    Parser( const uint8_t* begin, const uint8_t* end, const uint8_t* tree_start ) :
        _pos( begin ),
        _end( end ),
        _tree_start( tree_start )
    {}

    bool has_next_tag()
    {
        skip_zero_bytes();
        return _pos < _end;
    }

    size_t get_offset() const
    {
        return _pos - _tree_start;
    }

    Tlv::Status next( ShallowNode &node )
    {
        uint32_t tag = 0;
        uint32_t length = 0;
        uint8_t  byte;

        /* Step 1: Read Tag */

        // Read tag first byte
        if( !next_byte( byte ) )
            return Tlv::Status( Tlv::Status::BadArgument, get_offset(),
                "Unexpected error: no more tag at offset %d", static_cast<int>( get_offset() ) );

        tag = byte;

        // Read tag other bytes
        if( ( byte & multi_octet_tag_mask_ ) == multi_octet_tag_mask_ )
        {
            bool hasNext = true;
            for(size_t i = 1; i < sizeof(uint32_t) && hasNext; i++ )
            {
                if( !next_byte(byte) )
                    return Tlv::Status( Tlv::Status::UnexpectedEnd, get_offset(),
                        "Unexpected end of input while reading tag '%X' at offset %d", tag, static_cast<int>( get_offset() ) );

                tag = ( tag << 8 ) + byte;
                hasNext = (byte & more_octet_mask_);
            }

            if( hasNext )
            {
                return Tlv::Status( Tlv::Status::BadLength, get_offset(),
                    "Tag too long while reading tag '%X' at offset %d", tag, static_cast<int>( get_offset() ) );
            }
        }

        node.tag = tag;

        /* Step 2: Read Length */

        // Read tag length first byte
        if( !next_byte( byte ) )
            return Tlv::Status( Tlv::Status::UnexpectedEnd, get_offset(),
                "Unexpected end of input while reading length of tag '%X' at offset %d", tag, static_cast<int>( get_offset() ) );

        // Reag tag length other bytes
        if( byte & more_octet_mask_ )
        {
            size_t num_bytes = byte ^ more_octet_mask_;
            if( num_bytes > sizeof( length ) )
                return Tlv::Status( Tlv::Status::BadLength, get_offset(),
                    "Tag length of tag '%X' too large at offset %d", tag, static_cast<int>( get_offset() ) );

            for(size_t i = 0; i < num_bytes; i++ )
            {
                if( !next_byte( byte ) )
                    return Tlv::Status( Tlv::Status::UnexpectedEnd, get_offset(),
                        "Unexpected end of input while reading length of tag '%X' at offset %d", tag, static_cast<int>( get_offset() ) );

                length = ( length << 8 ) + byte;
            }
        }
        else
        {
            length = byte;
        }

        // Verify data bounds, advance parser _pos
        node.begin = _pos;
        const uint8_t* valueEnd = _pos + length;

        if( valueEnd > _end )
        {
            node.end = _end;
            _pos = _end;
            return Tlv::Status( Tlv::Status::UnexpectedEnd, get_offset(),
                "Unexpected end of input while reading data of tag '%X' at offset %d", tag, static_cast<int>( get_offset() ) );
        } else {
            node.end = valueEnd;
            _pos = valueEnd;
            return Tlv::Status( Tlv::Status::OK, get_offset() ); // Status ok
        }
    }
};

/*
 * FormattedParser
 */

class Tlv::FormattedParser
{
    std::string_view data;
    size_t pos;

    size_t curLine;
    size_t curLineStartPos;

    static constexpr const uint8_t multi_octet_tag_mask_ = 0x1F;
    static constexpr const uint8_t more_octet_mask_ = 0x80;

private:

    void read_spaces()
    {
        while(pos < data.size() && data.at(pos) == ' ')
        {
            pos++;
        }
    }

    int read_indet()
    {
        // indent defines tag hierarch (just whitespaces allowed)
        auto old_pos = pos;
        read_spaces();
        return pos - old_pos;
    }

    std::string_view read_hex( Tlv::Status &status )
    {
        // hex string for either tag or data, must be terminated by either whitespace, newline, or EOF
        auto is_hex_char = []( char c )
        {
            return ((c >= '0') && (c <= '9')) ||
                   ((c >= 'A') && (c <= 'F')) ||
                   ((c >= 'a') && (c <= 'f'));
        };

        size_t begin = pos;

        // read all hex chars
        while( pos < data.size() && is_hex_char(data.at(pos)) )
        {
            pos++;
        }
        size_t end = pos;

        // check if hex string is terminated correctly with whitespace, newline for enf of input (if exists at all)
        if( (end-begin) > 0 && end < data.size() )
        {
            if( data.at(pos) != ' ' && data.at(pos) != '\n' )
            {
                status = Tlv::Status( Tlv::Status::UnexpectedData, pos, "Unexpected char '%c' at line %u pos %u while parsing hex data",
                                      data.at(pos), (unsigned)curLine, (unsigned)get_cur_line_pos() );
                return std::string_view();
            }
        }

        // check size of hex string
        if( (end-begin) % 2 != 0 )
        {
            status = Tlv::Status( Tlv::Status::UnexpectedData, pos, "Unexpected size of hex encoded data at line %u pos %u",
                                  (unsigned)curLine, (unsigned)get_cur_line_pos() );
            return std::string_view();
        }

        return std::string_view( &data.at(begin), end-begin );
    }

    Tlv::Tag read_tag( Tlv::Status &status )
    {
        auto tagHexStr = read_hex( status );
        if( !status.ok() )
        {
            return Tlv::Tag();
        }
        else if( tagHexStr.empty() )
        {
            status = Tlv::Status( Tlv::Status::UnexpectedData, pos, "Unexpected end of tag number at line %u pos %u: expected hex encoded tag",
                                  (unsigned)curLine, (unsigned)get_cur_line_pos() );
            return Tlv::Tag();
        }

        // Hex 2 Bin
        auto tagBinVec = LibtlvUtil::unhexify( tagHexStr );

        // Read tag fist byte
        uint32_t tag = tagBinVec[0];
        // Read tag other bytes
        if( ( tagBinVec[0] & multi_octet_tag_mask_ ) == multi_octet_tag_mask_ )
        {
            bool hasNext = true;
            size_t i = 1;
            for(; i < sizeof(uint32_t) && hasNext; i++ )
            {
                if( i == tagBinVec.size() )
                {
                    status = Tlv::Status( Tlv::Status::BadTag, pos, "Unexpected end of tag number at line %u pos %u",
                                          (unsigned)curLine, (unsigned)get_cur_line_pos() );
                    return Tlv::Tag();
                }

                tag = ( tag << 8 ) + tagBinVec[i];
                hasNext = (tagBinVec[i] & more_octet_mask_);
            }

            if( hasNext )
            {
                status = Tlv::Status( Tlv::Status::BadTag, pos, "Tag too long while reading tag number '%X' at line %u pos %u",
                                      tag, (unsigned)curLine, (unsigned)get_cur_line_pos() );
                return Tlv::Tag();
            }

            if( i != tagBinVec.size())
            {
                status = Tlv::Status( Tlv::Status::BadTag, pos, "Unexpected trailing data while reading tag number at line %u",
                                      (unsigned)curLine );
            }
        }

        return Tlv::Tag( tag );
    }

    // optional hex data, emtpy string if not present
    std::string_view read_tag_data( Tlv::Status& status )
    {
        // read spaces before tag data
        read_spaces();
        return read_hex( status );
    }

    // optional comment, empty string if not present, no errors
    std::string_view read_tag_comment( Tlv::Status & )
    {
        // read spaces before comment
        read_spaces();

        // starts with "//"
        if( pos < (data.size() - 1) && data.at(pos) == '/' && data.at(pos+1) == '/' )
        {
            pos+=2;

            // read until newline or end of input
            size_t begin = pos;
            while( pos < data.size() && data.at(pos) != '\n' )
            {
                pos++;
            }
            size_t end = pos;
            return std::string_view( &data.at(begin), end-begin );
        }

        return std::string_view();
    };

    // end of tag andvances the line, ensures all other data was read
    void read_end_of_tag( Tlv::Status& status )
    {
        if( pos < data.size() && data.at( pos ) == '\n' )
        {
            pos++;
            curLine++;
            curLineStartPos = pos;
        }
        else if ( pos < data.size() )
        {
            status = Tlv::Status( Tlv::Status::UnexpectedData, pos, "Unexpected char '%c' at line %u pos %u",
                                  data.at(pos), (unsigned)curLine, (unsigned)get_cur_line_pos() );
        }
    }

public:

    struct ShallowNode
    {
        int line;
        int indent;
        Tlv::Tag tag;
        std::string_view hexData;
        std::string_view comment;
    };

    FormattedParser( std::string_view data ) :
        data( data ),
        pos( 0 ),
        curLine( 1 ),
        curLineStartPos( 0 )
    {}

    bool has_next_tag() { return data.size() > pos; }

    size_t get_cur_pos() { return pos; }
    size_t get_cur_line() { return curLine; }
    size_t get_cur_line_pos() { return pos - curLineStartPos + 1; }

    Tlv::Status next( ShallowNode& node )
    {
        Tlv::Status status;

        node.line = curLine;
        node.indent = read_indet();

        // Get Tag
        node.tag = read_tag( status );
        if( !status.ok() )
        {
            return status;
        }

        // Get Tag data (optional)
        node.hexData = read_tag_data( status );
        if( !status.ok() )
        {
            return status;
        }

        // Get Tag comment (optional)
        node.comment = read_tag_comment( status );
        if( !status.ok() )
        {
            return status;
        }

        // Read enf of line, ensure no junk data is left
        read_end_of_tag( status );
        return status;
    }


};

/**
 * Tlv
 */

Tlv::Tlv() :
    data_( std::make_shared<Data>() )
{}

Tlv::Tlv( const Tag tag ) :
    Tlv()
{
    data_->tag = tag;
}

Tlv::Tlv( const Tag tag, const Value &data ) :
    Tlv( tag )
{
    data_->value = data;
}

Tlv::Tlv( const Tag tag, const Value &&data ) :
    Tlv( tag )
{
    data_->value = std::move( data );
}

Tlv::Tlv( const Tag tag, const uint8_t *data, size_t size ) :
    Tlv( tag )
{
    data_->value = Value( data, data + size );
}

Tlv::Tlv( const Tag tag, const char *s ) :
    Tlv( tag, std::string( s ) )
{}

Tlv::Tlv( const Tag tag, const std::string &s ) :
    Tlv( tag )
{
    data_->value = Value( s.data(), s.data() + s.size() );
}

template<typename T>
void build_int_value_raw( Tlv::Value& buf, const T val )
{
    /*
     * Write as unsigned integer with MSB byte order,
     * without leading 0x00-bytes (not BER-TLV signed integer)
     */

    using U = std::make_unsigned_t<T>;
    const U uval = static_cast<U>(val);

    auto getByte = [&](int n) -> uint8_t
    {
        return static_cast<uint8_t>( uval >> ( n * 8 ) );
    };

    if ( val == 0 )
    {
        buf.push_back( 0x00 );
    }
    else
    {
        int i = sizeof( T ) - 1;

        // skip leading 0x00-bytes
        for( ; i >= 0 && getByte( i ) == 0x00; i-- );
        // store other bytes
        for( ; i >= 0; i-- )
        {
            buf.push_back( getByte( i ) );
        }
    }
}

template<typename T>
T read_int_value_raw( const Tlv::Value& buf )
{
    /*
     * Read as unsigned integer with MSB byte order (not BER-TLV signed integer)
     *
     * If buffer is bigger than the target value it will only read
     * the tail bytes in MSB order (overflow).
     */

    T val = 0;

    // start with tail bytes if target value would overflow (MSB order)
    size_t i = buf.size() > sizeof( T ) ? buf.size() - sizeof( T ) : 0;
    for( ; i < buf.size(); i++ )
    {
        if constexpr(sizeof(T) > 1)
        {
            val <<= 8;
        }
        val |= buf.at( i );
    }

    return val;
}

Tlv::Tlv( const Tag tag, bool b ) :
    Tlv( tag )
{
    data_->value.push_back( (uint8_t)b );
}

Tlv::Tlv( const Tag tag, int8_t i ) :
    Tlv( tag )
{
    data_->value.push_back( i );
}

Tlv::Tlv( const Tag tag, int16_t i ) :
    Tlv( tag )
{
    build_int_value_raw<int16_t>( data_->value, i );
}

Tlv::Tlv( const Tag tag, int32_t i ) :
    Tlv( tag )
{
    build_int_value_raw<int32_t>( data_->value, i );
}

Tlv::Tlv( const Tag tag, int64_t i ) :
    Tlv( tag )
{
    build_int_value_raw<int64_t>( data_->value, i );
}

Tlv::Tlv( const Tag tag, uint8_t i ) :
    Tlv( tag )
{
    data_->value.push_back( i );
}

Tlv::Tlv( const Tag tag, uint16_t i ) :
    Tlv( tag )
{
    build_int_value_raw<int16_t>( data_->value, i );
}

Tlv::Tlv( const Tag tag, uint32_t i ) :
    Tlv( tag )
{
    build_int_value_raw<int32_t>( data_->value, i );
}

Tlv::Tlv( const Tag tag, uint64_t i ) :
    Tlv( tag )
{
    build_int_value_raw<int64_t>( data_->value, i );
}

Tlv::Tlv( const Tag tag, Tlv &child ) :
    Tlv( tag )
{
    push_back( child );
}

Tlv::Tlv(const Tag tag, Tlv&& child ) :
    Tlv( tag )
{
    push_back( std::move(child) );
}

Tlv::Tlv( const Tlv &rhs ) :
    data_( rhs.data_ )
{}

Tlv::Tlv( Tlv &&rhs ) :
    data_( std::move(rhs.data_) )
{}

Tlv::Tlv( const std::shared_ptr<Data> &data ) :
    data_( data )
{}

Tlv::Tlv( std::shared_ptr<Data> &&data ) :
    data_( std::move(data) )
{}

Tlv::~Tlv()
{}

Tlv& Tlv::operator=( const Tlv &rhs )
{
    data_ = rhs.data_;
    return *this;
}

Tlv& Tlv::operator=( Tlv &&rhs )
{
    data_ = std::move( rhs.data_ );
    return *this;
}

bool Tlv::operator==( const Tlv &rhs ) const
{
    return *data_ == *rhs.data_;
}

bool Tlv::operator!=( const Tlv &rhs ) const
{
    return *data_ != *rhs.data_;
}

bool Tlv::identical(const Tlv& other) const
{
    return data_ == other.data_;
}

Tlv::operator bool() const
{
    return !empty();
}

Tlv Tlv::parse( const uint8_t *data, const size_t size, Status &s, int depth )
{
    Tlv tlv;
    s = tlv.parse( data, size, depth );
    return tlv;
}

Tlv Tlv::parse_all( const uint8_t *data, const size_t size, Status &s, int depth )
{
    Tlv root;
    s = root.parse_all( data, size, depth );
    return root;
}

Tlv::Status Tlv::parse( const uint8_t *data, const size_t size, int depth )
{
    reset();
    return _parse_one( *this, data, data + size, data, depth );
}

Tlv::Status Tlv::parse_all(const uint8_t* data, const size_t size, int depth )
{
    reset();
    return _parse( *this, data, data + size, data, depth );
}

Tlv Tlv::parse_formatted(const uint8_t *data, const size_t size, Status &s)
{
    Tlv root;
    s = root.parse_formatted( data, size );
    return root;
}

Tlv Tlv::parse_formatted(std::string_view data, Status &s)
{
    Tlv root;
    s = root.parse_formatted( data );
    return root;
}

Tlv::Status Tlv::parse_formatted(const uint8_t *data, const size_t size)
{
    reset();
    std::string_view formattedStr( reinterpret_cast<const char*>(data), size );
    return _parse_formatted( *this, formattedStr );
}

Tlv::Status Tlv::parse_formatted( std::string_view data )
{
    reset();
    return _parse_formatted( *this, data );
}

Tlv::Status Tlv::expand( int depth )
{
    Status s;
    if( data_->value.size() > 0 )
    {
        s = _parse( *this, data_->value.data(), data_->value.data() + data_->value.size() , &data_->value.front(), depth );

        if( s )
        {
            data_->value.clear();
        }
        else
        {
            data_->children.clear();
        }

    } else {
        s = Status( Status::BadArgument, 0, "Node has no value" );
    }

    return s;
}

std::vector<uint8_t> Tlv::dump() const
{
    struct BuildStackFrame
    {
        const Tlv* node;
        int nodeNumber;
        size_t parentOffset;
    };

    struct BuildElement
    {
        const Tlv* node;
        size_t size;
    };

    auto len_field_size = [](size_t len)
    {
        if( len <= 127 )
            return 1;
        else
            return 4 - __builtin_clz( len ) / 8 + 1;
    };

    int nodeNumber = 0;
    std::vector<BuildStackFrame>	buildStack;
    std::vector<BuildElement>		buildElements;
    buildStack.push_back( { this, -1, 0 } ); // start with root

    while( !buildStack.empty() )
    {
        auto &currentElement = buildStack.back();

        // unexplored node
        if( currentElement.nodeNumber == -1 )
        {
            // assign node number (dfs order)
            currentElement.nodeNumber = nodeNumber++;
            buildElements.push_back( { currentElement.node, 0 } );

            // leaf node: set payload size
            if( currentElement.node->data_->children.empty() )
            {
                buildElements[currentElement.nodeNumber].size += currentElement.node->data_->value.size();
            }
            // branch node: add children to stack
            else
            {
                auto &children = currentElement.node->data_->children;
                size_t off = 0;
                for( int r = children.size() -1; r >= 0; --r )
                {
                    buildStack.push_back({ &children[r], -1, ++off });
                }
            }
        }
        // already explored node, size is known, propagate to parent nodes
        else
        {
            if( currentElement.parentOffset > 0 )
            {
                // add own size to parent size + size for tag + len field
                auto &parentNodeNumber = buildStack[buildStack.size() - 1 - currentElement.parentOffset].nodeNumber;

                // this node only has a positive node size and len field if, the tag is not emtpy
                if( !currentElement.node->tag().empty() )
                {
                    buildElements[parentNodeNumber].size
                            += currentElement.node->tag().size()							   // tag size
                            + len_field_size( buildElements[currentElement.nodeNumber].size ); // len field size
                }
                buildElements[parentNodeNumber].size += buildElements[currentElement.nodeNumber].size; // payload size
            }
            buildStack.pop_back();
        }
    }

    size_t total_size = data_->tag.size() + len_field_size( buildElements[0].size ) + buildElements[0].size;

    std::vector<uint8_t> output;
    output.reserve(total_size);

    static auto build_tag = []( std::vector<uint8_t> &out, const Data &element, size_t size ) {
        if( !element.tag.empty() )
        {
            // Build tag
            for( int i = ( sizeof( element.tag._value ) - __builtin_clz( element.tag._value ) / 8 ) - 1; i >= 0; i-- )
            {
                out.push_back( ( element.tag._value >> ( i * 8 ) ) & 0xFF );
            }
            // Build length
            size_t len = size;
            if ( len <= 127 )
            {
                // Definite short form
                out.push_back( len & 0x7F );
            } else {
                // Definite long form
                int len_bytes = 4 - __builtin_clz( len ) / 8;
                out.push_back( (uint8_t)( 0x80 | len_bytes ) );
                for( int i = len_bytes - 1; i >= 0; i-- )
                {
                    out.push_back( ( len >> ( i * 8 ) ) & 0xFF );
                }
            }
        }
        // Append data
        out.insert( out.end(), element.value.begin(), element.value.end() );
    };


    for( auto &el : buildElements )
    {
        build_tag( output, *el.node->data_, el.size );
    }

    return output;
}

std::string Tlv::dump_formatted() const
{
    std::string output;

    auto is_printable_char = []( uint8_t byte )
    {
        // space ' ' 0x20 to tilde '~' 0x7E
        return byte >= 0x20 && byte <= 0x7E;
    };

    // skip encoding of root node, if it's just a container for child nodes
    bool skip_root = !has_tag();

    // encoding of one line, in dfs order
    auto append_node = [&]( const Tlv& tlv, int depth )
    {
        if( depth == 0 && skip_root )
            return Continue;

        output.append( (depth - skip_root) *4, ' ' );
        output.append( tlv.data_->tag.to_hex_string() );
        if( tlv.data_->value.size() > 0 )
        {
            output.append( " " );
            output.append( hexify( tlv.data_->value ) );

            // add ascii representation as comment, if printable
            if( std::all_of( tlv.data_->value.begin(), tlv.data_->value.end(), is_printable_char ) )
            {
                output.append( " // \"" );
                output.append( tlv.string() );
                output.append( "\"");
            }
        }
        output.append( "\n" );

        return TraversalAction::Continue;
    };

    _dfs_unsafe_depth( append_node );
    return output;
}

// Capacity

bool Tlv::empty() const
{
    return !data_ || data_->empty();
}

bool Tlv::has_tag() const
{
    return !data_->tag.empty();
}

bool Tlv::has_value() const
{
    return !data_->value.empty();
}

size_t Tlv::value_size() const
{
    return data_->value.size();
}

bool Tlv::has_parent() const
{
    return data_->parent != nullptr;
}

bool Tlv::has_children() const
{
    return !data_->children.empty();
}

size_t Tlv::num_children() const
{
    return data_->children.size();
}

size_t Tlv::tree_size() const
{
    size_t size = 0;
    _dfs_unsafe( [&]( const Tlv& ){ ++size; return Continue; } );
    return size;
}

Tlv::ChildIterator Tlv::begin()
{
    return data_->children.begin();
}

Tlv::ChildIterator Tlv::end()
{
    return data_->children.begin();
}

// Element access

Tlv::Tag Tlv::tag() const
{
    return data_->tag;
}

const Tlv::Value& Tlv::value() const
{
    return data_->value;
}

Tlv::Value& Tlv::value()
{
    return data_->value;
}

std::string Tlv::string() const
{
    return std::string( (const char*)data_->value.data(), data_->value.size() );
}

bool Tlv::boolean() const
{
    return std::any_of( data_->value.begin(), data_->value.end(), [](uint8_t byte) {
        return byte != 0x00;
    } );
}

int8_t Tlv::int8() const
{
    return read_int_value_raw<int8_t>( data_->value );
}

int16_t Tlv::int16() const
{
    return read_int_value_raw<int16_t>( data_->value );
}

int32_t Tlv::int32() const
{
    return read_int_value_raw<int32_t>( data_->value );
}

int64_t Tlv::int64() const
{
    return read_int_value_raw<int64_t>( data_->value );
}

uint8_t Tlv::uint8() const
{
    return read_int_value_raw<uint8_t>( data_->value );
}

uint16_t Tlv::uint16() const
{
    return read_int_value_raw<uint16_t>( data_->value );
}

uint32_t Tlv::uint32() const
{
    return read_int_value_raw<uint32_t>( data_->value );
}

uint64_t Tlv::uint64() const
{
    return read_int_value_raw<uint64_t>( data_->value );
}

const Tlv::ChildContainer& Tlv::children() const
{
    return data_->children;
}

Tlv Tlv::front() const
{
    return ( data_ && !data_->children.empty() ) ? Tlv( data_->children.front() ) : Tlv();
}

Tlv Tlv::back() const
{
    return ( data_ && !data_->children.empty() ) ? Tlv( data_->children.back() ) : Tlv();
}

void Tlv::set_value( const Value& value )
{
    data_->value = value;
    data_->children.clear();
}
void Tlv::set_value( Value&& value )
{
    data_->value = std::move(value);
    data_->children.clear();
}

void Tlv::set_tag( const Tag& tag )
{
    data_->tag = tag;
}

void Tlv::dfs( std::function<TraversalAction(Tlv&)> callback ) const
{
    if ( !callback )
    {
        return;
    }

    std::vector<Tlv> stack;
    stack.reserve( 4 );	 // start with a reasonable default size
    stack.push_back( *this );

    while( !stack.empty() )
    {
        Tlv currentNode( std::move( stack.back() ) );
        stack.pop_back();

        // visit node
        auto ret = callback( currentNode );
        switch( ret )
        {
            case Break: return;		// stop here
            case Prune: continue;   // continue, but skip subtree of current node
            case Continue: ;		// continue traversal
        }
        stack.insert( stack.end(), currentNode.data_->children.rbegin(), currentNode.data_->children.rend() );
    }
}

void Tlv::dfs( std::function<TraversalAction(Tlv&, int depth)> callback ) const
{
    if ( !callback )
    {
        return;
    }

    // track depth of node on stack
    std::vector<std::pair<Tlv, int>> stack;
    stack.reserve( 4 );	 // start with a reasonable default size
    stack.emplace_back(std::make_pair(*this, 0));

    while( !stack.empty() )
    {
        auto nodeElement( std::move( stack.back() ) );
        stack.pop_back();

        // visit node
        auto ret = callback( nodeElement.first, nodeElement.second );
        switch( ret )
        {
            case Break: return;		// stop here
            case Prune: continue;   // continue, but skip subtree of current node
            case Continue: ;		// continue traversal
        }

        auto& children = nodeElement.first.data_->children;
        for( auto it = children.rbegin(); it != children.rend(); ++it )
        {
            stack.emplace_back(*it, nodeElement.second +1 );
        }
    }
}

void Tlv::bfs( std::function<TraversalAction(Tlv&)> callback ) const
{
    if ( !callback )
    {
        return;
    }

    std::deque<Tlv> queue;
    queue.push_back( *this );

    while( !queue.empty() )
    {
        Tlv currentNode( std::move( queue.front() ) );
        queue.pop_front();

        // visit node
        auto ret = callback( currentNode );
        switch( ret )
        {
            case Break: return;		// stop here
            case Prune: continue;   // continue, but skip subtree of current node
            case Continue: ;		// continue traversal
        }

        // add children of current node to queue
        queue.insert( queue.end(), currentNode.data_->children.begin(), currentNode.data_->children.end() );
    }
}

void Tlv::bfs( std::function<TraversalAction(Tlv&, int depth)> callback ) const
{
    if ( !callback )
    {
        return;
    }

    // track depth of node in queue
    std::deque<std::pair<Tlv, int>> queue;
    queue.emplace_back(std::make_pair(*this, 0));

    while( !queue.empty() )
    {
        auto nodeElement( std::move( queue.back() ) );
        queue.pop_back();

        // visit node
        auto ret = callback( nodeElement.first, nodeElement.second );
        switch( ret )
        {
            case Break: return;		// stop here
            case Prune: continue;   // continue, but skip subtree of current node
            case Continue: ;		// continue traversal
        }

        auto& children = nodeElement.first.data_->children;
        for( auto it = children.rbegin(); it != children.rend(); ++it )
        {
            queue.emplace_back(*it, nodeElement.second +1 );
        }
    }
}

Tlv Tlv::find( const Tag tag, int maxDepth ) const
{
    // just iterate to find direct childen
    if( maxDepth == DirectChildren )
    {
        for( auto& child : data_->children )
        {
            if( child.tag() == tag )
            {
                return child;
            }
        }

        return Tlv();
    }
    // dfs search to find first deep match
    else
    {
        Tlv tlv;
        auto find_tag = [&]( const Tlv& child, int depth )
        {
            if( child.tag() == tag )
            {
                tlv = child;
                return TraversalAction::Break;
            }
            else if( depth == maxDepth )
            {
                return TraversalAction::Prune;
            }
            else
            {
                return TraversalAction::Continue;
            }
        };

        _dfs_unsafe_depth( find_tag );
        return tlv;
    }
}

std::vector<Tlv> Tlv::find_all( const Tag tag, int maxDepth, bool findNested ) const
{
    std::vector<Tlv> matches;

    // just iterate to find direct childen
    if( maxDepth == DirectChildren )
    {
        for( auto& child : data_->children )
        {
            if( child.tag() == tag )
            {
                matches.push_back( child );
            }
        }
    }
    // dfs search to find deep matches
    else
    {
        auto find_tag = [&]( const Tlv& child, int depth )
        {
            bool match = child.tag() == tag;
            if( match )
            {
                matches.push_back( child );
            }

            if( ( match && !findNested ) || depth == maxDepth )
            {
                return TraversalAction::Prune;
            }
            else
            {
                return TraversalAction::Continue;
            }
        };

        _dfs_unsafe_depth( find_tag );
    }
    return matches;
}

size_t Tlv::remove( const Tag tag )
{
    size_t num = 0;
    for( auto it = data_->children.begin(); it != data_->children.end(); ++it )
    {
        if ( it->tag() == tag )
        {
            it->data_->parent = nullptr;
            it = it->data_->children.erase( it );
            num++;
        }
    }
    return num;
}

// Modifiers

void Tlv::set_parent( Tlv &parent )
{
    parent.push_back( *this );
}

void Tlv::push_front( Tlv &child )
{
    data_->value.clear();
    child.detach();
    child.data_->parent = data_.get();
    data_->children.insert( data_->children.begin(), child );
}

void Tlv::push_front( Tlv&& child )
{
    data_->value.clear();
    child.detach();
    child.data_->parent = data_.get();
    data_->children.insert( data_->children.begin(), std::move( child ) );
}

void Tlv::push_back( Tlv& child )
{
    data_->value.clear();
    child.detach();
    child.data_->parent = data_.get();
    data_->children.push_back( child );
}

void Tlv::push_back( Tlv&& child )
{
    data_->value.clear();
    child.detach();
    child.data_->parent = data_.get();
    data_->children.push_back( std::move( child) );
}

void Tlv::pop_front()
{
    if ( !data_->children.empty() )
    {
        assert( data_->children.front().data_->parent == data_.get() );
        // unset parent and remove from children
        data_->children.front().data_->parent = nullptr;
        data_->children.erase( data_->children.begin() );
    }
}

void Tlv::pop_back()
{
    if ( !data_->children.empty() )
    {
        assert( data_->children.back().data_->parent == data_.get() );
        // unset parent and remove from children
        data_->children.back().data_->parent = nullptr;
        data_->children.pop_back();
    }
}

void Tlv::detach()
{
    if ( data_->parent )
    {
        for( auto it = data_->parent->children.begin(); it != data_->parent->children.end(); ++it )
        {
            if ( it->data_.get() == data_.get() )
            {
                data_->parent->children.erase( it );
                data_->parent = nullptr;
                return;
            }
        }
        /* Unreachable, because if this node has a parent, it must be child of it's parent. */
        assert( false );
    }
}

void Tlv::erase( const Tag tag )
{
    for( auto it = data_->children.begin(); it != data_->children.end(); ++it )
    {
        if ( it->data_->tag == tag )
        {
            it = it->data_->children.erase( it );
        }
    }
}

void Tlv::swap( Tlv &other )
{
    std::shared_ptr<Data> tmp = data_;
    data_ = other.data_;
    other.data_ = tmp;
}

void Tlv::reset()
{
    data_ = std::make_shared<Data>();
}

template< typename T >
inline void Tlv::_dfs_unsafe( T callback ) const
{
    std::vector<const Tlv*> stack;
    stack.reserve( 4 );				// start with a reasonable default size
    stack.push_back( this );

    while( !stack.empty() )
    {
        const Tlv* currentNode = stack.back();
        stack.pop_back();

        auto ret = callback( *currentNode );

        switch( ret )
        {
            case Break: return;		// stop here
            case Prune: continue;   // continue, but skip subtree of current node
            case Continue: ;		// continue traversal
        }

        // add children of current node to stack - first child must be on top of stack
        auto &children = currentNode->data_->children;
        for( int r = children.size() - 1; r >= 0; r-- )
        {
            stack.push_back(&children[r]);
        }
    }
}

template< typename T >
inline void Tlv::_dfs_unsafe_depth( T callback ) const
{
    std::vector<std::pair<const Tlv*, int>> stack;
    stack.reserve( 4 );				// start with a reasonable default size
    stack.emplace_back( this, 0 );

    while( !stack.empty() )
    {
        auto currentElement = stack.back();
        stack.pop_back();

        auto ret = callback( *(currentElement.first), currentElement.second );

        switch( ret )
        {
            case Break: return;		// stop here
            case Prune: continue;   // continue, but skip subtree of current node
            case Continue: ;		// continue traversal
        }

        // add children of current node to stack - first child must be on top of stack
        auto &children = currentElement.first->data_->children;
        for( int r = children.size() - 1; r >= 0; r-- )
        {
            stack.emplace_back(&children[r], currentElement.second + 1);
        }
    }
}

const Tlv::Status Tlv::_parse(Tlv& root, const uint8_t* begin, const uint8_t* end, const uint8_t* tree_begin, int maxDepth)
{
    if( maxDepth <= 0 )
    {
        return Status( Status::BadArgument, begin - tree_begin, "Minimum parse depth is 1" );
    }

    struct BacklogNode			// for incomplete nodes in backlog
    {
        Data* data;
        const uint8_t* begin;
        const uint8_t* end;
        int depth;
    };

    Status status;
    std::vector<BacklogNode> backlogStack;
    backlogStack.reserve(4); // start with reasonable default size
    std::vector<Parser::ShallowNode> nodeCache;
    nodeCache.reserve(4);    // start with a reasonable default size

    backlogStack.push_back( BacklogNode{ root.data_.get(), begin, end, 0 } );

    while( !backlogStack.empty() )
    {
        BacklogNode curNode = backlogStack.back();
        backlogStack.pop_back();
        int curChildDepth = curNode.depth + 1;

        Parser parser(curNode.begin, curNode.end, tree_begin);
        nodeCache.clear();

        while( parser.has_next_tag() )
        {
            nodeCache.emplace_back();
            status = parser.next( nodeCache.back() );

            // Abort on parse errors
            if( !status )
            {
                return status;
            }
        }

        curNode.data->children.reserve( nodeCache.size() );
        for( auto &cacheNode : nodeCache )
        {
            curNode.data->children.push_back( Tlv() );
            Data* childDataPtr = curNode.data->children.back().data_.get();
            childDataPtr->tag = cacheNode.tag;
            childDataPtr->parent = curNode.data;

            // Constructed nodes must be revisted for parsing of child nodes, unless max depth was reached
            if ( cacheNode.tag.constructed() && curChildDepth < maxDepth )
            {
                backlogStack.push_back( BacklogNode{childDataPtr, cacheNode.begin, cacheNode.end, curChildDepth} );
            }
            // Otherwise assign data
            else
            {
                childDataPtr->value.assign(cacheNode.begin, cacheNode.end);
            }
        }
    }

    // The rightmost node, might not be the last that is parsed. Set propper end posistion here.
    status.set_parsed_len( end - tree_begin );
    return status;
}

const Tlv::Status Tlv::_parse_one(Tlv& root, const uint8_t* begin, const uint8_t* end, const uint8_t* tree_begin, int maxDepth)
{
    if( maxDepth <= 0 )
    {
        return Status( Status::BadArgument, begin - tree_begin, "Minimum parse depth is 1" );
    }

    Status s;
    Parser parser(begin, end, tree_begin);
    if( parser.has_next_tag() )
    {
        Parser::ShallowNode shallowNode;
        s = parser.next( shallowNode );

        if( s )
        {
            root.data_->tag = shallowNode.tag;
            // Do we neet to continue parsing children?
            if( root.tag().constructed() && maxDepth -1 > 0 )
            {
                s = _parse( root, shallowNode.begin, shallowNode.end, tree_begin, maxDepth -1 );
            }
            else
            {
                root.data_->value.assign( shallowNode.begin, shallowNode.end );
            }

        }
    }

    return s;
}

const Tlv::Status Tlv::_parse_formatted(Tlv &root, std::string_view data)
{
    /* Note: The formatted parser parses tags linewise, indentation defines nesting.
     * Tags in the data-format appear in DFS order of the TLV-tree */
    FormattedParser parser( data );
    Tlv::Status status;

    struct ParentNode
    {
        Data* node;
        int   child_indent;   // indentation child tags
    };

    // The stack has the parent nodes chain
    std::vector<ParentNode> stack;
    stack.reserve( 4 );
    // Virtual root node without tag number has all children (per definition they have indent >= 0)
    stack.push_back({ root.data_.get(), -1 });

    while( parser.has_next_tag() )
    {
        // Parse one line / tag
        FormattedParser::ShallowNode node;
        status = parser.next( node );
        if( !status.ok() )
        {
            return status;
        }

        // Create Tlv node for decoded ShallowNode
        Tlv tlvNode( node.tag );
        if( !node.hexData.empty() )
        {
            tlvNode.set_value( LibtlvUtil::unhexify( node.hexData ));
        }

        /* Assertion due to DFS-order:
         * Each node that has child nodes, has at least one direct child tag that comes immediately after.
         * If a non direct child requires stack-unwind, it must already have other child nodes with
         * equal indentation */

        // Case 1: Child of current parent on stack
        if( node.indent >= stack.back().child_indent )
        {
            // Sibling of last tag
            if( stack.back().child_indent == node.indent )
            {
                stack.back().node->children.push_back( tlvNode );
            }
            // First child of root node (special case)
            else if( stack.back().child_indent == -1 )
            {
                stack.back().child_indent = node.indent;
                stack.back().node->children.push_back( tlvNode );
            }
            // Subtag of last tag,
            else if ( node.indent > stack.back().child_indent )
            {
                // The last child tag of the current parent, becomes new parent for this tag
                auto new_parent = stack.back().node->children.back().data_.get();

                // Enforce that parent is a constructred tag
                if( !new_parent->tag.constructed() )
                {
                    status = Tlv::Status( Tlv::Status::UnexpectedData, parser.get_cur_pos(),
                                          "Invalid subtag %X for non-construted parent tag %X on line %u",
                                          node.tag.value(), new_parent->tag.value(), (unsigned)parser.get_cur_line()-1 );
                    return status;
                }
                // Enforce that parent has no payload data
                if( !new_parent->value.empty() )
                {
                    status = Tlv::Status( Tlv::Status::UnexpectedData, parser.get_cur_pos(),
                                          "Invalid subtag %X for parent tag %X with payload data on line %u",
                                          node.tag.value(), new_parent->tag.value(), (unsigned)parser.get_cur_line()-1 );
                    return status;
                }

                // It must be pushed on stack
                stack.push_back( { new_parent, node.indent } );
                // This node with bigger indentation becomes first child of new parent
                stack.back().node->children.push_back( tlvNode );
            }
        }

        // Case 2: Sibling of a previous tag on stack
        else
        {
            // Unwind stack until finding the correct parent
            // Note that the root note will never be removed since it's child_indent (-1) is smaller than for any other node
            while( stack.back().child_indent > node.indent )
            {
                stack.pop_back();
            }

            // Enforce that all children of a tag use identical indentation
            if( stack.back().child_indent != node.indent )
            {
                status = Tlv::Status( Tlv::Status::UnexpectedData, parser.get_cur_pos(),
                                      "Tag %X on line %u has unexpected indentation.", node.tag.value(), (unsigned)parser.get_cur_line()-1 );
                return status;
            }

            // Set as child of current parent
            stack.back().node->children.push_back( tlvNode );
        }
    }

    status.set_parsed_len(parser.get_cur_pos());
    return status;
}