--- smsdaemon/SmsPdu.cpp	2008/06/12 15:16:12	66
+++ smsdaemon/SmsPdu.cpp	2008/12/07 00:59:05	132
@@ -16,181 +16,317 @@
 
 using namespace std;
 
-
 namespace SmsPdu
 {
 
 
-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)
 	{
-		buf[0] = input.at(i);
-		unsigned char tmp = atoi(buf);
-
-		if ( (i%2) == 0)
-		{
-			current = tmp;
-		}
-		else
-		{
-			current |= (tmp<<4);
-			result.push_back(current);
+		for (unsigned int i=1; i<input.length(); i+=2)
+	{
+			char tmp = input[i];
+			input[i] = input[i-1];
+			input[i-1] = tmp;
 		}
+		return input;
 	}
 
-	if ((input.length() % 2) == 1)
+	string EncodePhonenr(string input)
 	{
-		current |= 0xF0;
-		result.push_back(current);
+		if ( input.at(0) == '+' )
+			input.erase(0,1);
+
+		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;
 
-string HexformatVector(vector<unsigned char> vec)
-{
-	ostringstream os;
+		if (input.at(last) == 'F')
+			input.erase(last,1);
+		return input;
+	}
 
-	for (unsigned int i=0; i<vec.size(); ++i)
+
+
+
+	string HexformatVector(vector<unsigned char> vec)
 	{
-		os.width(2);
-		os.fill('0');
-		os << hex << uppercase << static_cast<unsigned int>(vec.at(i));
-	}
+		ostringstream os;
 
+		for (unsigned int i=0; i<vec.size(); ++i)
+		{
+			os.width(2);
+			os.fill('0');
+			os << hex << uppercase << static_cast<unsigned int>(vec.at(i));
+		}
 
-	return os.str();
-}
 
-std::string Encode8to7bit(vector<unsigned char> vec)
-{
-	string result;
+		return os.str();
+	}
 
-	return result;
-}
+	vector<unsigned char> HexDecodeString(string str)
+	{
+		vector<unsigned char> vec;
+		for (unsigned int i=0; i<str.length(); i+=2)
+			vec.push_back( strtol( str.substr(i,2).c_str(), 0, 16 ));
 
-vector<unsigned char> Encode7to8bit(std::string str)
-{
-	vector<unsigned char> buf;
+		return vec;
+	}
 
-	int shift = 0;
-	for (unsigned int i=0; i<str.size(); ++i)
+	std::string Decode8to7bit(vector<unsigned char> input, int shift_start)
 	{
-		unsigned char current = str.at(i) & 0x7F;
-		unsigned char next = ( (i+1)<str.size()) ? str.at(i+1) : 0;
-		next &= 0x7F;
+		string result;
 
-		current >>= shift;
-		next <<= (7-shift);
+		int shift = shift_start;
+		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;
 
-		unsigned char byte = current | next;
-		buf.push_back(byte);
+			current <<= shift;
 
-		if (shift == 6)
-			i++;
+			prev >>= (8-shift);
 
-		shift = (shift+1) % 7;
+			unsigned char byte = current | prev;
+			byte &= 0x7F;
 
-	}
+			result.append(1, byte);
 
-	return buf;
-}
+			if (shift == 6)
+				result.append(1, input.at(i) >> 1);
 
-vector<PduInfo> CreateSmsPdu(string to, string message, bool allowMultipart)
-{
-	bool multipart = allowMultipart && message.length() > 160;
+			shift = (shift+1) % 7;
 
-	const unsigned char UDHI = multipart ? 0x40 : 0;
+		}
+
+		return result;
+	}
 
-	srand(time(0));
-	unsigned char csms_ref = rand() % 128;
 
-	int part_count;
 
-	const int PDU_LEN = 153;
 
-	if (multipart)
+	vector<unsigned char> Encode7to8bit(std::string str, int shift_start)
 	{
-		if (message.length() > 800)
+		vector<unsigned char> buf;
+
+		int shift = shift_start;
+		for (unsigned int i=0; i<str.size(); ++i)
 		{
-			Common::instance()->logMessage("Trying to send multipart sms > 800 bytes !!!");
-			message = message.substr(0,800);
+			unsigned char current = str.at(i) & 0x7F;
+			unsigned char next = ( (i+1)<str.size()) ? str.at(i+1) : 0;
+			next &= 0x7F;
+
+			current >>= shift;
+			next <<= (7-shift);
+
+			unsigned char byte = current | next;
+			buf.push_back(byte);
+
+			if (shift == 6)
+				i++;
+
+			shift = (shift+1) % 7;
+
 		}
 
-		part_count = message.length() / PDU_LEN;
-		if (message.length() % PDU_LEN)
-			part_count++;
-	}
-	else
-	{
-		part_count = 1;
+		return buf;
 	}
 
-	vector<PduInfo> result;
-	for (int partnr = 0; partnr < part_count; ++partnr)
+	vector<PduInfo> CreateSmsPdu(string to, string message, bool allowMultipart)
 	{
-		vector<unsigned char> pdu;
+		bool multipart = allowMultipart && message.length() > 160;
+
+		const unsigned char UDHI = multipart ? 0x40 : 0;
 
-		pdu.push_back(0x00); // use SMSC from phone
-		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(0x81); // type of address (international nr  + ISDN/telephone range) - else try 0x81
+		unsigned char csms_ref = rand() % 128;
 
-		vector<unsigned char> phone = BcdEncode(to);
-		pdu.insert( pdu.end(), phone.begin(), phone.end());
+		int part_count;
 
-		pdu.push_back(0x00); // Protocol identifier
-		pdu.push_back(0x00); // Data coding scheme 
+		const int PDU_LEN = 153;
 
-		string message_part;
 		if (multipart)
 		{
-			message_part = message.substr(0, PDU_LEN);
-			message.erase(0, PDU_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);
+			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++;
 		}
 		else
 		{
-			if (message.length() > 160)
+			part_count = 1;
+		}
+
+		vector<PduInfo> result;
+		for (int partnr = 0; partnr < part_count; ++partnr)
+		{
+			vector<unsigned char> pdu;
+
+			pdu.push_back(0x00); // use SMSC from phone
+			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(0x81); // type of address (international nr  + ISDN/telephone range) - else try 0x81
+
+
+			vector<unsigned char> phone = HexDecodeString( EncodePhonenr(to ));
+			pdu.insert( pdu.end(), phone.begin(), phone.end());
+
+			pdu.push_back(0x00); // Protocol identifier
+			pdu.push_back(0x00); // Data coding scheme 
+
+			int shift_start = 0;
+			string message_part;
+			if (multipart)
 			{
-				message_part = message.substr(0,160); //truncate to 160
-				Common::instance()->logMessage("Truncated message");
+				message_part = message.substr(0, PDU_LEN);
+				message.erase(0, PDU_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);
+				//shift_start = 6;
 			}
 			else
 			{
-				message_part = message;
+				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
 			}
 
-			pdu.push_back( message_part.length() ); //UserDataLength
+			vector<unsigned char> userData = Encode7to8bit(message_part, shift_start);
+
+			pdu.insert( pdu.end(), userData.begin(), userData.end());
+
+			PduInfo info;
+			info.len = pdu.size()-1;
+			info.pdu = HexformatVector(pdu);
+			result.push_back(info);
+
+		}
+		return result;
+	}
+
+	typedef vector<unsigned char>::iterator char_it;
+
+	string DecodeRawPhonenr(char_it start, char_it end)
+	{
+		ostringstream os;
+
+		for (char_it it = start; it != end; ++it)
+		{
+			os.width(2);
+			os.fill('0');
+			os << hex << (int)(*it);
+		}
+		return DecodePhonenr(os.str());
+	}
+
+	string DecodeTimestamp(char_it start, char_it end)
+	{
+		string stamp = DecodeRawPhonenr(start,end);
+
+		ostringstream os;
+
+		os << stamp.substr(0,2) << "/"; //year
+		os << stamp.substr(2,2) << "/"; //month
+		os << stamp.substr(4,2) << ","; //day
+		os << stamp.substr(6,2) << ":"; //hour
+		os << stamp.substr(8,2) << ":"; //minute
+		os << stamp.substr(10,2) ; //second
+
+		int timezone = strtol(stamp.substr(12,2).c_str(), 0, 16);
+
+
+		if (timezone > 127)
+			os << "-";
+		else
+			os << "+";
+
+		os.width(2);
+		os.fill('0');
+		os << (timezone & 0x7F);
+
+		return os.str();
+	}
+
+
+	SMS ParseSmsPdu(std::string pdu_str)
+	{
+		SMS result;
+
+		vector<unsigned char> pdu = HexDecodeString(pdu_str);
+
+		vector<unsigned char>::iterator it = pdu.begin();
+
+		it += (*it++); // Skip smsc info
+
+		unsigned char deliver_first_octet = (*it++);
+
+		bool UDHI = (deliver_first_octet & 0x40) > 0;
+
+		unsigned char sender_len = (*it++);
+		if ( (sender_len % 2) == 1)
+			sender_len++;
+
+		++it; //ignore Type-Of-Address
+
+		result.sender = DecodeRawPhonenr( it, it+(sender_len/2) );
+
+		it += (sender_len/2);
+		++it; //protocol identifier
+		++it; //Data encoding
+
+		result.timestamp = DecodeTimestamp(it, it+7);
+		it += 7;
+
+
+		unsigned char data_len = (*it++);
+
+		int shift_start = 0;
+		if (UDHI)
+		{
+			int udh_len = (*it++); 
+			it += udh_len; //just ignore the User Data Header
+			data_len -= udh_len;
+
+			shift_start = udh_len+1; //make the 8to7bit decode start with the right shift level
 		}
 
-		vector<unsigned char> userData = Encode7to8bit(message_part);
 
-		pdu.insert( pdu.end(), userData.begin(), userData.end());
+		vector<unsigned char> user_data;
+		user_data.insert(user_data.end(), it, it+data_len);
+		
+		result.message = Decode8to7bit(user_data, shift_start).substr(0,data_len);
 
-		PduInfo info;
-		info.len = pdu.size()-1;
-		info.pdu = HexformatVector(pdu);
-		result.push_back(info);
+		result.message = Util::str_trim(result.message);
+		
 
+		return result;
 	}
-	return result;
-}
 
 }