--- smsdaemon/SmsPdu.cpp	2008/06/12 11:05:18	61
+++ smsdaemon/SmsPdu.cpp	2008/06/12 18:54:24	68
@@ -10,7 +10,7 @@
 #include <time.h>
 #include <stdlib.h> 
 
-
+#include "common.h"
 #include "util.h"
 
 
@@ -21,37 +21,38 @@
 {
 
 
-vector<unsigned char> BcdEncode(string input)
-{
-	char buf[2] = " ";
-	vector<unsigned char> result;
 
-	unsigned char current;
-	for (unsigned int i=0; i<input.length(); ++i)
+string SwitchChars(string input)
+{
+	for (unsigned int i=1; i<input.length(); i+=2)
 	{
-		buf[0] = input.at(i);
-		unsigned char tmp = atoi(buf);
-
-		if ( (i%2) == 0)
-		{
-			current = tmp;
-		}
-		else
-		{
-			current |= (tmp<<4);
-			result.push_back(current);
-		}
+		char tmp = input[i];
+		input[i] = input[i-1];
+		input[i-1] = tmp;
 	}
+	return input;
+}
 
-	if ((input.length() % 2) == 1)
-	{
-		current |= 0xF0;
-		result.push_back(current);
-	}
+string EncodePhonenr(string input)
+{
+	if ( (input.length() % 2) == 1)
+		input.append("F");
+	return SwitchChars(input);
+}
 
-	return result;
+string DecodePhonenr(string input)
+{
+	input = SwitchChars(input);
+	int last = input.length() -1;
+
+	if (input.at(last) == 'F')
+		input.erase(last,1);
+	return input;
 }
 
+
+
+
 string HexformatVector(vector<unsigned char> vec)
 {
 	ostringstream os;
@@ -67,13 +68,38 @@
 	return os.str();
 }
 
-std::string Encode8to7bit(vector<unsigned char> vec)
+std::string Decode8to7bit(vector<unsigned char> input)
 {
 	string result;
 
+	int shift = 0;
+	for (unsigned int i=0; i<input.size(); ++i)
+	{
+		unsigned char current = input.at(i);
+		unsigned char prev = (i>0) ? input.at(i-1) : 0;
+
+		current <<= shift;
+
+		prev >>= (8-shift);
+
+		unsigned char byte = current | prev;
+		byte &= 0x7F;
+
+		result.append(1, byte);
+
+		if (shift == 6)
+			result.append(1, input.at(i) >> 1);
+
+		shift = (shift+1) % 7;
+
+	}
+
 	return result;
 }
 
+
+
+
 vector<unsigned char> Encode7to8bit(std::string str)
 {
 	vector<unsigned char> buf;
@@ -116,6 +142,12 @@
 
 	if (multipart)
 	{
+		if (message.length() > 800)
+		{
+			Common::instance()->logMessage("Trying to send multipart sms > 800 bytes !!!");
+			message = message.substr(0,800);
+		}
+
 		part_count = message.length() / PDU_LEN;
 		if (message.length() % PDU_LEN)
 			part_count++;
@@ -134,9 +166,10 @@
 		pdu.push_back( 0x01|UDHI ); // first octet -- no timeout
 		pdu.push_back(0x00); // TP-MR message reference
 		pdu.push_back(to.length() ); //length of phone nr
-		pdu.push_back(0x91); // type of address (international nr  + ISDN/telephone range) - else try 0x81
+		pdu.push_back(0x81); // type of address (international nr  + ISDN/telephone range) - else try 0x81
 
-		vector<unsigned char> phone = BcdEncode(to);
+		
+		string phone = EncodePhonenr(to);
 		pdu.insert( pdu.end(), phone.begin(), phone.end());
 
 		pdu.push_back(0x00); // Protocol identifier
@@ -148,18 +181,27 @@
 			message_part = message.substr(0, PDU_LEN);
 			message.erase(0, PDU_LEN);
 
-			pdu.push_back( message_part.length()+ 6 );  //UserDataLength
-			pdu.push_back( 0x05 ); // UDH Len
+			pdu.push_back( message_part.length()+ 7 );  //UserDataLength
+			pdu.push_back( 0x06 ); // UDH Len
 			pdu.push_back( 0x00 ); // UDH Element Identifier
 			pdu.push_back( 0x03 ); // UDH element length
 			pdu.push_back( csms_ref ); // csms_ref reference
 			pdu.push_back( part_count );
 			pdu.push_back( partnr+1 );
+			pdu.push_back( 0x00);
 
 		}
 		else
 		{
-			message_part = message.substr(0,160); //truncate to 160
+			if (message.length() > 160)
+			{
+				message_part = message.substr(0,160); //truncate to 160
+				Common::instance()->logMessage("Truncated message");
+			}
+			else
+			{
+				message_part = message;
+			}
 
 			pdu.push_back( message_part.length() ); //UserDataLength
 		}