This version is extra defensive. I think we're doing a lot of useless
length checks to implement the _append_element approach, even though the
length of every ctlv can be known ahead.
---
src/stkutil.c | 196 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 196 insertions(+), 0 deletions(-)
diff --git a/src/stkutil.c b/src/stkutil.c
index e053e2c..e4dcdfb 100644
--- a/src/stkutil.c
+++ b/src/stkutil.c
@@ -2849,3 +2849,199 @@ void stk_command_free(struct stk_command *command)
g_free(command);
}
+
+struct stk_tlv_builder {
+ struct comprehension_tlv_builder ctlv;
+ unsigned char *value;
+ unsigned int len;
+ unsigned int max_len;
+};
+
+static inline gboolean stk_tlv_builder_init(struct stk_tlv_builder *iter,
+ unsigned char *pdu,
+ unsigned int size)
+{
+ iter->value = NULL;
+ iter->len = 0;
+
+ return comprehension_tlv_builder_init(&iter->ctlv, pdu, size);
+}
+
+static inline gboolean stk_tlv_open_container(struct stk_tlv_builder *iter,
+ gboolean cr,
+ unsigned char shorttag,
+ gboolean relocatable)
+{
+ if (comprehension_tlv_builder_next(&iter->ctlv, cr, shorttag) != TRUE)
+ return FALSE;
+
+ iter->len = 0;
+ iter->max_len = relocatable ? 0xff : 0x7f;
+ if (comprehension_tlv_builder_set_length(&iter->ctlv, iter->max_len) !=
+ TRUE)
+ return FALSE;
+
+ iter->value = comprehension_tlv_builder_get_data(&iter->ctlv);
+
+ return TRUE;
+}
+
+static inline gboolean stk_tlv_close_container(struct stk_tlv_builder *iter)
+{
+ return comprehension_tlv_builder_set_length(&iter->ctlv, iter->len);
+}
+
+static inline unsigned int stk_tlv_get_length(struct stk_tlv_builder *iter)
+{
+ return comprehension_tlv_builder_get_data(&iter->ctlv) -
+ iter->ctlv.pdu + iter->len;
+}
+
+static inline gboolean stk_tlv_append_byte(struct stk_tlv_builder *iter,
+ unsigned char num)
+{
+ if (iter->len >= iter->max_len)
+ return FALSE;
+
+ iter->value[iter->len++] = num;
+ return TRUE;
+}
+
+static inline gboolean stk_tlv_append_short(struct stk_tlv_builder *iter,
+ unsigned short num)
+{
+ if (iter->len + 2 > iter->max_len)
+ return FALSE;
+
+ iter->value[iter->len++] = num >> 8;
+ iter->value[iter->len++] = num & 0xff;
+ return TRUE;
+}
+
+static inline gboolean stk_tlv_append_text(struct stk_tlv_builder *iter,
+ int dcs, const char *text)
+{
+ unsigned int len;
+ unsigned char *gsm, *ucs2;
+ long written = 0;
+ gsize gwritten;
+
+ if (text == NULL)
+ return TRUE;
+
+ len = strlen(text);
+
+ switch (dcs) {
+ case 0x00:
+ gsm = convert_utf8_to_gsm(text, len, NULL, &written, 0);
+ if (gsm == NULL && len > 0)
+ return FALSE;
+ if (iter->len + (written * 7 + 7) / 8 >= iter->max_len) {
+ g_free(gsm);
+ return FALSE;
+ }
+
+ iter->value[iter->len++] = 0x00;
+
+ pack_7bit_own_buf(gsm, len, 0, FALSE, &written, 0,
+ iter->value + iter->len);
+ g_free(gsm);
+ if (written < 1 && len > 0)
+ return FALSE;
+ iter->len += written;
+
+ return TRUE;
+ case 0x04:
+ gsm = convert_utf8_to_gsm(text, len, NULL, &written, 0);
+ if (gsm == NULL && len > 0)
+ return FALSE;
+ if (iter->len + written >= iter->max_len) {
+ g_free(gsm);
+ return FALSE;
+ }
+
+ iter->value[iter->len++] = 0x04;
+
+ memcpy(iter->value + iter->len, gsm, written);
+ iter->len += written;
+
+ g_free(gsm);
+
+ return TRUE;
+ case 0x08:
+ ucs2 = (unsigned char *) g_convert((const gchar *) text, len,
+ "UCS-2BE", "UTF-8//TRANSLIT",
+ NULL, &gwritten, NULL);
+ if (ucs2 == NULL)
+ return FALSE;
+ if (iter->len + gwritten >= iter->max_len) {
+ g_free(ucs2);
+ return FALSE;
+ }
+
+ iter->value[iter->len++] = 0x08;
+
+ memcpy(iter->value + iter->len, ucs2, gwritten);
+ iter->len += gwritten;
+
+ g_free(ucs2);
+
+ return TRUE;
+ case -1:
+ /* Fake DCS to mean unpacked GSM alphabet if possible
+ * to encode the string and UCS2 if not. */
+ gsm = convert_utf8_to_gsm(text, len, NULL, &written, 0);
+
+ if (gsm == NULL && len > 0) {
+ /* Use UCS2. */
+ ucs2 = (unsigned char *) g_convert(
+ (const gchar *) text, len,
+ "UCS-2BE", "UTF-8//TRANSLIT",
+ NULL, &gwritten, NULL);
+ if (ucs2 == NULL)
+ return FALSE;
+ if (iter->len + gwritten >= iter->max_len) {
+ g_free(ucs2);
+ return FALSE;
+ }
+
+ iter->value[iter->len++] = 0x08;
+
+ memcpy(iter->value + iter->len, ucs2, gwritten);
+ iter->len += gwritten;
+
+ g_free(ucs2);
+
+ return TRUE;
+ }
+
+ if (iter->len + written >= iter->max_len) {
+ g_free(gsm);
+ return FALSE;
+ }
+
+ iter->value[iter->len++] = 0x04;
+
+ memcpy(iter->value + iter->len, gsm, written);
+ iter->len += written;
+
+ g_free(gsm);
+
+ return TRUE;
+ }
+
+ return FALSE;
+}
+
+static inline gboolean stk_tlv_append_bytes(struct stk_tlv_builder *iter,
+ const unsigned char *data,
+ unsigned int length)
+{
+ if (iter->len + length > iter->max_len)
+ return FALSE;
+
+ memcpy(iter->value + iter->len, data, length);
+ iter->len += length;
+
+ return TRUE;
+}
--
1.7.1.86.g0e460.dirty
_______________________________________________
ofono mailing list
[email protected]
http://lists.ofono.org/listinfo/ofono
