* [PATCH 01/27] Add utilities for encoding BTLVs and CTLVs.
@ 2010-05-16 14:01 Andrzej Zaborowski
2010-05-25 21:36 ` Denis Kenzior
0 siblings, 1 reply; 2+ messages in thread
From: Andrzej Zaborowski @ 2010-05-16 14:01 UTC (permalink / raw)
To: ofono
[-- Attachment #1: Type: text/plain, Size: 11737 bytes --]
This version merges ber_tlv_builder_next and _set_tag into one function,
same for comprehension tlvs. The ber_tlv_iter and ber_tlv_builder
structs are not merged because the pointers in _iter need to be const
else ugly casting is needed.
---
src/simutil.c | 310 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
src/simutil.h | 47 +++++++++
2 files changed, 357 insertions(+), 0 deletions(-)
diff --git a/src/simutil.c b/src/simutil.c
index b98c011..a0e824a 100644
--- 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 45b6847..53e7743 100644
--- a/src/simutil.h
+++ b/src/simutil.h
@@ -114,6 +114,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);
@@ -135,6 +154,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);
/*
@@ -167,6 +198,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);
--
1.7.1.86.g0e460.dirty
^ permalink raw reply related [flat|nested] 2+ messages in thread
end of thread, other threads:[~2010-05-25 21:36 UTC | newest]
Thread overview: 2+ messages (download: mbox.gz follow: Atom feed
-- links below jump to the message on this page --
2010-05-16 14:01 [PATCH 01/27] Add utilities for encoding BTLVs and CTLVs Andrzej Zaborowski
2010-05-25 21:36 ` Denis Kenzior
This is an external index of several public inboxes,
see mirroring instructions on how to clone and mirror
all data and code used by this external index.