All of lore.kernel.org
 help / color / mirror / Atom feed
* [PATCH 2/3][RfC] Add utilities for encoding BTLVs and CTLVs.
@ 2010-04-22  8:11 Andrzej Zaborowski
  2010-04-23 20:09 ` Denis Kenzior
  0 siblings, 1 reply; 4+ messages in thread
From: Andrzej Zaborowski @ 2010-04-22  8:11 UTC (permalink / raw)
  To: ofono

[-- Attachment #1: Type: text/plain, Size: 13552 bytes --]

---
 src/simutil.c |  396 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/simutil.h |   44 +++++++
 2 files changed, 440 insertions(+), 0 deletions(-)

diff --git a/src/simutil.c b/src/simutil.c
index 822938c..ab2e421 100644
--- a/src/simutil.c
+++ b/src/simutil.c
@@ -486,6 +486,402 @@ static const guint8 *ber_tlv_find_by_tag(const guint8 *pdu, guint8 in_tag,
 	return NULL;
 }
 
+gboolean ber_tlv_constr_init(struct ber_tlv_constr *constr,
+				unsigned char *pdu, unsigned int size)
+{
+	if (size < 2)
+		return FALSE;
+
+	constr->pdu = pdu;
+	constr->pos = 0;
+	constr->max = size;
+	constr->parent = NULL;
+
+	return TRUE;
+}
+
+static inline unsigned int ber_tlv_get_tag_len(unsigned char *start)
+{
+	int len = 1;
+
+	if (bit_field(start[0], 0, 4) == 0x1f)
+		while (start[len] & 0x80)
+			len++;
+
+	return len;
+}
+
+static inline unsigned int ber_tlv_get_len_len(unsigned char *start)
+{
+	return *start >= 0x80 ? *start - 0x7f : 1;
+}
+
+gboolean ber_tlv_constr_next(struct ber_tlv_constr *constr)
+{
+	unsigned int taglen = ber_tlv_get_tag_len(constr->pdu + constr->pos);
+	unsigned int lenlen = 1;
+	unsigned int len = constr->pdu[constr->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) |
+				constr->pdu[constr->pos + taglen + i];
+
+		lenlen += extended_bytes;
+	}
+
+	if (constr->pos + taglen + lenlen + len + 2 > constr->max)
+		return FALSE;
+
+	constr->pos += taglen + lenlen + len;
+	constr->pdu[constr->pos + 0] = 0; /* Tag */
+	constr->pdu[constr->pos + 1] = 0; /* Length */
+
+	return TRUE;
+}
+
+gboolean ber_tlv_constr_set_tag(struct ber_tlv_constr *constr,
+				enum ber_tlv_data_type class,
+				enum ber_tlv_data_encoding_type encoding,
+				unsigned short tag)
+{
+	if (tag < 0x1f)
+		constr->pdu[constr->pos] =
+			(class << 6) | (encoding << 5) | tag;
+	else {
+		int taglen = 0;
+		int i;
+
+		while (tag >> (taglen * 7))
+			taglen += 1;
+
+		if (constr->pos + taglen + 2 > constr->max)
+			return FALSE;
+
+		constr->pdu[constr->pos] =
+			(class << 6) | (encoding << 5) | 0x1f;
+
+		for (i = 1; i < taglen; i += 1)
+			constr->pdu[constr->pos + i] = 0x80 |
+				(tag >> ((taglen - i)) * 7);
+
+		constr->pdu[constr->pos + i++] = tag & 0x7f;
+		constr->pdu[constr->pos + i] = 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.  */
+static unsigned char *ber_tlv_constr_update_len(struct ber_tlv_constr *constr,
+						unsigned int new_len,
+						unsigned int pos)
+{
+	unsigned char *tlv = constr->pdu + pos;
+	unsigned int taglen = ber_tlv_get_tag_len(tlv);
+	unsigned int lenlen = 1, new_lenlen = 1;
+	unsigned int len = tlv[taglen];
+	unsigned int tlv_len, new_tlv_len;
+	unsigned int added_len = 0;
+
+	/* 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;
+	}
+
+	tlv_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_tlv_len = taglen + new_lenlen + new_len;
+
+	/* Check there is enough space */
+	if (constr->pos > pos)
+		added_len = constr->pos - (pos + tlv_len);
+
+	if (pos + new_tlv_len + added_len > constr->max)
+		return NULL;
+
+	if (constr->parent) {
+		unsigned char *new_pdu = ber_tlv_constr_update_len(
+						constr->parent,
+						pos + new_tlv_len + added_len,
+						constr->parent_pos);
+		if (new_pdu == NULL)
+			return NULL;
+
+		constr->pdu = new_pdu;
+		tlv = constr->pdu + pos;
+	}
+
+	if (added_len > 0 && new_tlv_len != tlv_len) {
+		memmove(tlv + new_tlv_len, tlv + tlv_len, added_len);
+		constr->pos += new_tlv_len - tlv_len;
+	}
+
+	if (new_lenlen != lenlen)
+		memmove(tlv + taglen + new_lenlen, tlv + taglen + lenlen,
+				tlv_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 tlv + taglen + new_lenlen;
+}
+
+gboolean ber_tlv_constr_set_length(struct ber_tlv_constr *constr,
+					unsigned int new_len)
+{
+	return ber_tlv_constr_update_len(constr, new_len, constr->pos) != NULL;
+}
+
+unsigned char *ber_tlv_constr_get_data(struct ber_tlv_constr *constr)
+{
+	unsigned char *tlv = constr->pdu + constr->pos;
+	unsigned int taglen = ber_tlv_get_tag_len(tlv);
+	unsigned int lenlen = ber_tlv_get_len_len(tlv + taglen);
+
+	return tlv + taglen + lenlen;
+}
+
+gboolean ber_tlv_constr_recurse(struct ber_tlv_constr *constr,
+				struct ber_tlv_constr *recurse)
+{
+	unsigned char *end = constr->pdu + constr->max;
+	unsigned char *data = ber_tlv_constr_get_data(constr);
+	gboolean r = ber_tlv_constr_init(recurse, data, end - data);
+
+	if (!r)
+		return FALSE;
+
+	recurse->parent = constr;
+	recurse->parent_pos = constr->pos;
+
+	return TRUE;
+}
+
+gboolean ber_tlv_constr_recurse_comprehension(struct ber_tlv_constr *constr,
+				struct comprehension_tlv_constr *recurse)
+{
+	unsigned char *end = constr->pdu + constr->max;
+	unsigned char *data = ber_tlv_constr_get_data(constr);
+	gboolean r = comprehension_tlv_constr_init(recurse, data, end - data);
+
+	if (!r)
+		return FALSE;
+
+	recurse->parent = constr;
+	recurse->parent_pos = constr->pos;
+
+	return TRUE;
+}
+
+gboolean comprehension_tlv_constr_init(struct comprehension_tlv_constr *constr,
+					unsigned char *pdu,
+					unsigned int size)
+{
+	if (size < 2)
+		return FALSE;
+
+	constr->pdu = pdu;
+	constr->pos = 0;
+	constr->max = size;
+	constr->parent = NULL;
+
+	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_constr_next(struct comprehension_tlv_constr *constr)
+{
+	unsigned int taglen =
+		comprehension_tlv_get_tag_len(constr->pdu + constr->pos);
+	unsigned int lenlen = 1;
+	unsigned int len = constr->pdu[constr->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) |
+				constr->pdu[constr->pos + taglen + i];
+
+		lenlen += extended_bytes;
+	}
+
+	if (constr->pos + taglen + lenlen + len + 2 > constr->max)
+		return FALSE;
+
+	constr->pos += taglen + lenlen + len;
+	constr->pdu[constr->pos + 0] = 0; /* Tag */
+	constr->pdu[constr->pos + 1] = 0; /* Length */
+
+	return TRUE;
+}
+
+gboolean comprehension_tlv_constr_set_tag(
+					struct comprehension_tlv_constr *constr,
+					gboolean cr, unsigned short tag)
+{
+	if (tag < 0x7f)
+		constr->pdu[constr->pos] = (cr ? 0x80 : 0x00) | tag;
+	else {
+		if (constr->pos + 4 > constr->max)
+			return FALSE;
+
+		constr->pdu[constr->pos] = 0x7f;
+		constr->pdu[constr->pos + 1] = (cr ? 0x80 : 0x00) | (tag >> 8);
+		constr->pdu[constr->pos + 2] = tag & 0xff;
+		constr->pdu[constr->pos + 3] = 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.  */
+static unsigned char *comprehension_tlv_constr_update_len(
+					struct comprehension_tlv_constr *constr,
+					unsigned int new_len,
+					unsigned int pos)
+{
+	unsigned char *tlv = constr->pdu + 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;
+	unsigned int added_len = 0;
+
+	/* 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 (constr->pos > pos)
+		added_len = constr->pos - (pos + ctlv_len);
+
+	if (pos + new_ctlv_len + added_len > constr->max)
+		return NULL;
+
+	if (constr->parent) {
+		unsigned char *new_pdu = ber_tlv_constr_update_len(
+						constr->parent,
+						pos + new_ctlv_len + added_len,
+						constr->parent_pos);
+		if (new_pdu == NULL)
+			return NULL;
+
+		constr->pdu = new_pdu;
+		tlv = constr->pdu + pos;
+	}
+
+	if (added_len > 0) {
+		memmove(tlv + new_ctlv_len, tlv + ctlv_len, added_len);
+		constr->pos += new_ctlv_len - ctlv_len;
+	}
+
+	if (new_lenlen > lenlen)
+		memmove(tlv + taglen + new_lenlen, tlv + taglen + lenlen,
+				new_lenlen - lenlen);
+
+	/* 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 tlv + taglen + new_lenlen;
+}
+
+gboolean comprehension_tlv_constr_set_length(
+					struct comprehension_tlv_constr *constr,
+					unsigned int new_len)
+{
+	return comprehension_tlv_constr_update_len(constr,
+							new_len,
+							constr->pos) != NULL;
+}
+
+unsigned char *comprehension_tlv_constr_get_data(
+				struct comprehension_tlv_constr *constr)
+{
+	unsigned char *tlv = constr->pdu + constr->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 7590cca..60db647 100644
--- a/src/simutil.h
+++ b/src/simutil.h
@@ -114,6 +114,22 @@ struct ber_tlv_iter {
 	const unsigned char *data;
 };
 
+struct ber_tlv_constr {
+	unsigned int max;
+	unsigned int pos;
+	unsigned char *pdu;
+	struct ber_tlv_constr *parent;
+	unsigned int parent_pos;
+};
+
+struct comprehension_tlv_constr {
+	unsigned int max;
+	unsigned int pos;
+	unsigned char *pdu;
+	struct ber_tlv_constr *parent;
+	unsigned int parent_pos;
+};
+
 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);
@@ -132,6 +148,18 @@ unsigned int comprehension_tlv_iter_get_length(
 const unsigned char *comprehension_tlv_iter_get_data(
 					struct comprehension_tlv_iter *iter);
 
+gboolean comprehension_tlv_constr_init(struct comprehension_tlv_constr *constr,
+					unsigned char *pdu, unsigned int size);
+gboolean comprehension_tlv_constr_next(struct comprehension_tlv_constr *constr);
+gboolean comprehension_tlv_constr_set_tag(
+					struct comprehension_tlv_constr *constr,
+					gboolean cr, unsigned short tag);
+gboolean comprehension_tlv_constr_set_length(
+					struct comprehension_tlv_constr *constr,
+					unsigned int len);
+unsigned char *comprehension_tlv_constr_get_data(
+				struct comprehension_tlv_constr *constr);
+
 void ber_tlv_iter_init(struct ber_tlv_iter *iter, const unsigned char *pdu,
 			unsigned int len);
 /*
@@ -164,6 +192,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_constr_init(struct ber_tlv_constr *constr,
+				unsigned char *pdu, unsigned int size);
+gboolean ber_tlv_constr_set_tag(struct ber_tlv_constr *constr,
+				enum ber_tlv_data_type class,
+				enum ber_tlv_data_encoding_type encoding,
+				unsigned short tag);
+gboolean ber_tlv_constr_set_length(struct ber_tlv_constr *constr,
+					unsigned int len);
+unsigned char *ber_tlv_constr_get_data(struct ber_tlv_constr *constr);
+gboolean ber_tlv_constr_next(struct ber_tlv_constr *constr);
+gboolean ber_tlv_constr_recurse(struct ber_tlv_constr *constr,
+				struct ber_tlv_constr *recurse);
+gboolean ber_tlv_constr_recurse_comprehension(struct ber_tlv_constr *constr,
+				struct comprehension_tlv_constr *recurse);
+
+
 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.6.1


^ permalink raw reply related	[flat|nested] 4+ messages in thread

end of thread, other threads:[~2010-04-26 15:28 UTC | newest]

Thread overview: 4+ messages (download: mbox.gz follow: Atom feed
-- links below jump to the message on this page --
2010-04-22  8:11 [PATCH 2/3][RfC] Add utilities for encoding BTLVs and CTLVs Andrzej Zaborowski
2010-04-23 20:09 ` Denis Kenzior
2010-04-26 14:22   ` andrzej zaborowski
2010-04-26 15:28     ` 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.