Fix gsm_7bit_{en,de}code function and sms user_data calculations

With the 7bit alphabet the user_data_len field needs to be in
septets (characters) and not in octets occupied.

2 files changed, 43 insertions, 36 deletions

diff --git a/openbsc/src/gsm_04_11.c b/openbsc/src/gsm_04_11.c
index 9b3f44ec8..05e55d4dc 100644
--- a/openbsc/src/gsm_04_11.c
+++ b/openbsc/src/gsm_04_11.c
@@ -315,8 +315,13 @@ static int gsm340_gen_tpdu(struct msgb *msg, struct gsm_sms *sms)
 	*smsp = sms->user_data_len;
 
 	/* generate TP-UD */
-	smsp = msgb_put(msg, sms->user_data_len);
-	memcpy(smsp, sms->user_data, sms->user_data_len);
+	/* FIXME: Handle DSC of UCS2 or 8/bit default */
+	if (gsm338_get_sms_alphabet(sms->data_coding_scheme) == DCS_7BIT_DEFAULT) {
+		/* Warning, user_data_len indicates the amount of septets
+		 * (characters), we need amount of octets occupied */
+		smsp = msgb_put(msg, ceil(sms->user_data_len*7/8.0));
+		memcpy(smsp, sms->user_data, ceil(sms->user_data_len*7/8.0));
+	}
 
 	return msg->len - old_msg_len;
 }
diff --git a/openbsc/src/gsm_utils.c b/openbsc/src/gsm_utils.c
index 185918ef9..de18dba26 100644
--- a/openbsc/src/gsm_utils.c
+++ b/openbsc/src/gsm_utils.c
@@ -30,48 +30,50 @@
 /* GSM 03.38 6.2.1 Charachter packing */
 int gsm_7bit_decode(char *text, const u_int8_t *user_data, u_int8_t length)
 {
-	u_int8_t d_off = 0, b_off = 0;
-	u_int8_t i;
-
-	for (i=0;i<length;i++) {
-		text[i] = ((user_data[d_off] + (user_data[d_off+1]<<8)) & (0x7f<<b_off))>>b_off;
-		b_off += 7;
-		if (b_off >= 8) {
-			d_off += 1;
-			b_off -= 8;
-		}
-	}
-	text[i] = '\0';
-	return 0;
+	int i = 0;
+	int l = 0;
+
+        /* FIXME: We need to account for user data headers here */
+	i += l;
+	for (; i < length; i ++)
+		*(text ++) =
+			((user_data[(i * 7 + 7) >> 3] <<
+			  (7 - ((i * 7 + 7) & 7))) |
+			 (user_data[(i * 7) >> 3] >>
+			  ((i * 7) & 7))) & 0x7f;
+	*text = '\0';
+
+	return i - l;
 }
 
+
 /* GSM 03.38 6.2.1 Charachter packing */
 int gsm_7bit_encode(u_int8_t *result, const char *data)
 {
-	int i;
-	u_int8_t d_off = 0, b_off = 0;
-	const int length = strlen(data);
-	int out_length = (length * 8)/7;
-
-	memset(result, 0, out_length);
-
-	for (i = 0; i < length; ++i) {
-		u_int8_t first  = (data[i] & 0x7f) << b_off;
-		u_int8_t second = (data[i] & 0x7f) >> (8 - b_off);
-    
-		result[d_off] |= first;
-		if (second != 0)
-			result[d_off + 1] = second;
-
-		b_off += 7;
-
-		if (b_off >= 8) {
-			d_off += 1;
-			b_off -= 8;
+	int i,j = 0;
+	unsigned char ch1, ch2;
+	int shift = 0;
+
+	for ( i=0; i<strlen(data); i++ ) {
+
+		ch1 = data[i] & 0x7F;
+		ch1 = ch1 >> shift;
+		ch2 = data[(i+1)] & 0x7F;
+		ch2 = ch2 << (7-shift);
+
+		ch1 = ch1 | ch2;
+
+		result[j++] = ch1;
+
+		shift++;
+
+		if ((shift == 7) && (i+1<strlen(data))) {
+			shift = 0;
+			i++;
 		}
 	}
 
-	return out_length;
+	return i;
 }
 
 /* determine power control level for given dBm value, as indicated