3 files changed, 55 insertions, 19 deletions

diff --git a/src/simutil.c b/src/simutil.c
index 7fbf0b7b..035ca29b 100644
--- a/src/simutil.c
+++ b/src/simutil.c
@@ -1134,6 +1134,59 @@ void sim_extract_bcd_number(const unsigned char *buf, int len, char *out)
 	out[i*2] = '\0';
 }
 
+static inline int to_semi_oct(char in)
+{
+	int digit;
+
+	switch (in) {
+	case '0':
+	case '1':
+	case '2':
+	case '3':
+	case '4':
+	case '5':
+	case '6':
+	case '7':
+	case '8':
+	case '9':
+		digit = in - '0';
+		break;
+	case '*':
+		digit = 10;
+		break;
+	case '#':
+		digit = 11;
+		break;
+	case 'C':
+	case 'c':
+		digit = 12;
+		break;
+	case '?':
+		digit = 13;
+		break;
+	case 'E':
+	case 'e':
+		digit = 14;
+		break;
+	default:
+		digit = -1;
+		break;
+	}
+
+	return digit;
+}
+
+void sim_encode_bcd_number(const char *number, unsigned char *out)
+{
+	while (number[0] != '\0' && number[1] != '\0') {
+		*out = to_semi_oct(*number++);
+		*out++ |= to_semi_oct(*number++) << 4;
+	}
+
+	if (*number)
+		*out = to_semi_oct(*number) | 0xf0;
+}
+
 gboolean sim_adn_parse(const unsigned char *data, int length,
 			struct ofono_phone_number *ph, char **identifier)
 {
diff --git a/src/simutil.h b/src/simutil.h
index 80020260..29194ca3 100644
--- a/src/simutil.h
+++ b/src/simutil.h
@@ -248,6 +248,7 @@ static inline enum sim_file_access file_access_condition_decode(int bcd)
 }
 
 void sim_extract_bcd_number(const unsigned char *buf, int len, char *out);
+void sim_encode_bcd_number(const char *number, unsigned char *out);
 
 gboolean sim_adn_parse(const unsigned char *data, int length,
 			struct ofono_phone_number *ph, char **identifier);
diff --git a/src/smsutil.c b/src/smsutil.c
index af9632a0..e634764e 100644
--- a/src/smsutil.c
+++ b/src/smsutil.c
@@ -79,34 +79,16 @@ static inline int to_semi_oct(char in)
 
 	switch (in) {
 	case '0':
-		digit = 0;
-		break;
 	case '1':
-		digit = 1;
-		break;
 	case '2':
-		digit = 2;
-		break;
 	case '3':
-		digit = 3;
-		break;
 	case '4':
-		digit = 4;
-		break;
 	case '5':
-		digit = 5;
-		break;
 	case '6':
-		digit = 6;
-		break;
 	case '7':
-		digit = 7;
-		break;
 	case '8':
-		digit = 8;
-		break;
 	case '9':
-		digit = 9;
+		digit = in - '0';
 		break;
 	case '*':
 		digit = 10;