--- smsdaemon/SmsPdu.cpp	2008/06/12 15:16:12	66
+++ smsdaemon/SmsPdu.cpp	2008/06/12 18:54:24	68
@@ -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;
@@ -142,7 +168,8 @@
 		pdu.push_back(to.length() ); //length of phone nr
 		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