simutil: Add sim_encode_bcd_number

diff --git a/src/simutil.c b/src/simutil.c
index 7fbf0b7..035ca29 100644 (file)
--- 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 8002026..29194ca 100644 (file)
--- 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 af9632a..e634764 100644 (file)
--- 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;