simutil: Add utilities for encoding BTLV and CTLV

diff --git a/src/simutil.c b/src/simutil.c
index b98c011..a0e824a 100644 (file)
--- a/src/simutil.c
+++ b/src/simutil.c
@@ -498,6 +498,316 @@ static const guint8 *ber_tlv_find_by_tag(const guint8 *pdu, guint8 in_tag,
        return NULL;
 }
 
+#define MAX_BER_TLV_HEADER 8
+
+gboolean ber_tlv_builder_init(struct ber_tlv_builder *builder,
+                               unsigned char *pdu, unsigned int size)
+{
+       if (size < MAX_BER_TLV_HEADER)
+               return FALSE;
+
+       builder->pdu = pdu;
+       builder->pos = 0;
+       builder->max = size;
+       builder->parent = NULL;
+       builder->tag = 0xff;
+       builder->len = 0;
+
+       return TRUE;
+}
+
+static void ber_tlv_builder_write_header(struct ber_tlv_builder *builder)
+{
+       int header_len = 0;
+       int start = builder->pos + MAX_BER_TLV_HEADER;
+
+       /* Write length at end of the header space */
+       if (builder->len <= 0x7f)
+               builder->pdu[start - ++header_len] = builder->len;
+       else {
+               while (builder->len >> (header_len * 8)) {
+                       builder->pdu[start - 1 - header_len] =
+                               builder->len >> (header_len * 8);
+                       header_len++;
+               }
+               builder->pdu[start - 1 - header_len] = 0x80 + header_len;
+               header_len++;
+       }
+
+       /* Write the tag before length */
+       if (builder->tag < 0x1f)
+               builder->pdu[start - ++header_len] =
+                       (builder->class << 6) |
+                       (builder->encoding << 5) |
+                       builder->tag;
+       else {
+               int i = 0;
+
+               while (builder->tag >> (i * 7)) {
+                       builder->pdu[start - ++header_len] =
+                               i ? 0x80 | (builder->tag >> (i * 7)) :
+                               (builder->tag & 0x7f);
+                       i++;
+               }
+
+               builder->pdu[start - ++header_len] =
+                       (builder->class << 6) | (builder->encoding << 5) | 0x1f;
+       }
+
+       /* Pad with stuff bytes */
+       if (header_len < MAX_BER_TLV_HEADER)
+               memset(builder->pdu + builder->pos, 0xff,
+                               MAX_BER_TLV_HEADER - header_len);
+}
+
+gboolean ber_tlv_builder_next(struct ber_tlv_builder *builder,
+                               enum ber_tlv_data_type class,
+                               enum ber_tlv_data_encoding_type encoding,
+                               unsigned int new_tag)
+{
+       if (builder->tag != 0xff) {
+               ber_tlv_builder_write_header(builder);
+
+               builder->pos += MAX_BER_TLV_HEADER + builder->len;
+       }
+
+       if (builder->pos + MAX_BER_TLV_HEADER > builder->max)
+               return FALSE;
+
+       if (builder->parent)
+               if (ber_tlv_builder_set_length(builder->parent, builder->pos +
+                                       MAX_BER_TLV_HEADER) == FALSE)
+                       return FALSE;
+
+       builder->class = class;
+       builder->encoding = encoding;
+       builder->tag = new_tag;
+       builder->len = 0;
+
+       return TRUE;
+}
+
+/* Resize the TLV because the content of Value field needs more space.  If
+ * this TLV is part of another TLV, resize that one too.  */
+gboolean ber_tlv_builder_set_length(struct ber_tlv_builder *builder,
+                                       unsigned int new_len)
+{
+       if (builder->pos + MAX_BER_TLV_HEADER + new_len > builder->max)
+               return FALSE;
+
+       if (builder->parent)
+               if (ber_tlv_builder_set_length(builder->parent,
+                                       builder->pos + MAX_BER_TLV_HEADER +
+                                       new_len) == FALSE)
+                       return FALSE;
+
+       builder->len = new_len;
+
+       return TRUE;
+}
+
+unsigned char *ber_tlv_builder_get_data(struct ber_tlv_builder *builder)
+{
+       return builder->pdu + builder->pos + MAX_BER_TLV_HEADER;
+}
+
+gboolean ber_tlv_builder_recurse(struct ber_tlv_builder *builder,
+                                       struct ber_tlv_builder *recurse)
+{
+       unsigned char *end = builder->pdu + builder->max;
+       unsigned char *data = ber_tlv_builder_get_data(builder);
+
+       if (ber_tlv_builder_init(recurse, data, end - data) == FALSE)
+               return FALSE;
+
+       recurse->parent = builder;
+
+       return TRUE;
+}
+
+gboolean ber_tlv_builder_recurse_comprehension(struct ber_tlv_builder *builder,
+                               struct comprehension_tlv_builder *recurse)
+{
+       unsigned char *end = builder->pdu + builder->max;
+       unsigned char *data = ber_tlv_builder_get_data(builder);
+
+       if (comprehension_tlv_builder_init(recurse, data, end - data) == FALSE)
+               return FALSE;
+
+       recurse->parent = builder;
+
+       return TRUE;
+}
+
+void ber_tlv_builder_optimize(struct ber_tlv_builder *builder,
+                               unsigned char **pdu, unsigned int *len)
+{
+       ber_tlv_builder_write_header(builder);
+
+       if (pdu == NULL)
+               return;
+
+       *len = builder->pos + MAX_BER_TLV_HEADER + builder->len;
+       *pdu = builder->pdu;
+
+       while (**pdu == 0xff) {
+               (*len)--;
+               (*pdu)++;
+       }
+}
+
+gboolean comprehension_tlv_builder_init(
+                               struct comprehension_tlv_builder *builder,
+                               unsigned char *pdu, unsigned int size)
+{
+       if (size < 2)
+               return FALSE;
+
+       builder->pdu = pdu;
+       builder->pos = 0;
+       builder->max = size;
+       builder->parent = NULL;
+
+       builder->pdu[0] = 0;
+       builder->pdu[1] = 0;
+
+       return TRUE;
+}
+
+static inline unsigned int comprehension_tlv_get_tag_len(unsigned char *start)
+{
+       return bit_field(*start, 0, 7) == 0x7f ? 3 : 1;
+}
+
+static inline unsigned int comprehension_tlv_get_len_len(unsigned char *start)
+{
+       return *start >= 0x80 ? *start - 0x7f : 1;
+}
+
+gboolean comprehension_tlv_builder_next(
+                               struct comprehension_tlv_builder *builder,
+                               gboolean cr, unsigned short tag)
+{
+       unsigned int taglen = 0;
+       unsigned int lenlen = 0;
+       unsigned int len = 0;
+
+       if (builder->pdu[builder->pos] != 0) {
+               taglen = comprehension_tlv_get_tag_len(builder->pdu +
+                                                       builder->pos);
+               lenlen = 1;
+               len = builder->pdu[builder->pos + taglen];
+
+               if (len >= 0x80) {
+                       unsigned int extended_bytes = len - 0x80;
+                       unsigned int i;
+
+                       for (len = 0, i = 1; i <= extended_bytes; i++)
+                               len = (len << 8) |
+                                       builder->pdu[builder->pos + taglen + i];
+
+                       lenlen += extended_bytes;
+               }
+       }
+
+       if (builder->pos + taglen + lenlen + len + (tag < 0x7f ? 1 : 3) + 1 >
+                       builder->max)
+               return FALSE;
+
+       builder->pos += taglen + lenlen + len;
+       if (tag < 0x7f) {
+               builder->pdu[builder->pos + 0] = (cr ? 0x80 : 0x00) | tag;
+               builder->pdu[builder->pos + 1] = 0; /* Length */
+       } else {
+               if (builder->pos + 4 > builder->max)
+                       return FALSE;
+
+               builder->pdu[builder->pos + 0] = 0x7f;
+               builder->pdu[builder->pos + 1] = (cr ? 0x80 : 0) | (tag >> 8);
+               builder->pdu[builder->pos + 2] = tag & 0xff;
+               builder->pdu[builder->pos + 3] = 0; /* Length */
+       }
+
+       return TRUE;
+}
+
+/* Resize the TLV because the content of Value field needs more space.  If
+ * this TLV is part of another TLV, resize that one too.  */
+gboolean comprehension_tlv_builder_set_length(
+                               struct comprehension_tlv_builder *builder,
+                               unsigned int new_len)
+{
+       unsigned char *tlv = builder->pdu + builder->pos;
+       unsigned int taglen = comprehension_tlv_get_tag_len(tlv);
+       unsigned int lenlen = 1, new_lenlen = 1;
+       unsigned int len = tlv[taglen];
+       unsigned int ctlv_len, new_ctlv_len;
+
+       /* How much space do we occupy now */
+       if (len >= 0x80) {
+               unsigned int extended_bytes = len - 0x80;
+               unsigned int i;
+
+               for (len = 0, i = 1; i <= extended_bytes; i++)
+                       len = (len << 8) | tlv[taglen + i];
+
+               lenlen += extended_bytes;
+       }
+
+       ctlv_len = taglen + lenlen + len;
+
+       /* How much do we need */
+       if (new_len >= 0x80) {
+               unsigned int extended_bytes = 0;
+               while (new_len >> (extended_bytes * 8))
+                       extended_bytes += 1;
+               new_lenlen += extended_bytes;
+       }
+
+       new_ctlv_len = taglen + new_lenlen + new_len;
+
+       /* Check there is enough space */
+       if (builder->pos + new_ctlv_len > builder->max)
+               return FALSE;
+
+       if (builder->parent)
+               if (ber_tlv_builder_set_length(builder->parent, builder->pos +
+                                       new_ctlv_len) == FALSE)
+                       return FALSE;
+
+       len = MIN(len, new_len);
+       if (len > 0 && new_lenlen != lenlen)
+               memmove(tlv + taglen + new_lenlen, tlv + taglen + lenlen, len);
+
+       /* Write new length */
+       if (new_len < 0x80)
+               tlv[taglen] = new_len;
+       else {
+               unsigned int extended_bytes = 0;
+               unsigned int i;
+               while (new_len >> (extended_bytes * 8))
+                       extended_bytes += 1;
+
+               for (i = 1; i <= extended_bytes; i++)
+                       tlv[taglen + i] =
+                               (new_len >> ((extended_bytes - i) * 8)) & 0xff;
+
+               tlv[taglen] = 0x80 + extended_bytes;
+       }
+
+       return TRUE;
+}
+
+unsigned char *comprehension_tlv_builder_get_data(
+                               struct comprehension_tlv_builder *builder)
+{
+       unsigned char *tlv = builder->pdu + builder->pos;
+       unsigned int taglen = comprehension_tlv_get_tag_len(tlv);
+       unsigned int lenlen = comprehension_tlv_get_len_len(tlv + taglen);
+
+       return tlv + taglen + lenlen;
+}
+
 static char *sim_network_name_parse(const unsigned char *buffer, int length,
                                        gboolean *add_ci)
 {

diff --git a/src/simutil.h b/src/simutil.h
index 144bf12..c64d275 100644 (file)
--- a/src/simutil.h
+++ b/src/simutil.h
@@ -115,6 +115,25 @@ struct ber_tlv_iter {
        const unsigned char *data;
 };
 
+struct ber_tlv_builder {
+       unsigned int max;
+       unsigned int pos;
+       unsigned char *pdu;
+       struct ber_tlv_builder *parent;
+
+       unsigned int tag;
+       enum ber_tlv_data_type class;
+       enum ber_tlv_data_encoding_type encoding;
+       unsigned int len;
+};
+
+struct comprehension_tlv_builder {
+       unsigned int max;
+       unsigned int pos;
+       unsigned char *pdu;
+       struct ber_tlv_builder *parent;
+};
+
 void simple_tlv_iter_init(struct simple_tlv_iter *iter,
                                const unsigned char *pdu, unsigned int len);
 gboolean simple_tlv_iter_next(struct simple_tlv_iter *iter);
@@ -136,6 +155,18 @@ const unsigned char *comprehension_tlv_iter_get_data(
 void comprehension_tlv_iter_copy(struct comprehension_tlv_iter *from,
                                        struct comprehension_tlv_iter *to);
 
+gboolean comprehension_tlv_builder_init(
+                               struct comprehension_tlv_builder *builder,
+                               unsigned char *pdu, unsigned int size);
+gboolean comprehension_tlv_builder_next(
+                               struct comprehension_tlv_builder *builder,
+                               gboolean cr, unsigned short tag);
+gboolean comprehension_tlv_builder_set_length(
+                               struct comprehension_tlv_builder *builder,
+                               unsigned int len);
+unsigned char *comprehension_tlv_builder_get_data(
+                               struct comprehension_tlv_builder *builder);
+
 void ber_tlv_iter_init(struct ber_tlv_iter *iter, const unsigned char *pdu,
                        unsigned int len);
 /*
@@ -168,6 +199,22 @@ void ber_tlv_iter_recurse_simple(struct ber_tlv_iter *iter,
 void ber_tlv_iter_recurse_comprehension(struct ber_tlv_iter *iter,
                                        struct comprehension_tlv_iter *recurse);
 
+gboolean ber_tlv_builder_init(struct ber_tlv_builder *builder,
+                               unsigned char *pdu, unsigned int size);
+gboolean ber_tlv_builder_next(struct ber_tlv_builder *builder,
+                               enum ber_tlv_data_type class,
+                               enum ber_tlv_data_encoding_type encoding,
+                               unsigned int new_tag);
+gboolean ber_tlv_builder_set_length(struct ber_tlv_builder *builder,
+                                       unsigned int len);
+unsigned char *ber_tlv_builder_get_data(struct ber_tlv_builder *builder);
+gboolean ber_tlv_builder_recurse(struct ber_tlv_builder *builder,
+                                       struct ber_tlv_builder *recurse);
+gboolean ber_tlv_builder_recurse_comprehension(struct ber_tlv_builder *builder,
+                               struct comprehension_tlv_builder *recurse);
+void ber_tlv_builder_optimize(struct ber_tlv_builder *builder,
+                               unsigned char **pdu, unsigned int *len);
+
 struct sim_eons *sim_eons_new(int pnn_records);
 void sim_eons_add_pnn_record(struct sim_eons *eons, int record,
                                const guint8 *tlv, int length);