#include <stdio.h>
#include <errno.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>

extern int emsg(const char *format, ...);
extern int imsg(const char *format, ...);
extern int dmsg(const char *format, ...);

#include "lib.h"
#include "lprintf.h"
#ifdef REGANS_THREADS
#	include "../adt/thread.h"
#else
#	include "thread.h"
#endif

#include "comlib.h"
#include "sms_gsm.h"
#include "smslib.h"

#ifndef FALSE
#define FALSE 0
#endif

#ifndef TRUE
#define TRUE !FALSE
#endif

#ifdef WIN32
#	define vsnprintf _vsnprintf
#endif

#define CTRLZ 26
#define MAX_WAITING_LINES 500
#define MAX_STORED 100

char *pdu_mode_status[] = {
	"REC UNREAD","REC READ","STO UNSENT","STO SENT","ALL"
};

struct sms_data_gsm {
	Mutex *lock_queue;

	Mutex *lock_com;
	Comport *com;

	GsmMessage *stored[MAX_STORED];
	int nstored,donestored;	

	char *localphone;
	
	/*
	char *endl;
	int nendl;
	*/

	char *r_endl;
	char *w_endl;
	int r_nendl;
	int w_nendl;

	Thread *reader;	

	Mutex *lock_buffer;
	int bflen,bfsz;
	char *buffer;

	int ispdu;
	int usedcs;
	int ext7bit;
	int min_signal;
	int signal_timeout;
	int stopping;

	FILE *flog;
};

struct gsm_message {
	char memory[BFSZ];					/* only present if msg obtained by notification */

	int index;

	char status[BFSZ];
	char phone[BFSZ];
	char alpha[BFSZ];					/* optional, empty if not given */
	char timestamp[BFSZ];

	int type_of_originating_address;	/* optional, 0 if not given */
	int first_octet_of_sms;				/* optional, 0 if not given */
	int protocal_identifier;			/* optional, 0 if not given */
	int data_coding_sceme;				/* optional, 0 if not given */
	int message_reference;				/* only reqd for SMS-SUBMIT stored in CMGL */
	
	char service_center[BFSZ];			/* optional, empty if not given */
	int type_of_service_center;			/* optional, 0 if not given */

	int length;							/* optional, 0 if not given */
	
	char message[BFSZ];
};

int gsm_write_raw(SmsGsm *gsm, int *errcode, char *bf, int length);
int gsm_write(SmsGsm *gsm, int *errcode, char *format, ...);

enum {
	GRL_INCOMING	= 0x1,
	GRL_RAW			= 0x2,
	GRL_SKIP_BLANK	= 0x4,
	GRL_EXPECT_GT	= 0x8
};

int gsm_read_raw(SmsGsm *gsm, int *errcode, int type, long timeout, char *bf, int bfsz);
#define gsm_read_data(a,b,c,d)			gsm_read_raw(a,b,GRL_RAW,30000,c,d)

int gsm_read_line(SmsGsm *gsm, int *errcode, int type, long timeout, char *bf, int bfsz);
#define gsm_read(a,b,c,d)				gsm_read_line(a,b,GRL_SKIP_BLANK,30000,c,d)
#define gsm_read_incoming(a,b,c,d,e)	gsm_read_line(a,b,GRL_INCOMING|GRL_SKIP_BLANK,c,d,e)

#define gsm_iserror(x,y,z) gsm_iserrorx(x,y,z,NULL)
int gsm_iserrorx(SmsGsm *gsm, char *bf, int *errcode, int *iscme);

int gsm_clear_ring(SmsGsm *gsm, int *errcode);
int gsm_process_message_info(SmsGsm *gsm, int *errcode, GsmMessage *msg, char *info, int iscmgl);
int gsm_process_message_notify(SmsGsm *gsm, int *errcode, GsmMessage *msg, char *info);
int gsm_process_encoded(int ucs2, int ext7bit, char *message, int length, int max);
int gsm_create_encoded(int ucs2, int ext7bit, char *message, int length, int max);

int th_reader(SmsGsm *gsm) {
	char bf[BFSZ];
	int bytes;

	imsg("th_reader: %d",th_id());
	while(!gsm->stopping) {
		bytes = com_read(gsm->com,bf,BFSZ);
		if (bytes == -1) {
			emsg("Error reading from com, %d:%s",errno,strerror(errno));
			break;
		}
		if (bytes == 0) {
			lib_msleep(50);
			continue;
		}

		th_mutex_get_queue(gsm->lock_buffer);
		//dmsg("th_reader: %d bytes",bytes);
		if (gsm->bfsz < gsm->bflen+bytes+1) {
			//dmsg("th_reader: expand %d -> %d",gsm->bfsz,gsm->bflen+bytes);
			gsm->buffer = realloc(gsm->buffer,gsm->bflen+bytes+1);
			gsm->bfsz = gsm->bflen+bytes+1;
		}
		memcpy(&gsm->buffer[gsm->bflen],bf,bytes);
		gsm->buffer[gsm->bflen += bytes] = '\0';
		//dmsg("th_reader: ready %d bytes",gsm->bflen);
		th_mutex_release(gsm->lock_buffer);
	}
	dmsg("th_reader: exiting");
	return 0;
}
void pdu_encode_int(char *pdu, int v, int *i) {
	pdu[(*i)++] = "0123456789ABCDEF"[v / 16];
	pdu[(*i)++] = "0123456789ABCDEF"[v % 16];
}
int pdu_decode_int(char *pdu, int *i) {
	int v;
	
	v = char2byte(&pdu[(*i)]);
	(*i) += 2;

	return v;
}
void pdu_flip(char *pdu, char **dst, int *i) {
	*(*dst+1) = pdu[(*i)++];
	**dst = pdu[(*i)++];
	(*dst) += 2;
}
void pdu_encode_addr(char *addr, char *dst, int *i) {
	char *tmp,*ptr;
	int l;

	l = strlen(addr);
	tmp = malloc(l+2);	

	if (*addr != '+') strcpy(tmp,addr);
	else {
		strcpy(tmp,&addr[1]);
		l--;
	}

	if (l % 2) pdu_encode_int(dst,l+1,i);
	else pdu_encode_int(dst,l,i);	
	
	if (*addr != '+') 
		/* unknown number */
		pdu_encode_int(dst,0x81,i);
	else 
		/* international number */
		pdu_encode_int(dst,0x91,i);

	if (l % 2) {
		tmp[l++] = 'F';
		tmp[l] = 0;
	}

	for(l = 0, ptr = &dst[*i]; tmp[l]; )
		pdu_flip(tmp,&ptr,&l);
	
	*i += l;

	free(tmp);
}
void pdu_decode_addr_real(char *pdu, char *dst, int *i, int sca);
void pdu_decode_sc_addr(char *pdu, char *dst, int *i) {
	pdu_decode_addr_real(pdu,dst,i,TRUE);
}
void pdu_decode_addr(char *pdu, char *dst, int *i) {
	pdu_decode_addr_real(pdu,dst,i,FALSE);
}
void pdu_decode_addr_real(char *pdu, char *dst, int *i, int sca) {
	int length,type,start;

	length = pdu_decode_int(pdu,i);
	start = *i;
	type = pdu_decode_int(pdu,i);
	if (length == 0) return ;
	
	if (sca) length *= 2;	
	else start = *i;

	if (type & 0x90) *dst++ = '+';

	while((*i - start) < length)
		pdu_flip(pdu,&dst,i);

	if (dst[-1] == 'F') dst--;
	*dst = 0;
}
void pdu_decode_time(char *pdu, char *dst, int *i) {
	char *pold = dst;

	/* YEAR */
	pdu_flip(pdu,&dst,i); *dst++ = '/';
	/* MONTH */
	pdu_flip(pdu,&dst,i); *dst++ = '/';
	/* DAY */
	pdu_flip(pdu,&dst,i); *dst++ = ',';
	/* HOUR */
	pdu_flip(pdu,&dst,i); *dst++ = ':';
	/* MINUTE */
	pdu_flip(pdu,&dst,i); *dst++ = ':';
	/* SECCOND */
	pdu_flip(pdu,&dst,i); 	

	/* Look for sign */
	if (pdu[*i] & 0x80) *dst++ = '-';
	else *dst++ = '+';

	/* Mask it out */
	pdu[*i] = (pdu[*i] & 0x7F);

	pold = dst;

	/* TIMEZONE */
	pdu_flip(pdu,&dst,i);

	{
		int tz = atoi(pold);
		sprintf(pold,"%02d",tz/4);
	}
}

unsigned int seven_bit_to_unicode[] = {
0x0040,0x00A3,0x0024,0x00A5,0x00E8,0x00E9,0x00F9,0x00EC,0x00F2,0x00E7,0x000A,0x00D8,0x00F8,0x000D,0x00C5,0x00E5,
0x0394,0x005F,0x03A6,0x0393,0x039B,0x03A9,0x03A0,0x03A8,0x03A3,0x0398,0x039E,0x001B,0x00C6,0x00E6,0x00DF,0x00C9,
0x0020,0x0021,0x0022,0x0023,0x00A4,0x0025,0x0026,0x0027,0x0028,0x0029,0x002A,0x002B,0x002C,0x002D,0x002E,0x002F,
0x0030,0x0031,0x0032,0x0033,0x0034,0x0035,0x0036,0x0037,0x0038,0x0039,0x003A,0x003B,0x003C,0x003D,0x003E,0x003F,
0x00A1,0x0041,0x0042,0x0043,0x0044,0x0045,0x0046,0x0047,0x0048,0x0049,0x004A,0x004B,0x004C,0x004D,0x004E,0x004F,
0x0050,0x0051,0x0052,0x0053,0x0054,0x0055,0x0056,0x0057,0x0058,0x0059,0x005A,0x00C4,0x00D6,0x00D1,0x00DC,0x00A7,
0x00BF,0x0061,0x0062,0x0063,0x0064,0x0065,0x0066,0x0067,0x0068,0x0069,0x006A,0x006B,0x006C,0x006D,0x006E,0x006F,
0x0070,0x0071,0x0072,0x0073,0x0074,0x0075,0x0076,0x0077,0x0078,0x0079,0x007A,0x00E4,0x00F6,0x00F1,0x00FC,0x00E0,

0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x000A,0x0000,0x0000,0x0000,0x0000,0x0000,
0x0000,0x0000,0x0000,0x0000,0x005E,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x001B,0x0000,0x0000,0x0000,0x0000,
0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x007B,0x007D,0x0000,0x0000,0x0000,0x0000,0x0000,0x005C,
0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x005B,0x007E,0x005D,0x0000,
0x007C,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,
0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,
0x0000,0x0000,0x0000,0x0000,0x0000,0x20AC,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,
0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000
};

/* approx list:
0x00 == no conversion.
0x1B == excape char to seccond table (add 127 to lookup value on next char).
0x20 in position 0x1B on seccond table as space is used for unknown characters.
*/
unsigned char seven_bit_to_ascii[] = {
0x40,0xA3,0x24,0xA5,0xE8,0xE9,0xF9,0xEC,0xF2,0xE7,0x0A,0xD8,0xF8,0x0D,0xC5,0xE5,
0x7F,0x5F,0xE8,0xE2,0x5E,0xEA,0xE3,0xE7,0xE4,0xE9,0xF0,0x1B,0xC6,0xE6,0xDF,0xC9,
0x20,0x21,0x22,0x23,0xA4,0x25,0x26,0x27,0x28,0x29,0x2A,0x2B,0x2C,0x2D,0x2E,0x2F,
0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39,0x3A,0x3B,0x3C,0x3D,0x3E,0x3F,
0xA1,0x41,0x42,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,0x4B,0x4C,0x4D,0x4E,0x4F,
0x50,0x51,0x52,0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5A,0xC4,0xD6,0xD1,0xDC,0xA7,
0xBF,0x61,0x62,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,0x6B,0x6C,0x6D,0x6E,0x6F,
0x70,0x71,0x72,0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7A,0xE4,0xF6,0xF1,0xFC,0xE0,
/* seccond table */
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0A,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x5E,0x00,0x00,0x00,0x00,0x00,0x00,0x20,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7B,0x7D,0x00,0x00,0x00,0x00,0x00,0x5C,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x5B,0x7E,0x5D,0x00,
0x7C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
};

unsigned char ascii_to_seven_bit[] = {
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0A,0x00,0x00,0x0D,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1B,0x00,0x00,0x00,0x00,
0x20,0x21,0x22,0x23,0x02,0x25,0x26,0x27,0x28,0x29,0x2A,0x2B,0x2C,0x2D,0x2E,0x2F,
0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39,0x3A,0x3B,0x3C,0x3D,0x3E,0x3F,
0x00,0x41,0x42,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,0x4B,0x4C,0x4D,0x4E,0x4F,
0x50,0x51,0x52,0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5A,0xBC,0xAF,0xBE,0x14,0x11,
0x00,0x61,0x62,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,0x6B,0x6C,0x6D,0x6E,0x6F,
0x70,0x71,0x72,0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7A,0xA8,0xC0,0xA9,0xBD,0x10,

0xE5,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x40,0x00,0x01,0x24,0x03,0x00,0x5F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x60,
0x00,0x00,0x00,0x00,0x5B,0x0E,0x1C,0x00,0x00,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x5D,0x00,0x00,0x00,0x00,0x5C,0x00,0x0B,0x00,0x00,0x00,0x5E,0x00,0x00,0x1E,
0x7F,0x00,0x13,0x16,0x18,0x0F,0x1D,0x09,0x04,0x05,0x15,0x00,0x07,0x00,0x00,0x00,
0x1A,0x7D,0x08,0x00,0x00,0x00,0x7C,0x00,0x0C,0x06,0x00,0x00,0x7E,0x00,0x00,0x00
};

#define hextobin(c) ((c) <= '9'?(c)-'0':((c)&~0x20)-'a'+10)

int pdu_decode_content_16bit(char *pdu, char *dst, int *i) {
	char *p = &pdu[*i];
	int length = 0;

	dmsg("16bit decode, first 2 chars are: (%c%c)",*p,*(p+1)); p += 2;
	while(*p) {
		*dst = hextobin(*p) << 4; p++;
		*dst += hextobin(*p) << 4; p++;
		*dst += hextobin(*p) << 4; p++;
		*dst++ += hextobin(*p); p++;
		length++;
	}
	return length;
}
int pdu_decode_content_8bit(char *pdu, char *dst, int *i) {
	int length = 0;
	while(pdu[*i]) {
		*dst++ = pdu_decode_int(pdu,i);
		length++;
	}
	*dst = '\0';
	return length;
}
int pdu_decode_content_7bit(char *pdu, char *dst, int *i) {
	unsigned char v, tail, c;
	int octet, septets, off;
	int length;
	
	septets = pdu_decode_int(pdu,i);
	length = septets;

	octet = off = 0;
	while(septets > 0) {
		c = pdu_decode_int(pdu,i);
		switch(octet % 7) {
		case 0:
			v = (c&0x7F);
			tail = (c&0x80)>>7;
			break;
		case 1:
			v = (c&0x3F) << 1;
			v |= tail;
			tail = (c&0xC0) >> 6;
			break;
		case 2:
			v = (c&0x1F) << 2;
			v |= tail;
			tail = (c&0xE0) >> 5;
			break;
		case 3:
			v = (c&0x0F) << 3;
			v |= tail;
			tail = (c&0xF0) >> 4;
			break;
		case 4:
			v = (c&0x07) << 4;
			v |= tail;
			tail = (c&0xF8) >> 3;
			break;
		case 5:
			v = (c&0x03) << 5;
			v |= tail;
			tail = (c&0xFC) >> 2;
			break;
		case 6:
			v = (c&0x01) << 6;
			v |= tail;
			tail = (c&0xFE) >> 1;
			
			if (v == 0x1B) 
				off = 128;
			else {
				*dst++ = seven_bit_to_ascii[v+off];
				off = 0;
			}
			
			septets--;

			v = tail;
			tail = 0;
			break;
		}

		if (septets == 0) break;

		if (v == 0x1B) 
			off = 128;
		else {
			*dst++ = seven_bit_to_ascii[v+off];
			off = 0;
		}
		septets--;
		octet++;
	}
	*dst++ = 0;

	return length;
}
int pdu_decode_content(int dcs, char *pdu, char *dst, int *i) {
	if (dcs == 0x0) return pdu_decode_content_7bit(pdu,dst,i);
	else if (dcs == 0x4) return pdu_decode_content_8bit(pdu,dst,i);
	else if (dcs == 0x8) return pdu_decode_content_16bit(pdu,dst,i);
	emsg("Data Coding Sceme Unknown (%x)",dcs);
	return 0;
}
void pdu_encode_content_16bit(char *dst, char *content, int *i) {
	int l;

	l = strlen(content);
	if (l > 140) l = 140;

	pdu_encode_int(dst,l*2,i);

	while(*content && l--) {
		dst[(*i)++] = "0123456789ABCDEF"[(unsigned char)*content/4096];
		dst[(*i)++] = "0123456789ABCDEF"[(unsigned char)*content/256];
		dst[(*i)++] = "0123456789ABCDEF"[(unsigned char)*content/16];
		dst[(*i)++] = "0123456789ABCDEF"[(unsigned char)*content++%16];
	}
	dst[*i] = '\0';
}
void pdu_encode_content_8bit(char *dst, char *content, int *i) {
	int l;

	l = strlen(content);
	if (l > 140) l = 140;

	while(*content && l--)
		pdu_encode_int(dst,*content++,i);
}
void pdu_encode_content_7bit(char *dst, char *content, int *i) {
	unsigned char c,v,escape;
	int l,septet,j;

	l = strlen(content);
	if (l > 160) l = 160;
	else {
		for(j = 0; j < l; j++) {
			escape = content[j];
			if (ascii_to_seven_bit[escape] & 0x80)
				l++;
		}
	}

	pdu_encode_int(dst,l,i);
	escape = 0x80;
	septet = 0;

	while(l) {
		c = *content;
		if (ascii_to_seven_bit[c] & 0x80) {
			escape = ascii_to_seven_bit[c];
			escape -= 0x80;
			c = 0x1B;
			content++;
		}
		else {
			if (escape == 0x80) {
				c = (ascii_to_seven_bit[c] & 0x7F);
				content++;
			}
			else {
				c = escape;
				escape = 0x80;
			}
		}
		
		switch(septet % 8) {
		case 0:
			v = c;
			break;
		case 1:
			v |= ((c & 0x01) << 7);
			pdu_encode_int(dst,v,i);
			v = c >> 1;
			break;
		case 2:
			v |= ((c & 0x03) << 6);
			pdu_encode_int(dst,v,i);
			v = c >> 2;
			break;
		case 3:
			v |= ((c & 0x07) << 5);
			pdu_encode_int(dst,v,i);
			v = c >> 3;
			break;
		case 4:
			v |= ((c & 0x0F) << 4);
			pdu_encode_int(dst,v,i);
			v = c >> 4;
			break;
		case 5:
			v |= ((c & 0x1F) << 3);
			pdu_encode_int(dst,v,i);
			v = c >> 5;			
			break;
		case 6:
			v |= ((c & 0x3F) << 2);
			pdu_encode_int(dst,v,i);
			v = c >> 6;			
			break;
		case 7:
			v |= ((c & 0x7F) << 1);
			pdu_encode_int(dst,v,i);
			v = c >> 7;			
			break;
		};		
		septet++;
		l--;
	}
	if ((septet % 8) != 0)
		pdu_encode_int(dst,v,i);
	dst[*i] = '\0';
}
void pdu_encode_content(int dcs, char *dst, char *content, int *i) {
	if (dcs == 0x0) pdu_encode_content_7bit(dst,content,i);
	else if (dcs == 0x4) pdu_encode_content_8bit(dst,content,i);
	else if (dcs == 0x8) pdu_encode_content_16bit(dst,content,i);
	else emsg("Data Coding Sceme Unknown (%x)",dcs);
}
/*
06              <- sca length == 6
91              <- sca type   == 81 = national, 91 = international
5669050010      <- sca = "6596500001"

04              <- first octet == 04 = deliver, 11 = send

06              <- length of OA
99              <- type of OA == 81 = national, 91 = international
8888880000      <- OA

30105111209023  <- timestamp

0C              <- user data len

D4F29C0E6A97E7F3F0B90C  <- user data "Test message"
*/


/*
void pdu_decode_string(char *p, char *d, int l) {
	d[l] = '\0';
	while(l) {
		if (l%2 == 0) d[l-1] = p[l-2];
		else d[l-1] = p[l];
		l--;
	}
}
*/
int gsm_pdu_is_submit(GsmMessage *msg)
{
	return msg && (msg->first_octet_of_sms & 0x01);
}

int gsm_decode_pdu(GsmMessage *msg, char *pdu) {
	int i = 0;

	dmsg("Decode PDU [%s]",pdu);
	/* SERVICE CENTER ADDRESS */
	pdu_decode_sc_addr(pdu,msg->service_center,&i);
	dmsg("PDU service_center[%s]",msg->service_center);
	/* FIRST OCTET */
	msg->first_octet_of_sms = pdu_decode_int(pdu,&i);
	dmsg("PDU first_octet[%d]",msg->first_octet_of_sms);
	if (gsm_pdu_is_submit(msg)) {
		/* SMS-SUBMIT */

		/* MESSAGE REFERENCE */
		msg->message_reference = pdu_decode_int(pdu,&i);
		dmsg("PDU protocal_identifier[%d]",msg->protocal_identifier);

		/* DESTINATION ADDRESS */
		pdu_decode_addr(pdu,msg->phone,&i);
		dmsg("PDU destination phone[%s]",msg->phone);

		/* PROTOCOL ID */
		msg->protocal_identifier = pdu_decode_int(pdu,&i);
		dmsg("PDU protocal_identifier[%d]",msg->protocal_identifier);

		/* DATA CODING SCEME */
		msg->data_coding_sceme = pdu_decode_int(pdu,&i);
		dmsg("PDU data_coding_sceme[%d]",msg->data_coding_sceme);

		if (msg->first_octet_of_sms & 0x10) {
			/* VALIDITY PERIOD */
			dmsg("PDU has validity period");

			if (msg->first_octet_of_sms & 0x08) {
				/* semi-octet */
				pdu_decode_time(pdu,msg->timestamp,&i);
				dmsg("PDU validity period[%s]",msg->timestamp);
			}
			else {
				/* integer */
				pdu_decode_int(pdu,&i);
			}
		}

		/* CONTENT */
		msg->length = pdu_decode_content(msg->data_coding_sceme,pdu,msg->message,&i);
		dmsg("PDU content[%d][%s]",msg->length,msg->message);	
	}
	else {
		/* SMS-DELIVER */

		/* ORIGINATING ADDRESS */
		pdu_decode_addr(pdu,msg->phone,&i);
		dmsg("PDU originating phone[%s]",msg->phone);

		/* PROTOCOL ID */
		msg->protocal_identifier = pdu_decode_int(pdu,&i);
		dmsg("PDU protocal_identifier[%d]",msg->protocal_identifier);

		/* DATA CODING SCEME */
		msg->data_coding_sceme = pdu_decode_int(pdu,&i);
		dmsg("PDU data_coding_sceme[%d]",msg->data_coding_sceme);

		/* TIME */
		pdu_decode_time(pdu,msg->timestamp,&i);
		dmsg("PDU timestamp[%s]",msg->timestamp);

		/* CONTENT */
		msg->length = pdu_decode_content(msg->data_coding_sceme,pdu,msg->message,&i);
		dmsg("PDU content[%d][%s]",msg->length,msg->message);	
	}
	return TRUE;
}
int gsm_encode_pdu(int dcs, char *to, char *message, char *dst, int bfsz) {
	static int upto;
	int i = 0;

	/* SERVICE CENTER ADDRESS */
	pdu_encode_int(dst,0x0,&i);
	/* FIRST OCTET */
	pdu_encode_int(dst,0x1,&i);
	/* MESSAGE REFERENCE */
	pdu_encode_int(dst,upto++%256,&i);
	/* DESTINATION ADDRESS */
	pdu_encode_addr(to,dst,&i);
	/* PROTOCOL ID */
	pdu_encode_int(dst,0x0,&i);
	/* DATA CODING SCEME */
	pdu_encode_int(dst,dcs,&i);
	/* CONTENT */
	pdu_encode_content(dcs,dst,message,&i);

	return (i-1)/2;
}
int has_baud(char *p[], int n, int val) {
	int min,max,i;
	char *s;

	for(i = 0; i < n; i++) {
		s = strchr(p[i],'-');
		if (s) {
			min = atoi(p[i]); max = atoi(s+1);
			if (val >= min && val <= max) return TRUE;
		}
		if (atoi(p[i]) == val) return TRUE;
	}
	return FALSE;
}
char *make_bearer(char *cbst, int baud, char *bearer, int bfsz) {
	char *pp[20],*s;
	int speed,name,ce;
	int np,i;

	if (!bearer || !cbst) return NULL;
	*bearer = '\0';
	speed = name = ce = -1;

	//+CBST: (0-8,12,14,65,66,68,70,71,75),(0),(0-3)
	//+CBST: (0-7,65,66,68,70,71),0,(0-1)
	np = token_split(cbst,pp,"()",20);
	//if (np < 6) return NULL;

	for(i = 0; i < np; i++) {
		for(s = pp[i]; *s == ','; s++);
		if (!isdigit(*s)) continue;
		if (speed == -1) speed = *s-'0';
		else if (name == -1) name = *s-'0';
		else if (ce == -1) ce = *s-'0';
	}

	if (speed == -1 || name == -1 || ce == -1)
		return NULL;

	if (speed != 0) {
		char *sp[50];
		int ns;

		ns = token_split(pp[1],sp,",",50);
		switch(baud) {
		case CB_115200:
		case CB_57600:
		case CB_38400:
			if (has_baud(sp,ns,16)) { speed = 16; break; }
			break;
		case CB_19200:
			if (has_baud(sp,ns,15)) { speed = 15; break; }
			break;
		case CB_9600:
			if (has_baud(sp,ns,12)) { speed = 12; break; }
			if (has_baud(sp,ns,7))  { speed = 7; break; }
			break;
		case CB_4800:
			if (has_baud(sp,ns,6)) { speed = 6; break; }
			break;
		case CB_2400:
			if (has_baud(sp,ns,5)) { speed = 5; break; }
			if (has_baud(sp,ns,4)) { speed = 4; break; }
			break;
		case CB_1200:
			if (has_baud(sp,ns,3)) { speed = 3; break; }
			if (has_baud(sp,ns,2)) { speed = 2; break; }
			break;
		default: case CB_110: case CB_300: case CB_600:
			speed = 1;
			break;
		}
	}
	lprintf(bearer,bfsz,"AT+CBST=%d,%d,%d",speed,name,ce);
	return bearer;
}

char *make_cnmi(char *cnmi, char *res, int bfsz)
{
	
	char *pp[20];
	char *s,*n,*d;
	int i,o,np;
	int lo,hi;

	*res = 0;

	//+CNMI: (0-3),(0-3),(0-3),(0-2),(0,1)
	//+CNMI: (3),(0,1,2,3),(0,2),(0,1,2),(0)
	//+CNMI: (0-3),(0,1),(0,2,3),(0,2),(1)
	np = token_split(cnmi,pp,"()",20);
	for(i = o = 0; i < np; i++) {
		if (pp[i][0] == '+') continue;
		s = mystrtok(pp[i],",",&n);
		if (!s) continue;
		o++;
		if (o == 2) {
			for(; s; s = mystrtok(NULL,",",&n)) {
				d = strchr(s,'-');
				if (d) {
					*d++ = 0;
					lo = atoi(s);
					hi = atoi(d);
					if (lo <= 1 && hi >= 1) {
						s = "1";
						break;
					}
				}
				if (atoi(s) == 1) break;
			}
		}
		if (o==1) s = "1"; // chrisp hack so it always listens
		if (!s) return NULL; //failed mode not supported		
		if (res[0]) lpcat(res,bfsz,",%d",atoi(s));		
		else lprintf(res,bfsz,"%d",atoi(s));		
	}

	return res;
}

#define trimmed(d,s) ncpy(bf,s,BFSZ-1); d = strdup(strtrm(bf));

/* should NOT return NULL as struct is needed in order to find out what errcode means */
SmsGsm *sms_gsm_init(Sms *sms, GsmInit *init, int *errcode) {
	SmsGsm *gsm = calloc(1,sizeof(SmsGsm));
	char bf[BFSZ],tmp[BFSZ];
	int iscme,retry;
	char *p;
	int nloop = 0;
	gsm->com = com_new();

	dmsg("Opening %s (BAUD=%d)...",init->port,init->baud);
	if (!com_open(gsm->com,init->port,init->baud)) {
		if (errcode) *errcode = SMS_GSM_OPEN_ERROR;
		goto error;
	}
	
	if (init->debug_file) {
		dmsg("Com port debug file (%s)",init->debug_file);
		gsm->flog = fopen(init->debug_file,"a");
		if (!gsm->flog) emsg("Failed to append to '%s', %s",init->debug_file,strerror(errno));
		for(p = init->debug_file; *p; p++);
		for(; p != init->debug_file; p--) {
			if (issep(*p)) {
				p = init->debug_file;
				break;
			}
			if (*p == '.') break;
		}
		if (p == init->debug_file) lprintf(bf,BFSZ-1,"%s_raw",p-init->debug_file);
		else lprintf(bf,BFSZ-1,"%.*s_raw%s",(p-init->debug_file),init->debug_file,p);
		if (init->dodebug) com_debug(gsm->com,bf,init->crlf);
		else com_dump(gsm->com,bf);
	}

	/*
	if (init->crlf) {
		dmsg("Using (\\r\\n) as end of line");
		gsm->endl = strdup("\r\n");
	}
	else {
		dmsg("Using (\\n) as end of line");
		gsm->endl = strdup("\n");
	}
	gsm->nendl = strlen(gsm->endl);
	*/
	gsm->r_endl = "\r\n";
	gsm->r_nendl = 2;
	gsm->w_endl = "\r";
	gsm->w_nendl = 1;	

	if (init->localphone) {
		trimmed(gsm->localphone,init->localphone);
		dmsg("Modem No is (%s)",init->localphone);
	}
	else {
		if (errcode) *errcode = SMS_GSM_SETUP_ERROR;
		emsg("NO MODEM NUMBER!");
		goto error;
	}

	gsm->lock_com = th_mutex_new("USING_GSM_MODEM");
	gsm->lock_buffer = th_mutex_new("USING_READ_BUFFER");
	gsm->lock_queue = th_mutex_new("GSM_SEND_QUEUE");

	gsm->reader = th_new(th_reader,gsm);

	if (init->init_string) {
		/* Initialize auto-baud */
		if (!gsm_write(gsm,errcode,"%s%s",init->init_string,gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (strcmp(bf,init->init_string)==0 && !gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	}

	/* Turn off echo */
again:
	if (!gsm_write(gsm,errcode,"ATE0%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (strcmp(bf,"ATE0")==0 && !gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (strcmp(bf,"OK") != 0) {
		if (strstr(bf,"515")!=NULL && nloop<200) {
			nloop++;
			emsg("Waiting for modem to initialize %s\n",bf);
			lib_msleep(5000);
			goto again;
		}
		if (errcode) *errcode = SMS_GSM_ECHO_ERROR;
		goto error;
	}

	/* query "line termination character" */
	if (!gsm_write(gsm,errcode,"ATS3?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

	/* query "response formatting character" */
	if (!gsm_write(gsm,errcode,"ATS4?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

	/* query error codes */
	if (!gsm_write(gsm,errcode,"AT+CMEE=?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (!gsm_iserror(gsm,bf,errcode)) {
		/* +CMEE: (0,1) (or unverified) +CMEE: (0,1,2) (or unverified) +CMEE: (0-2) 
		we might want to use level 2 if possible */
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}
	/* Set error codes ON */
	if (!gsm_write(gsm,errcode,"AT+CMEE=1%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode))
		emsg("CMEE=1 failed, modem is non compliant, continue anyway.");

	/* query Bearer type */
	if (!gsm_write(gsm,errcode,"AT+CBST=?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
// don't care if this fails, chrisp...
	if (gsm_iserror(gsm,bf,errcode)) goto error;
	if (init->cbst) ncpy(tmp,init->cbst,BFSZ-1);
	else if (!make_bearer(bf,init->baud,tmp,BFSZ)) {
		emsg("Failed to make bearer from (%s)",bf);
		goto error;
	}
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;

	/* set Bearer type */
	if (!gsm_write(gsm,errcode,"%s%s",tmp,gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	
	// Chrisp, don't care about response to this, lets hope it works anyway.
	// if (gsm_iserror(gsm,bf,errcode)) goto error;

	/* Bearer type
	if (init->mode & MODE_CBST_ONE) {
		if (!gsm_write(gsm,errcode,"AT+CBST=0,0,1%s",gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

		if (!gsm_write(gsm,errcode,"AT+CBST=7,0,1%s",gsm->w_endl)) goto error;
	}
	else {
		if (!gsm_write(gsm,errcode,"AT+CBST=0,0,0%s",gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

		if (!gsm_write(gsm,errcode,"AT+CBST=7,0,0%s",gsm->w_endl)) goto error;
	}
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;
	*/

	/* Check if we need a PIN number */
	if (!gsm_write(gsm,errcode,"AT+CPIN?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserrorx(gsm,bf,errcode,&iscme)) {
		if (iscme && (*errcode == 3 || *errcode == 4)) ;
		else if (*errcode != SMS_GSM_MISC_ERROR) {
			if (iscme) (*errcode) += 300;
			goto error;
		}
		*errcode = 0;
	}
	
	if (strcmp(&bf[7],"READY")==0 || iscme || strcmp(bf,"ERROR") == 0) {
		dmsg("No PIN required or CPIN not understood, continue...");
		/* +CPIN: READY */
	}
	else if (strncmp(&bf[7],"SIM PIN",7) == 0) {
		/* +CPIN: SIM PIN */
		emsg("PIN required (%s)",bf);
		if(!init->pin) {
			if (errcode) *errcode = SMS_GSM_PIN_ERROR;
			emsg("gsm_pin REQUIRED");
			goto error;
		}
		if (!gsm_write(gsm,errcode,"AT+CPIN=%s%s",init->pin,gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}
	else if (strncmp(&bf[7],"SIM PUK",7) == 0) {
		/* +CPIN: SIM PUK */
		emsg("PIN/PUK required (%s)",bf);
		if(!init->pin) {
			if (errcode) *errcode = SMS_GSM_PIN_ERROR;
			emsg("gsm_pin REQUIRED");
			goto error;
		}
		else if (!init->puk) {
			if (errcode) *errcode = SMS_GSM_PIN_ERROR;
			emsg("gsm_puk REQUIRED");
			goto error;
		}
		if (!gsm_write(gsm,errcode,"AT+CPIN=%s,%s%s",init->puk,init->pin,gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}
	else if (strncmp(&bf[7],"SIM PIN2",8) == 0) {
		/* +CPIN: SIM PIN2 */
		dmsg("PIN2 required (%s)",bf);
		if(!init->pin2) {
			if (errcode) *errcode = SMS_GSM_PIN_ERROR;
			emsg("gsm_pin2 REQUIRED");
			goto error;
		}
		if (!gsm_write(gsm,errcode,"AT+CPIN2=%s%s",init->pin2,gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}
	else if (strncmp(&bf[7],"SIM PUK2",8) == 0) {
		/* +CPIN: SIM PUK2 */
		dmsg("PUK2 required (%s)",bf);
		if(!init->pin2) {
			if (errcode) *errcode = SMS_GSM_PIN_ERROR;
			emsg("gsm_pin2 REQUIRED");
			goto error;
		}
		else if (!init->puk2) {
			if (errcode) *errcode = SMS_GSM_PIN_ERROR;
			emsg("gsm_puk2 REQUIRED");
			goto error;
		}
		if (!gsm_write(gsm,errcode,"AT+CPIN2=%s,%s%s",init->puk2,init->pin2,gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}
	else {
		/*
		PH-SIM PIN
		PH-FSIM PIN
		PH-FSIM PUK		
		PH-NET PIN
		PH-NET PUK
		PH-NETSUB PIN
		PH-NETSUB PUK
		PH-SP PIN
		PH-SP PUK
		PH-CORP PIN
		PH-CORP PUK
		*/
		emsg("Unsupported PIN check response (%s), continue anyway...",bf);		
	}

	if (strcmp(bf,"ERROR") != 0 && init->mode & MODE_CPIN_OK) {
		dmsg("cpin_ok true, reading..");
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}

	for(retry = 0; retry < 25; retry++) {
		/* Query SMS mode */
		if (!gsm_write(gsm,errcode,"AT+CSMS=?%s",gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (!gsm_iserror(gsm,bf,errcode)) break;;
		/* +CSMS: (0,1) */
		imsg("AT+CSMS=? (%d) error: %s",retry,bf);
		lib_msleep(500);
	}
	if (retry == 25) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

	for(retry = 0; retry < 25; retry++) {
		/* Check if it can handle SMS */
		if (!gsm_write(gsm,errcode,"AT+CSMS=0%s",gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (!gsm_iserror(gsm,bf,errcode)) break;
		imsg("AT+CSMS=0 (%d) error: %s",retry,bf);
		lib_msleep(500);
	}
	if (retry == 25) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

	if (init->mode & MODE_8BIT) gsm->usedcs = 0x4;
	else if (init->mode & MODE_16BIT) gsm->usedcs = 0x8;
	else gsm->usedcs = 0x0;

	if (init->mode & MODE_EXTENDED)
		gsm->ext7bit = TRUE;

	/* Query character set */
	if (!gsm_write(gsm,errcode,"AT+CSCS=?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;
	/* +CSCS: ("GSM","PCCP437") */
	if (init->dcs) {
		if (!strstr(bf,init->dcs)) {
			emsg("Character set (%s) is unsupported (%s)",init->dcs,bf);
			init->dcs = NULL;
		}
	}
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;

	/* query service centre address (SMSC) */
	if (!gsm_write(gsm,errcode,"AT+CSCA?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) imsg("No Service Centre address (%s)",bf);
	else {
		dmsg("Current Service Centre address (%s)",bf);
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	}

	/* set service centre address (SMSC) */
	if (init->sca) {
		dmsg("Setting Service Centre Address (%s)",init->sca);
		if (!gsm_write(gsm,errcode,"AT+CSCA=%s%s",init->sca,gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}

	/* query available modes */
	if (!gsm_write(gsm,errcode,"AT+CMGF=?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;

	if (init->mode & MODE_PDU) {
		/* Set pdu mode */
		if (!gsm_write(gsm,errcode,"AT+CMGF=0%s",gsm->w_endl)) goto error;
		gsm->ispdu = TRUE;
	}
	else {
		/* Set text mode */
		if (!gsm_write(gsm,errcode,"AT+CMGF=1%s",gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;

		/* Set character set */
		if (init->dcs) {
			if (!gsm_write(gsm,errcode,"AT+CSCS=\"%s\"%s",init->dcs,gsm->w_endl)) goto error;
		}
		else if (gsm->usedcs == 0x8) {
			if (!gsm_write(gsm,errcode,"AT+CSCS=\"UCS2\"%s",gsm->w_endl)) goto error;
		}
		else {
			if (!gsm_write(gsm,errcode,"AT+CSCS=\"GSM\"%s",gsm->w_endl)) goto error;
		}
	}
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) {
		// Chrisp change, continue if this fails as doesn't seem critical
		imsg("Failed to set character set, probably not critical");
		// goto error;
	}

	/* Query notification of new messages */
	if (!gsm_write(gsm,errcode,"AT+CNMI=?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) {
		imsg("AT+CNMI=? failed (%s) query current mode..");
		if (!gsm_write(gsm,errcode,"AT+CNMI?%s",gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) {
			lprintf(tmp,BFSZ,"1,1,0,0,1");
			imsg("AT+CNMI? failed (%s) lets guess (%s)",bf,tmp);
		}
		else {
			for (p = strchr(bf,':'); p && isspace(*p); p++) {}
			lprintf(tmp,BFSZ,"%s",p);
			imsg("AT+CNMI? result (%s) lets try (%s)",bf,tmp);
			//read OK
			if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
			if (gsm_iserror(gsm,bf,errcode)) goto error;
		}
	}
	else {
		/* +CNMI: (0-3),(0-3),(0-3),(0,1),(0,1) */
		if (!make_cnmi(bf,tmp,BFSZ)) {
			emsg("Failed to make CNMI from (%s)",bf);
			goto error;
		}
		//read OK
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}
	/* Enable notification of new messages */	
	if (!gsm_write(gsm,errcode,"AT+CNMI=%s%s",tmp,gsm->w_endl)) goto error;
	/*
	if (init->mode & MODE_CNMI_FLUSH) {
		if (!gsm_write(gsm,errcode,"AT+CNMI=1,1,0,0,0%s",gsm->w_endl)) goto error;
	}
	else {
		if (!gsm_write(gsm,errcode,"AT+CNMI=1,1,0,0,1%s",gsm->w_endl)) goto error;
	}*/
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;
	
	/* check signal */
	if (!gsm_write(gsm,errcode,"AT+CSQ=?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;
	dmsg("Signal range (%s)",bf);
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

	if (!gsm_write(gsm,errcode,"AT+CSQ%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;
	dmsg("Signal level now (%s)",bf);
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

	gsm->min_signal = init->min_signal;
	gsm->signal_timeout = init->signal_timeout;

	/*
	if (!gsm_write(gsm,errcode,"AT+CSDH?%s",gsm->w_endl)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,errcode)) goto error;
	if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
	*/

	if (!(init->mode & MODE_PDU)) {
		/* Query text mode parameters */
		if (!gsm_write(gsm,errcode,"AT+CSDH=?%s",gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;

		/* Show text mode parameters */
		if (!gsm_write(gsm,errcode,"AT+CSDH=1%s",gsm->w_endl)) goto error;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;
	}

	return gsm;
error:
	if (*errcode == SMS_NO_ERROR) *errcode = SMS_GSM_PROTO_ERROR;
	emsg("Error with %s init, cleanup...",init->port);
	if (gsm->reader) {
		gsm->stopping = TRUE;
		lib_msleep(200);
		th_kill(gsm->reader,0);
		th_free(&gsm->reader);
	}
	com_close(gsm->com);
	com_free(&gsm->com);

	/* Still return it, as we need it to get 
	the error message assoc with this failure */
	return gsm;
}

void sms_gsm_free(SmsGsm **gsm) {
	if (gsm && *gsm) {
		if ((*gsm)->reader) {
			(*gsm)->stopping = TRUE;
			lib_msleep(200);
			th_kill((*gsm)->reader,0);
			th_free(&(*gsm)->reader);
		}

		if ((*gsm)->lock_com) th_mutex_free((*gsm)->lock_com);
		if ((*gsm)->lock_buffer) th_mutex_free((*gsm)->lock_buffer);
		if ((*gsm)->lock_queue) th_mutex_free((*gsm)->lock_queue);

		com_free(&(*gsm)->com);
		free((*gsm)->localphone);

		free(*gsm);
		*gsm = NULL;
	}
}

GsmMessage *sms_gsm_wait(SmsGsm *gsm, long timeout_msec, int *errcode) {
	GsmMessage *msg = NULL;
	char bf[BFSZ],*ptr;

	if (!gsm->donestored) {
		//dmsg("sms_gsm_wait: checking for stored messages");
		th_mutex_get_queue(gsm->lock_com);

		/* Ask for all stored messages */
		if (gsm->ispdu) {
			if (!gsm_write(gsm,errcode,"AT+CMGL=4%s",gsm->w_endl)) goto error;
		}
		else {
			if (!gsm_write(gsm,errcode,"AT+CMGL=\"ALL\"%s",gsm->w_endl)) goto error;
		}
		if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		if (gsm_iserror(gsm,bf,errcode)) goto error;

		while(TRUE) {
			if (strcmp(bf,"OK") == 0) break;

			gsm->stored[gsm->nstored] = calloc(1,sizeof(GsmMessage));
			gsm->stored[gsm->nstored]->index = atoi(&bf[7]);

			/* Read msg info */
			for(ptr = &bf[7]; *ptr && *ptr != ','; ptr++);
			if (!gsm_process_message_info(gsm,errcode,gsm->stored[gsm->nstored],ptr+1,TRUE)) 
				emsg("Didn't like the message line (%s)",bf);
			
			ptr = bf;
			if (gsm->ispdu) {
				/* read Service Center ph number length */				
				if (gsm_read_data(gsm,errcode,bf,2) != 2) goto error;				
				bf[2] = 0;
				gsm->stored[gsm->nstored]->length += (atoi(bf) * 2);
				dmsg("PDU length (including SCA) (%d)",gsm->stored[gsm->nstored]->length);
				ptr = &bf[2];
			}

			/* Read message text/pdu */
			if (gsm_read_data(gsm,errcode,ptr,gsm->stored[gsm->nstored]->length) != gsm->stored[gsm->nstored]->length) goto error;
			ptr[gsm->stored[gsm->nstored]->length] = '\0';
			//stop using ptr here, use bf instead

			if (gsm->ispdu) {
				if (!gsm_decode_pdu(gsm->stored[gsm->nstored],bf))
					emsg("Could not decode PDU (%s)",bf);

				/* skip the stored sent message */
				if (gsm_pdu_is_submit(gsm->stored[gsm->nstored])) {
					dmsg("Skipping stored PDU it's not an incoming message");
					if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
					continue;
				}
			}
			else {
				gsm->stored[gsm->nstored]->length = gsm_process_encoded((gsm->usedcs==0x8),gsm->ext7bit,bf,gsm->stored[gsm->nstored]->length,BFSZ);
				ncpy(gsm->stored[gsm->nstored]->message,bf,BFSZ-1);
			}
			gsm->nstored++;

			if (!gsm_read(gsm,errcode,bf,BFSZ)) goto error;
		}
		gsm->donestored = TRUE;
		//dmsg("sms_gsm_wait: stored messages done");
error:
		th_mutex_release(gsm->lock_com);
		//dmsg("sms_gsm_wait: stored message check complete");
	}

	if (gsm->nstored) {
		//dmsg("sms_gsm_wait: we have a stored message");
		msg = gsm->stored[--gsm->nstored];
		gsm->stored[gsm->nstored] = NULL;

		th_mutex_get_queue(gsm->lock_com);
		/* Delete incoming/stored message*/
		if (gsm_write(gsm,errcode,"AT+CMGD=%d%s",msg->index,gsm->w_endl)) {
			if (gsm_read(gsm,errcode,bf,BFSZ))
				gsm_iserror(gsm,bf,errcode);
		}
		th_mutex_release(gsm->lock_com);
		//dmsg("sms_gsm_wait: return stored message");
		return msg;
	}

	while (TRUE) {
		//dmsg("sms_gsm_wait: read_incoming, get mutex");
		if (th_mutex_wait(gsm->lock_com,timeout_msec)) {
			//dmsg("sms_gsm_wait: read_incoming, got mutex");
			if (!gsm_read_incoming(gsm,errcode,timeout_msec,bf,BFSZ)) {
				//dmsg("sms_gsm_wait: read_incoming, release mutex");
				th_mutex_release(gsm->lock_com);
				//dmsg("sms_gsm_wait: read_incoming, none");
				break;
			}
			//dmsg("sms_gsm_wait: read_incoming, release mutex");
			th_mutex_release(gsm->lock_com);
			//dmsg("sms_gsm_wait: incoming message(%s)",bf);
			msg = calloc(1,sizeof(GsmMessage));
			if (!gsm_process_message_notify(gsm,errcode,msg,&bf[7])) {
				free(&msg);
				msg = NULL;
				continue;
			}
		}
		break;
	}

	return msg;
}

int sms_gsm_send(SmsGsm *gsm, void *data, char *from, char *to, char *message) {
	int errcode = SMS_NO_ERROR,len;
	char bf[BFSZ],msg[BFSZ];

	dmsg("Sending GSM, from=(%s) to=(%s) message=(%.10s...) aquire lock...",from,to,message);
	th_mutex_get_queue(gsm->lock_com);
	dmsg("Sending GSM, lock aquired");

	if (gsm->min_signal) {
		int signal,r;

		for(r = 0; ; r++) {
			if (gsm->signal_timeout && r == gsm->signal_timeout) break;

			if (!gsm_write(gsm,&errcode,"AT+CSQ%s",gsm->w_endl)) goto error;
			if (!gsm_read(gsm,&errcode,bf,BFSZ)) goto error;
			if (gsm_iserror(gsm,bf,&errcode)) goto error;
			//+CSQ: 17,3
			signal = atoi(&bf[6]);
			if (!gsm_read(gsm,&errcode,bf,BFSZ)) goto error;

			if (signal >= gsm->min_signal) break;
			dmsg("Signal strength (%d) is too weak (< %d)",signal,gsm->min_signal);
			lib_msleep(1000);
		}
		dmsg("Waited %d secs for signal to improve",r);
		if (r == gsm->signal_timeout) {
			emsg("Signal strength (%d) is too weak (< %d)",signal,gsm->min_signal);
			goto error;
		}
		imsg("Signal strength (%d) is ok (> %d)",signal,gsm->min_signal);
	}

	if (gsm->ispdu) {
		len = gsm_encode_pdu(gsm->usedcs,to,message,msg,BFSZ);
		if (!gsm_write(gsm,&errcode,"AT+CMGS=%d%s",len,gsm->w_endl)) goto error;
		len = len*2+2;
	}
	else {
		// 'from' is ignored cos it's always the same for a gsm modem
		if (gsm->usedcs==0x8) {
			ncpy(bf,to,BFSZ-1);
			gsm_create_encoded(TRUE,gsm->ext7bit,bf,strlen(bf),BFSZ);
			to = bf;
		}
		if (!gsm_write(gsm,&errcode,"AT+CMGS=\"%s\"%s",to,gsm->w_endl)) goto error;
		ncpy(msg,message,BFSZ-1); len = strlen(msg);
		len = gsm_create_encoded((gsm->usedcs==0x8),gsm->ext7bit,msg,len,140);
		//while(len > 139 || msg[len-1] == 0x1B) len--;
	}
	msg[len++] = CTRLZ;

	// expect blank line 
	gsm_read_line(gsm,&errcode,0,30000,bf,BFSZ);
	if (errcode != SMS_NO_ERROR) goto error;
	// expect "> " without newline(s)
	if (!gsm_read_raw(gsm,&errcode,GRL_RAW|GRL_EXPECT_GT,30000,bf,2)) goto error;
	//if (gsm_read_data(gsm,&errcode,bf,2) != 2) goto error;
	bf[2] = '\0';
	if (strcmp(bf,"> ")!=0) {
		if (!gsm_read(gsm,&errcode,&bf[2],BFSZ-2)) goto error;
		if (gsm_iserror(gsm,bf,&errcode)) goto error;
		errcode = SMS_GSM_PROTO_ERROR;
		goto error;
	}

	if (!gsm_write_raw(gsm,&errcode,msg,len)) goto error;
	if (!gsm_read(gsm,&errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,&errcode)) goto error;
	if (!gsm_read(gsm,&errcode,bf,BFSZ)) goto error;	
	dmsg("Sending GSM, sent");
error:
	th_mutex_release(gsm->lock_com);
	dmsg("Sending GSM, end");
	return errcode;
}
// CHRISP, IT IS IN THIS FUNCTION.  
int sms_gsm_recv(SmsGsm *gsm, GsmMessage *msg) {
	int errcode = SMS_NO_ERROR;
	char bf[BFSZ];

	/* if doesn't have memory specified it was stored and has already been read*/
	if (!msg->memory[0]) return errcode;

	th_mutex_get_queue(gsm->lock_com);

	/* Read incoming message*/
	if (!gsm_write(gsm,&errcode,"AT+CMGR=%d%s",msg->index,gsm->w_endl)) goto error;	
	if (!gsm_read(gsm,&errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,&errcode)) goto error;
	if (!gsm_process_message_info(gsm,&errcode,msg,&bf[7],FALSE)) goto error;

	if (gsm_read_data(gsm,&errcode,bf,msg->length) != msg->length) goto error;
	bf[msg->length] = '\0';

	if (gsm->ispdu) {
		if (!gsm_decode_pdu(msg,bf))
			emsg("Could not decode PDU (%s)",bf);
	}
	else {
		msg->length = gsm_process_encoded((gsm->usedcs==0x8),gsm->ext7bit,bf,msg->length,BFSZ);
		ncpy(msg->message,bf,BFSZ-1);
	}

	if (!gsm_read(gsm,&errcode,bf,BFSZ)) goto error;
	if (strcmp(bf,"OK")!=0) emsg("Expcted 'OK' got '%s'",bf);

	/* Delete incoming/stored message*/
	if (!gsm_write(gsm,&errcode,"AT+CMGD=%d%s",msg->index,gsm->w_endl)) goto error;
	if (!gsm_read(gsm,&errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,&errcode)) goto error;

error:
	th_mutex_release(gsm->lock_com);
	return errcode;
}
int sms_gsm_accept(SmsGsm *gsm, GsmMessage *msg, int code, char *reason) {
	return SMS_NO_ERROR;
}
int sms_gsm_reject(SmsGsm *gsm, GsmMessage *msg, int code, char *reason) {
	int errcode = SMS_NO_ERROR;
	char bf[BFSZ];

	th_mutex_get_queue(gsm->lock_com);

	/* Delete incoming/stored message*/
	if (!gsm_write(gsm,&errcode,"AT+CMGD=%d%s",msg->index,gsm->w_endl)) goto error;
	if (!gsm_read(gsm,&errcode,bf,BFSZ)) goto error;
	if (gsm_iserror(gsm,bf,&errcode)) goto error;

error:
	th_mutex_release(gsm->lock_com);
	return errcode;
}

char *sms_gsm_error(SmsGsm *gsm, int errcode, char *bf, int bfsz) {
	char tmp[BFSZ];
	int serr;

	if (!bf) return "";
	bf[0] = '\0';

	if (errcode == SMS_NO_ERROR) return bf;	

#ifdef WIN32
	serr = GetLastError();
#else
	serr = errno;
#endif
	if (errcode == SMS_GSM_OPEN_ERROR) lprintf(bf,bfsz-1,"Error opening. OSError(%s)",system_error(serr,tmp,BFSZ));
	else if (errcode == SMS_GSM_SETUP_ERROR) lprintf(bf,bfsz-1,"Error setting up. OSError(%s)",system_error(serr,tmp,BFSZ));
	else if (errcode == SMS_GSM_READ_ERROR) lprintf(bf,bfsz-1,"Error reading. OSError(%s)",system_error(serr,tmp,BFSZ));
	else if (errcode == SMS_GSM_WRITE_ERROR) lprintf(bf,bfsz-1,"Error writing. OSError(%s)",system_error(serr,tmp,BFSZ));
	else if (errcode == SMS_GSM_MISC_ERROR) lprintf(bf,bfsz-1,"Error without error code. OSError(%s)",system_error(serr,tmp,BFSZ));
	else if (errcode == SMS_GSM_PROTO_ERROR) lprintf(bf,bfsz-1,"Error unexpected line. OSError(%s)",system_error(serr,tmp,BFSZ));
	else if (errcode == SMS_GSM_ECHO_ERROR) lprintf(bf,bfsz-1,"Error with ATEO (Echo). OSError(%s)",system_error(serr,tmp,BFSZ));
	else if (errcode == SMS_GSM_PIN_ERROR) lprintf(bf,bfsz-1,"Error authenticating with phone, pin required. Check smsgate.log for ERROR noting setting required.");
	else {
		errcode -= 1;

		if (errcode >= 0 && errcode <= 127) {
			/*0...127 GSM 04.11 Annex E-2 values*/
			if (errcode==0) lprintf(bf,bfsz-1,"[%d] unspecified GSM radio link layer error",errcode);
			else if (errcode==1) lprintf(bf,bfsz-1,"[%d] unassigned number",errcode);
			else if (errcode==2) lprintf(bf,bfsz-1,"[%d] operator determined barring",errcode);
			else if (errcode==3) lprintf(bf,bfsz-1,"[%d] call barred",errcode);
			else if (errcode==4) lprintf(bf,bfsz-1,"[%d] CUG reject",errcode);
			else if (errcode==5) lprintf(bf,bfsz-1,"[%d] transfer rejected",errcode);
			else if (errcode==6) lprintf(bf,bfsz-1,"[%d] destination out of service",errcode);
			else if (errcode==7) lprintf(bf,bfsz-1,"[%d] unidentified subscriber",errcode);
			else if (errcode==8) lprintf(bf,bfsz-1,"[%d] facility rejected",errcode);
			else if (errcode==9) lprintf(bf,bfsz-1,"[%d] unknown subscriber",errcode);
			else if (errcode==10) lprintf(bf,bfsz-1,"[%d] network out of order",errcode);
			else if (errcode==11) lprintf(bf,bfsz-1,"[%d] temporary failure",errcode);
			else if (errcode==12) lprintf(bf,bfsz-1,"[%d] congestion",errcode);
			else if (errcode==13) lprintf(bf,bfsz-1,"[%d] resources unavailable, unspecified",errcode);
			else if (errcode==14) lprintf(bf,bfsz-1,"[%d] requested facility not implemented",errcode);
			else if (errcode==15) lprintf(bf,bfsz-1,"[%d] invalid SM transfer reference value",errcode);
			else if (errcode==16) lprintf(bf,bfsz-1,"[%d] invalid message, unspecified",errcode);
			else if (errcode==17) lprintf(bf,bfsz-1,"[%d] mandatory information element missing",errcode);
			else if (errcode==18) lprintf(bf,bfsz-1,"[%d] message type non-existent or not implemented ",errcode);
			else if (errcode==19) lprintf(bf,bfsz-1,"[%d] SM not compatible with SM transfer state or message type non-existent or not implemented",errcode);
			else if (errcode==20) lprintf(bf,bfsz-1,"[%d] information element non-existent or not implemented",errcode);
			else if (errcode==21) lprintf(bf,bfsz-1,"[%d] invalid information element contents",errcode);
			else if (errcode==22) lprintf(bf,bfsz-1,"[%d] SM not compatible with SM transfer state",errcode);
			else if (errcode==23) lprintf(bf,bfsz-1,"[%d] protocol error, unspecified",errcode);
			else if (errcode==24) lprintf(bf,bfsz-1,"[%d] interworking failure, unspecified",errcode);
			else if (errcode>=25 && errcode<=49) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==50) lprintf(bf,bfsz-1,"[%d] unspecified GSM transport layer error",errcode);
			else if (errcode==51) lprintf(bf,bfsz-1,"[%d] telematic interworking not supported",errcode);
			else if (errcode==52) lprintf(bf,bfsz-1,"[%d] short message Type 0 not supported",errcode);
			else if (errcode==53) lprintf(bf,bfsz-1,"[%d] cannot replace short message",errcode);
			else if (errcode>=54 && errcode<=65) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==66) lprintf(bf,bfsz-1,"[%d] unspecified TP-PID error",errcode);
			else if (errcode==67) lprintf(bf,bfsz-1,"[%d] data coding scheme not supported",errcode);
			else if (errcode==68) lprintf(bf,bfsz-1,"[%d] message class not supported",errcode);
			else if (errcode>=69 && errcode<=81) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==82) lprintf(bf,bfsz-1,"[%d] unspecified TP-DCS error",errcode);
			else if (errcode==83) lprintf(bf,bfsz-1,"[%d] command cannot be actioned",errcode);
			else if (errcode>=84 && errcode<=97) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==98) lprintf(bf,bfsz-1,"[%d] unspecified TP-Command error",errcode);
			else if (errcode==99) lprintf(bf,bfsz-1,"[%d] TPDU not supported",errcode);
			else if (errcode>=100 && errcode<=114) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==115) lprintf(bf,bfsz-1,"[%d] SMSC busy",errcode);
			else if (errcode==116) lprintf(bf,bfsz-1,"[%d] no SMSC subscription",errcode);
			else if (errcode==117) lprintf(bf,bfsz-1,"[%d] SMSC system failure",errcode);
			else if (errcode==118) lprintf(bf,bfsz-1,"[%d] invalid SME address",errcode);
			else if (errcode==119) lprintf(bf,bfsz-1,"[%d] destination SME barred",errcode);
			else if (errcode>=120 && errcode<=126) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==127) lprintf(bf,bfsz-1,"[%d] memory capacity exceeded",errcode);
		}
		else if (errcode>=128 && errcode<=255) {
			/* 128...255 GSM 03.40 subclause 9.2.3.22 values*/

			/* TP-PID errors*/
			if (errcode==128) lprintf(bf,bfsz-1,"[%d] Telematic interworking not supported",errcode);
			else if (errcode==129) lprintf(bf,bfsz-1,"[%d] Short message type 0 not supported",errcode);
			else if (errcode==130) lprintf(bf,bfsz-1,"[%d] Cannot replace short message",errcode);
			else if (errcode>=131 && errcode<=142) lprintf(bf,bfsz-1,"[%d] Reserved", errcode);
			else if (errcode==143) lprintf(bf,bfsz-1,"[%d] Unspecified TP-PID error",errcode);

			/* TP-DCS errors*/
			else if (errcode==144) lprintf(bf,bfsz-1,"[%d] Data coding sceme (alphabet) not supported",errcode);
			else if (errcode==145) lprintf(bf,bfsz-1,"[%d] Message class not supported",errcode);
			else if (errcode>=146 && errcode<=158) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==159) lprintf(bf,bfsz-1,"[%d] Unspecified TP-DCS error",errcode);

			/* TP-Command errors*/
			else if (errcode==160) lprintf(bf,bfsz-1,"[%d] Command cannot be actioned",errcode);
			else if (errcode==161) lprintf(bf,bfsz-1,"[%d] Command unsupported",errcode);
			else if (errcode>=162 && errcode<=174) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==175) lprintf(bf,bfsz-1,"[%d] Unspecified TP-Command error",errcode);
			else if (errcode==176) lprintf(bf,bfsz-1,"[%d] TPDU not supported",errcode);
			else if (errcode>=177 && errcode <=191) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==192) lprintf(bf,bfsz-1,"[%d] SC busy",errcode);
			else if (errcode==193) lprintf(bf,bfsz-1,"[%d] No SC subscription",errcode);
			else if (errcode==194) lprintf(bf,bfsz-1,"[%d] SC system failure",errcode);
			else if (errcode==195) lprintf(bf,bfsz-1,"[%d] Invalid SME address",errcode);
			else if (errcode==196) lprintf(bf,bfsz-1,"[%d] Destination SME barred",errcode);
			else if (errcode==197) lprintf(bf,bfsz-1,"[%d] SM Rejected-Duplicate SM",errcode);
			else if (errcode>=198 && errcode<=207) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode==208) lprintf(bf,bfsz-1,"[%d] SIM SMS storage full",errcode);
			else if (errcode==209) lprintf(bf,bfsz-1,"[%d] No SMS storage capability in SIM",errcode);
			else if (errcode==210) lprintf(bf,bfsz-1,"[%d] Error in MS",errcode);
			else if (errcode==211) lprintf(bf,bfsz-1,"[%d] Memory Capacity Exceeded",errcode);
			else if (errcode>=212 && errcode<=223) lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
			else if (errcode>=224 && errcode<=254) lprintf(bf,bfsz-1,"[%d] Values specific to an aplication",errcode);
			else if (errcode==255) lprintf(bf,bfsz-1,"[%d] Unspecified error cause",errcode);
		}
		else if (errcode>=256 && errcode<=511) {
			if (errcode==300) lprintf(bf,bfsz-1,"[%d] 300 ME failure",errcode);
			else if (errcode==301) lprintf(bf,bfsz-1,"[%d] SMS service of ME reserved",errcode);
			else if (errcode==302) lprintf(bf,bfsz-1,"[%d] operation not allowed",errcode);
			else if (errcode==303) lprintf(bf,bfsz-1,"[%d] operation not supported",errcode);
			else if (errcode==304) lprintf(bf,bfsz-1,"[%d] invalid PDU mode parameter",errcode);
			else if (errcode==305) lprintf(bf,bfsz-1,"[%d] invalid text mode parameter",errcode);
			else if (errcode==310) lprintf(bf,bfsz-1,"[%d] SIM not inserted",errcode);
			else if (errcode==311) lprintf(bf,bfsz-1,"[%d] SIM PIN required",errcode);
			else if (errcode==312) lprintf(bf,bfsz-1,"[%d] PH-SIM PIN required",errcode);
			else if (errcode==313) lprintf(bf,bfsz-1,"[%d] SIM failure",errcode);
			else if (errcode==314) lprintf(bf,bfsz-1,"[%d] SIM busy",errcode);
			else if (errcode==315) lprintf(bf,bfsz-1,"[%d] SIM wrong",errcode);
			else if (errcode==316) lprintf(bf,bfsz-1,"[%d] SIM PUK required",errcode);
			else if (errcode==317) lprintf(bf,bfsz-1,"[%d] SIM PIN2 required",errcode);
			else if (errcode==318) lprintf(bf,bfsz-1,"[%d] SIM PUK2 required",errcode);
			else if (errcode==320) lprintf(bf,bfsz-1,"[%d] memory failure",errcode);
			else if (errcode==321) lprintf(bf,bfsz-1,"[%d] invalid memory index",errcode);
			else if (errcode==322) lprintf(bf,bfsz-1,"[%d] memory full",errcode);
			else if (errcode==330) lprintf(bf,bfsz-1,"[%d] SMSC address unknown",errcode);
			else if (errcode==331) lprintf(bf,bfsz-1,"[%d] no network service",errcode);
			else if (errcode==332) lprintf(bf,bfsz-1,"[%d] network timeout",errcode);
			else if (errcode==340) lprintf(bf,bfsz-1,"[%d] no +CNMA acknowledgement expected",errcode);
			else if (errcode==500) lprintf(bf,bfsz-1,"[%d] unknown error",errcode);
			else lprintf(bf,bfsz-1,"[%d] Reserved",errcode);
		}
		else lprintf(bf,bfsz-1,"[%d] Manufacturer specific",errcode);	
	}

	return bf;
}
char *sms_gsm_fromip(SmsGsm *gsm, GsmMessage *msg) {
	return "127.0.0.1";
}
char *sms_gsm_from(SmsGsm *gsm, GsmMessage *msg) {
	return msg->phone;
}
char *sms_gsm_to(SmsGsm *gsm, GsmMessage *msg) {
	return gsm->localphone;
}
char *sms_gsm_body(SmsGsm *gsm, GsmMessage *msg) {
	return msg->message;
}
void sms_gsm_msgfree(SmsGsm *gsm, GsmMessage **msg) {
	if (gsm && msg && *msg) {
		free(*msg);
		*msg = NULL;
	}
}

int gsm_write_raw(SmsGsm *gsm, int *errcode, char *bf, int length) {
	int r;

	r = com_write(gsm->com,bf,length);
	if (r != length) {
		if (errcode) *errcode = SMS_GSM_WRITE_ERROR;
		return FALSE;
	}

	if (gsm->flog) {
		char tmp[BFSZ];
		struct tm tm;
		time_t t;

		time(&t);
		fprintf(gsm->flog,"%.15s WRITE[%.*s]\n",safe_asctime(safe_localtime(&t,&tm),tmp)+4,length,bf);
		fflush(gsm->flog);
	}

	return TRUE;
}
int gsm_write(SmsGsm *gsm, int *errcode, char *format, ...) {
	va_list args;
	char bf[BFSZ];
	int length,r;

	va_start(args,format);
	length = vsnprintf(bf,BFSZ,format,args);
	if (length == -1 || length == BFSZ) bf[length = BFSZ-1] = '\0';
	va_end(args);

	r = com_write(gsm->com,bf,length);
	if (r != length) {
		if (errcode) *errcode = SMS_GSM_WRITE_ERROR;
		return FALSE;
	}

	if (gsm->flog) {
		char tmp[BFSZ];
		struct tm tm;
		time_t t;

		time(&t);
		for(length--; iseol(bf[length]); length--) bf[length] = '\0';		
		fprintf(gsm->flog,"%.15s WRITE[%s]\n",safe_asctime(safe_localtime(&t,&tm),tmp)+4,bf);
		fflush(gsm->flog);
	}

	return TRUE;
}

#define	bfrmv(b,p,s,l) {		\
	memmove(p,&p[s],l-(p-b)-s); \
	b[l -= s] = '\0';			\
}

int gsm_read_raw(SmsGsm *gsm, int *errcode, int type, long timeout, char *bf, int bfsz) {
	int i,gotdata,waited,len = 0;
	char *eol = NULL;

	//if (type & GRL_INCOMING) dmsg("gsm_read_raw: read_incoming (%d) timeout=%d bf=0x%08x bfsz=%d",type,timeout,bf,bfsz);

	if (bfsz == 0) return 0;

	for(i = waited = gotdata = 0; waited < timeout; th_mutex_release(gsm->lock_buffer), lib_msleep(50), waited += 50) {
		//if (type & GRL_INCOMING) dmsg("gsm_read_raw: loop waited=%d timeout=%d i=%d",waited,timeout,i);
		th_mutex_get_queue(gsm->lock_buffer);
		
		for(; i < gsm->bflen && !gotdata; ) {
			eol = strstr(&gsm->buffer[i],gsm->r_endl);
			if (eol) {
				*eol = '\0';
				//if (type & GRL_INCOMING) dmsg("gsm_read_raw: eol found i=%d (%s)",i,&gsm->buffer[i]);

				//dmsg("READ_RAW[%d][%d][%s]",type,i,&gsm->buffer[i]);

				if ((type & GRL_SKIP_BLANK) && eol == &gsm->buffer[i]) {
					bfrmv(gsm->buffer,eol,gsm->r_nendl,gsm->bflen);
					continue;
				}

				if (strcmp(&gsm->buffer[i],"^SYSSTART") == 0) {
					// skip this unless doing normal read
					if (!(type & GRL_INCOMING) && !(type & GRL_RAW)) {
						*eol = gsm->r_endl[0];
						i = (eol-gsm->buffer)+gsm->r_nendl;
						continue;
					}
				}

				if (strcmp(&gsm->buffer[i],"RING") == 0) {
					//skip this unless doing incoming read
					if (type & GRL_INCOMING) {
						gotdata = TRUE;
						break;
					}
					*eol = gsm->r_endl[0];
					i = (eol-gsm->buffer)+gsm->r_nendl;
					continue;
				}				

				//+CRING: VOICE
				if (strncmp(&gsm->buffer[i],"+CRING:",7) == 0) {
					//skip this unless doing incoming read
					if (type & GRL_INCOMING) {
						gotdata = TRUE;
						break;
					}
					*eol = gsm->r_endl[0];
					i = (eol-gsm->buffer)+gsm->r_nendl;
					continue;
				}

				//+CLIP: "0823782452",129
				if (strncmp(&gsm->buffer[i],"+CLIP:",6) == 0) {
					//skip this unless doing incoming read
					if (type & GRL_INCOMING) {
						gotdata = TRUE;
						break;
					}
					*eol = gsm->r_endl[0];
					i = (eol-gsm->buffer)+gsm->r_nendl;
					continue;
				}

				if (strncmp(&gsm->buffer[i],"+CMTI:",6) == 0) {
					//skip this unless doing incoming read
					if (type & GRL_INCOMING) {
						gotdata = TRUE;
						break;
					}
					*eol = gsm->r_endl[0];
					i = (eol-gsm->buffer)+gsm->r_nendl;
					continue;
				}

				if (!(type & GRL_INCOMING) && !(type & GRL_RAW)) {
					//if doing normal read, this is it
					gotdata = TRUE;
					break;
				}

				//reset for next loop, and/or raw read
				*eol = gsm->r_endl[0]; eol = NULL;
			}
			if (type & GRL_RAW) {
				//dmsg("READ_RAW[%d][%d][%d][%d][%s]",type,gsm->bflen,i,bfsz,&gsm->buffer[i]);
				if (gsm->bflen-i < bfsz) break;
				if ((type & GRL_EXPECT_GT) && gsm->buffer[i] != '>') break;
				if (gsm->flog) {
					char tmp[BFSZ];
					struct tm tm;
					time_t t;

					time(&t);
					fprintf(gsm->flog,"%.15s RAW  [%.*s]\n",safe_asctime(safe_localtime(&t,&tm),tmp)+4,bfsz,&gsm->buffer[i]);
					fflush(gsm->flog);
				}
				gotdata = TRUE;
				len = bfsz;
				break;
			}
			break;
		}

		if (gotdata) break;

		/*
		if (type & GRL_INCOMING) {
			//look for incoming message line
			for(i = gotdata = 0; i < gsm->bflen && !gotdata; ) {
				eol = strstr(&gsm->buffer[i],gsm->r_endl);
				if (!eol) break;
				*eol = '\0';
				
				dmsg("INCOMING[%d][%s]",i,&gsm->buffer[i]);

				if (strncmp(&gsm->buffer[i],"+CMTI:",6) != 0 && 
					strcmp(&gsm->buffer[i],"RING") != 0) {
					*eol = gsm->r_endl[0];
					i = (eol-gsm->buffer)+gsm->r_nendl;
					continue;
				}

				gotdata = TRUE;
				break;
			}
			if (gotdata) break;
			continue;
		}

		if (type & GRL_RAW) {
			//read a raw number of bytes
			if (gsm->bflen-i < bfsz) continue;
			for(gotdata = 0; !gotdata && i < gsm->bflen; ) {
				if (gsm->buffer[i] == '+')
			}
			if (gsm->flog) {
				fprintf(gsm->flog,"RAW  [%.*s]\n",bfsz,&gsm->buffer[i]);
				fflush(gsm->flog);
			}
			len = bfsz;
			break;
		}

		//normal read a line
		for(i = gotdata = 0; !gotdata && i < gsm->bflen; ) {
			eol = strstr(&gsm->buffer[i],gsm->r_endl);
			if (!eol) break;
			*eol = '\0';

			if ((type & GRL_SKIP_BLANK) && eol == &gsm->buffer[i]) {
				bfrmv(gsm->buffer,eol,gsm->r_nendl,gsm->bflen);
				continue;
			}
			if (strcmp(&gsm->buffer[i],"^SYSSTART") == 0) {
				if (gsm->flog) {
					fprintf(gsm->flog,"INIT[%s]\n",&gsm->buffer[i]);
					fflush(gsm->flog);
				}
				bfrmv(gsm->buffer,eol,9+gsm->r_nendl,gsm->bflen);
				dmsg("gsm_read_raw[%d]: ~SYSSTART %d bytes left",type,gsm->bflen);
				continue;
			}
			if (strcmp(&gsm->buffer[i],"RING")==0) {
				if (gsm->flog) {
					fprintf(gsm->flog,"RING[%s]\n",&gsm->buffer[i]);
					fflush(gsm->flog);
				}
				bfrmv(gsm->buffer,eol,4+gsm->r_nendl,gsm->bflen);
				dmsg("gsm_read_raw[%d]: RING %d bytes left",type,gsm->bflen);
				continue;
			}
			if (strcmp(&gsm->buffer[i],"+CMTI:")==0) {
				*eol = gsm->r_endl[0];
				i = (eol-gsm->buffer)+gsm->r_nendl;
				continue;
			}
			gotdata = TRUE;
		}
		if (gotdata) break;
		*/
	}
	if (waited >= timeout) {
		//if (type & GRL_INCOMING) dmsg("gsm_read_raw: timed out %d >= %d",waited,timeout);
		if (!(type & GRL_INCOMING)) {
			FILE *f;

			th_mutex_get_queue(gsm->lock_buffer);
			dmsg("gsm_read_raw[%d]: timeout %d > %d, %d bytes (%.*s)",type,waited,timeout,gsm->bflen,gsm->bflen,gsm->buffer);
#ifdef WIN32
			if (gsm->bflen) {
				f = fopen("\\smsgate\\log\\timeout.log","wb");
				if (!f) emsg("\\smsgate\\log\\timeout.log, %s",strerror(errno));
				else {
					fwrite(gsm->buffer,gsm->bflen,1,f);
					fclose(f);
				}
			}
			else {
				remove("\\smsgate\\log\\timeout.log");
			}
#endif
			th_mutex_release(gsm->lock_buffer);

			*errcode = SMS_GSM_READ_ERROR;
		}
		/*i = j = 0;*/		
	}
	else {
		if (eol) {
			len = (eol-gsm->buffer)+gsm->r_nendl;
			*eol = gsm->r_endl[0];			
		}
		memcpy(bf,&gsm->buffer[i],len);
		bfrmv(gsm->buffer,(&gsm->buffer[i]),len,gsm->bflen);
		/*
		memcpy(bf,&gsm->buffer[i],j-i);
		if (j != gsm->bflen) memmove(&gsm->buffer[i],&gsm->buffer[j],gsm->bflen-j);
		gsm->bflen -= (j-i);
		//dmsg("gsm_read_raw: data (%d)(%d)(%.*s)",type,j-i,j-i,bf);
		//dmsg("gsm_read_raw: %d bytes left",gsm->bflen);
		*/
		th_mutex_release(gsm->lock_buffer);
	}
	//if (type & GRL_INCOMING) dmsg("gsm_read_raw: returning %d",len);
	return len;
}
int gsm_read_line(SmsGsm *gsm, int *errcode, int type, long timeout, char *bf, int bfsz) {
	int bytes;

	while(TRUE) {
		//if (type & GRL_INCOMING) dmsg("gsm_read_line: read_incoming (%d) timeout=%d bf=0x%08x bfsz=%d",type,timeout,bf,bfsz);
		bytes = gsm_read_raw(gsm,errcode,type,timeout,bf,bfsz);
		//if (type & GRL_INCOMING) dmsg("gsm_read_line: read_incoming %d bytes (%.*s)",bytes,bytes,bf);
		if (!bytes) return bytes;

		for(bytes--; bytes != 0 && iseol(bf[bytes]); bytes--);
		if (iseol(bf[bytes])) bf[bytes] = '\0';
		else bf[++bytes] = '\0';

		if ((type & GRL_INCOMING)) {			
			if (strcmp(bf,"RING")==0) {
				gsm_clear_ring(gsm,errcode);
				continue;
			}

			if (strncmp(bf,"+CRING:",7)==0) {
				gsm_clear_ring(gsm,errcode);
				continue;
			}

			if (strncmp(bf,"+CLIP:",6)==0) {
				//just ignore
				continue;
			}

		}
		/* RMH; read_raw is called directly for RAW reads now.
		if ((type & GRL_RAW) && bf[0] != '>') {
			if (bf[0] == 'R' && bf[1] == 'I') gsm_clear_ring(gsm,errcode);
			continue;
		}*/
		if (gsm->flog) {
			char tmp[BFSZ];
			struct tm tm;
			time_t t;

			time(&t);
			fprintf(gsm->flog,"%.15s READ%d[%.*s]\n",safe_asctime(safe_localtime(&t,&tm),tmp)+4,type,bytes,bf);
			fflush(gsm->flog);
		}

		break;
	}

	return bytes;
}

/*
int gsm_read_raw(SmsGsm *gsm, int *errcode, int type, long timeout, char *bf, int bfsz) {
	int i,waited,bflen;

	waited = bflen = 0;
	bf[0] = '\0';

	while(TRUE) {
		th_mutex_get_queue(gsm->lock_lines);
		if (!gsm->nlines) {
			th_mutex_release(gsm->lock_lines);
			if (waited > timeout) {
				//emsg("gsm_read_raw timed out (%d)>(%d)",waited,timeout);
				if (errcode && !(type & GRL_INCOMING)) *errcode = SMS_GSM_READ_ERROR;
				return 0;
			}
			lib_msleep(100);
			waited += 100;
			continue;
		}
		for(i = 0; i < gsm->nlines; i++) {
			// Ignore blank lines
			if (gsm->lines[i][0] == '\0' && !(type & BLANK)) {
				memmove(&gsm->lines[i],&gsm->lines[i+1],(gsm->nlines-i)*sizeof(char *));
				memmove(&gsm->linelen[i],&gsm->linelen[i+1],(gsm->nlines-i)*sizeof(char *));
				gsm->nlines--;
				i--;
				continue;
			}

			// Handle weird Siemens TC35 message
			// ^SYSSTART
			if (strcmp(gsm->lines[i],"^SYSSTART")==0) {
				if (gsm->flog) {
					fprintf(gsm->flog,"INIT[%s]\n",gsm->lines[i]);
					fflush(gsm->flog);
				}
				memmove(&gsm->lines[i],&gsm->lines[i+1],(gsm->nlines-i)*sizeof(char *));
				memmove(&gsm->linelen[i],&gsm->linelen[i+1],(gsm->nlines-i)*sizeof(char *));
				gsm->nlines--;
				i--;
				continue;
			}

			// Handle RING messages
			if (strcmp(gsm->lines[i],"RING")==0) {
				if (gsm->flog) {
					fprintf(gsm->flog,"RING[%s]\n",gsm->lines[i]);
					fflush(gsm->flog);
				}
				gsm_clear_ring(gsm,errcode);
				memmove(&gsm->lines[i],&gsm->lines[i+1],(gsm->nlines-i)*sizeof(char *));
				memmove(&gsm->linelen[i],&gsm->linelen[i+1],(gsm->nlines-i)*sizeof(char *));
				gsm->nlines--;
				i--;
				continue;
			}

			if (strncmp(gsm->lines[i],"+CMTI:",6) == 0) {
				if (type & NORMAL) continue;
			}
			else if (type & GRL_INCOMING)
				continue;

			memcpy(bf,gsm->lines[i],gsm->linelen[i]+1);
			bflen = gsm->linelen[i];

			memmove(&gsm->lines[i],&gsm->lines[i+1],(gsm->nlines-i)*sizeof(char *));
			memmove(&gsm->linelen[i],&gsm->linelen[i+1],(gsm->nlines-i)*sizeof(char *));
			gsm->nlines--;

			if (gsm->flog) {
				fprintf(gsm->flog,"READ [%s]\n",bf);
				fflush(gsm->flog);
			}
			break;
		}
		th_mutex_release(gsm->lock_lines);
		if (bf[0] == '\0') continue;
		break;
	}
	return bflen;
}
*/
int gsm_iserrorx(SmsGsm *gsm, char *bf, int *errcode, int *iscme) {
	if (iscme) *iscme = FALSE;
	if (errcode) *errcode = 0;
	if (bf) {
		if (strncmp(bf,"+CMS ERROR:",11)==0) {
			if (errcode) *errcode = atoi(&bf[12]) + 1;
			return TRUE;
		}
		if (strncmp(bf,"+CME ERROR:",11)==0) {
			if (errcode) *errcode = atoi(&bf[12]) + 1;
			if (iscme) *iscme = TRUE;
			return TRUE;
		}
		if (strcmp(bf,"ERROR")==0) {
			if (errcode) *errcode = SMS_GSM_MISC_ERROR;
			return TRUE;
		}
	}
	return FALSE;
}
int gsm_clear_ring(SmsGsm *gsm, int *errcode) {
	/*if (th_mutex_get_queue(gsm->lock_com)) {*/
		char bf[BFSZ];

		/*if (!gsm_write(gsm,errcode,"ATA\n")) return FALSE;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) return FALSE;*/
		if (!gsm_write(gsm,errcode,"ATH0%s",gsm->w_endl)) return FALSE;
		if (!gsm_read(gsm,errcode,bf,BFSZ)) return FALSE;
		/*th_mutex_release(gsm->lock_com);*/
		return TRUE;
	/*}
	return FALSE;*/
}
/* gsm_process_message_info
Expects "<stat>","<oa>",["<alpha>"],"<scts>"[,<tooa>,<fo>,<pid>,<dcs>,"<sca>",<tosca>,<length>] 
0,,27
with NO \n

  eg.
+CMGR: <stat>,      <oa>,[<alpha>],<scts> [,<tooa>,<fo>,<pid>,<dcs>,<sca>,<tosca>,<length>]<CR><LF><data>
+CMGR: "REC UNREAD","+6596238266",,"03/04/14,09:28:08+32",145,4,0,0,"+6596197777",145,39

+CMGL: <index>,<stat>,<oa/da>,[<alpha>],[<scts>] [,<tooa/toda>,<length>]<CR><LF><data>
+CMGL: 1,"REC READ","+6596238266",,"03/04/15,12:06:59+32",145,41
*/
int gsm_process_message_info(SmsGsm *gsm, int *errcode, GsmMessage *msg, char *info, int iscmgl) {
	int index;
	char *ptr;

	if (gsm->ispdu) {
		ncpy(msg->status,pdu_mode_status[atoi(info)],BFSZ-1);
		dmsg("Message 'status' (%d)(%s)",atoi(info),msg->status);
		
		for(ptr = info; *ptr && *ptr != ','; ptr++);
		if (*ptr=='\0') {
			if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
			return FALSE;
		}

		if (*(ptr+1) == ',') msg->alpha[0] = '\0';
		else {
			for(ptr += 2, index = 0; *ptr && *ptr != '\"' && index < BFSZ; ptr++, index++)
				msg->alpha[index] = *ptr;
			msg->alpha[index] = '\0';
			dmsg("Message 'alpha' (%s)",msg->alpha);

			if (*ptr=='\0') {
				if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
				return FALSE;
			}
		}				

		ptr += 2;

		msg->length = (atoi(ptr) * 2);
		dmsg("PDU length (%d)",msg->length);

		return TRUE;
	}

	/* text mode */		
	for (ptr = &info[1], index = 0; *ptr && *ptr != '\"' && index < BFSZ; ptr++, index++)
		msg->status[index] = *ptr;
	msg->status[index] = '\0';
	dmsg("Message 'status' (%s)",msg->status);
	
	if (*ptr=='\0') {
		if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
		return FALSE;
	}

	for (ptr += 3, index = 0; *ptr && *ptr != '\"' && index < BFSZ; ptr++, index++)
		msg->phone[index] = *ptr;
	msg->phone[index] = '\0';
	if (gsm->usedcs==0x8) {
		dmsg("Decoding 'from' UCS2 (%s)",msg->phone);
		gsm_process_encoded(TRUE,gsm->ext7bit,msg->phone,index,BFSZ);
	}
	dmsg("Message 'from' (%s)",msg->phone);

	if (*ptr=='\0') {
		if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
		return FALSE;
	}
	
	ptr++;
	if (*(ptr+1) != ',') {
		for(ptr += 2, index = 0; *ptr && *ptr != '\"' && index < BFSZ; ptr++, index++)
			msg->alpha[index] = *ptr;
		msg->alpha[index] = '\0';
		dmsg("Message 'alpha' (%s)",msg->alpha);
	}
	else msg->alpha[0] = '\0';

	if (*ptr=='\0') {
		if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
		return FALSE;
	}
	
	for (ptr += 3, index = 0; *ptr && *ptr != '\"' && index < BFSZ; ptr++, index++)
		msg->timestamp[index] = *ptr;
	msg->timestamp[index] = '\0';
	dmsg("Message 'timestamp' (%s)",msg->timestamp);
	ptr++;

	/* Rest is optional */
	if (*ptr=='\0') return TRUE;

	ptr++;
	if (*ptr && *ptr != ',') {
		msg->type_of_originating_address = atoi(ptr);
		dmsg("Message 'type of originating address' (%d)",msg->type_of_originating_address);
		for ( ptr++; *ptr && *ptr!=','; ptr++ );
	}
	if (*ptr=='\0') {
		if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
		return FALSE;
	}

	if (!iscmgl) {
		ptr++;
		if (*ptr && *ptr!=',') {
			msg->first_octet_of_sms = atoi(ptr);
			dmsg("Message 'first octet of sms' (%d)",msg->first_octet_of_sms);
			for ( ptr++; *ptr && *ptr!=','; ptr++ );
		}
		if (*ptr=='\0') {
			if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
			return FALSE;
		}

		ptr++;
		if (*ptr && *ptr!=',') {
			msg->protocal_identifier = atoi(ptr);
			dmsg("Message 'protocal identifier' (%d)",msg->protocal_identifier);
			for ( ptr++; *ptr && *ptr!=','; ptr++ );
		}
		if (*ptr=='\0') {
			if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
			return FALSE;
		}

		ptr++;
		if (*ptr && *ptr!=',') {
			msg->data_coding_sceme = atoi(ptr);
			dmsg("Message 'data coding sceme' (%d)",msg->data_coding_sceme);
			for ( ptr++; *ptr && *ptr!=','; ptr++ );
		}
		if (*ptr=='\0') {
			if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
			return FALSE;
		}

		ptr++;
		if (*ptr == '\"') {
			for ( ptr++, index=0; *ptr && *ptr!='\"' && index<BFSZ; index++,ptr++ )
				msg->service_center[index] = *ptr;
			msg->service_center[index] = '\0';
			if (gsm->usedcs==0x8) {
				dmsg("Decoding 'service center' UCS2 (%s)",msg->service_center);
				gsm_process_encoded(TRUE,gsm->ext7bit,msg->service_center,index,BFSZ);
			}
			dmsg("Message 'service center' (%s)",msg->service_center);
			if (*ptr=='\"') ptr++;
		}
		if (*ptr=='\0') {
			if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
			return FALSE;
		}

		ptr++;
		if (*ptr && *ptr!=',') {
			msg->type_of_service_center = atoi(ptr);
			dmsg("Message 'type of service center' (%d)",msg->type_of_service_center);
			for ( ptr++; *ptr && *ptr!=','; ptr++ );
		}
		if (*ptr=='\0') {
			if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
			return FALSE;
		}
	}

	ptr++;
	if (*ptr && *ptr!=',') {
		if (gsm->usedcs == 0x8) {
			msg->length = atoi(ptr) * 4;
			dmsg("Message 'length' UCS2 (%d)",msg->length);
		}
		else {
			msg->length = atoi(ptr);
			dmsg("Message 'length' (%d)",msg->length);
		}		
	}

	return TRUE;
}
/* gsm_process_message_notify
Expects "<mem>",<index>
with NO \n
*/
int gsm_process_message_notify(SmsGsm *gsm, int *errcode, GsmMessage *msg, char *info) {
	int index;
	char *ptr;

	imsg("process notify (%s)\n",info);
	for (ptr = &info[1], index = 0; *ptr && *ptr != '\"' && index < BFSZ; ptr++, index++)
		msg->memory[index] = *ptr;
	
	if (*ptr=='\0') {
		imsg("process notify failed, did not find what we wanted (%s)\n",info);
		if (errcode) *errcode = SMS_GSM_PROTO_ERROR;
		return FALSE;
	}

	msg->index = atoi(ptr+2);

	dmsg("Incoming message (%s)(%d)",msg->memory,msg->index);

	return TRUE;
}

/* for some reason one of the modems I encountered leaves 
encoded 7bit characters in the string? */
int gsm_process_encoded(int ucs2, int ext7bit, char *message, int length, int max) {
	dmsg("gsm_process_encoded ucs2=(%d) ext7bit=(%d) length=(%d) max=(%d) message=(%s)",ucs2,ext7bit,length,max,message);
	if (ucs2) {		
		unsigned int v;
		int i,j,x;

		dmsg("16bit decode, first 2 chars are: (%c%c)",message[0],message[1]);
		j = 2;
		for(i = 0; j < length && i < max; ) {
			if (!isalnum(message[j])) break;
			v = hextobin(message[j]) << 4; j++;
			
			if (!isalnum(message[j])) break;
			v += hextobin(message[j]) << 4; j++;

			if (!isalnum(message[j])) break;
			v += hextobin(message[j]) << 4; j++;

			if (!isalnum(message[j])) break;
			v += hextobin(message[j]); j++;

			for(x = 0; x < 256; x++) {
				if (seven_bit_to_unicode[x] == v) {
					v = seven_bit_to_ascii[x];
					break;
				}
			}
			message[i++] = v;
		}
		message[i] = '\0';
		length = i;
	}
	else {
		unsigned char v = 0;
		char *ptr;
		int l;

		l = length;
		for(ptr = message; l; ptr++, l--) {
			if (*ptr >= 0x20 || *ptr == '\r' || *ptr == '\n') continue;
			if (*ptr == 0x1B) {
				memmove(ptr,ptr+1,l--);
				length--;
				v = 128;			
			}
			*ptr = seven_bit_to_ascii[*ptr+v];
			v = 0;
		}
		*ptr = '\0';
	}
	return length;
}

int gsm_create_encoded(int ucs2, int ext7bit, char *message, int length, int max) {
	unsigned char v = 0;

	if (ucs2) {
		int i,j = length*4;

		for(i = 0; length < j; ) {
			if (i + 4 >= max) break;
			memmove(&message[i+4],&message[i+1],length-i-1);
			length += 3;

			v = message[i];
			message[i++] = "0123456789ABCDEF"[v/4096];
			message[i++] = "0123456789ABCDEF"[v/256];
			message[i++] = "0123456789ABCDEF"[v/16];
			message[i++] = "0123456789ABCDEF"[v%16];
		}
		message[i] = '\0';
		length = i;
	}
	else {		
		char *ptr;
		int l;

		l = length>max?max:length;
		for(ptr = message; l; ptr++, l--) {
			v = ascii_to_seven_bit[(unsigned char)*ptr];

			if (v == '\r' || v == '\n') continue;
			if (v >= 0x20 && v <= 127) continue;
			if (v > 127) {
				if (ext7bit) {
					memmove(ptr+1,ptr,l+1);
					*ptr++ = 0x1B;
					length++;						
				}
				v -= 128;
			}
			*ptr = v;
		}
		*ptr = '\0';
	}
	return length>max?max:length;
}

#ifdef _DEBUG
void test_pdu(int mode, char *xpdu) {
	char pdu[BFSZ],bf[BFSZ],tmp[BFSZ];
	GsmMessage *msg;

	strcpy(pdu,xpdu);
	if (mode == TEST_RECV) {		
		msg = calloc(1,sizeof(GsmMessage));
		gsm_decode_pdu(msg,pdu);
		printf("PDU from[%s]\n",msg->phone);
		printf("PDU date[%s]\n",msg->timestamp);
		printf("PDU decoded [%s]\n",msg->message);
		strcpy(bf,msg->message);
		gsm_encode_pdu(msg->data_coding_sceme,"",bf,tmp,BFSZ);
		if (strcmp(&pdu[44],&tmp[14])==0) printf("Encode matches\n");
		else printf("Encode doesn't match\n");
	}
	else {
		int dcs,i = 0,x;

		/* SERVICE CENTER ADDRESS */
		x = pdu_decode_int(pdu,&i);
		/* FIRST OCTET */
		x = pdu_decode_int(pdu,&i); /* no validity period */
		/* MESSAGE REFERENCE */
		x = pdu_decode_int(pdu,&i);
		/* DESTINATION ADDRESS */
		pdu_decode_addr(pdu,bf,&i);
		/* PROTOCOL ID */
		x = pdu_decode_int(pdu,&i);
		/* DATA CODING SCEME */
		dcs = pdu_decode_int(pdu,&i);
		/* CONTENT */
		pdu_decode_content(dcs,pdu,bf,&i);
	}
}
void test_txt(int iscmgl, char *cmgr) {
	GsmMessage *msg;
	SmsGsm *g;
	int e;

	msg = calloc(1,sizeof(GsmMessage));
	g = calloc(1,sizeof(SmsGsm));
	if (!gsm_process_message_info(g,&e,msg,(iscmgl)?&cmgr[9]:&cmgr[7],iscmgl))
		printf("TXT decode failed\n");
	else {
		printf("TXT from[%s]\n",msg->phone);
		printf("TXT date[%s]\n",msg->timestamp);
		printf("TXT length [%d]\n",msg->length);
		printf("TXT decoded [%s]\n",msg->message);
	}
}
void decode_txt_msg(int ucs2, char *message, int length) {
}
void test_txt_msg(int ucs2, int ext7bit, char *message, int length) {
	int i,elength;
	char bf[BFSZ];

	memcpy(bf,message,length);
	for(i = 0; i < length; i++) {
		printf("<%02x>",(unsigned char)message[i]);
	}
	printf("\n");

	elength = gsm_create_encoded(ucs2,ext7bit,bf,length,BFSZ);
	
	for(i = 0; i < elength; i++) {
		printf("<%02x>",(unsigned char)bf[i]);
	}
	printf("\n");

	gsm_process_encoded(ucs2,ext7bit,bf,elength,BFSZ);

	if (memcmp(message,bf,length)!=0) printf("Encode-decode failed.\n");		
	else printf("Encode-decode ok.\n");
}
#endif
