util: Add a "sim string" encoding utility.

3 files changed, 59 insertions, 15 deletions

diff --git a/src/simutil.c b/src/simutil.c
index f980bf69..7291729e 100644
--- a/src/simutil.c
+++ b/src/simutil.c
@@ -1232,29 +1232,24 @@ void sim_adn_build(unsigned char *data, int length,
 			const char *identifier)
 {
 	int number_len = strlen(ph->number);
-	unsigned char *gsm_identifier = NULL;
-	long gsm_bytes;
-	long alpha_length;
+	unsigned char *alpha = NULL;
+	int alpha_written = 0;
+	int alpha_length;
 
 	alpha_length = length - 14;
 
 	/* Alpha-Identifier field */
 	if (alpha_length > 0) {
-		memset(data, 0xff, alpha_length);
-
 		if (identifier)
-			gsm_identifier = convert_utf8_to_gsm(identifier,
-					-1, NULL, &gsm_bytes, 0);
-
-		if (gsm_identifier) {
-			memcpy(data, gsm_identifier,
-				MIN(gsm_bytes, alpha_length));
-			g_free(gsm_identifier);
+			alpha = utf8_to_sim_string(identifier, alpha_length,
+							&alpha_written);
+		if (alpha) {
+			memcpy(data, alpha, alpha_written);
+			g_free(alpha);
 		}
 
-		/* TODO: figure out when the identifier needs to
-		 * be encoded in UCS2 and do this.
-		 */
+		memset(data + alpha_written, 0xff,
+				alpha_length - alpha_written);
 		data += alpha_length;
 	}
 
diff --git a/src/util.c b/src/util.c
index e5ce7b3a..fd8b3059 100644
--- a/src/util.c
+++ b/src/util.c
@@ -1215,3 +1215,49 @@ char *sim_string_to_utf8(const unsigned char *buffer, int length)
 
 	return utf8;
 }
+
+unsigned char *utf8_to_sim_string(const char *utf,
+					int max_length, int *out_length)
+{
+	unsigned char *result;
+	unsigned char *ucs2;
+	long gsm_bytes;
+	gsize converted;
+
+	result = convert_utf8_to_gsm(utf, -1, NULL, &gsm_bytes, 0);
+	if (result) {
+		if (gsm_bytes > max_length) {
+			gsm_bytes = max_length;
+			while (gsm_bytes && result[gsm_bytes - 1] == 0x1b)
+				gsm_bytes -= 1;
+		}
+
+		*out_length = gsm_bytes;
+		return result;
+	}
+
+	/* NOTE: UCS2 formats with an offset are never used */
+
+	ucs2 = (guint8 *) g_convert(utf, -1, "UCS-2BE//TRANSLIT", "UTF-8",
+					NULL, &converted, NULL);
+
+	if (!ucs2)
+		return NULL;
+
+	if (max_length != -1 && (int) converted + 1 > max_length)
+		converted = (max_length - 1) & ~1;
+
+	result = g_try_malloc(converted + 1);
+	if (!result) {
+		g_free(ucs2);
+		return NULL;
+	}
+
+	*out_length = converted + 1;
+
+	result[0] = 0x80;
+	memcpy(&result[1], ucs2, converted);
+	g_free(ucs2);
+
+	return result;
+}
diff --git a/src/util.h b/src/util.h
index 2835b767..9da81aa9 100644
--- a/src/util.h
+++ b/src/util.h
@@ -77,3 +77,6 @@ unsigned char *pack_7bit(const unsigned char *in, long len, int byte_offset,
 				long *items_written, unsigned char terminator);
 
 char *sim_string_to_utf8(const unsigned char *buffer, int length);
+
+unsigned char *utf8_to_sim_string(const char *utf,
+					int max_length, int *out_length);