--- chan_unistim.c.orig	2011-10-11 11:33:06.000000000 -0500
+++ chan_unistim.c	2011-10-11 11:42:22.000000000 -0500
@@ -73,20 +73,25 @@
 #include "asterisk/musiconhold.h"
 #include "asterisk/causes.h"
 #include "asterisk/indications.h"
+#include "asterisk/features.h"
+#include "asterisk/astobj2.h"
+#include "asterisk/astdb.h"
 
-/*! Beware, G729 and G723 are not supported by asterisk, except with the proper licence */
-#define CAPABILITY AST_FORMAT_ALAW | AST_FORMAT_ULAW    /* | AST_FORMAT_G729A | AST_FORMAT_G723_1 */
+#define CAPABILITY AST_FORMAT_ALAW | AST_FORMAT_ULAW
 
 #define DEFAULTCONTEXT	  "default"
 #define DEFAULTCALLERID	 "Unknown"
 #define DEFAULTCALLERNAME       " "
 #define DEFAULTHEIGHT	 3
 #define USTM_LOG_DIR	    "unistimHistory"
+#define USTM_LANG_DIR	    "unistimLang"
 
 /*! Size of the transmit buffer */
 #define MAX_BUF_SIZE	    64
 /*! Number of slots for the transmit queue */
 #define MAX_BUF_NUMBER	  50
+/*! Number of digits displayed on screen */
+#define MAX_SCREEN_NUMBER   15
 /*! Try x times before removing the phone */
 #define NB_MAX_RETRANSMIT       8
 /*! Nb of milliseconds waited when no events are scheduled */
@@ -94,7 +99,9 @@
 /*! Wait x milliseconds before resending a packet */
 #define RETRANSMIT_TIMER	2000
 /*! How often the mailbox is checked for new messages */
-#define TIMER_MWI	       10000
+#define TIMER_MWI	       5000
+/*! How often the mailbox is checked for new messages */
+#define TIMER_DIAL	       4000
 /*! Not used */
 #define DEFAULT_CODEC	   0x00
 #define SIZE_PAGE	       4096
@@ -103,13 +110,15 @@
 #define MAX_ENTRY_LOG	   30
 
 #define SUB_REAL		0
-#define SUB_THREEWAY	    1
-#define MAX_SUBS		2
+#define SUB_RING                1
+#define SUB_THREEWAY            2
+#define SUB_ONHOLD              3
+
+
 
 enum autoprovision {
 	AUTOPROVISIONING_NO = 0,
 	AUTOPROVISIONING_YES,
-	AUTOPROVISIONING_DB,
 	AUTOPROVISIONING_TN
 };
 
@@ -185,6 +194,9 @@
 
 #define FAV_MAX_LENGTH		  0x0A
 
+#define FAVNUM                    6
+#define FAV_LINE_ICON         FAV_ICON_ONHOOK_BLACK
+
 static void dummy(char *unused, ...)
 {
 	return;
@@ -224,17 +236,9 @@
 static struct io_context *io;
 static struct sched_context *sched;
 static struct sockaddr_in public_ip = { 0, };
-/*! give the IP address for the last packet received */
-static struct sockaddr_in address_from;
-/*! size of the sockaddr_in (in WSARecvFrom) */
-static unsigned int size_addr_from = sizeof(address_from);
-/*! Receive buffer address */
-static unsigned char *buff;
+static unsigned char *buff; /*! Receive buffer address */
 static int unistim_reloading = 0;
 AST_MUTEX_DEFINE_STATIC(unistim_reload_lock);
-AST_MUTEX_DEFINE_STATIC(usecnt_lock);
-static int usecnt = 0;
-/* extern char ast_config_AST_LOG_DIR[AST_CONFIG_MAX_PATH]; */
 
 /*! This is the thread for the monitor which checks for input on the channels
  * which are not currently in use.  */
@@ -256,7 +260,9 @@
 	STATE_DIALPAGE,
 	STATE_RINGING,
 	STATE_CALL,
+	STATE_SELECTOPTION,
 	STATE_SELECTCODEC,
+	STATE_SELECTLANGUAGE,
 	STATE_CLEANING,
 	STATE_HISTORY
 };
@@ -307,75 +313,36 @@
 	KEY_INDEX = 0x7f
 };
 
-struct tone_zone_unistim {
-	char country[3];
-	int freq1;
-	int freq2;
+enum charset {
+	LANG_DEFAULT,
+	ISO_8859_1,
+	ISO_8859_2,
+	ISO_8859_4,
+	ISO_8859_5,
+	ISO_2022_JP,
 };
 
-static const struct tone_zone_unistim frequency[] = {
-	{"us", 350, 440},
-	{"fr", 440, 0},
-	{"au", 413, 438},
-	{"nl", 425, 0},
-	{"uk", 350, 440},
-	{"fi", 425, 0},
-	{"es", 425, 0},
-	{"jp", 400, 0},
-	{"no", 425, 0},
-	{"at", 420, 0},
-	{"nz", 400, 0},
-	{"tw", 350, 440},
-	{"cl", 400, 0},
-	{"se", 425, 0},
-	{"be", 425, 0},
-	{"sg", 425, 0},
-	{"il", 414, 0},
-	{"br", 425, 0},
-	{"hu", 425, 0},
-	{"lt", 425, 0},
-	{"pl", 425, 0},
-	{"za", 400, 0},
-	{"pt", 425, 0},
-	{"ee", 425, 0},
-	{"mx", 425, 0},
-	{"in", 400, 0},
-	{"de", 425, 0},
-	{"ch", 425, 0},
-	{"dk", 425, 0},
-	{"cn", 450, 0},
-	{"--", 0, 0}
-};
+static const int dtmf_row[] = { 697,  770,  852,  941 };
+static const float dtmf_col[] = { 1209, 1336, 1477, 1633 };
 
 struct wsabuf {
 	u_long len;
 	unsigned char *buf;
 };
 
-struct systemtime {
-	unsigned short w_year;
-	unsigned short w_month;
-	unsigned short w_day_of_week;
-	unsigned short w_day;
-	unsigned short w_hour;
-	unsigned short w_minute;
-	unsigned short w_second;
-	unsigned short w_milliseconds;
-};
-
 struct unistim_subchannel {
 	ast_mutex_t lock;
-	/*! SUBS_REAL or SUBS_THREEWAY */
-	unsigned int subtype;
-	/*! Asterisk channel used by the subchannel */
-	struct ast_channel *owner;
-	/*! Unistim line */
-	struct unistim_line *parent;
-	/*! RTP handle */
-	struct ast_rtp_instance *rtp;
+	unsigned int subtype;		/*! SUB_REAL, SUB_RING, SUB_THREEWAY or SUB_ONHOLD */
+	struct ast_channel *owner;	/*! Asterisk channel used by the subchannel */
+	struct unistim_line *parent;	/*! Unistim line */
+	struct ast_rtp_instance *rtp;	/*! RTP handle */
+	int softkey;			/*! Softkey assigned */
+	pthread_t ss_thread;		/*! unistim_ss thread handle */
 	int alreadygone;
 	char ringvolume;
 	char ringstyle;
+	int moh;					/*!< Music on hold in progress */
+	AST_LIST_ENTRY(unistim_subchannel) list;
 };
 
 /*!
@@ -383,62 +350,45 @@
  */
 struct unistim_line {
 	ast_mutex_t lock;
-	/*! Like 200 */
-	char name[80];
-	/*! Like USTM/200\@black */
-	char fullname[80];
-	/*! pointer to our current connection, channel... */
-	struct unistim_subchannel *subs[MAX_SUBS];
-	/*! Extension where to start */
-	char exten[AST_MAX_EXTENSION];
-	/*! Context to start in */
-	char context[AST_MAX_EXTENSION];
-	/*! Language for asterisk sounds */
-	char language[MAX_LANGUAGE];
-	/*! CallerID Number */
-	char cid_num[AST_MAX_EXTENSION];
-	/*! Mailbox for MWI */
-	char mailbox[AST_MAX_EXTENSION];
-	/*! Used by MWI */
-	int lastmsgssent;
-	/*! Used by MWI */
-	time_t nextmsgcheck;
-	/*! MusicOnHold class */
-	char musicclass[MAX_MUSICCLASS];
-	/*! Call group */
-	unsigned int callgroup;
-	/*! Pickup group */
-	unsigned int pickupgroup;
-	/*! Account code (for billing) */
-	char accountcode[80];
-	/*! AMA flags (for billing) */
-	int amaflags;
-	/*! Codec supported */
-	format_t capability;
-	/*! Parkinglot */
-	char parkinglot[AST_MAX_CONTEXT];
+	char name[80]; /*! Like 200 */
+	char fullname[80]; /*! Like USTM/200\@black */
+	char exten[AST_MAX_EXTENSION]; /*! Extension where to start */
+	char cid_num[AST_MAX_EXTENSION]; /*! CallerID Number */
+	char mailbox[AST_MAX_EXTENSION]; /*! Mailbox for MWI */
+	int lastmsgssent; /*! Used by MWI */
+	time_t nextmsgcheck; /*! Used by MWI */
+	char musicclass[MAX_MUSICCLASS]; /*! MusicOnHold class */
+	unsigned int callgroup; /*! Call group */
+	unsigned int pickupgroup; /*! Pickup group */
+	char accountcode[AST_MAX_ACCOUNT_CODE]; /*! Account code (for billing) */
+	int amaflags; /*! AMA flags (for billing) */
+	format_t capability; /*! Codec supported */
+	char parkinglot[AST_MAX_CONTEXT]; /*! Parkinglot */
 	struct unistim_line *next;
 	struct unistim_device *parent;
+	AST_LIST_ENTRY(unistim_line) list;
 };
 
 /*! 
  * \brief A device containing one or more lines 
  */
 static struct unistim_device {
+	ast_mutex_t lock;
 	int receiver_state;	      /*!< state of the receiver (see ReceiverState) */
 	int size_phone_number;	  /*!< size of the phone number */
-	char phone_number[16];	  /*!< the phone number entered by the user */
-	char redial_number[16];	 /*!< the last phone number entered by the user */
-	int phone_current;		      /*!< Number of the current phone */
-	int pos_fav;			    /*!< Position of the displayed favorites (used for scrolling) */
+	char context[AST_MAX_EXTENSION]; /*!< Context to start in */
+	char phone_number[AST_MAX_EXTENSION];	  /*!< the phone number entered by the user */
+	char redial_number[AST_MAX_EXTENSION];	 /*!< the last phone number entered by the user */
 	char id[18];			    /*!< mac address of the current phone in ascii */
 	char name[DEVICE_NAME_LEN];     /*!< name of the device */
-	int softkeylinepos;		     /*!< position of the line softkey (default 0) */
-	char softkeylabel[6][11];       /*!< soft key label */
-	char softkeynumber[6][16];      /*!< number dialed when the soft key is pressed */
-	char softkeyicon[6];	    /*!< icon number */
-	char softkeydevice[6][16];      /*!< name of the device monitored */
-	struct unistim_device *sp[6];   /*!< pointer to the device monitored by this soft key */
+	char softkeylabel[FAVNUM][11];       /*!< soft key label */
+	char softkeynumber[FAVNUM][AST_MAX_EXTENSION];      /*!< number dialed when the soft key is pressed */
+	char softkeyicon[FAVNUM];	    /*!< icon number */
+	char softkeydevice[FAVNUM][16];      /*!< name of the device monitored */
+	struct unistim_subchannel *ssub[FAVNUM];
+	struct unistim_line *sline[FAVNUM];
+	struct unistim_device *sp[FAVNUM];   /*!< pointer to the device monitored by this soft key */
+	char language[MAX_LANGUAGE];    /*!< Language for asterisk sounds */
 	int height;							/*!< The number of lines the phone can display */
 	char maintext0[25];		     /*!< when the phone is idle, display this string on line 0 */
 	char maintext1[25];		     /*!< when the phone is idle, display this string on line 1 */
@@ -450,27 +400,31 @@
 	struct ast_tone_zone *tz;	       /*!< Tone zone for res_indications (ring, busy, congestion) */
 	char ringvolume;			/*!< Ring volume */
 	char ringstyle;			 /*!< Ring melody */
+	char cwvolume;			/*!< Ring volume on call waiting */
+	char cwstyle;			 /*!< Ring melody on call waiting */
+	time_t nextdial;		/*!< Timer used for dial by timeout */
 	int rtp_port;			   /*!< RTP port used by the phone */
 	int rtp_method;			 /*!< Select the unistim data used to establish a RTP session */
 	int status_method;		      /*!< Select the unistim packet used for sending status text */
 	char codec_number;		      /*!< The current codec used to make calls */
 	int missed_call;			/*!< Number of call unanswered */
 	int callhistory;			/*!< Allowed to record call history */
+        int sharp_dial;				/*!< Execute Dial on '#' or not */
 	char lst_cid[TEXT_LENGTH_MAX];  /*!< Last callerID received */
 	char lst_cnm[TEXT_LENGTH_MAX];  /*!< Last callername recevied */
 	char call_forward[AST_MAX_EXTENSION];   /*!< Forward number */
 	int output;				     /*!< Handset, headphone or speaker */
 	int previous_output;	    /*!< Previous output */
 	int volume;				     /*!< Default volume */
+        int selected;                           /*!< softkey selected */
 	int mute;				       /*!< Mute mode */
-	int moh;					/*!< Music on hold in progress */
 	int nat;					/*!< Used by the obscure ast_rtp_setnat */
 	enum autoprov_extn extension;   /*!< See ifdef EXTENSION for valid values */
 	char extension_number[11];      /*!< Extension number entered by the user */
 	char to_delete;			 /*!< Used in reload */
-	time_t start_call_timestamp;    /*!< timestamp for the length calculation of the call */
 	struct ast_silence_generator *silence_generator;
-	struct unistim_line *lines;
+	AST_LIST_HEAD(,unistim_subchannel) subs; /*!< pointer to our current connection, channel... */
+	AST_LIST_HEAD(,unistim_line) lines;
 	struct ast_ha *ha;
 	struct unistimsession *session;
 	struct unistim_device *next;
@@ -497,6 +451,19 @@
 	struct unistimsession *next;
 } *sessions = NULL;
 
+struct unistim_menu_item {
+	char *label;
+	int state;
+	void (*handle_option)(struct unistimsession *);
+};
+
+struct unistim_languages {
+	char *label;
+	char *lang_short;
+	int encoding;
+	struct ao2_container *trans;
+};
+
 /*!
  * \page Unistim datagram formats
  *
@@ -515,6 +482,8 @@
 
 static const unsigned char packet_recv_firm_version[] =
 	{ 0x00, 0x00, 0x00, 0x13, 0x9a, 0x0a, 0x02 };
+static const unsigned char packet_recv_it_type[] =
+	{ 0x00, 0x00, 0x00, 0x13, 0x9a, 0x04, 0x03 };
 static const unsigned char packet_recv_pressed_key[] =
 	{ 0x00, 0x00, 0x00, 0x13, 0x99, 0x04, 0x00 };
 static const unsigned char packet_recv_pick_up[] =
@@ -628,9 +597,9 @@
 };
 static const unsigned char packet_send_Contrast[] =
 	{ 0x17, 0x04, 0x24, /*Contrast */ 0x08 };
-static const unsigned char packet_send_StartTimer[] =
-	{ 0x17, 0x05, 0x0b, 0x05, 0x00, 0x17, 0x08, 0x16, /* Text */ 0x44, 0x75, 0x72, 0xe9,
-0x65 };
+static const unsigned char packet_send_start_timer[] =
+	{ 0x17, 0x05, 0x0b, /*Timer option*/0x05, /* Timer ID */0x00, 0x17, 0x08, 0x16,
+	/* Text */ 0x44, 0x75, 0x72, 0xe9, 0x65 };
 static const unsigned char packet_send_stop_timer[] = { 0x17, 0x05, 0x0b, 0x02, 0x00 };
 static const unsigned char packet_send_icon[] = { 0x17, 0x05, 0x14, /*pos */ 0x00, /*icon */ 0x25 };      /* display an icon in front of the text zone */
 static const unsigned char packet_send_S7[] = { 0x17, 0x06, 0x0f, 0x30, 0x07, 0x07 };
@@ -662,6 +631,23 @@
 	{ 0x17, 0x0b, 0x19, /* pos [08|28|48|68] */ 0x00, /* text */ 0x20, 0x20, 0x20, 0x20,
 0x20, 0x20, 0x20 /* end_text */  };
 
+/* Multiple character set support */
+/* ISO-8859-1 - Western European) */
+static const unsigned char packet_send_charset_iso_8859_1[] =  
+	{ 0x17, 0x08, 0x21, 0x1b, 0x2d, 0x41, 0x1b, 0x00 };
+/* ISO-8859-2 - Central European) */
+static const unsigned char packet_send_charset_iso_8859_2[] =  
+	{ 0x17, 0x08, 0x21, 0x1b, 0x2d, 0x42, 0x1b, 0x00 };
+/* ISO-8859-4 - Baltic) */
+static const unsigned char packet_send_charset_iso_8859_4[] =  
+	{ 0x17, 0x08, 0x21, 0x1b, 0x2d, 0x44, 0x1b, 0x00 };
+/* ISO 8859-5 - cyrilic */
+static const unsigned char packet_send_charset_iso_8859_5[] =  
+	{ 0x17, 0x08, 0x21, 0x1b, 0x2d, 0x4c, 0x1b, 0x00 };
+/* Japaneese (ISO-2022-JP ?) */
+static const unsigned char packet_send_charset_iso_2022_jp[] = 
+	{ 0x17, 0x08, 0x21, 0x1b, 0x29, 0x49, 0x1b, 0x7e };
+
 static const unsigned char packet_send_led_update[] = { 0x19, 0x04, 0x00, 0x00 };
 
 static const unsigned char packet_send_query_basic_manager_04[] = { 0x1a, 0x04, 0x01, 0x04 };
@@ -684,7 +670,7 @@
 static int unload_module(void);
 static int reload_config(void);
 static void show_main_page(struct unistimsession *pte);
-static struct ast_channel *unistim_request(const char *type, format_t format, const struct ast_channel *requestor, 
+static struct ast_channel *unistim_request(const char *type, format_t format, const struct ast_channel *requestor,
 	void *data, int *cause);
 static int unistim_call(struct ast_channel *ast, char *dest, int timeout);
 static int unistim_hangup(struct ast_channel *ast);
@@ -703,7 +689,7 @@
 	char *line1);
 static void change_callerid(struct unistimsession *pte, int type, char *callerid);
 
-static const struct ast_channel_tech unistim_tech = {
+static struct ast_channel_tech unistim_tech = {
 	.type = channel_type,
 	.description = tdesc,
 	.capabilities = CAPABILITY,
@@ -722,15 +708,125 @@
 	.bridge = ast_rtp_instance_bridge,
 };
 
-static void display_last_error(const char *sz_msg)
+static void send_start_rtp(struct unistim_subchannel *);
+
+static void send_callerid_screen(struct unistimsession *, struct unistim_subchannel *);
+static void key_favorite(struct unistimsession *, char);
+
+static void handle_select_codec(struct unistimsession *);
+static void handle_select_language(struct unistimsession *);
+static int find_language(const char*);
+
+static int unistim_free_sub(struct unistim_subchannel *);
+
+struct unistim_menu_item options_menu[] =
 {
-	time_t cur_time;
-	
-	time(&cur_time);
+	{"Change codec", STATE_SELECTCODEC, handle_select_codec},
+	{"Language", STATE_SELECTLANGUAGE, handle_select_language},
+	{NULL, 0, NULL}
+};
+
+struct unistim_languages options_languages[] =
+{
+	{"English", "en", ISO_8859_1, NULL},
+	{"Russian", "ru", ISO_8859_5, NULL},
+	{NULL, NULL, 0, NULL}
+};
+
+struct ustm_lang_entry {
+	const char *str_orig;
+	const char *str_trans;
+};
+
+static int lang_hash_fn(const void *obj, const int flags)
+{
+	const struct ustm_lang_entry *entry = obj;
+	return ast_str_hash(entry->str_orig);
+}
+
+static int lang_cmp_fn(void *obj, void *arg, int flags)
+{
+	struct ustm_lang_entry *entry1 = obj;
+	struct ustm_lang_entry *entry2 = arg;
+
+	return (!strcmp(entry1->str_orig, entry2->str_orig)) ? (CMP_MATCH | CMP_STOP) : 0;
+}
+
+static const char *ustmtext(const char *str, struct unistimsession *pte) 
+{
+	struct ustm_lang_entry *lang_entry;
+	struct ustm_lang_entry le_search;
+	struct unistim_languages *lang = NULL;
+	if (pte->device) {
+		lang = &options_languages[find_language(pte->device->language)];
+	}
+	if (!lang)
+		return str;
+	/* Check if specified language exists */
+	if (!lang->trans) {
+		char tmp[1024], *p, *p_orig = NULL, *p_trans = NULL;
+		FILE *f;
+
+		lang->trans = ao2_container_alloc(8, lang_hash_fn, lang_cmp_fn);
+		snprintf(tmp, sizeof(tmp), "%s/%s/%s.po", ast_config_AST_VAR_DIR,
+			 USTM_LANG_DIR, lang->lang_short);
+		f = fopen(tmp, "r");
+		if (!f) {
+			return str;
+		}
+		while (fgets(tmp, sizeof(tmp), f)) {
+			if (!(p = strchr(tmp, '\n'))) {
+				ast_log(LOG_ERROR, "input line too long.\n");
+			}
+			*p = '\0';
+			if (!(p = strchr(tmp, '"')))
+				continue;
+			if (tmp == strstr(tmp, "msgid")) {
+				p_orig = ast_strdup(p + 1);
+				p = strchr(p_orig, '"');
+			} else if (tmp == strstr(tmp, "msgstr")) {
+				p_trans = ast_strdup(p + 1);
+				p = strchr(p_trans, '"');
+			} else {
+				continue;
+			}
+			*p = '\0';
+			if (!p_trans || !p_orig)
+				continue;
+			if (ast_strlen_zero(p_trans)) {
+				ast_free(p_trans);
+				ast_free(p_orig);
+				p_trans = NULL;
+				p_orig = NULL;
+				continue;
+			}
+			if (!(lang_entry = ao2_alloc(sizeof(*lang_entry), NULL))) {
+				fclose(f);
+				return str;
+			}
+
+			lang_entry->str_trans = p_trans;
+			lang_entry->str_orig = p_orig;
+			ao2_link(lang->trans, lang_entry);
+			p_trans = NULL;
+			p_orig = NULL;
+		}
+
+		fclose(f);
+	}
 
+	le_search.str_orig = str;
+	lang_entry = ao2_find(lang->trans, &le_search, OBJ_POINTER);
+	if (lang_entry)
+		return lang_entry->str_trans;
+
+	return str;
+}
+
+static void display_last_error(const char *sz_msg)
+{
 	/* Display the error message */
-	ast_log(LOG_WARNING, "%s %s : (%u) %s\n", ctime(&cur_time), sz_msg, errno,
-			strerror(errno));
+	ast_log(LOG_WARNING, "%s : (%u) %s\n", sz_msg, errno, strerror(errno));
 }
 
 static unsigned int get_tick_count(void)
@@ -889,6 +985,7 @@
 	}
 	ast_mutex_init(&s->lock);
 	ast_mutex_lock(&sessionlock);
+	s->device = NULL;
 	s->next = sessions;
 	sessions = s;
 
@@ -956,8 +1053,8 @@
 	BUFFSEND;
 	if (unistimdebug)
 		ast_verb(0, "Sending start timer\n");
-	memcpy(buffsend + SIZE_HEADER, packet_send_StartTimer, sizeof(packet_send_StartTimer));
-	send_client(SIZE_HEADER + sizeof(packet_send_StartTimer), buffsend, pte);
+	memcpy(buffsend + SIZE_HEADER, packet_send_start_timer, sizeof(packet_send_start_timer));
+	send_client(SIZE_HEADER + sizeof(packet_send_start_timer), buffsend, pte);
 }
 
 static void send_stop_timer(struct unistimsession *pte)
@@ -969,7 +1066,7 @@
 	send_client(SIZE_HEADER + sizeof(packet_send_stop_timer), buffsend, pte);
 }
 
-static void Sendicon(unsigned char pos, unsigned char status, struct unistimsession *pte)
+static void send_icon(unsigned char pos, unsigned char status, struct unistimsession *pte)
 {
 	BUFFSEND;
 	if (unistimdebug)
@@ -1027,9 +1124,9 @@
 
 /* Positions for favorites
  |--------------------|
- |  5	    2    |
- |  4	    1    |
- |  3	    0    |
+ |  5	         2    | <-- not on screen in i2002
+ |  4	         1    |
+ |  3	         0    |
 */
 
 /* status (icons) : 00 = nothing, 2x/3x = see parser.h, 4x/5x = blink fast, 6x/7x = blink slow */
@@ -1046,44 +1143,127 @@
 	buffsend[10] = pos;
 	buffsend[24] = pos;
 	buffsend[25] = status;
-	i = strlen(text);
+	i = strlen(ustmtext(text, pte));
 	if (i > FAV_MAX_LENGTH)
 		i = FAV_MAX_LENGTH;
-	memcpy(buffsend + FAV_MAX_LENGTH + 1, text, i);
+	memcpy(buffsend + FAV_MAX_LENGTH + 1, ustmtext(text, pte), i);
 	send_client(SIZE_HEADER + sizeof(packet_send_favorite), buffsend, pte);
 }
 
+static void send_favorite_short(unsigned char pos, unsigned char status, struct unistimsession *pte) {
+	send_favorite(pos, status, pte, pte->device->softkeylabel[pos]);
+	return;
+}
+
+static void send_favorite_selected(unsigned char status, struct unistimsession *pte) {
+	if (pte->device->selected != -1)
+		send_favorite(pte->device->selected, status, pte, pte->device->softkeylabel[pte->device->selected]);
+	return;
+}
+
+static int soft_key_visible(struct unistim_device* d, unsigned char num)
+{
+	if(d->height == 1 && num%3 == 2) {
+		return 0;
+	}
+	return 1;
+}
+
 static void refresh_all_favorite(struct unistimsession *pte)
 {
-	int i = 0;
+	unsigned char i = 0;
+	char data[256];
+	struct unistim_line *line;
+	line = AST_LIST_FIRST(&pte->device->lines);
 
 	if (unistimdebug)
 		ast_verb(0, "Refreshing all favorite\n");
-	for (i = 0; i < 6; i++) {
-		if ((pte->device->softkeyicon[i] <= FAV_ICON_HEADPHONES_ONHOLD) &&
-			(pte->device->softkeylinepos != i))
-			send_favorite((unsigned char) i, pte->device->softkeyicon[i] + 1, pte,
-						 pte->device->softkeylabel[i]);
-		else
-			send_favorite((unsigned char) i, pte->device->softkeyicon[i], pte,
-						 pte->device->softkeylabel[i]);
+	for (i = 0; i < FAVNUM; i++) {
+		unsigned char status = pte->device->softkeyicon[i];
+		
+		if (!soft_key_visible(pte->device, i)) {
+			continue;
+		}
+		if (!strcasecmp(pte->device->softkeylabel[i], "DND") && line) {
+			if (!ast_db_get("DND", line->name, data, sizeof(data))) {
+				status = FAV_ICON_SPEAKER_ONHOOK_WHITE;
+			}
+		}
+
+		send_favorite_short(i, status, pte);
+	}
+}
+
+static int is_key_favorite(struct unistim_device *d, int fav)
+{
+	if ((fav < 0) && (fav > 5))
+		return 0;
+	if (d->sline[fav])
+		return 0;
+	if (d->softkeynumber[fav][0] == '\0')
+		return 0;
+	return 1;
+}
+
+static int is_key_line(struct unistim_device *d, int fav)
+{
+	if ((fav < 0) && (fav > 5))
+		return 0;
+	if (!d->sline[fav])
+		return 0;
+	if (is_key_favorite(d, fav))
+		return 0;
+	return 1;
+}
+
+static int get_active_softkey(struct unistimsession *pte)
+{
+	return pte->device->selected;
+}
+
+static int get_avail_softkey(struct unistimsession *pte, const char* name)
+{
+	int i;
 
+	if (!is_key_line(pte->device, pte->device->selected)) {
+		pte->device->selected = -1;
+	}
+	for (i = 0; i < FAVNUM; i++) {
+		if (pte->device->selected != -1 && pte->device->selected != i) {
+			continue;
+		}
+		if (!soft_key_visible(pte->device, i))
+			continue;
+		if (pte->device->ssub[i])
+			continue;
+		if (is_key_line(pte->device, i)) {
+			if (name && strcmp(name, pte->device->sline[i]->name)) {
+				continue;
+			}
+			if (unistimdebug)
+				ast_verb(0, "Found softkey %d for device %s\n", i, name);
+			return i;
+		}
 	}
+	return -1;
 }
 
+
 /* Change the status for this phone (pte) and update for each phones where pte is bookmarked
  * use FAV_ICON_*_BLACK constant in status parameters */
 static void change_favorite_icon(struct unistimsession *pte, unsigned char status)
 {
 	struct unistim_device *d = devices;
 	int i;
-	/* Update the current phone */
-	if (pte->state != STATE_CLEANING)
-		send_favorite(pte->device->softkeylinepos, status, pte,
-					 pte->device->softkeylabel[pte->device->softkeylinepos]);
+	/* Update the current phone line softkey icon */
+	if (pte->state != STATE_CLEANING) {
+		int softkeylinepos = get_active_softkey(pte);
+		if (softkeylinepos != -1)
+			send_favorite_short(softkeylinepos, status, pte);
+	}
 	/* Notify other phones if we're in their bookmark */
 	while (d) {
-		for (i = 0; i < 6; i++) {
+		for (i = 0; i < FAVNUM; i++) {
 			if (d->sp[i] == pte->device) {  /* It's us ? */
 				if (d->softkeyicon[i] != status) {      /* Avoid resending the same icon */
 					d->softkeyicon[i] = status;
@@ -1096,30 +1276,32 @@
 	}
 }
 
-static int RegisterExtension(const struct unistimsession *pte)
+static int register_extension(const struct unistimsession *pte)
 {
+	struct unistim_line *line;
+	line = AST_LIST_FIRST(&pte->device->lines);
 	if (unistimdebug)
 		ast_verb(0, "Trying to register extension '%s' into context '%s' to %s\n",
-					pte->device->extension_number, pte->device->lines->context,
-					pte->device->lines->fullname);
-	return ast_add_extension(pte->device->lines->context, 0,
+					pte->device->extension_number, pte->device->context,
+					line->fullname);
+	return ast_add_extension(pte->device->context, 0,
 							 pte->device->extension_number, 1, NULL, NULL, "Dial",
-							 pte->device->lines->fullname, 0, "Unistim");
+							 line->fullname, 0, "Unistim");
 }
 
-static int UnregisterExtension(const struct unistimsession *pte)
+static int unregister_extension(const struct unistimsession *pte)
 {
 	if (unistimdebug)
 		ast_verb(0, "Trying to unregister extension '%s' context '%s'\n",
-					pte->device->extension_number, pte->device->lines->context);
-	return ast_context_remove_extension(pte->device->lines->context,
+					pte->device->extension_number, pte->device->context);
+	return ast_context_remove_extension(pte->device->context,
 										pte->device->extension_number, 1, "Unistim");
 }
 
 /* Free memory allocated for a phone */
 static void close_client(struct unistimsession *s)
 {
-	struct unistim_subchannel *sub;
+	struct unistim_subchannel *sub = NULL;
 	struct unistimsession *cur, *prev = NULL;
 	ast_mutex_lock(&sessionlock);
 	cur = sessions;
@@ -1134,22 +1316,31 @@
 		if (cur->device) {	      /* This session was registered ? */
 			s->state = STATE_CLEANING;
 			if (unistimdebug)
-				ast_verb(0, "close_client session %p device %p lines %p sub %p\n",
-							s, s->device, s->device->lines,
-							s->device->lines->subs[SUB_REAL]);
+				ast_verb(0, "close_client session %p device %p\n", s, s->device);
 			change_favorite_icon(s, FAV_ICON_NONE);
-			sub = s->device->lines->subs[SUB_REAL];
-			if (sub) {
+			ast_mutex_lock(&s->device->lock);
+			AST_LIST_LOCK(&s->device->subs);
+			AST_LIST_TRAVERSE_SAFE_BEGIN(&s->device->subs, sub, list) {
+				if (!sub)
+					continue;
 				if (sub->owner) {       /* Call in progress ? */
 					if (unistimdebug)
 						ast_verb(0, "Aborting call\n");
 					ast_queue_hangup_with_cause(sub->owner, AST_CAUSE_NETWORK_OUT_OF_ORDER);
+				} else {
+					if (unistimdebug)
+						ast_debug(1, "Released sub %d of channel %s@%s\n", sub->subtype, sub->parent->name, s->device->name);
+					AST_LIST_REMOVE_CURRENT(list);
+					unistim_free_sub(sub);
 				}
-			} else
-				ast_log(LOG_WARNING, "Freeing a client with no subchannel !\n");
+			}
+			AST_LIST_TRAVERSE_SAFE_END;
+			AST_LIST_UNLOCK(&s->device->subs);
+
 			if (!ast_strlen_zero(s->device->extension_number))
-				UnregisterExtension(s);
+				unregister_extension(s);
 			cur->device->session = NULL;
+			ast_mutex_unlock(&s->device->lock);
 		} else {
 			if (unistimdebug)
 				ast_verb(0, "Freeing an unregistered client\n");
@@ -1212,6 +1403,12 @@
 {
 	int i;
 	BUFFSEND;
+	if (!text) {
+		ast_log(LOG_ERROR, "Asked to display NULL text (pos %d, inverse flag %d)\n", pos, inverse);
+		return;
+	}
+	if (pte->device && pte->device->height == 1 && pos != TEXT_LINE0)
+		return;
 	if (unistimdebug)
 		ast_verb(0, "Sending text at pos %d, inverse flag %d\n", pos, inverse);
 	memcpy(buffsend + SIZE_HEADER, packet_send_text, sizeof(packet_send_text));
@@ -1371,6 +1568,11 @@
 
 }
 
+static void send_idle_clock(struct unistimsession *pte)
+{
+	send_text(TEXT_LINE0, TEXT_NORMAL, pte, "");
+}
+
 static void send_date_time(struct unistimsession *pte)
 {
 	BUFFSEND;
@@ -1449,6 +1651,43 @@
 	return;
 }
 
+static void send_charset_update(struct unistimsession *pte, int charset)
+{
+	const unsigned char* packet_send_charset;
+	int packet_size;
+	BUFFSEND;
+	if (unistimdebug)
+		ast_verb(0, "Sending set default charset\n");
+	if (charset == LANG_DEFAULT) {
+		charset = options_languages[find_language(pte->device->language)].encoding;
+	}
+	switch (charset) {
+	case ISO_8859_2:
+		packet_send_charset = packet_send_charset_iso_8859_2;
+		packet_size = sizeof(packet_send_charset_iso_8859_2);
+		break;
+	case ISO_8859_4:
+		packet_send_charset = packet_send_charset_iso_8859_4;
+		packet_size = sizeof(packet_send_charset_iso_8859_4);
+		break;
+	case ISO_8859_5:
+		packet_send_charset = packet_send_charset_iso_8859_5;
+		packet_size = sizeof(packet_send_charset_iso_8859_5);
+		break;
+	case ISO_2022_JP:
+		packet_send_charset = packet_send_charset_iso_2022_jp;
+		packet_size = sizeof(packet_send_charset_iso_2022_jp);
+		break;
+	case ISO_8859_1:
+	default:
+		packet_send_charset = packet_send_charset_iso_8859_1;
+		packet_size = sizeof(packet_send_charset_iso_8859_1);
+	}
+	memcpy(buffsend + SIZE_HEADER, packet_send_charset, packet_size);
+	send_client(SIZE_HEADER + packet_size, buffsend, pte);
+	return;
+}
+
 static void rcv_resume_connection_with_server(struct unistimsession *pte)
 {
 	BUFFSEND;
@@ -1474,7 +1713,6 @@
 			s->device = d;
 			d->session = s;
 			d->codec_number = DEFAULT_CODEC;
-			d->pos_fav = 0;
 			d->missed_call = 0;
 			d->receiver_state = STATE_ONHOOK;
 			break;
@@ -1489,37 +1727,123 @@
 	return 1;
 }
 
-static int alloc_sub(struct unistim_line *l, int x)
+static void unistim_line_copy(struct unistim_line *dst, struct unistim_line *src)
+{
+	memcpy(dst, src, sizeof(*dst)); /* this over writes the cap ptr, so we have to reset it */
+}
+
+static struct unistim_line *unistim_line_destroy(struct unistim_line *l)
+{
+	if (!l) {
+		return NULL;
+	}
+	ast_free(l);
+	return NULL;
+}
+
+static struct unistim_line *unistim_line_alloc(void)
+{
+	struct unistim_line *l;
+	if (!(l = ast_calloc(1, sizeof(*l)))) {
+		return NULL;
+	}
+	return l;
+}
+
+static int unistim_free_sub(struct unistim_subchannel *sub) {
+	if (unistimdebug)
+		ast_debug(1, "Released sub %d of channel %s@%s\n", sub->subtype, sub->parent->name, sub->parent->parent->name);
+	if (sub->ss_thread != AST_PTHREADT_NULL) {
+		//pthread_kill(sub->ss_thread, SIGURG);
+		//pthread_join(sub->ss_thread, NULL);
+	}
+	ast_mutex_destroy(&sub->lock);
+	ast_free(sub);
+	return 0;
+}
+
+static struct unistim_subchannel *unistim_alloc_sub(struct unistim_device *d, int x)
 {
 	struct unistim_subchannel *sub;
 	if (!(sub = ast_calloc(1, sizeof(*sub))))
-		return 0;
+		return NULL;
 
 	if (unistimdebug)
-		ast_verb(3, "Allocating UNISTIM subchannel #%d on %s@%s ptr=%p\n", x, l->name, l->parent->name, sub);
-	sub->parent = l;
+		ast_verb(3, "Allocating UNISTIM subchannel #%d on %s ptr=%p\n", x, d->name, sub);
+	sub->parent = NULL;
+	sub->ss_thread = AST_PTHREADT_NULL;
 	sub->subtype = x;
-	l->subs[x] = sub;
+	AST_LIST_LOCK(&d->subs);
+	AST_LIST_INSERT_TAIL(&d->subs, sub, list);
+	AST_LIST_UNLOCK(&d->subs);
 	ast_mutex_init(&sub->lock);
-	return 1;
+	return sub;
 }
 
-static int unalloc_sub(struct unistim_line *p, int x)
+static int unistim_unalloc_sub(struct unistim_device *d, struct unistim_subchannel *sub)
 {
-	if (!x) {
-		ast_log(LOG_WARNING, "Trying to unalloc the real channel %s@%s?!?\n", p->name,
-				p->parent->name);
-		return -1;
+	struct unistim_subchannel *s;
+
+	AST_LIST_LOCK(&d->subs);
+	AST_LIST_TRAVERSE_SAFE_BEGIN(&d->subs, s, list) {
+		if (!s)
+			continue;
+		if (s != sub)
+			continue;
+		AST_LIST_REMOVE_CURRENT(list);
+		unistim_free_sub(sub);
 	}
-	if (unistimdebug)
-		ast_debug(1, "Released sub %d of channel %s@%s\n", x, p->name,
-				p->parent->name);
-	ast_mutex_destroy(&p->lock);
-	ast_free(p->subs[x]);
-	p->subs[x] = 0;
+	AST_LIST_TRAVERSE_SAFE_END;
+	AST_LIST_UNLOCK(&d->subs);
 	return 0;
 }
 
+static const char *subtype_tostr(const int type) 
+{
+	switch (type) {
+	case SUB_REAL:
+		return "REAL";
+	case SUB_ONHOLD:
+		return "ONHOLD";
+	case SUB_RING:
+		return "RINGING";
+	case SUB_THREEWAY:
+		return "THREEWAY";
+	}
+	return "UNKNOWN";
+}
+
+static const char *ptestate_tostr(const int type) 
+{
+	switch (type) {
+	case STATE_INIT:
+		return "INIT";
+	case STATE_AUTHDENY:
+		return "AUTHDENY";
+	case STATE_MAINPAGE:
+		return "MAINPAGE";
+	case STATE_EXTENSION:
+		return "EXTENSION";
+	case STATE_DIALPAGE:
+		return "DIALPAGE";
+	case STATE_RINGING:
+		return "RINGING";
+	case STATE_CALL:
+		return "CALL";
+	case STATE_SELECTOPTION:
+		return "SELECTOPTION";
+	case STATE_SELECTCODEC:
+		return "SELECTCODEC";
+	case STATE_SELECTLANGUAGE:
+		return "SELECTLANGUAGE";
+	case STATE_CLEANING:
+		return "CLEARING";
+	case STATE_HISTORY:
+		return "HISTORY";
+	}
+	return "UNKNOWN";
+}
+
 static void rcv_mac_addr(struct unistimsession *pte, const unsigned char *buf)
 {
 	BUFFSEND;
@@ -1545,60 +1869,69 @@
 		case AUTOPROVISIONING_YES:
 			{
 				struct unistim_device *d, *newd;
-				struct unistim_line *newl;
+				struct unistim_line *newl = NULL, *l;
 				if (unistimdebug)
 					ast_verb(0, "New phone, autoprovisioning on\n");
 				/* First : locate the [template] section */
 				ast_mutex_lock(&devicelock);
 				d = devices;
 				while (d) {
-					if (!strcasecmp(d->name, "template")) {
-						/* Found, cloning this entry */
-						if (!(newd = ast_malloc(sizeof(*newd)))) {
-							ast_mutex_unlock(&devicelock);
-							return;
-						}
-
-						memcpy(newd, d, sizeof(*newd));
-						if (!(newl = ast_malloc(sizeof(*newl)))) {
-							ast_free(newd);
-							ast_mutex_unlock(&devicelock);
-							return;
-						}
-
-						memcpy(newl, d->lines, sizeof(*newl));
-						if (!alloc_sub(newl, SUB_REAL)) {
-							ast_free(newd);
-							ast_free(newl);
-							ast_mutex_unlock(&devicelock);
-							return;
+					if (strcasecmp(d->name, "template")) {
+						d = d->next;
+						continue;
+					}
+					/* Found, cloning this entry */
+					if (!(newd = ast_malloc(sizeof(*newd)))) {
+						ast_mutex_unlock(&devicelock);
+						return;
+					}
+					memcpy(newd, d, sizeof(*newd));
+					ast_mutex_init(&newd->lock);
+					newd->lines.first = NULL;
+					newd->lines.last = NULL;
+					AST_LIST_LOCK(&d->lines);
+					AST_LIST_TRAVERSE(&d->lines, l, list) {
+						if (!(newl = unistim_line_alloc())) {
+							break;
 						}
-						/* Ok, now updating some fields */
-						ast_copy_string(newd->id, addrmac, sizeof(newd->id));
-						ast_copy_string(newd->name, addrmac, sizeof(newd->name));
-						if (newd->extension == EXTENSION_NONE)
-							newd->extension = EXTENSION_ASK;
-						newd->lines = newl;
-						newd->receiver_state = STATE_ONHOOK;
-						newd->session = pte;
-						newd->to_delete = -1;
-						pte->device = newd;
-						newd->next = NULL;
+						unistim_line_copy(l, newl);
 						newl->parent = newd;
-						strcpy(newl->name, d->lines->name);
-						snprintf(d->lines->name, sizeof(d->lines->name), "%d",
-								 atoi(d->lines->name) + 1);
+						ast_copy_string(newl->name, l->name, sizeof(newl->name));
 						snprintf(newl->fullname, sizeof(newl->fullname), "USTM/%s@%s",
 								 newl->name, newd->name);
-						/* Go to the end of the linked chain */
-						while (d->next) {
-							d = d->next;
-						}
-						d->next = newd;
-						d = newd;
-						break;
+						snprintf(l->name, sizeof(l->name), "%d", atoi(l->name) + 1);
+
+						AST_LIST_LOCK(&newd->lines);
+						AST_LIST_INSERT_TAIL(&newd->lines, newl, list);
+						AST_LIST_UNLOCK(&newd->lines);
+					}
+					AST_LIST_UNLOCK(&d->lines);
+					if (!newl) {
+						ast_free(newd);
+						ast_mutex_unlock(&devicelock);
+					}
+
+					/* Ok, now updating some fields */
+					ast_copy_string(newd->id, addrmac, sizeof(newd->id));
+					ast_copy_string(newd->name, addrmac, sizeof(newd->name));
+					if (newd->extension == EXTENSION_NONE) {
+						newd->extension = EXTENSION_ASK;
+					}
+
+					newd->receiver_state = STATE_ONHOOK;
+					newd->session = pte;
+					newd->language[0] = '\0';
+					newd->to_delete = -1;
+					newd->next = NULL;
+					pte->device = newd;
+
+					/* Go to the end of the linked chain */
+					while (d->next) {
+						d = d->next;
 					}
-					d = d->next;
+					d->next = newd;
+					d = newd;
+					break;
 				}
 				ast_mutex_unlock(&devicelock);
 				if (!d) {
@@ -1610,17 +1943,28 @@
 		case AUTOPROVISIONING_TN:
 			pte->state = STATE_AUTHDENY;
 			break;
-		case AUTOPROVISIONING_DB:
-			ast_log(LOG_WARNING,
-					"Autoprovisioning with database is not yet functional\n");
-			break;
 		default:
 			ast_log(LOG_WARNING, "Internal error : unknown autoprovisioning value = %d\n",
 					autoprovisioning);
 		}
 	}
 	if (pte->state != STATE_AUTHDENY) {
+		struct unistim_line *line;
+		struct unistim_subchannel *sub;
+		
 		ast_verb(3, "Device '%s' successfuly registered\n", pte->device->name);
+		
+		AST_LIST_LOCK(&pte->device->subs);
+		AST_LIST_TRAVERSE_SAFE_BEGIN(&pte->device->subs, sub, list) {
+			if (sub) {
+				ast_log(LOG_ERROR, "Subchannel lost sice reboot. Hanged channel may apear!\n");
+				AST_LIST_REMOVE_CURRENT(list);
+				ast_free(sub);
+			}
+		}
+		AST_LIST_TRAVERSE_SAFE_END;
+		AST_LIST_UNLOCK(&pte->device->subs);
+		
 		switch (pte->device->extension) {
 		case EXTENSION_NONE:
 			pte->state = STATE_MAINPAGE;
@@ -1631,16 +1975,17 @@
 				pte->state = STATE_EXTENSION;
 			else {
 				/* Yes, because of a phone reboot. We don't ask again for the TN */
-				if (RegisterExtension(pte))
+				if (register_extension(pte))
 					pte->state = STATE_EXTENSION;
 				else
 					pte->state = STATE_MAINPAGE;
 			}
 			break;
 		case EXTENSION_LINE:
-			ast_copy_string(pte->device->extension_number, pte->device->lines->name,
+			line = AST_LIST_FIRST(&pte->device->lines);
+			ast_copy_string(pte->device->extension_number, line->name,
 							sizeof(pte->device->extension_number));
-			if (RegisterExtension(pte))
+			if (register_extension(pte))
 				pte->state = STATE_EXTENSION;
 			else
 				pte->state = STATE_MAINPAGE;
@@ -1725,20 +2070,18 @@
 
 	snprintf(tmp, sizeof(tmp), "%s/%s", ast_config_AST_LOG_DIR, USTM_LOG_DIR);
 	if (ast_mkdir(tmp, 0770)) {
-		if (errno != EEXIST) {
-			display_last_error("Unable to create directory for history");
-			return -1;
-		}
+		ast_log(LOG_WARNING, "Unable to create directory for history");
+                return -1;
 	}
 
 	ast_localtime(&now, &atm, NULL);
 	if (ismissed) {
 		if (way == 'i')
-			strcpy(tmp2, "Miss");
+			ast_copy_string(tmp2, ustmtext("Miss", pte), sizeof(tmp2));
 		else
-			strcpy(tmp2, "Fail");
+			ast_copy_string(tmp2, ustmtext("Fail", pte), sizeof(tmp2));
 	} else
-		strcpy(tmp2, "Answ");
+		ast_copy_string(tmp2, ustmtext("Answ", pte), sizeof(tmp2));
 	snprintf(line1, sizeof(line1), "%04d/%02d/%02d %02d:%02d:%02d %s",
 			 atm.tm_year + 1900, atm.tm_mon + 1, atm.tm_mday, atm.tm_hour,
 			 atm.tm_min, atm.tm_sec, tmp2);
@@ -1856,41 +2199,14 @@
 	return 0;
 }
 
-static void cancel_dial(struct unistimsession *pte)
-{
-	send_no_ring(pte);
-	pte->device->missed_call++;
-	write_history(pte, 'i', 1);
-	show_main_page(pte);
-	return;
-}
-
-static void swap_subs(struct unistim_line *p, int a, int b)
+static void unistim_quiet_chan(struct ast_channel *chan)
 {
-/*  struct ast_channel *towner; */
-	struct ast_rtp_instance *rtp;
-	int fds;
-
-	if (unistimdebug)
-		ast_verb(0, "Swapping %d and %d\n", a, b);
-
-	if ((!p->subs[a]->owner) || (!p->subs[b]->owner)) {
-		ast_log(LOG_WARNING,
-				"Attempted to swap subchannels with a null owner : sub #%d=%p sub #%d=%p\n",
-				a, p->subs[a]->owner, b, p->subs[b]->owner);
-		return;
+	if (chan && chan->_state == AST_STATE_UP) {
+		if (ast_test_flag(chan, AST_FLAG_MOH))
+			ast_moh_stop(chan);
+		else if (chan->generatordata)
+			ast_deactivate_generator(chan);
 	}
-	rtp = p->subs[a]->rtp;
-	p->subs[a]->rtp = p->subs[b]->rtp;
-	p->subs[b]->rtp = rtp;
-
-	fds = p->subs[a]->owner->fds[0];
-	p->subs[a]->owner->fds[0] = p->subs[b]->owner->fds[0];
-	p->subs[b]->owner->fds[0] = fds;
-
-	fds = p->subs[a]->owner->fds[1];
-	p->subs[a]->owner->fds[1] = p->subs[b]->owner->fds[1];
-	p->subs[b]->owner->fds[1] = fds;
 }
 
 static int attempt_transfer(struct unistim_subchannel *p1, struct unistim_subchannel *p2)
@@ -1898,7 +2214,7 @@
 	int res = 0;
 	struct ast_channel
 	 *chana = NULL, *chanb = NULL, *bridgea = NULL, *bridgeb = NULL, *peera =
-		NULL, *peerb = NULL, *peerc = NULL;
+		NULL, *peerb = NULL, *peerc = NULL, *peerd = NULL;
 
 	if (!p1->owner || !p2->owner) {
 		ast_log(LOG_WARNING, "Transfer attempted without dual ownership?\n");
@@ -1913,37 +2229,27 @@
 		peera = chana;
 		peerb = chanb;
 		peerc = bridgea;
+		peerd = bridgeb;
 	} else if (bridgeb) {
 		peera = chanb;
 		peerb = chana;
 		peerc = bridgeb;
+		peerd = bridgea;
 	}
-
+	
 	if (peera && peerb && peerc && (peerb != peerc)) {
-		/*ast_quiet_chan(peera);
-		   ast_quiet_chan(peerb);
-		   ast_quiet_chan(peerc);
-		   ast_quiet_chan(peerd); */
-
-		if (peera->cdr && peerb->cdr) {
-			peerb->cdr = ast_cdr_append(peerb->cdr, peera->cdr);
-		} else if (peera->cdr) {
-			peerb->cdr = peera->cdr;
-		}
-		peera->cdr = NULL;
-
-		if (peerb->cdr && peerc->cdr) {
-			peerb->cdr = ast_cdr_append(peerb->cdr, peerc->cdr);
-		} else if (peerc->cdr) {
-			peerb->cdr = peerc->cdr;
-		}
-		peerc->cdr = NULL;
-
+		unistim_quiet_chan(peera);
+		unistim_quiet_chan(peerb);
+		unistim_quiet_chan(peerc);
+		if (peerd)
+			unistim_quiet_chan(peerd); 
+	    
+		ast_log(LOG_NOTICE, "UNISTIM transfer: trying to masquerade %s into %s\n", peerc->name, peerb->name);
 		if (ast_channel_masquerade(peerb, peerc)) {
 			ast_log(LOG_WARNING, "Failed to masquerade %s into %s\n", peerb->name,
 					peerc->name);
 			res = -1;
-		}
+		} 
 		return res;
 	} else {
 		ast_log(LOG_NOTICE,
@@ -1976,42 +2282,143 @@
 	memcpy(data, callerid, size);
 }
 
-static void close_call(struct unistimsession *pte)
+static struct unistim_subchannel* get_sub(struct unistim_device *device, int type) 
 {
-	struct unistim_subchannel *sub;
-	struct unistim_line *l = pte->device->lines;
+	struct unistim_subchannel *sub = NULL;
 
-	sub = pte->device->lines->subs[SUB_REAL];
-	send_stop_timer(pte);
-	if (sub->owner) {
-		sub->alreadygone = 1;
-		if (l->subs[SUB_THREEWAY]) {
-			l->subs[SUB_THREEWAY]->alreadygone = 1;
-			if (attempt_transfer(sub, l->subs[SUB_THREEWAY]) < 0)
-				ast_verb(0, "attempt_transfer failed.\n");
-		} else
-			ast_queue_hangup(sub->owner);
-	} else {
-		if (l->subs[SUB_THREEWAY]) {
-			if (l->subs[SUB_THREEWAY]->owner)
-				ast_queue_hangup_with_cause(l->subs[SUB_THREEWAY]->owner, AST_CAUSE_NORMAL_CLEARING);
-			else
-				ast_log(LOG_WARNING, "threeway sub without owner\n");
-		} else
-			ast_verb(0, "USTM(%s@%s-%d) channel already destroyed\n", sub->parent->name,
-						sub->parent->parent->name, sub->subtype);
+	AST_LIST_LOCK(&device->subs);
+	AST_LIST_TRAVERSE(&device->subs, sub, list) {
+		if (!sub)
+			continue;
+		if (sub->subtype == type) {
+			break;
+		}
 	}
-	change_callerid(pte, 0, pte->device->redial_number);
-	change_callerid(pte, 1, "");
-	write_history(pte, 'o', pte->device->missed_call);
-	pte->device->missed_call = 0;
-	show_main_page(pte);
-	return;
+	AST_LIST_UNLOCK(&device->subs);
+
+	return sub;
 }
 
-static void IgnoreCall(struct unistimsession *pte)
+static void sub_start_silence(struct unistimsession *pte, struct unistim_subchannel *sub) 
 {
-	send_no_ring(pte);
+	/* Silence our channel */
+	if (!pte->device->silence_generator) {
+		pte->device->silence_generator =
+			ast_channel_start_silence_generator(sub->owner);
+		if (pte->device->silence_generator == NULL)
+			ast_log(LOG_WARNING, "Unable to start a silence generator.\n");
+		else if (unistimdebug)
+			ast_verb(0, "Starting silence generator\n");
+	}
+
+}
+
+static void sub_stop_silence(struct unistimsession *pte, struct unistim_subchannel *sub) 
+{
+	/* Stop the silence generator */
+	if (pte->device->silence_generator) {
+		if (unistimdebug)
+			ast_verb(0, "Stopping silence generator\n");
+		if (sub->owner)
+			ast_channel_stop_silence_generator(sub->owner, pte->device->silence_generator);
+		else
+			ast_log(LOG_WARNING, "Trying to stop silence generator on a null channel !\n");
+		pte->device->silence_generator = NULL;
+	}
+}
+
+static void sub_hold(struct unistimsession *pte, struct unistim_subchannel *sub) 
+{
+	if (!sub) {
+		return;
+	}
+	sub->moh = 1;
+	sub->subtype = SUB_ONHOLD;
+	send_favorite_short(sub->softkey, FAV_ICON_ONHOLD_BLACK + FAV_BLINK_SLOW, pte);
+	send_select_output(pte, pte->device->output, pte->device->volume, MUTE_ON);
+	send_stop_timer(pte);
+	if (sub->owner) {
+		ast_queue_control_data(sub->owner, AST_CONTROL_HOLD, NULL, 0);
+		send_end_call(pte);
+	}
+	return;
+}
+
+static void sub_unhold(struct unistimsession *pte, struct unistim_subchannel *sub) 
+{
+	struct unistim_subchannel *sub_real;
+
+	sub_real = get_sub(pte->device, SUB_REAL);
+	if (sub_real) {
+	    sub_hold(pte, sub_real);
+	}
+
+	sub->moh = 0;
+	sub->subtype = SUB_REAL;
+	send_favorite_short(sub->softkey, FAV_ICON_OFFHOOK_BLACK, pte);
+	send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
+	send_start_timer(pte);
+	if (sub->owner) {
+		ast_queue_control_data(sub->owner, AST_CONTROL_UNHOLD, NULL, 0);
+		if (sub->rtp)
+			send_start_rtp(sub);
+	}
+	return;
+}
+
+static void close_call(struct unistimsession *pte)
+{
+	struct unistim_subchannel *sub, *sub_transf;
+
+	sub = get_sub(pte->device, SUB_REAL);
+	sub_transf = get_sub(pte->device, SUB_THREEWAY);
+	send_stop_timer(pte);
+	if (!sub) {
+		ast_log(LOG_WARNING, "Close call without sub\n");
+		return;
+	}
+	send_favorite_short(sub->softkey, FAV_LINE_ICON, pte);
+	if (sub->owner) {
+		sub->alreadygone = 1;
+		if (sub_transf) {
+			sub_transf->alreadygone = 1;
+			if (attempt_transfer(sub, sub_transf) < 0)
+				ast_verb(0, "attempt_transfer failed.\n");
+		} else
+			ast_queue_hangup(sub->owner);
+	} else {
+		if (sub_transf) {
+			if (sub_transf->owner)
+				ast_queue_hangup_with_cause(sub_transf->owner, AST_CAUSE_NORMAL_CLEARING);
+			else
+				ast_log(LOG_WARNING, "threeway sub without owner\n");
+		} else {
+			ast_verb(0, "USTM(%s@%s-%d) channel already destroyed\n", sub->parent->name,
+						pte->device->name, sub->softkey);
+		}
+	}        
+	change_callerid(pte, 0, pte->device->redial_number);
+	change_callerid(pte, 1, "");
+	write_history(pte, 'o', pte->device->missed_call);
+	pte->device->missed_call = 0;
+	show_main_page(pte);
+	return;
+}
+
+static void ignore_call(struct unistimsession *pte)
+{
+	send_no_ring(pte);
+	return;
+}
+
+static void discard_call(struct unistimsession *pte)
+{
+	struct unistim_subchannel* sub;
+	sub = get_sub(pte->device, SUB_RING);
+	if (!sub)
+	    return;
+	
+	ast_queue_hangup_with_cause(sub->owner, AST_CAUSE_NORMAL_CLEARING);
 	return;
 }
 
@@ -2023,7 +2430,12 @@
 	struct unistimsession *s = l->parent->session;
 	int res;
 
-	ast_verb(3, "Starting switch on '%s@%s-%d' to %s\n", l->name, l->parent->name, sub->subtype, s->device->phone_number);
+/*      Better to fail, then completely hang a phone in upredictable state
+	if (!s) { 
+		return NULL;
+	}
+*/
+	ast_verb(3, "Starting switch on '%s@%s-%d' to %s\n", l->name, l->parent->name, sub->softkey, s->device->phone_number);
 	ast_copy_string(chan->exten, s->device->phone_number, sizeof(chan->exten));
 	ast_copy_string(s->device->redial_number, s->device->phone_number,
 					sizeof(s->device->redial_number));
@@ -2031,85 +2443,52 @@
 	res = ast_pbx_run(chan);
 	if (res) {
 		ast_log(LOG_WARNING, "PBX exited non-zero\n");
-		send_tone(s, 1000, 0);;
+		send_tone(s, 1000, 0);
 	}
 	return NULL;
 }
 
-static void start_rtp(struct unistim_subchannel *sub)
+static int find_rtp_port(struct unistim_subchannel *s) 
 {
-	BUFFSEND;
+	struct unistim_subchannel *sub = NULL;
+	int rtp_start = s->parent->parent->rtp_port;
+	struct ast_sockaddr us_tmp;
 	struct sockaddr_in us = { 0, };
+
+	AST_LIST_LOCK(&s->parent->parent->subs);
+	AST_LIST_TRAVERSE(&s->parent->parent->subs, sub, list) {
+		if (!sub)
+			continue;
+		if (sub->rtp) {
+			ast_rtp_instance_get_remote_address(sub->rtp, &us_tmp);
+			ast_sockaddr_to_sin(&us_tmp, &us);
+			if (htons(us.sin_port)) {
+				rtp_start = htons(us.sin_port) + 1;
+				break;
+			}
+		}
+	}
+	AST_LIST_UNLOCK(&s->parent->parent->subs);
+	return rtp_start;
+}
+
+static void send_start_rtp(struct unistim_subchannel *sub) 
+{
+	BUFFSEND;
+
+	int codec;
 	struct sockaddr_in public = { 0, };
+	struct sockaddr_in us = { 0, };
 	struct sockaddr_in sin = { 0, };
-	format_t codec;
-	struct sockaddr_in sout = { 0, };
 	struct ast_sockaddr us_tmp;
 	struct ast_sockaddr sin_tmp;
-	struct ast_sockaddr sout_tmp;
+	struct unistimsession *pte;
 
-	/* Sanity checks */
-	if (!sub) {
-		ast_log(LOG_WARNING, "start_rtp with a null subchannel !\n");
-		return;
-	}
-	if (!sub->parent) {
-		ast_log(LOG_WARNING, "start_rtp with a null line !\n");
-		return;
-	}
-	if (!sub->parent->parent) {
-		ast_log(LOG_WARNING, "start_rtp with a null device !\n");
-		return;
-	}
-	if (!sub->parent->parent->session) {
-		ast_log(LOG_WARNING, "start_rtp with a null session !\n");
-		return;
-	}
-	sout = sub->parent->parent->session->sout;
-
-	ast_mutex_lock(&sub->lock);
-	/* Allocate the RTP */
-	if (unistimdebug)
-		ast_verb(0, "Starting RTP. Bind on %s\n", ast_inet_ntoa(sout.sin_addr));
-	ast_sockaddr_from_sin(&sout_tmp, &sout);
-	sub->rtp = ast_rtp_instance_new("asterisk", sched, &sout_tmp, NULL);
-	if (!sub->rtp) {
-		ast_log(LOG_WARNING, "Unable to create RTP session: %s binaddr=%s\n",
-				strerror(errno), ast_inet_ntoa(sout.sin_addr));
-		ast_mutex_unlock(&sub->lock);
-		return;
-	}
-	ast_rtp_instance_set_prop(sub->rtp, AST_RTP_PROPERTY_RTCP, 1);
-	if (sub->owner) {
-		sub->owner->fds[0] = ast_rtp_instance_fd(sub->rtp, 0);
-		sub->owner->fds[1] = ast_rtp_instance_fd(sub->rtp, 1);
-	}
-	ast_rtp_instance_set_qos(sub->rtp, qos.tos_audio, qos.cos_audio, "UNISTIM RTP");
-	ast_rtp_instance_set_prop(sub->rtp, AST_RTP_PROPERTY_NAT, sub->parent->parent->nat);
-
-	/* Create the RTP connection */
 	ast_rtp_instance_get_local_address(sub->rtp, &us_tmp);
 	ast_sockaddr_to_sin(&us_tmp, &us);
-	sin.sin_family = AF_INET;
-	/* Setting up RTP for our side */
-	memcpy(&sin.sin_addr, &sub->parent->parent->session->sin.sin_addr,
-		   sizeof(sin.sin_addr));
-	sin.sin_port = htons(sub->parent->parent->rtp_port);
-	ast_sockaddr_from_sin(&sin_tmp, &sin);
-	ast_rtp_instance_set_remote_address(sub->rtp, &sin_tmp);
-	if (!(sub->owner->nativeformats & sub->owner->readformat)) {
-		format_t fmt;
-		char tmp[256];
-		fmt = ast_best_codec(sub->owner->nativeformats);
-		ast_log(LOG_WARNING,
-				"Our read/writeformat has been changed to something incompatible: %s, using %s best codec from %s\n",
-				ast_getformatname(sub->owner->readformat),
-				ast_getformatname(fmt),
-				ast_getformatname_multiple(tmp, sizeof(tmp), sub->owner->nativeformats));
-		sub->owner->readformat = fmt;
-		sub->owner->writeformat = fmt;
-	}
-	codec = ast_rtp_codecs_payload_code(ast_rtp_instance_get_codecs(sub->rtp), 1, sub->owner->readformat);
+	ast_rtp_instance_get_remote_address(sub->rtp, &sin_tmp);
+	ast_sockaddr_to_sin(&sin_tmp, &sin);
+
 	/* Setting up RTP of the phone */
 	if (public_ip.sin_family == 0)  /* NAT IP override ?   */
 		memcpy(&public, &us, sizeof(public));   /* No defined, using IP from recvmsg  */
@@ -2122,35 +2501,35 @@
 		ast_verb(0, "Starting phone RTP stack. Our public IP is %s\n",
 					ast_inet_ntoa(public.sin_addr));
 	}
+
+	pte = sub->parent->parent->session;
+	codec = ast_rtp_codecs_payload_code(ast_rtp_instance_get_codecs(sub->rtp), 1, sub->owner->readformat);
 	if ((sub->owner->readformat == AST_FORMAT_ULAW) ||
 		(sub->owner->readformat == AST_FORMAT_ALAW)) {
 		if (unistimdebug)
-			ast_verb(0, "Sending packet_send_rtp_packet_size for codec %s\n", ast_getformatname(codec));
+			ast_verb(0, "Sending packet_send_rtp_packet_size for codec %d\n", codec);
 		memcpy(buffsend + SIZE_HEADER, packet_send_rtp_packet_size,
 			   sizeof(packet_send_rtp_packet_size));
 		buffsend[10] = (int) codec & 0xffffffffLL;
-		send_client(SIZE_HEADER + sizeof(packet_send_rtp_packet_size), buffsend,
-				   sub->parent->parent->session);
+		send_client(SIZE_HEADER + sizeof(packet_send_rtp_packet_size), buffsend, pte);
 	}
 	if (unistimdebug)
 		ast_verb(0, "Sending Jitter Buffer Parameters Configuration\n");
 	memcpy(buffsend + SIZE_HEADER, packet_send_jitter_buffer_conf,
 		   sizeof(packet_send_jitter_buffer_conf));
-	send_client(SIZE_HEADER + sizeof(packet_send_jitter_buffer_conf), buffsend,
-			   sub->parent->parent->session);
-	if (sub->parent->parent->rtp_method != 0) {
+	send_client(SIZE_HEADER + sizeof(packet_send_jitter_buffer_conf), buffsend, pte);
+	if (pte->device->rtp_method != 0) {
 		uint16_t rtcpsin_port = htons(us.sin_port) + 1; /* RTCP port is RTP + 1 */
 
 		if (unistimdebug)
-			ast_verb(0, "Sending OpenAudioStreamTX using method #%d\n",
-						sub->parent->parent->rtp_method);
-		if (sub->parent->parent->rtp_method == 3)
+			ast_verb(0, "Sending OpenAudioStreamTX using method #%d\n", pte->device->rtp_method);
+		if (pte->device->rtp_method == 3)
 			memcpy(buffsend + SIZE_HEADER, packet_send_open_audio_stream_tx3,
 				   sizeof(packet_send_open_audio_stream_tx3));
 		else
 			memcpy(buffsend + SIZE_HEADER, packet_send_open_audio_stream_tx,
 				   sizeof(packet_send_open_audio_stream_tx));
-		if (sub->parent->parent->rtp_method != 2) {
+		if (pte->device->rtp_method != 2) {
 			memcpy(buffsend + 28, &public.sin_addr, sizeof(public.sin_addr));
 			buffsend[20] = (htons(sin.sin_port) & 0xff00) >> 8;
 			buffsend[21] = (htons(sin.sin_port) & 0x00ff);
@@ -2169,18 +2548,17 @@
 			buffsend[11] = codec;
 		}
 		buffsend[12] = codec;
-		send_client(SIZE_HEADER + sizeof(packet_send_open_audio_stream_tx), buffsend,
-				   sub->parent->parent->session);
+		send_client(SIZE_HEADER + sizeof(packet_send_open_audio_stream_tx), buffsend, pte);
 
 		if (unistimdebug)
 			ast_verb(0, "Sending OpenAudioStreamRX\n");
-		if (sub->parent->parent->rtp_method == 3)
+		if (pte->device->rtp_method == 3)
 			memcpy(buffsend + SIZE_HEADER, packet_send_open_audio_stream_rx3,
 				   sizeof(packet_send_open_audio_stream_rx3));
 		else
 			memcpy(buffsend + SIZE_HEADER, packet_send_open_audio_stream_rx,
 				   sizeof(packet_send_open_audio_stream_rx));
-		if (sub->parent->parent->rtp_method != 2) {
+		if (pte->device->rtp_method != 2) {
 			memcpy(buffsend + 28, &public.sin_addr, sizeof(public.sin_addr));
 			buffsend[20] = (htons(sin.sin_port) & 0xff00) >> 8;
 			buffsend[21] = (htons(sin.sin_port) & 0x00ff);
@@ -2199,8 +2577,7 @@
 			buffsend[12] = codec;
 		}
 		buffsend[11] = codec;
-		send_client(SIZE_HEADER + sizeof(packet_send_open_audio_stream_rx), buffsend,
-				   sub->parent->parent->session);
+		send_client(SIZE_HEADER + sizeof(packet_send_open_audio_stream_rx), buffsend, pte);
 	} else {
 		uint16_t rtcpsin_port = htons(us.sin_port) + 1; /* RTCP port is RTP + 1 */
 
@@ -2234,73 +2611,155 @@
 		buffsend[46] = (htons(sin.sin_port) & 0x00ff);
 		buffsend[47] = (rtcpsin_port & 0xff00) >> 8;
 		buffsend[48] = (rtcpsin_port & 0x00ff);
-		send_client(SIZE_HEADER + sizeof(packet_send_call), buffsend,
-				   sub->parent->parent->session);
+		send_client(SIZE_HEADER + sizeof(packet_send_call), buffsend, pte);
 	}
-	ast_mutex_unlock(&sub->lock);
 }
 
-static void SendDialTone(struct unistimsession *pte)
+static void start_rtp(struct unistim_subchannel *sub)
 {
-	int i;
-	/* No country defined ? Using US tone */
-	if (ast_strlen_zero(pte->device->country)) {
-		if (unistimdebug)
-			ast_verb(0, "No country defined, using US tone\n");
-		send_tone(pte, 350, 440);
+	struct sockaddr_in sin = { 0, };
+	struct sockaddr_in sout = { 0, };
+	struct ast_sockaddr sin_tmp;
+	struct ast_sockaddr sout_tmp;
+
+	/* Sanity checks */
+	if (!sub) {
+		ast_log(LOG_WARNING, "start_rtp with a null subchannel !\n");
 		return;
 	}
-	if (strlen(pte->device->country) != 2) {
-		if (unistimdebug)
-			ast_verb(0, "Country code != 2 char, using US tone\n");
-		send_tone(pte, 350, 440);
+	if (!sub->parent) {
+		ast_log(LOG_WARNING, "start_rtp with a null line!\n");
 		return;
 	}
-	i = 0;
-	while (frequency[i].freq1) {
-		if ((frequency[i].country[0] == pte->device->country[0]) &&
-			(frequency[i].country[1] == pte->device->country[1])) {
-			if (unistimdebug)
-				ast_verb(0, "Country code found (%s), freq1=%d freq2=%d\n",
-							frequency[i].country, frequency[i].freq1, frequency[i].freq2);
-			send_tone(pte, frequency[i].freq1, frequency[i].freq2);
+	if (!sub->parent->parent) {
+		ast_log(LOG_WARNING, "start_rtp with a null device!\n");
+		return;
+	}
+	if (!sub->parent->parent->session) {
+		ast_log(LOG_WARNING, "start_rtp with a null session!\n");
+		return;
+	}
+	if (!sub->owner) {
+		ast_log(LOG_WARNING, "start_rtp with a null asterisk channel!\n");
+		return;
+	}
+	sout = sub->parent->parent->session->sout;
+	sout.sin_family = AF_INET;
+	ast_mutex_lock(&sub->lock);
+	/* Allocate the RTP */
+	if (unistimdebug)
+		ast_verb(0, "Starting RTP. Bind on %s\n", ast_inet_ntoa(sout.sin_addr));
+	ast_sockaddr_from_sin(&sout_tmp, &sout);
+	sub->rtp = ast_rtp_instance_new("asterisk", sched, &sout_tmp, NULL);
+	if (!sub->rtp) {
+		ast_log(LOG_WARNING, "Unable to create RTP session: %s binaddr=%s\n",
+				strerror(errno), ast_inet_ntoa(sout.sin_addr));
+		ast_mutex_unlock(&sub->lock);
+		return;
+	}
+	ast_rtp_instance_set_prop(sub->rtp, AST_RTP_PROPERTY_RTCP, 1);
+	sub->owner->fds[0] = ast_rtp_instance_fd(sub->rtp, 0);
+	sub->owner->fds[1] = ast_rtp_instance_fd(sub->rtp, 1);
+	ast_rtp_instance_set_qos(sub->rtp, qos.tos_audio, qos.cos_audio, "UNISTIM RTP");
+	ast_rtp_instance_set_prop(sub->rtp, AST_RTP_PROPERTY_NAT, sub->parent->parent->nat);
+
+	/* Create the RTP connection */
+	sin.sin_family = AF_INET;
+	/* Setting up RTP for our side */
+	memcpy(&sin.sin_addr, &sub->parent->parent->session->sin.sin_addr,
+		   sizeof(sin.sin_addr));
+
+	sin.sin_port = htons(find_rtp_port(sub));
+	ast_sockaddr_from_sin(&sin_tmp, &sin);
+	ast_rtp_instance_set_remote_address(sub->rtp, &sin_tmp);
+	if (!(sub->owner->nativeformats & sub->owner->readformat)) {
+		format_t fmt;
+		char tmp[256];
+		fmt = ast_best_codec(sub->owner->nativeformats);
+		ast_log(LOG_WARNING,
+				"Our read/writeformat has been changed to something incompatible: %s, using %s best codec from %s\n",
+				ast_getformatname(sub->owner->readformat),
+				ast_getformatname(fmt),
+				ast_getformatname_multiple(tmp, sizeof(tmp), sub->owner->nativeformats));
+                
+                sub->owner->readformat = fmt;
+                sub->owner->writeformat = fmt;
+	}
+	send_start_rtp(sub);
+	ast_mutex_unlock(&sub->lock);
+}
+
+static void send_dial_tone(struct unistimsession *pte)
+{
+	struct ast_tone_zone_sound *ts = NULL;
+	struct ast_tone_zone_part tone_data;
+	char *s = NULL;
+	char *ind;
+
+	if ((ts = ast_get_indication_tone(pte->device->tz, "dial"))) {
+		ind = ast_strdupa(ts->data);
+		s = strsep(&ind, ",");
+		ast_tone_zone_part_parse(s, &tone_data);
+		if (tone_data.modulate) {
+			tone_data.freq2 = 0;
 		}
-		i++;
+		send_tone(pte, tone_data.freq1, tone_data.freq2);
+		if (unistimdebug)
+			ast_verb(0, "Country code found (%s), freq1=%d freq2=%d\n",
+							pte->device->tz->country, tone_data.freq1, tone_data.freq2);
+
+		ts = ast_tone_zone_sound_unref(ts);
 	}
 }
 
+static void show_phone_number(struct unistimsession *pte)
+{
+	char tmp[TEXT_LENGTH_MAX + 1];
+	const char *tmp_number = ustmtext("Number:", pte);
+	int line, tmp_copy, offset = 0, i;
+
+	pte->device->phone_number[pte->device->size_phone_number] = '\0';
+	if  (pte->device->size_phone_number > MAX_SCREEN_NUMBER) {
+		offset = pte->device->size_phone_number - MAX_SCREEN_NUMBER - 1;
+		if (offset > strlen(tmp_number))
+			offset = strlen(tmp_number);
+		tmp_copy = strlen(tmp_number) - offset + 1;
+		if (tmp_copy > sizeof(tmp))
+			tmp_copy = sizeof(tmp);
+		memcpy(tmp, tmp_number + offset, tmp_copy);
+	} else {
+		ast_copy_string(tmp, tmp_number, sizeof(tmp));
+	}
+
+	offset = (pte->device->size_phone_number >= TEXT_LENGTH_MAX) ? (pte->device->size_phone_number - TEXT_LENGTH_MAX +1) : 0;
+	if (pte->device->size_phone_number) {
+		memcpy(tmp + strlen(tmp), pte->device->phone_number + offset, pte->device->size_phone_number - offset + 1);
+	}
+	offset = strlen(tmp);
+
+	for (i = strlen(tmp); i < TEXT_LENGTH_MAX; i++) {
+		tmp[i] = '.';
+	}
+	tmp[i] = '\0';
+
+	line = (pte->device->height == 1) ? TEXT_LINE0 : TEXT_LINE2;
+	send_text(line, TEXT_NORMAL, pte, tmp);
+	send_blink_cursor(pte);
+	send_cursor_pos(pte, (unsigned char) (line + offset));
+	send_led_update(pte, 0);
+}
+
 static void handle_dial_page(struct unistimsession *pte)
 {
 	pte->state = STATE_DIALPAGE;
 	if (pte->device->call_forward[0] == -1) {
 		send_text(TEXT_LINE0, TEXT_NORMAL, pte, "");
-		send_text(TEXT_LINE1, TEXT_NORMAL, pte, "Enter forward");
-		send_text_status(pte, "ForwardCancel BackSpcErase");
+		send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("Enter forward", pte));
+		send_text_status(pte, ustmtext("Fwd    Cancel BackSp Erase", pte));
 		if (pte->device->call_forward[1] != 0) {
-			char tmp[TEXT_LENGTH_MAX + 1];
-
 			ast_copy_string(pte->device->phone_number, pte->device->call_forward + 1,
 							sizeof(pte->device->phone_number));
-			pte->device->size_phone_number = strlen(pte->device->phone_number);
-			if (pte->device->size_phone_number > 15)
-				pte->device->size_phone_number = 15;
-			strcpy(tmp, "Number : ...............");
-			memcpy(tmp + 9, pte->device->phone_number, pte->device->size_phone_number);
-
-			if (pte->device->height == 1) {
-				send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmp);
-				send_blink_cursor(pte);
-				send_cursor_pos(pte,
-						  (unsigned char) (TEXT_LINE0 + 0x09 +
-										   pte->device->size_phone_number));
-			} else {
-				send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmp);
-				send_blink_cursor(pte);
-				send_cursor_pos(pte,
-						  (unsigned char) (TEXT_LINE2 + 0x09 +
-										   pte->device->size_phone_number));
-			}
-
+			show_phone_number(pte);
 			send_led_update(pte, 0);
 			return;
 		}
@@ -2310,200 +2769,294 @@
 			send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
 		else
 			send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
-		SendDialTone(pte);
+		send_dial_tone(pte);
 
 		if (pte->device->height > 1) {
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Enter the number to dial");
-			send_text(TEXT_LINE1, TEXT_NORMAL, pte, "and press Call");
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Enter the number to dial", pte));
+			send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("and press Call", pte));
 		}
-		send_text_status(pte, "Call   Redial BackSpcErase");
+		send_text_status(pte, ustmtext("Call   Redial BackSp Erase", pte));
 	}
 
-	if (pte->device->height == 1) {
-		send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Number : ...............");
-		send_blink_cursor(pte);
-		send_cursor_pos(pte, TEXT_LINE0 + 0x09);
-	} else {
-		send_text(TEXT_LINE2, TEXT_NORMAL, pte, "Number : ...............");
-		send_blink_cursor(pte);
-		send_cursor_pos(pte, TEXT_LINE2 + 0x09);
-	}
 	pte->device->size_phone_number = 0;
 	pte->device->phone_number[0] = 0;
+	show_phone_number(pte);
 	change_favorite_icon(pte, FAV_ICON_PHONE_BLACK);
-	Sendicon(TEXT_LINE0, FAV_ICON_NONE, pte);
+	send_icon(TEXT_LINE0, FAV_ICON_NONE, pte);
 	pte->device->missed_call = 0;
 	send_led_update(pte, 0);
 	return;
 }
 
-/* Step 1 : Music On Hold for peer, Dialing screen for us */
-static void TransferCallStep1(struct unistimsession *pte)
+static void swap_subs(struct unistim_subchannel *a, struct unistim_subchannel *b)
 {
-	struct unistim_subchannel *sub;
-	struct unistim_line *p = pte->device->lines;
+/*  struct ast_channel *towner; */
+	struct ast_rtp_instance *rtp;
+	int fds;
 
-	sub = p->subs[SUB_REAL];
+	if (unistimdebug)
+		ast_verb(0, "Swapping %p and %p\n", a, b);
 
-	if (!sub->owner) {
-		ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
+	if ((!a->owner) || (!b->owner)) {
+		ast_log(LOG_WARNING,
+				"Attempted to swap subchannels with a null owner : sub #%p=%p sub #%p=%p\n",
+				a, a->owner, b, b->owner);
 		return;
 	}
-	if (p->subs[SUB_THREEWAY]) {
-		if (unistimdebug)
-			ast_verb(0, "Transfer canceled, hangup our threeway channel\n");
-		if (p->subs[SUB_THREEWAY]->owner)
-			ast_queue_hangup_with_cause(p->subs[SUB_THREEWAY]->owner, AST_CAUSE_NORMAL_CLEARING);
-		else
-			ast_log(LOG_WARNING, "Canceling a threeway channel without owner\n");
+	rtp = a->rtp;
+	a->rtp = b->rtp;
+	b->rtp = rtp;
+
+	fds = a->owner->fds[0];
+	a->owner->fds[0] = b->owner->fds[0];
+	b->owner->fds[0] = fds;
+
+	fds = a->owner->fds[1];
+	a->owner->fds[1] = b->owner->fds[1];
+	b->owner->fds[1] = fds;
+}
+
+/* Step 1 : Music On Hold for peer, Dialing screen for us */
+static void transfer_call_step1(struct unistimsession *pte)
+{
+	struct unistim_subchannel *sub, *sub_trans;
+	struct unistim_device *d = pte->device;
+
+	sub = get_sub(d, SUB_REAL);
+	sub_trans = get_sub(d, SUB_THREEWAY);
+
+	if (!sub || !sub->owner) {
+		ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
 		return;
 	}
 	/* Start music on hold if appropriate */
-	if (pte->device->moh)
+	if (sub->moh)
 		ast_log(LOG_WARNING, "Transfer with peer already listening music on hold\n");
 	else {
-		if (ast_bridged_channel(p->subs[SUB_REAL]->owner)) {
-			ast_moh_start(ast_bridged_channel(p->subs[SUB_REAL]->owner),
-						  pte->device->lines->musicclass, NULL);
-			pte->device->moh = 1;
+		if (ast_bridged_channel(sub->owner)) {
+			ast_moh_start(ast_bridged_channel(sub->owner),
+						  sub->parent->musicclass, NULL);
+			sub->moh = 1;
+			sub->subtype = SUB_THREEWAY;
 		} else {
 			ast_log(LOG_WARNING, "Unable to find peer subchannel for music on hold\n");
 			return;
 		}
 	}
-	/* Silence our channel */
-	if (!pte->device->silence_generator) {
-		pte->device->silence_generator =
-			ast_channel_start_silence_generator(p->subs[SUB_REAL]->owner);
-		if (pte->device->silence_generator == NULL)
-			ast_log(LOG_WARNING, "Unable to start a silence generator.\n");
-		else if (unistimdebug)
-			ast_verb(0, "Starting silence generator\n");
-	}
+	sub_start_silence(pte, sub);
 	handle_dial_page(pte);
 }
 
+static void transfer_cancel_step2(struct unistimsession *pte)
+{
+	struct unistim_subchannel *sub, *sub_trans;
+	struct unistim_device *d = pte->device;
+
+	sub = get_sub(d, SUB_REAL);
+	sub_trans = get_sub(d, SUB_THREEWAY);
+
+	if (!sub || !sub->owner) {
+		ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
+		return;
+	}
+	if (sub_trans) {
+		if (unistimdebug)
+			ast_verb(0, "Transfer canceled, hangup our threeway channel\n");
+		if (sub->owner) {
+			swap_subs(sub, sub_trans);
+			ast_moh_stop(ast_bridged_channel(sub_trans->owner));
+			sub_trans->moh = 0;
+			sub_trans->subtype = SUB_REAL;
+			sub->subtype = SUB_THREEWAY;
+			ast_queue_hangup_with_cause(sub->owner, AST_CAUSE_NORMAL_CLEARING);
+                } else
+			ast_log(LOG_WARNING, "Canceling a threeway channel without owner\n");
+		return;
+	}
+}
+
 /* From phone to PBX */
-static void HandleCallOutgoing(struct unistimsession *s)
+static void handle_call_outgoing(struct unistimsession *s)
 {
 	struct ast_channel *c;
 	struct unistim_subchannel *sub;
-	pthread_t t;
+	int softkey;
+
 	s->state = STATE_CALL;
-	sub = s->device->lines->subs[SUB_REAL];
-	if (!sub) {
-		ast_log(LOG_NOTICE, "No available lines on: %s\n", s->device->name);
+	s->sout.sin_family = AF_INET;
+
+	sub = get_sub(s->device, SUB_THREEWAY);
+	if (sub) {
+		/* If sub for threway call created than we use transfer behaviuor */
+		struct unistim_subchannel *sub_trans = NULL;
+		struct unistim_device *d = s->device;
+
+		sub_trans = get_sub(d, SUB_REAL);
+		if (sub_trans) {
+			ast_log(LOG_WARNING, "Can't transfer while active subchannel exists!\n");
+			return;
+		}
+		if (!sub->owner) {
+			ast_log(LOG_WARNING, "Unable to find subchannel with music on hold\n");
+			return;
+		}
+
+		sub_trans = unistim_alloc_sub(d, SUB_REAL);
+		if (!sub_trans) {
+			ast_log(LOG_WARNING, "Unable to allocate three-way subchannel\n");
+			return;
+		}
+		sub_trans->parent = sub->parent;
+		sub_stop_silence(s, sub);
+		send_tone(s, 0, 0);
+		/* Make new channel */
+		c = unistim_new(sub_trans, AST_STATE_DOWN, NULL);
+		if (!c) {
+			ast_log(LOG_WARNING, "Cannot allocate new structure on channel %p\n", sub->parent);
+			return;
+		}
+		/* Swap things around between the three-way and real call */
+		swap_subs(sub, sub_trans);
+		send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
+		if (s->device->height == 1) {
+			send_text(TEXT_LINE0, TEXT_NORMAL, s, s->device->phone_number);
+		} else {
+			send_text(TEXT_LINE0, TEXT_NORMAL, s, ustmtext("Calling (pre-transfer)", s));
+			send_text(TEXT_LINE1, TEXT_NORMAL, s, s->device->phone_number);
+			send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("Dialing...", s));
+		}
+		send_text_status(s, ustmtext("TransfrCancel", s));
+
+		if (ast_pthread_create(&sub->ss_thread, NULL, unistim_ss, c)) {
+			ast_log(LOG_WARNING, "Unable to start simple switch on channel %p\n", c);
+			sub->ss_thread = AST_PTHREADT_NULL;
+			ast_hangup(c);
+			return;
+		}
+		if (unistimdebug)
+			ast_verb(0, "Started three way call on channel %p (%s) subchan %d\n",
+				 sub_trans->owner, sub_trans->owner->name, sub_trans->subtype);
 		return;
 	}
+
+	softkey = get_avail_softkey(s, NULL);
+	if (softkey == -1) {
+		ast_log(LOG_WARNING, "Have no avail softkey for calling\n");
+		return;
+	}
+	sub = get_sub(s->device, SUB_REAL);
+	if (sub) { /* have already call assigned */
+		sub_hold(s, sub); /* Need to put on hold */
+	}
+	sub = unistim_alloc_sub(s->device, SUB_REAL);
+	sub->parent = s->device->sline[softkey];
+	s->device->ssub[softkey] = sub;
+	sub->softkey = softkey;
+
+	if (unistimdebug)
+		ast_verb(0, "Using softkey %d, line %p\n", sub->softkey, sub->parent);
+	send_favorite_short(sub->softkey, FAV_ICON_OFFHOOK_BLACK, s);
+	s->device->selected = -1;
 	if (!sub->owner) {		      /* A call is already in progress ? */
 		c = unistim_new(sub, AST_STATE_DOWN, NULL);   /* No, starting a new one */
-		if (c) {
-			/* Need to start RTP before calling ast_pbx_run */
-			if (!sub->rtp)
-				start_rtp(sub);
-			send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
+		if (!sub->rtp) /* Need to start RTP before calling ast_pbx_run */
+			start_rtp(sub);
+		if (!strcmp(s->device->phone_number, ast_pickup_ext())) {
+			if (unistimdebug)
+				ast_verb(0, "Try to pickup in unistim_new\n");
 
+			send_text(TEXT_LINE0, TEXT_NORMAL, s, "");
+			send_text_status(s, ustmtext("       Transf        Hangup", s));
+			send_start_timer(s);
+			if (ast_pickup_call(c)) {
+				ast_log(LOG_NOTICE, "Nothing to pick up\n");
+				c->hangupcause = AST_CAUSE_CALL_REJECTED;
+			} else {
+				c->hangupcause = AST_CAUSE_NORMAL_CLEARING;
+			}
+			ast_hangup(c);
+			c = NULL;
+                } else if (c) {
+			send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
+			send_tone(s, 0, 0); /* Dialing empty number should also stop dial tone */
 			if (s->device->height == 1) {
-				send_text(TEXT_LINE0, TEXT_NORMAL, s, s->device->phone_number);
+				if (strlen(s->device->phone_number) > 0) {
+					send_text(TEXT_LINE0, TEXT_NORMAL, s, s->device->phone_number);
+				} else {
+					send_text(TEXT_LINE0, TEXT_NORMAL, s, ustmtext("Calling...", s));
+				}
 			} else {
-				send_text(TEXT_LINE0, TEXT_NORMAL, s, "Calling :");
+				send_text(TEXT_LINE0, TEXT_NORMAL, s, ustmtext("Calling :", s));
 				send_text(TEXT_LINE1, TEXT_NORMAL, s, s->device->phone_number);
-				send_text(TEXT_LINE2, TEXT_NORMAL, s, "Dialing...");
+				send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("Dialing...", s));
 			}
-			send_text_status(s, "Hangup");
+			send_text_status(s, ustmtext("                     Hangup", s));
 
 			/* start switch */
-			if (ast_pthread_create(&t, NULL, unistim_ss, c)) {
-				display_last_error("Unable to create switch thread");
+			if (ast_pthread_create(&sub->ss_thread, NULL, unistim_ss, c)) {
+				ast_log(LOG_WARNING, "Unable to create switch thread\n");
+				sub->ss_thread = AST_PTHREADT_NULL;
 				ast_queue_hangup_with_cause(c, AST_CAUSE_SWITCH_CONGESTION);
 			}
 		} else
 			ast_log(LOG_WARNING, "Unable to create channel for %s@%s\n",
 					sub->parent->name, s->device->name);
-	} else {					/* We already have a call, so we switch in a threeway call */
-
-		if (s->device->moh) {
-			struct unistim_subchannel *subchannel;
-			struct unistim_line *p = s->device->lines;
-			subchannel = p->subs[SUB_REAL];
-
-			if (!subchannel->owner) {
-				ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
-				return;
-			}
-			if (p->subs[SUB_THREEWAY]) {
-				ast_log(LOG_WARNING,
-						"Can't transfer while an another transfer is taking place\n");
-				return;
-			}
-			if (!alloc_sub(p, SUB_THREEWAY)) {
-				ast_log(LOG_WARNING, "Unable to allocate three-way subchannel\n");
-				return;
-			}
-			/* Stop the silence generator */
-			if (s->device->silence_generator) {
-				if (unistimdebug)
-					ast_verb(0, "Stopping silence generator\n");
-				ast_channel_stop_silence_generator(subchannel->owner,
-												   s->device->silence_generator);
-				s->device->silence_generator = NULL;
-			}
-			send_tone(s, 0, 0);
-			/* Make new channel */
-			c = unistim_new(p->subs[SUB_THREEWAY], AST_STATE_DOWN, NULL);
-			if (!c) {
-				ast_log(LOG_WARNING, "Cannot allocate new structure on channel %p\n", p);
-				return;
-			}
-			/* Swap things around between the three-way and real call */
-			swap_subs(p, SUB_THREEWAY, SUB_REAL);
-			send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
-
-			if (s->device->height == 1) {
-				send_text(TEXT_LINE0, TEXT_NORMAL, s, s->device->phone_number);
-			} else {
-				send_text(TEXT_LINE0, TEXT_NORMAL, s, "Calling (pre-transfer)");
-				send_text(TEXT_LINE1, TEXT_NORMAL, s, s->device->phone_number);
-				send_text(TEXT_LINE2, TEXT_NORMAL, s, "Dialing...");
-			}
-			send_text_status(s, "TransfrCancel");
-
-			if (ast_pthread_create(&t, NULL, unistim_ss, p->subs[SUB_THREEWAY]->owner)) {
-				ast_log(LOG_WARNING, "Unable to start simple switch on channel %p\n", p);
-				ast_hangup(c);
-				return;
-			}
-			if (unistimdebug)
-				ast_verb(0, "Started three way call on channel %p (%s) subchan %d\n",
-					 p->subs[SUB_THREEWAY]->owner, p->subs[SUB_THREEWAY]->owner->name,
-					 p->subs[SUB_THREEWAY]->subtype);
-		} else
-			ast_debug(1, "Current sub [%s] already has owner\n", sub->owner->name);
+	} else {
+		ast_debug(1, "Current sub [%s] already has owner\n", sub->owner->name);
 	}
 	return;
 }
 
 /* From PBX to phone */
-static void HandleCallIncoming(struct unistimsession *s)
+static void handle_call_incoming(struct unistimsession *s)
 {
-	struct unistim_subchannel *sub;
+	struct unistim_subchannel *sub = NULL;
+	int i;
+
 	s->state = STATE_CALL;
 	s->device->missed_call = 0;
 	send_no_ring(s);
-	sub = s->device->lines->subs[SUB_REAL];
+	sub = get_sub(s->device, SUB_RING); /* Put other SUB_REAL call on hold */
 	if (!sub) {
-		ast_log(LOG_NOTICE, "No available lines on: %s\n", s->device->name);
+		ast_log(LOG_WARNING, "No ringing lines on: %s\n", s->device->name);
+		return;
+	}
+	/* Change icons for all ringing keys */
+	for (i = 0; i < FAVNUM; i++) {
+		if (!s->device->ssub[i])  /* No sub assigned - skip */
+			continue;
+		if (s->device->ssub[i]->subtype == SUB_REAL)
+			sub_hold(s, s->device->ssub[i]);
+		if (s->device->ssub[i] != sub)
+			continue;
+		if (sub->softkey == i) /* If softkey assigned at this moment - do not erase */
+			continue;
+		if (sub->softkey < 0) { /* If softkey not defined - first one used */
+			sub->softkey = i;
+			continue;
+		}
+		send_favorite_short(i, FAV_LINE_ICON, s);
+		s->device->ssub[i] = NULL;
+	}
+	if (sub->softkey < 0) {
+		ast_log(LOG_WARNING, "Can not assign softkey for incoming call on: %s\n", s->device->name);
 		return;
-	} else if (unistimdebug)
+	}
+	send_favorite_short(sub->softkey, FAV_ICON_OFFHOOK_BLACK, s);
+	sub->parent = s->device->sline[sub->softkey];
+	sub->subtype = SUB_REAL;
+	if (unistimdebug)
 		ast_verb(0, "Handle Call Incoming for %s@%s\n", sub->parent->name,
 					s->device->name);
 	start_rtp(sub);
 	if (!sub->rtp)
 		ast_log(LOG_WARNING, "Unable to create channel for %s@%s\n", sub->parent->name,
 				s->device->name);
-	ast_queue_control(sub->owner, AST_CONTROL_ANSWER);
-	send_text(TEXT_LINE2, TEXT_NORMAL, s, "is on-line");
-	send_text_status(s, "Hangup Transf");
+	if (sub->owner) {
+		ast_queue_control(sub->owner, AST_CONTROL_ANSWER);
+	}
+	send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("is on-line", s));
+	send_text_status(s, ustmtext("       Transf        Hangup", s));
 	send_start_timer(s);
 
 	if ((s->device->output == OUTPUT_HANDSET) &&
@@ -2511,7 +3064,6 @@
 		send_select_output(s, OUTPUT_SPEAKER, s->device->volume, MUTE_OFF);
 	else
 		send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
-	s->device->start_call_timestamp = time(0);
 	write_history(s, 'i', 0);
 	return;
 }
@@ -2520,8 +3072,10 @@
 {
 	struct ast_frame f = { .frametype = AST_FRAME_DTMF, .subclass.integer = digit, .src = "unistim" };
 	struct unistim_subchannel *sub;
-	sub = pte->device->lines->subs[SUB_REAL];
-	if (!sub->owner || sub->alreadygone) {
+        int row, col;
+
+	sub = get_sub(pte->device, SUB_REAL);
+	if (!sub || !sub->owner || sub->alreadygone) {
 		ast_log(LOG_WARNING, "Unable to find subchannel in dtmf senddigit\n");
 		return -1;
 	}
@@ -2531,65 +3085,80 @@
 
 	if (unistimdebug)
 		ast_verb(0, "Send Digit %c\n", digit);
-	switch (digit) {
-	case '0':
-		send_tone(pte, 941, 1336);
-		break;
-	case '1':
-		send_tone(pte, 697, 1209);
-		break;
-	case '2':
-		send_tone(pte, 697, 1336);
-		break;
-	case '3':
-		send_tone(pte, 697, 1477);
-		break;
-	case '4':
-		send_tone(pte, 770, 1209);
-		break;
-	case '5':
-		send_tone(pte, 770, 1336);
-		break;
-	case '6':
-		send_tone(pte, 770, 1477);
-		break;
-	case '7':
-		send_tone(pte, 852, 1209);
-		break;
-	case '8':
-		send_tone(pte, 852, 1336);
-		break;
-	case '9':
-		send_tone(pte, 852, 1477);
-		break;
-	case 'A':
-		send_tone(pte, 697, 1633);
-		break;
-	case 'B':
-		send_tone(pte, 770, 1633);
-		break;
-	case 'C':
-		send_tone(pte, 852, 1633);
-		break;
-	case 'D':
-		send_tone(pte, 941, 1633);
-		break;
-	case '*':
-		send_tone(pte, 941, 1209);
-		break;
-	case '#':
-		send_tone(pte, 941, 1477);
-		break;
-	default:
+
+	row = (digit-'1')%3;
+	col = (digit-'1'-row)/3;
+	if (digit >= '1' && digit <='9')
+		send_tone(pte, dtmf_row[row], dtmf_col[col]);
+	else if (digit >= 'A' && digit <= 'D')
+		send_tone(pte, dtmf_row[digit-'A'], dtmf_col[3]);
+	else if (digit == '*')
+		send_tone(pte, dtmf_row[3], dtmf_col[0]);
+	else if (digit == '0')
+		send_tone(pte, dtmf_row[3], dtmf_col[1]);
+	else if (digit == '#')
+		send_tone(pte, dtmf_row[3], dtmf_col[2]);
+	else
 		send_tone(pte, 500, 2000);
-	}
+
 	usleep(150000);			 /* XXX Less than perfect, blocking an important thread is not a good idea */
 	send_tone(pte, 0, 0);
 	return 0;
 }
 
+static void handle_key_fav(struct unistimsession *pte, char keycode) 
+{
+	int keynum = keycode - KEY_FAV0;
+	struct unistim_subchannel *sub;
+
+	sub = get_sub(pte->device, SUB_REAL);
+
+	/* Make an action on selected favorite key */
+	if (!pte->device->ssub[keynum]) { /* Key have no assigned call */
+		send_favorite_selected(FAV_LINE_ICON, pte);
+		if (is_key_line(pte->device, keynum)) {
+			if (unistimdebug)
+				ast_verb(0, "Handle line w/o sub - dialpage\n");
+			pte->device->selected = keynum;
+			sub_hold(pte, sub); /* Put active call on hold */
+			send_stop_timer(pte);
+			handle_dial_page(pte);
+		} else if (is_key_favorite(pte->device, keynum)) {
+			/* Put active call on hold in handle_call_outgoing function, after preparation and
+			 checking if lines available for calling */
+			if (unistimdebug)
+				ast_verb(0, "Handle favorite w/o sub - dialing\n");
+			if ((pte->device->output == OUTPUT_HANDSET) &&
+				(pte->device->receiver_state == STATE_ONHOOK))
+				send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
+			else
+				send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
+
+			key_favorite(pte, keycode);
+		}
+	} else {
+		sub = pte->device->ssub[keynum];
+		/* Favicon have assigned sub, activate it and put current on hold */
+		if (sub->subtype == SUB_REAL) {
+			sub_hold(pte, sub);
+			show_main_page(pte);
+		} else if (sub->subtype == SUB_RING) {
+			sub->softkey = keynum;
+			handle_call_incoming(pte);
+		} else if (sub->subtype == SUB_ONHOLD) {
+			if (pte->state == STATE_DIALPAGE){
+				send_tone(pte, 0, 0);
+			}
+			send_callerid_screen(pte, sub);
+			sub_unhold(pte, sub);
+			pte->state = STATE_CALL;
+		}
+	}
+}
+
 static void key_call(struct unistimsession *pte, char keycode)
 {
+	struct unistim_subchannel *sub = NULL;
 	if ((keycode >= KEY_0) && (keycode <= KEY_SHARP)) {
 		if (keycode == KEY_SHARP)
 			keycode = '#';
@@ -2601,12 +3170,31 @@
 		return;
 	}
 	switch (keycode) {
-	case KEY_HANGUP:
-	case KEY_FUNC1:
-		close_call(pte);
+	case KEY_FUNC1: 
+		if (get_sub(pte->device, SUB_THREEWAY)) {
+			close_call(pte);
+		}
 		break;
 	case KEY_FUNC2:
-		TransferCallStep1(pte);
+		if (get_sub(pte->device, SUB_THREEWAY)) {
+			transfer_cancel_step2(pte);
+		} else {
+			transfer_call_step1(pte);
+		}
+		break;
+	case KEY_HANGUP:
+	case KEY_FUNC4: 
+		if (!get_sub(pte->device, SUB_THREEWAY)) {
+			close_call(pte);
+		}
+		break;
+	case KEY_FAV0:
+	case KEY_FAV1:
+	case KEY_FAV2:
+	case KEY_FAV3:
+	case KEY_FAV4:
+	case KEY_FAV5:
+		handle_key_fav(pte, keycode);
 		break;
 	case KEY_HEADPHN:
 		if (pte->device->output == OUTPUT_HEADPHONE)
@@ -2622,82 +3210,83 @@
 							 MUTE_OFF);
 		break;
 	case KEY_MUTE:
-		if (!pte->device->moh) {
+		sub = get_sub(pte->device, SUB_REAL);
+		if (!sub || !sub->owner) {
+			ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
+			return;
+		}
+		if (!sub->moh) {
 			if (pte->device->mute == MUTE_ON)
 				send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
 			else
 				send_select_output(pte, pte->device->output, pte->device->volume, MUTE_ON);
 			break;
 		}
+		break;
 	case KEY_ONHOLD:
-		{
-			struct unistim_subchannel *sub;
-			struct ast_channel *bridgepeer = NULL;
-			sub = pte->device->lines->subs[SUB_REAL];
-			if (!sub->owner) {
-				ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
-				return;
+		sub = get_sub(pte->device, SUB_REAL);
+		if (!sub) {
+			if(pte->device->ssub[pte->device->selected]) {
+				sub_hold(pte, pte->device->ssub[pte->device->selected]);
 			}
-			if ((bridgepeer = ast_bridged_channel(sub->owner))) {
-				if (pte->device->moh) {
-					ast_moh_stop(bridgepeer);
-					pte->device->moh = 0;
-					send_select_output(pte, pte->device->output, pte->device->volume,
-									 MUTE_OFF);
-				} else {
-					ast_moh_start(bridgepeer, pte->device->lines->musicclass, NULL);
-					pte->device->moh = 1;
-					send_select_output(pte, pte->device->output, pte->device->volume,
-									 MUTE_ON);
-				}
-			} else
-				ast_log(LOG_WARNING,
-						"Unable to find peer subchannel for music on hold\n");
-			break;
-		}
+		} else
+			sub_hold(pte, sub);
+		break;
 	}
 	return;
 }
 
 static void key_ringing(struct unistimsession *pte, char keycode)
 {
-	if (keycode == KEY_FAV0 + pte->device->softkeylinepos) {
-		HandleCallIncoming(pte);
-		return;
-	}
 	switch (keycode) {
+	case KEY_FAV0:
+	case KEY_FAV1:
+	case KEY_FAV2:
+	case KEY_FAV3:
+	case KEY_FAV4:
+	case KEY_FAV5:
+		handle_key_fav(pte, keycode);
+		break;
+	case KEY_FUNC3:
+		ignore_call(pte);
+		break;
 	case KEY_HANGUP:
 	case KEY_FUNC4:
-		IgnoreCall(pte);
+		discard_call(pte);
 		break;
 	case KEY_FUNC1:
-		HandleCallIncoming(pte);
+		handle_call_incoming(pte);
+		break;
+	case KEY_LOUDSPK:
+		pte->device->output = OUTPUT_SPEAKER;
+		handle_call_incoming(pte);
+		break;
+	case KEY_HEADPHN:
+		pte->device->output = OUTPUT_HEADPHONE;
+		handle_call_incoming(pte);
 		break;
 	}
 	return;
 }
 
-static void Keyfavorite(struct unistimsession *pte, char keycode)
+static void key_favorite(struct unistimsession *pte, char keycode)
 {
-	int fav;
-
-	if ((keycode < KEY_FAV1) && (keycode > KEY_FAV5)) {
+	int fav = keycode - KEY_FAV0;
+	if (!is_key_favorite(pte->device, fav)) {
 		ast_log(LOG_WARNING, "It's not a favorite key\n");
 		return;
 	}
-	if (keycode == KEY_FAV0)
-		return;
-	fav = keycode - KEY_FAV0;
-	if (pte->device->softkeyicon[fav] == 0)
-		return;
 	ast_copy_string(pte->device->phone_number, pte->device->softkeynumber[fav],
 					sizeof(pte->device->phone_number));
-	HandleCallOutgoing(pte);
+	handle_call_outgoing(pte);
 	return;
 }
 
 static void key_dial_page(struct unistimsession *pte, char keycode)
 {
+	struct unistim_subchannel *sub = get_sub(pte->device, SUB_THREEWAY);
+
+	pte->device->nextdial = 0;
 	if (keycode == KEY_FUNC3) {
 		if (pte->device->size_phone_number <= 1)
 			keycode = KEY_FUNC4;
@@ -2706,49 +3295,36 @@
 			keycode = pte->device->phone_number[pte->device->size_phone_number] + 0x10;
 		}
 	}
+	if (keycode == KEY_SHARP && pte->device->sharp_dial == 1) {
+		keycode = KEY_FUNC1;
+	}
 	if ((keycode >= KEY_0) && (keycode <= KEY_SHARP)) {
-		char tmpbuf[] = "Number : ...............";
-		int i = 0;
+		int i = pte->device->size_phone_number;
 
-		if (pte->device->size_phone_number >= 15)
-			return;
 		if (pte->device->size_phone_number == 0)
 			send_tone(pte, 0, 0);
-		while (i < pte->device->size_phone_number) {
-			tmpbuf[i + 9] = pte->device->phone_number[i];
-			i++;
-		}
-		if (keycode == KEY_SHARP)
+
+		if (keycode == KEY_SHARP) 
 			keycode = '#';
 		else if (keycode == KEY_STAR)
 			keycode = '*';
 		else
 			keycode -= 0x10;
-		tmpbuf[i + 9] = keycode;
 		pte->device->phone_number[i] = keycode;
 		pte->device->size_phone_number++;
 		pte->device->phone_number[i + 1] = 0;
-		if (pte->device->height == 1) {
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmpbuf);
+		show_phone_number(pte);
+		
+		if (ast_exists_extension(NULL, pte->device->context, pte->device->phone_number, 1, NULL) && 
+			!ast_matchmore_extension(NULL, pte->device->context, pte->device->phone_number, 1, NULL)) {
+		    keycode = KEY_FUNC1;
 		} else {
-			send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmpbuf);
+		    pte->device->nextdial = get_tick_count() + TIMER_DIAL;
 		}
-		send_blink_cursor(pte);
-		send_cursor_pos(pte, (unsigned char) (TEXT_LINE2 + 0x0a + i));
-		return;
 	}
 	if (keycode == KEY_FUNC4) {
-
 		pte->device->size_phone_number = 0;
-		if (pte->device->height == 1) {
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Number : ...............");
-			send_blink_cursor(pte);
-			send_cursor_pos(pte, TEXT_LINE0 + 0x09);
-		} else {
-			send_text(TEXT_LINE2, TEXT_NORMAL, pte, "Number : ...............");
-			send_blink_cursor(pte);
-			send_cursor_pos(pte, TEXT_LINE2 + 0x09);
-		}
+		show_phone_number(pte);
 		return;
 	}
 
@@ -2759,6 +3335,8 @@
 			show_main_page(pte);
 		} else if ((keycode == KEY_FUNC2) || (keycode == KEY_HANGUP)) {
 			pte->device->call_forward[0] = '\0';
+			send_led_update(pte, 0x08);
+			send_led_update(pte, 0x10);
 			show_main_page(pte);
 		}
 		return;
@@ -2770,40 +3348,38 @@
 		ast_copy_string(pte->device->phone_number, pte->device->redial_number,
 						sizeof(pte->device->phone_number));
 	case KEY_FUNC1:
-		HandleCallOutgoing(pte);
+		handle_call_outgoing(pte);
 		break;
 	case KEY_HANGUP:
-		if (pte->device->lines->subs[SUB_REAL]->owner) {
-			/* Stop the silence generator */
-			if (pte->device->silence_generator) {
-				if (unistimdebug)
-					ast_verb(0, "Stopping silence generator\n");
-				ast_channel_stop_silence_generator(pte->device->lines->subs[SUB_REAL]->
-												   owner, pte->device->silence_generator);
-				pte->device->silence_generator = NULL;
-			}
+		if (sub && sub->owner) {
+			struct ast_channel *bridgepeer = NULL;
+
+			sub_stop_silence(pte, sub);
 			send_tone(pte, 0, 0);
-			ast_moh_stop(ast_bridged_channel(pte->device->lines->subs[SUB_REAL]->owner));
-			pte->device->moh = 0;
+			if ((bridgepeer = ast_bridged_channel(sub->owner))) {
+				ast_moh_stop(bridgepeer);
+			}
+			sub->moh = 0;
+			sub->subtype = SUB_REAL;
 			pte->state = STATE_CALL;
 
-			if (pte->device->height == 1) {
-				send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Dial Cancel,back to priv. call.");
-			} else {
-				send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Dialing canceled,");
-				send_text(TEXT_LINE1, TEXT_NORMAL, pte, "switching back to");
-				send_text(TEXT_LINE2, TEXT_NORMAL, pte, "previous call.");
-			}
-			send_text_status(pte, "Hangup Transf");
-		} else
+			send_text_status(pte, ustmtext("       Transf        Hangup", pte));
+			send_callerid_screen(pte, sub);
+		} else {                        
+			send_led_update(pte, 0x08);
+			send_led_update(pte, 0x10);
 			show_main_page(pte);
+                }
 		break;
+	case KEY_FAV0:
 	case KEY_FAV1:
 	case KEY_FAV2:
 	case KEY_FAV3:
 	case KEY_FAV4:
 	case KEY_FAV5:
-		Keyfavorite(pte, keycode);
+		send_favorite_selected(FAV_LINE_ICON, pte);
+		pte->device->selected = -1;
+		handle_key_fav(pte, keycode);
 		break;
 	case KEY_LOUDSPK:
 		if (pte->device->output == OUTPUT_SPEAKER) {
@@ -2828,16 +3404,57 @@
 	return;
 }
 
+static void handle_select_option(struct unistimsession *pte)
+{
+	char tmp[128];
+
+	if (pte->state != STATE_SELECTOPTION) {
+		pte->state = STATE_SELECTOPTION;
+		pte->size_buff_entry = 1;
+		pte->buff_entry[0] = 0; /* Position in menu */
+	}
+	snprintf(tmp, sizeof(tmp), "%d. %s", pte->buff_entry[0] + 1, ustmtext(options_menu[(int)pte->buff_entry[0]].label, pte));
+	send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmp);
+	send_text_status(pte, ustmtext("Select               Cancel", pte));
+	return;
+}
+
+static void key_select_option(struct unistimsession *pte, char keycode)
+{
+	switch (keycode) {
+	case KEY_DOWN:
+		pte->buff_entry[0]++;
+		if (options_menu[(int)pte->buff_entry[0]].label == NULL)
+			pte->buff_entry[0]--;
+		break;
+	case KEY_UP:
+		if (pte->buff_entry[0] > 0)
+			pte->buff_entry[0]--;
+		break;
+	case KEY_LOUDSPK:
+	case KEY_FUNC1:
+		options_menu[(int)pte->buff_entry[0]].handle_option(pte);
+		return;
+	case KEY_HANGUP:
+	case KEY_FUNC4:
+		show_main_page(pte);
+		return;
+	}
+
+	handle_select_option(pte);
+	return;
+}
+
 #define SELECTCODEC_START_ENTRY_POS 15
 #define SELECTCODEC_MAX_LENGTH 2
 #define SELECTCODEC_MSG "Codec number : .."
-static void HandleSelectCodec(struct unistimsession *pte)
+static void handle_select_codec(struct unistimsession *pte)
 {
 	char buf[30], buf2[5];
 
 	pte->state = STATE_SELECTCODEC;
-	strcpy(buf, "Using codec ");
-	sprintf(buf2, "%d", pte->device->codec_number);
+	ast_copy_string(buf, ustmtext("Using codec", pte), sizeof(buf));
+	snprintf(buf2, sizeof(buf2), " %d", pte->device->codec_number);
 	strcat(buf, buf2);
 	strcat(buf, " (G711u=0,");
 
@@ -2847,7 +3464,7 @@
 	send_blink_cursor(pte);
 	send_cursor_pos(pte, TEXT_LINE2 + SELECTCODEC_START_ENTRY_POS);
 	pte->size_buff_entry = 0;
-	send_text_status(pte, "Select BackSpcErase  Cancel");
+	send_text_status(pte, ustmtext("Select BackSp Erase  Cancel", pte));
 	return;
 }
 
@@ -2905,19 +3522,82 @@
 	return;
 }
 
+static int find_language(const char* lang)
+{
+	int i = 0;
+	while (options_languages[i].lang_short != NULL) {
+		if(!strcmp(options_languages[i].lang_short, lang))
+			return i;
+		i++;
+	}
+	return 0;
+}
+
+static void handle_select_language(struct unistimsession *pte)
+{
+	char tmp_language[40];
+	struct unistim_languages lang;
+
+	if (pte->state != STATE_SELECTLANGUAGE) {
+		pte->state = STATE_SELECTLANGUAGE;
+		pte->size_buff_entry = 1;
+		pte->buff_entry[0] = find_language(pte->device->language);
+	}
+	lang = options_languages[(int)pte->buff_entry[0]];
+	ast_copy_string(tmp_language, pte->device->language, sizeof(tmp_language));
+	ast_copy_string(pte->device->language, lang.lang_short, sizeof(pte->device->language));
+	send_charset_update(pte, lang.encoding);
+	send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext(lang.label, pte));
+
+	ast_copy_string(pte->device->language, tmp_language, sizeof(pte->device->language));
+	lang = options_languages[find_language(pte->device->language)];
+	send_charset_update(pte, lang.encoding);
+	send_text_status(pte, ustmtext("Select               Cancel", pte));
+	return;
+}
+
+static void key_select_language(struct unistimsession *pte, char keycode)
+{
+	switch (keycode) {
+	case KEY_DOWN:
+		pte->buff_entry[0]++;
+		if (options_languages[(int)pte->buff_entry[0]].label == NULL)
+			pte->buff_entry[0]--;
+		break;
+	case KEY_UP:
+		if (pte->buff_entry[0] > 0)
+			pte->buff_entry[0]--;
+		break;
+	case KEY_FUNC1:
+		ast_copy_string(pte->device->language, options_languages[(int)pte->buff_entry[0]].lang_short, sizeof(pte->device->language));
+		send_charset_update(pte, options_languages[(int)pte->buff_entry[0]].encoding);
+		refresh_all_favorite(pte);
+		show_main_page(pte);
+		return;
+	case KEY_HANGUP:
+	case KEY_FUNC4:
+		handle_select_option(pte);
+		return;
+	}
+
+	handle_select_language(pte);
+	return;
+}
+
+
 #define SELECTEXTENSION_START_ENTRY_POS 0
 #define SELECTEXTENSION_MAX_LENGTH 10
 #define SELECTEXTENSION_MSG ".........."
-static void ShowExtensionPage(struct unistimsession *pte)
+static void show_extension_page(struct unistimsession *pte)
 {
 	pte->state = STATE_EXTENSION;
 
-	send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Please enter a Terminal");
-	send_text(TEXT_LINE1, TEXT_NORMAL, pte, "Number (TN) :");
+	send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Please enter a Terminal", pte));
+	send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("Number (TN) :", pte));
 	send_text(TEXT_LINE2, TEXT_NORMAL, pte, SELECTEXTENSION_MSG);
 	send_blink_cursor(pte);
 	send_cursor_pos(pte, TEXT_LINE2 + SELECTEXTENSION_START_ENTRY_POS);
-	send_text_status(pte, "Enter  BackSpcErase");
+	send_text_status(pte, ustmtext("Enter  BackSpcErase", pte));
 	pte->size_buff_entry = 0;
 	return;
 }
@@ -2972,7 +3652,6 @@
 						pte->device = d;
 						d->session = pte;
 						d->codec_number = DEFAULT_CODEC;
-						d->pos_fav = 0;
 						d->missed_call = 0;
 						d->receiver_state = STATE_ONHOOK;
 						strcpy(d->id, pte->macaddr);
@@ -2989,8 +3668,8 @@
 				d = d->next;
 			}
 			ast_mutex_unlock(&devicelock);
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Invalid Terminal Number.");
-			send_text(TEXT_LINE1, TEXT_NORMAL, pte, "Please try again :");
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Invalid Terminal Number.", pte));
+			send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("Please try again :", pte));
 			send_cursor_pos(pte,
 						  (unsigned char) (TEXT_LINE2 + SELECTEXTENSION_START_ENTRY_POS +
 										   pte->size_buff_entry));
@@ -2998,9 +3677,9 @@
 		} else {
 			ast_copy_string(pte->device->extension_number, pte->buff_entry,
 							pte->size_buff_entry + 1);
-			if (RegisterExtension(pte)) {
-				send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Invalid extension.");
-				send_text(TEXT_LINE1, TEXT_NORMAL, pte, "Please try again :");
+			if (register_extension(pte)) {
+				send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Invalid extension.", pte));
+				send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("Please try again :", pte));
 				send_cursor_pos(pte,
 							  (unsigned char) (TEXT_LINE2 +
 											   SELECTEXTENSION_START_ENTRY_POS +
@@ -3020,12 +3699,14 @@
 	return;
 }
 
-static int ReformatNumber(char *number)
+/*
+static int reformat_number(char *number)
 {
 	int pos = 0, i = 0, size = strlen(number);
 
 	for (; i < size; i++) {
-		if ((number[i] >= '0') && (number[i] <= '9')) {
+		if (((number[i] >= '0') && (number[i] <= '9')) ||
+			(number[i] == '*') || (number[i] == '#') || (number[i] == '+')) {
 			if (i == pos) {
 				pos++;
 				continue;
@@ -3037,6 +3718,7 @@
 	number[pos] = 0;
 	return pos;
 }
+*/
 
 static void show_entry_history(struct unistimsession *pte, FILE ** f)
 {
@@ -3049,7 +3731,12 @@
 		return;
 	}
 	line[sizeof(line) - 1] = '\0';
-	send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+	if (pte->device->height == 1) {
+		if (pte->buff_entry[3] == 1)
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+	} else {
+		send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+	}
 	if (fread(line, TEXT_LENGTH_MAX, 1, *f) != 1) {
 		display_last_error("Can't read callerid entry");
 		fclose(*f);
@@ -3057,37 +3744,48 @@
 	}
 	line[sizeof(line) - 1] = '\0';
 	ast_copy_string(pte->device->lst_cid, line, sizeof(pte->device->lst_cid));
-	send_text(TEXT_LINE1, TEXT_NORMAL, pte, line);
+	if (pte->device->height == 1) {
+		if (pte->buff_entry[3] == 2)
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+	} else {
+		send_text(TEXT_LINE1, TEXT_NORMAL, pte, line);
+	}
 	if (fread(line, TEXT_LENGTH_MAX, 1, *f) != 1) {
 		display_last_error("Can't read callername entry");
 		fclose(*f);
 		return;
 	}
 	line[sizeof(line) - 1] = '\0';
-	send_text(TEXT_LINE2, TEXT_NORMAL, pte, line);
+	if (pte->device->height == 1) {
+		if (pte->buff_entry[3] == 3)
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+        } else {
+                send_text(TEXT_LINE2, TEXT_NORMAL, pte, line);
+        }
 	fclose(*f);
 
-	snprintf(line, sizeof(line), "Call %03d/%03d", pte->buff_entry[2],
+	snprintf(line, sizeof(line), "%s %03d/%03d", ustmtext("Call", pte), pte->buff_entry[2],
 			 pte->buff_entry[1]);
 	send_texttitle(pte, line);
 
 	if (pte->buff_entry[2] == 1)
-		strcpy(func1, "       ");
+		ast_copy_string(func1, "       ", sizeof(func1));
 	else
-		strcpy(func1, "Prvious");
+		ast_copy_string(func1, ustmtext("Prev   ", pte), sizeof(func1));
 	if (pte->buff_entry[2] >= pte->buff_entry[1])
-		strcpy(func2, "       ");
+		ast_copy_string(func2, "       ", sizeof(func2));
 	else
-		strcpy(func2, "Next   ");
-	if (ReformatNumber(pte->device->lst_cid))
-		strcpy(func3, "Redial ");
+		ast_copy_string(func2, ustmtext("Next   ", pte), sizeof(func2));
+	if (strlen(pte->device->lst_cid))
+		ast_copy_string(func3, ustmtext("Redial ", pte), sizeof(func3));
 	else
-		strcpy(func3, "       ");
-	snprintf(status, sizeof(status), "%s%s%sCancel", func1, func2, func3);
+		ast_copy_string(func3, "       ", sizeof(func3));
+
+	snprintf(status, sizeof(status), "%s%s%s%s", func1, func2, func3, ustmtext("Cancel", pte));
 	send_text_status(pte, status);
 }
 
-static char OpenHistory(struct unistimsession *pte, char way, FILE ** f)
+static char open_history(struct unistimsession *pte, char way, FILE ** f)
 {
 	char tmp[AST_CONFIG_MAX_PATH];
 	char count;
@@ -3122,12 +3820,13 @@
 		return;
 	if (!pte->device->callhistory)
 		return;
-	count = OpenHistory(pte, way, &f);
+	count = open_history(pte, way, &f);
 	if (!count)
 		return;
 	pte->buff_entry[0] = way;
 	pte->buff_entry[1] = count;
 	pte->buff_entry[2] = 1;
+	pte->buff_entry[3] = 1;
 	show_entry_history(pte, &f);
 	pte->state = STATE_HISTORY;
 }
@@ -3135,60 +3834,74 @@
 static void show_main_page(struct unistimsession *pte)
 {
 	char tmpbuf[TEXT_LENGTH_MAX + 1];
-
+	const char *text;
 
 	if ((pte->device->extension == EXTENSION_ASK) &&
 		(ast_strlen_zero(pte->device->extension_number))) {
-		ShowExtensionPage(pte);
+		show_extension_page(pte);
 		return;
 	}
 
 	pte->state = STATE_MAINPAGE;
 
 	send_tone(pte, 0, 0);
+	send_stop_timer(pte); /* case of holding call */
 	send_select_output(pte, pte->device->output, pte->device->volume, MUTE_ON_DISCRET);
-	pte->device->lines->lastmsgssent = 0;
-	send_favorite(pte->device->softkeylinepos, FAV_ICON_ONHOOK_BLACK, pte,
-				 pte->device->softkeylabel[pte->device->softkeylinepos]);
+	send_led_update(pte, 0x08);
+	send_led_update(pte, 0x10);
+
 	if (!ast_strlen_zero(pte->device->call_forward)) {
 		if (pte->device->height == 1) {
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Forwarding ON");
+			char tmp_field[100];
+			snprintf(tmp_field, sizeof(tmp_field), "%s %s", ustmtext("Fwd to:", pte), pte->device->call_forward);
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmp_field);
 		} else {
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Call forwarded to :");
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Call forwarded to :", pte));
 			send_text(TEXT_LINE1, TEXT_NORMAL, pte, pte->device->call_forward);
 		}
-		Sendicon(TEXT_LINE0, FAV_ICON_REFLECT + FAV_BLINK_SLOW, pte);
-		send_text_status(pte, "Dial   Redial NoForwd");
+		send_icon(TEXT_LINE0, FAV_ICON_REFLECT + FAV_BLINK_SLOW, pte);
+		send_text_status(pte, ustmtext("Dial   Redial NoFwd  ", pte));
 	} else {
 		if ((pte->device->extension == EXTENSION_ASK) ||
 			(pte->device->extension == EXTENSION_TN))
-			send_text_status(pte, "Dial   Redial ForwardUnregis");
+			send_text_status(pte, ustmtext("Dial   Redial Fwd    Unregis", pte));
 		else
-			send_text_status(pte, "Dial   Redial Forward");
+			send_text_status(pte, ustmtext("Dial   Redial Fwd    Pickup", pte));
 
 		send_text(TEXT_LINE1, TEXT_NORMAL, pte, pte->device->maintext1);
-		if (pte->device->missed_call == 0)
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, pte->device->maintext0);
-		else {
-			sprintf(tmpbuf, "%d unanswered call(s)", pte->device->missed_call);
+		if (pte->device->missed_call == 0) {
+			send_date_time2(pte);
+			send_idle_clock(pte);
+			if (strlen(pte->device->maintext0))
+				send_text(TEXT_LINE0, TEXT_NORMAL, pte, pte->device->maintext0);
+		} else {
+			if (pte->device->missed_call == 1) {
+				text = ustmtext("unanswered call", pte);
+			} else {
+				text = ustmtext("unanswered calls", pte);
+			}
+			snprintf(tmpbuf, sizeof(tmpbuf), "%d %s", pte->device->missed_call, text);
 			send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmpbuf);
-			Sendicon(TEXT_LINE0, FAV_ICON_CALL_CENTER + FAV_BLINK_SLOW, pte);
+			send_icon(TEXT_LINE0, FAV_ICON_CALL_CENTER + FAV_BLINK_SLOW, pte);
 		}
 	}
-	if (ast_strlen_zero(pte->device->maintext2)) {
-		strcpy(tmpbuf, "IP : ");
-		strcat(tmpbuf, ast_inet_ntoa(pte->sin.sin_addr));
-		send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmpbuf);
-	} else
-		send_text(TEXT_LINE2, TEXT_NORMAL, pte, pte->device->maintext2);
+	if (pte->device->height > 1) {
+		if (ast_strlen_zero(pte->device->maintext2)) {
+			strcpy(tmpbuf, "IP : ");
+			strcat(tmpbuf, ast_inet_ntoa(pte->sin.sin_addr));
+			send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmpbuf);
+		} else
+			send_text(TEXT_LINE2, TEXT_NORMAL, pte, pte->device->maintext2);
+	}
+
 	send_texttitle(pte, pte->device->titledefault);
-	change_favorite_icon(pte, FAV_ICON_ONHOOK_BLACK);
+	change_favorite_icon(pte, FAV_LINE_ICON);
 }
 
 static void key_main_page(struct unistimsession *pte, char keycode)
 {
 	if (pte->device->missed_call) {
-		Sendicon(TEXT_LINE0, FAV_ICON_NONE, pte);
+		send_icon(TEXT_LINE0, FAV_ICON_NONE, pte);
 		pte->device->missed_call = 0;
 	}
 	if ((keycode >= KEY_0) && (keycode <= KEY_SHARP)) {
@@ -3198,6 +3911,7 @@
 	}
 	switch (keycode) {
 	case KEY_FUNC1:
+		pte->device->selected = get_avail_softkey(pte, NULL);
 		handle_dial_page(pte);
 		break;
 	case KEY_FUNC2:
@@ -3211,7 +3925,7 @@
 
 		ast_copy_string(pte->device->phone_number, pte->device->redial_number,
 						sizeof(pte->device->phone_number));
-		HandleCallOutgoing(pte);
+		handle_call_outgoing(pte);
 		break;
 	case KEY_FUNC3:
 		if (!ast_strlen_zero(pte->device->call_forward)) {
@@ -3219,7 +3933,7 @@
 			memmove(pte->device->call_forward + 1, pte->device->call_forward,
 					sizeof(pte->device->call_forward));
 			pte->device->call_forward[0] = '\0';
-			Sendicon(TEXT_LINE0, FAV_ICON_NONE, pte);
+			send_icon(TEXT_LINE0, FAV_ICON_NONE, pte);
 			pte->device->output = OUTPUT_HANDSET;   /* Seems to be reseted somewhere */
 			show_main_page(pte);
 			break;
@@ -3229,9 +3943,9 @@
 		break;
 	case KEY_FUNC4:
 		if (pte->device->extension == EXTENSION_ASK) {
-			UnregisterExtension(pte);
+			unregister_extension(pte);
 			pte->device->extension_number[0] = '\0';
-			ShowExtensionPage(pte);
+			show_extension_page(pte);
 		} else if (pte->device->extension == EXTENSION_TN) {
 			ast_mutex_lock(&devicelock);
 			strcpy(pte->device->id, pte->device->extension_number);
@@ -3240,26 +3954,24 @@
 			pte->device->session = NULL;
 			pte->device = NULL;
 			ast_mutex_unlock(&devicelock);
-			ShowExtensionPage(pte);
-		}
+			show_extension_page(pte);
+		} else { /* Pickup function */
+			pte->device->selected = -1;
+			ast_copy_string(pte->device->phone_number, ast_pickup_ext(),
+						sizeof(pte->device->phone_number));
+			handle_call_outgoing(pte);
+                }
 		break;
 	case KEY_FAV0:
-		handle_dial_page(pte);
-		break;
 	case KEY_FAV1:
 	case KEY_FAV2:
 	case KEY_FAV3:
 	case KEY_FAV4:
 	case KEY_FAV5:
-		if ((pte->device->output == OUTPUT_HANDSET) &&
-			(pte->device->receiver_state == STATE_ONHOOK))
-			send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
-		else
-			send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
-		Keyfavorite(pte, keycode);
+		handle_key_fav(pte, keycode);
 		break;
 	case KEY_CONF:
-		HandleSelectCodec(pte);
+		handle_select_option(pte);
 		break;
 	case KEY_LOUDSPK:
 		send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
@@ -3284,44 +3996,41 @@
 	FILE *f;
 	char count;
 	long offset;
+	int flag = 0;
 
 	switch (keycode) {
-	case KEY_UP:
 	case KEY_LEFT:
+		if (pte->device->height == 1) {
+			if (pte->buff_entry[3] <= 1)
+				return;
+			pte->buff_entry[3]--;
+			flag = 1;
+			break;
+		}
+	case KEY_UP:
 	case KEY_FUNC1:
 		if (pte->buff_entry[2] <= 1)
 			return;
 		pte->buff_entry[2]--;
-		count = OpenHistory(pte, pte->buff_entry[0], &f);
-		if (!count)
-			return;
-		offset = ((pte->buff_entry[2] - 1) * TEXT_LENGTH_MAX * 3);
-		if (fseek(f, offset, SEEK_CUR)) {
-			display_last_error("Unable to seek history entry.");
-			fclose(f);
-			return;
-		}
-		show_entry_history(pte, &f);
+		flag = 1;
 		break;
-	case KEY_DOWN:
 	case KEY_RIGHT:
+		if (pte->device->height == 1) {
+			if (pte->buff_entry[3] == 3)
+				return;
+			pte->buff_entry[3]++;
+			flag = 1;
+			break;
+		}
+	case KEY_DOWN:
 	case KEY_FUNC2:
 		if (pte->buff_entry[2] >= pte->buff_entry[1])
 			return;
 		pte->buff_entry[2]++;
-		count = OpenHistory(pte, pte->buff_entry[0], &f);
-		if (!count)
-			return;
-		offset = ((pte->buff_entry[2] - 1) * TEXT_LENGTH_MAX * 3);
-		if (fseek(f, offset, SEEK_CUR)) {
-			display_last_error("Unable to seek history entry.");
-			fclose(f);
-			return;
-		}
-		show_entry_history(pte, &f);
+		flag = 1;
 		break;
 	case KEY_FUNC3:
-		if (!ReformatNumber(pte->device->lst_cid))
+		if (ast_strlen_zero(pte->device->lst_cid))
 			break;
 		ast_copy_string(pte->device->redial_number, pte->device->lst_cid,
 						sizeof(pte->device->redial_number));
@@ -3344,6 +4053,20 @@
 			show_history(pte, 'i');
 		break;
 	}
+
+	if (flag) {
+		count = open_history(pte, pte->buff_entry[0], &f);
+		if (!count)
+			return;
+		offset = ((pte->buff_entry[2] - 1) * TEXT_LENGTH_MAX * 3);
+		if (fseek(f, offset, SEEK_CUR)) {
+			display_last_error("Unable to seek history entry.");
+			fclose(f);
+			return;
+		}
+		show_entry_history(pte, &f);
+	}
+
 	return;
 }
 
@@ -3379,18 +4102,24 @@
 	send_client(SIZE_HEADER + sizeof(packet_send_S7), buffsend, pte);
 	send_led_update(pte, 0);
 	send_ping(pte);
+	if (unistimdebug)
+		ast_verb(0, "Sending init language\n");
+	if (pte->device) {
+		send_charset_update(pte, options_languages[find_language(pte->device->language)].encoding);
+	}
 	if (pte->state < STATE_MAINPAGE) {
 		if (autoprovisioning == AUTOPROVISIONING_TN) {
-			ShowExtensionPage(pte);
+			show_extension_page(pte);
 			return;
 		} else {
 			int i;
 			char tmp[30];
 
-			for (i = 1; i < 6; i++)
+			for (i = 1; i < 6; i++) {
 				send_favorite(i, 0, pte, "");
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Sorry, this phone is not");
-			send_text(TEXT_LINE1, TEXT_NORMAL, pte, "registered in unistim.cfg");
+			}
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Phone is not registered", pte));
+			send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("in unistim.conf", pte));
 			strcpy(tmp, "MAC = ");
 			strcat(tmp, pte->macaddr);
 			send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmp);
@@ -3417,14 +4146,26 @@
 		rcv_resume_connection_with_server(pte);
 		return;
 	}
-	if (memcmp(buf + SIZE_HEADER, packet_recv_firm_version, sizeof(packet_recv_firm_version)) ==
-		0) {
+	if (memcmp(buf + SIZE_HEADER, packet_recv_firm_version, sizeof(packet_recv_firm_version)) == 0) {
 		buf[size] = 0;
 		if (unistimdebug)
 			ast_verb(0, "Got the firmware version : '%s'\n", buf + 13);
 		init_phone_step2(pte);
 		return;
 	}
+	if (memcmp(buf + SIZE_HEADER, packet_recv_it_type, sizeof(packet_recv_it_type)) == 0) {
+		char type = buf[13];
+		if (unistimdebug)
+			ast_verb(0, "Got the equipment type: '%d'\n", type);
+		switch (type) {
+		case 0x03: /* i2002 */
+			if (pte->device) {
+				pte->device->height = 1;
+			}
+			break;
+		}
+		return;
+	}
 	if (memcmp(buf + SIZE_HEADER, packet_recv_mac_addr, sizeof(packet_recv_mac_addr)) == 0) {
 		rcv_mac_addr(pte, buf);
 		return;
@@ -3444,8 +4185,8 @@
 		char keycode = buf[13];
 
 		if (unistimdebug)
-			ast_verb(0, "Key pressed : keycode = 0x%.2x - current state : %d\n", keycode,
-						pte->state);
+			ast_verb(0, "Key pressed: keycode = 0x%.2x - current state: %s\n", keycode,
+						ptestate_tostr(pte->state));
 
 		switch (pte->state) {
 		case STATE_INIT:
@@ -3471,9 +4212,15 @@
 		case STATE_EXTENSION:
 			key_select_extension(pte, keycode);
 			break;
+		case STATE_SELECTOPTION:
+			key_select_option(pte, keycode);
+			break;
 		case STATE_SELECTCODEC:
 			key_select_codec(pte, keycode);
 			break;
+		case STATE_SELECTLANGUAGE:
+			key_select_language(pte, keycode);
+			break;
 		case STATE_HISTORY:
 			key_history(pte, keycode);
 			break;
@@ -3492,13 +4239,14 @@
 			send_select_output(pte, OUTPUT_HEADPHONE, pte->device->volume, MUTE_OFF);
 		else
 			send_select_output(pte, OUTPUT_HANDSET, pte->device->volume, MUTE_OFF);
-		if (pte->state == STATE_RINGING)
-			HandleCallIncoming(pte);
-		else if ((pte->state == STATE_DIALPAGE) || (pte->state == STATE_CALL))
+		if (pte->state == STATE_RINGING) {
+			handle_call_incoming(pte);
+		} else if ((pte->state == STATE_DIALPAGE) || (pte->state == STATE_CALL))
 			send_select_output(pte, OUTPUT_HANDSET, pte->device->volume, MUTE_OFF);
 		else if (pte->state == STATE_EXTENSION) /* We must have a TN before calling */
 			return;
 		else {
+			pte->device->selected = get_avail_softkey(pte, NULL);
 			send_select_output(pte, OUTPUT_HANDSET, pte->device->volume, MUTE_OFF);
 			handle_dial_page(pte);
 		}
@@ -3510,14 +4258,15 @@
 		if (!pte->device)
 			return;
 		pte->device->receiver_state = STATE_ONHOOK;
-		if (pte->state == STATE_CALL)
-			close_call(pte);
-		else if (pte->device->lines->subs[SUB_REAL]->owner)
-			close_call(pte);
-		else if (pte->state == STATE_EXTENSION)
+		if (pte->state == STATE_CALL) {
+			if (pte->device->output != OUTPUT_SPEAKER) {
+				close_call(pte);
+			}
+		} else if (pte->state == STATE_EXTENSION) {
 			return;
-		else
+		} else {
 			show_main_page(pte);
+		}
 		return;
 	}
 	strcpy(tmpbuf, ast_inet_ntoa(pte->sin.sin_addr));
@@ -3703,28 +4452,71 @@
 		ast_log(LOG_WARNING, "Unistim callback function called without a device\n");
 		return NULL;
 	}
-	if (!sub->parent->parent->session) {
-		ast_log(LOG_WARNING, "Unistim callback function called without a session\n");
-		return NULL;
+	ast_mutex_lock(&sub->parent->parent->lock);
+	if (!sub->parent->parent->session) {
+		ast_log(LOG_WARNING, "Unistim callback function called without a session\n");
+		return NULL;
+	}
+	ast_mutex_unlock(&sub->parent->parent->lock);
+	return sub->parent->parent->session;
+}
+
+static void send_callerid_screen(struct unistimsession *pte, struct unistim_subchannel *sub) 
+{
+	char *cidname_str;
+	char *cidnum_str;
+
+	if (!sub) {
+		return;
+	}
+	if (sub->owner) {
+		if (sub->owner->connected.id.number.valid && sub->owner->connected.id.number.str) {
+			cidnum_str = sub->owner->connected.id.number.str;
+		} else {
+			cidnum_str = DEFAULTCALLERID;
+		}
+		change_callerid(pte, 0, cidnum_str);
+		if (strlen(cidnum_str) == 0) {
+			cidnum_str = DEFAULTCALLERID;
+		}
+
+		if (sub->owner->connected.id.name.valid && sub->owner->connected.id.name.str) {
+			cidname_str = sub->owner->connected.id.name.str;
+		} else {
+			cidname_str = DEFAULTCALLERNAME;
+		}
+		change_callerid(pte, 1, cidname_str);
+		if (strlen(cidname_str) == 0) {
+			cidname_str = DEFAULTCALLERNAME;
+		}
+
+		if (pte->device->height == 1) {
+			char tmpstr[256];
+			snprintf(tmpstr, sizeof(tmpstr), "%s %s", cidnum_str, ustmtext(cidname_str, pte));
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmpstr);
+		} else {
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, cidname_str);
+			send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext(cidnum_str, pte));
+		}
 	}
-	return sub->parent->parent->session;
 }
 
 /*--- unistim_call: Initiate UNISTIM call from PBX ---*/
 /*      used from the dial() application      */
 static int unistim_call(struct ast_channel *ast, char *dest, int timeout)
 {
-	int res = 0;
-	struct unistim_subchannel *sub;
+	int res = 0, i;
+	struct unistim_subchannel *sub, *sub_real;
 	struct unistimsession *session;
+	char ringstyle, ringvolume;
 
 	session = channel_to_session(ast);
 	if (!session) {
 		ast_log(LOG_ERROR, "Device not registered, cannot call %s\n", dest);
 		return -1;
 	}
-
 	sub = ast->tech_pvt;
+	sub_real = get_sub(session->device, SUB_REAL);
 	if ((ast->_state != AST_STATE_DOWN) && (ast->_state != AST_STATE_RESERVED)) {
 		ast_log(LOG_WARNING, "unistim_call called on %s, neither down nor reserved\n",
 				ast->name);
@@ -3733,157 +4525,151 @@
 
 	if (unistimdebug)
 		ast_verb(3, "unistim_call(%s)\n", ast->name);
-
 	session->state = STATE_RINGING;
-	Sendicon(TEXT_LINE0, FAV_ICON_NONE, session);
-
-	if (sub->owner) {
-		if (sub->owner->connected.id.number.valid
-			&& sub->owner->connected.id.number.str) {
-			if (session->device->height == 1) {
-				send_text(TEXT_LINE0, TEXT_NORMAL, session, sub->owner->connected.id.number.str);
-			} else {
-				send_text(TEXT_LINE1, TEXT_NORMAL, session, sub->owner->connected.id.number.str);
-			}
-			change_callerid(session, 0, sub->owner->connected.id.number.str);
+	send_callerid_screen(session, sub);
+	if (ast_strlen_zero(ast->call_forward)) { /* Send ring only if no call forward, otherwise short ring will apear */
+		send_text(TEXT_LINE2, TEXT_NORMAL, session, ustmtext("is calling you.", session));
+		send_text_status(session, ustmtext("Accept        Ignore Hangup", session));
+
+		if (sub_real) {
+			ringstyle = session->device->cwstyle;
+			ringvolume = session->device->cwvolume;
 		} else {
-			if (session->device->height == 1) {
-				send_text(TEXT_LINE0, TEXT_NORMAL, session, DEFAULTCALLERID);
-			} else {
-				send_text(TEXT_LINE1, TEXT_NORMAL, session, DEFAULTCALLERID);
-			}
-			change_callerid(session, 0, DEFAULTCALLERID);
+			ringstyle = (sub->ringstyle == -1) ? session->device->ringstyle : sub->ringstyle;
+			ringvolume = (sub->ringvolume == -1) ? session->device->ringvolume : sub->ringvolume;
 		}
-		if (sub->owner->connected.id.name.valid
-			&& sub->owner->connected.id.name.str) {
-			send_text(TEXT_LINE0, TEXT_NORMAL, session, sub->owner->connected.id.name.str);
-			change_callerid(session, 1, sub->owner->connected.id.name.str);
-		} else {
-			send_text(TEXT_LINE0, TEXT_NORMAL, session, DEFAULTCALLERNAME);
-			change_callerid(session, 1, DEFAULTCALLERNAME);
+		send_ring(session, ringvolume, ringstyle);
+		change_favorite_icon(session, FAV_ICON_SPEAKER_ONHOOK_BLACK + FAV_BLINK_FAST);
+		/* Send call identification to all */
+		for (i = 0; i < FAVNUM; i++) {
+			if (!soft_key_visible(session->device, i))
+				continue;
+			if (session->device->ssub[i])
+				continue;
+			if (is_key_line(session->device, i) && !strcmp(sub->parent->name, session->device->sline[i]->name)) {
+				if (unistimdebug)
+					ast_verb(0, "Found softkey %d for line %s\n", i, sub->parent->name);
+				send_favorite_short(i, FAV_ICON_SPEAKER_ONHOOK_BLACK + FAV_BLINK_FAST, session);
+				session->device->ssub[i] = sub;
+			}
 		}
 	}
-	send_text(TEXT_LINE2, TEXT_NORMAL, session, "is calling you.");
-	send_text_status(session, "Accept              Ignore");
-
-	if (sub->ringstyle == -1)
-		send_ring(session, session->device->ringvolume, session->device->ringstyle);
-	else {
-		if (sub->ringvolume == -1)
-			send_ring(session, session->device->ringvolume, sub->ringstyle);
-		else
-			send_ring(session, sub->ringvolume, sub->ringstyle);
-	}
-	change_favorite_icon(session, FAV_ICON_SPEAKER_ONHOOK_BLACK + FAV_BLINK_FAST);
-
 	ast_setstate(ast, AST_STATE_RINGING);
 	ast_queue_control(ast, AST_CONTROL_RINGING);
 	return res;
 }
 
+static int unistim_hangup_clean(struct ast_channel *ast, struct unistim_subchannel *sub) {
+	ast_mutex_lock(&sub->lock);
+	ast->tech_pvt = NULL;
+	sub->owner = NULL;
+	sub->alreadygone = 0;
+	ast_mutex_unlock(&sub->lock);
+	if (sub->rtp) {
+		if (unistimdebug)
+			ast_verb(0, "Destroying RTP session\n");
+		ast_rtp_instance_destroy(sub->rtp);
+		sub->rtp = NULL;
+	}
+	return 0;
+}
+
 /*--- unistim_hangup: Hangup UNISTIM call */
 static int unistim_hangup(struct ast_channel *ast)
 {
-	struct unistim_subchannel *sub;
+	struct unistim_subchannel *sub = NULL, *sub_real = NULL, *sub_trans = NULL;
 	struct unistim_line *l;
+	struct unistim_device *d;
 	struct unistimsession *s;
+	int i;
 
 	s = channel_to_session(ast);
 	sub = ast->tech_pvt;
+	l = sub->parent;
+	d = l->parent;
 	if (!s) {
 		ast_debug(1, "Asked to hangup channel not connected\n");
-		ast_mutex_lock(&sub->lock);
-		sub->owner = NULL;
-		ast->tech_pvt = NULL;
-		sub->alreadygone = 0;
-		ast_mutex_unlock(&sub->lock);
-		if (sub->rtp) {
-			if (unistimdebug)
-				ast_verb(0, "Destroying RTP session\n");
-			ast_rtp_instance_destroy(sub->rtp);
-			sub->rtp = NULL;
-		}
+		unistim_hangup_clean(ast, sub);
 		return 0;
 	}
-	l = sub->parent;
+	refresh_all_favorite(s); /* Update favicons in case of DND keys */
 	if (unistimdebug)
-		ast_verb(0, "unistim_hangup(%s) on %s@%s\n", ast->name, l->name, l->parent->name);
-
-	if ((l->subs[SUB_THREEWAY]) && (sub->subtype == SUB_REAL)) {
+		ast_verb(0, "unistim_hangup(%s) on %s@%s (STATE_%s)\n", ast->name, l->name, l->parent->name, ptestate_tostr(s->state));
+	sub_trans = get_sub(d, SUB_THREEWAY);
+	if (sub_trans && (sub_trans->owner) && (sub->subtype == SUB_REAL) &&
+		(sub->alreadygone == 0)) {
 		if (unistimdebug)
-			ast_verb(0, "Real call disconnected while talking to threeway\n");
-		sub->owner = NULL;
-		ast->tech_pvt = NULL;
+			ast_verb(0, "Threeway call disconnected, switching to real call\n");
+		if (ast_bridged_channel(sub_trans->owner)) {
+			ast_moh_stop(ast_bridged_channel(sub_trans->owner));
+		}
+		sub_trans->moh = 0;
+		sub_trans->subtype = SUB_REAL;
+		swap_subs(sub_trans, sub);
+
+		send_text_status(s, ustmtext("       Transf        Hangup", s));
+		send_callerid_screen(s, sub_trans);
+		unistim_hangup_clean(ast, sub);
+		unistim_unalloc_sub(d, sub);
 		return 0;
 	}
-	if ((l->subs[SUB_REAL]->owner) && (sub->subtype == SUB_THREEWAY) &&
+	sub_real = get_sub(d, SUB_REAL);
+	if (sub_real && (sub_real->owner) && (sub->subtype == SUB_THREEWAY) &&
 		(sub->alreadygone == 0)) {
 		if (unistimdebug)
-			ast_verb(0, "threeway call disconnected, switching to real call\n");
-		send_text(TEXT_LINE0, TEXT_NORMAL, s, "Three way call canceled,");
-		send_text(TEXT_LINE1, TEXT_NORMAL, s, "switching back to");
-		send_text(TEXT_LINE2, TEXT_NORMAL, s, "previous call.");
-		send_text_status(s, "Hangup Transf");
-		ast_moh_stop(ast_bridged_channel(l->subs[SUB_REAL]->owner));
-		swap_subs(l, SUB_THREEWAY, SUB_REAL);
-		l->parent->moh = 0;
-		ast_mutex_lock(&sub->lock);
-		sub->owner = NULL;
-		ast->tech_pvt = NULL;
-		ast_mutex_unlock(&sub->lock);
-		unalloc_sub(l, SUB_THREEWAY);
+			ast_verb(0, "Real call disconnected, stay in call\n");
+      
+		send_text_status(s, ustmtext("       Transf        Hangup", s));
+		send_callerid_screen(s, sub_real);
+		unistim_hangup_clean(ast, sub);
+		unistim_unalloc_sub(d, sub);
 		return 0;
 	}
-	ast_mutex_lock(&sub->lock);
-	sub->owner = NULL;
-	ast->tech_pvt = NULL;
-	sub->alreadygone = 0;
-	ast_mutex_unlock(&sub->lock);
-	if (!s) {
-		if (unistimdebug)
-			ast_verb(0, "Asked to hangup channel not connected (no session)\n");
-		if (sub->rtp) {
-			if (unistimdebug)
-				ast_verb(0, "Destroying RTP session\n");
-			ast_rtp_instance_destroy(sub->rtp);
-			sub->rtp = NULL;
+
+	if (sub->subtype == SUB_REAL) {
+		sub_stop_silence(s, sub);
+		send_end_call(s); /* Send end call packet only if ending active call, in other way sound should be loosed */
+	} else if (sub->subtype == SUB_RING) {
+		send_no_ring(s);
+		for (i = 0; i < FAVNUM; i++) {
+			if (!soft_key_visible(s->device, i))
+				continue;
+			if (d->ssub[i] != sub)
+				continue;
+			if (is_key_line(d, i) && !strcmp(l->name, d->sline[i]->name)) {
+				send_favorite_short(i, FAV_LINE_ICON, s);
+				d->ssub[i] = NULL;
+			}
 		}
-		return 0;
 	}
-	if (sub->subtype == SUB_REAL) {
-		/* Stop the silence generator */
-		if (s->device->silence_generator) {
-			if (unistimdebug)
-				ast_verb(0, "Stopping silence generator\n");
-			if (sub->owner)
-				ast_channel_stop_silence_generator(sub->owner,
-												   s->device->silence_generator);
-			else
-				ast_log(LOG_WARNING,
-						"Trying to stop silence generator on a null channel !\n");
-			s->device->silence_generator = NULL;
+	sub->moh = 0;
+	if (sub->softkey >= 0)
+		send_favorite_short(sub->softkey, FAV_LINE_ICON, s);
+
+	/* Delete assign sub to softkey */
+	for (i = 0; i < FAVNUM; i++) {
+		if (d->ssub[i] == sub) {
+			d->ssub[i] = NULL;
+			break;
 		}
 	}
-	l->parent->moh = 0;
-	send_no_ring(s);
-	send_end_call(s);
-	if (sub->rtp) {
-		if (unistimdebug)
-			ast_verb(0, "Destroying RTP session\n");
-		ast_rtp_instance_destroy(sub->rtp);
-		sub->rtp = NULL;
-	} else if (unistimdebug)
-		ast_verb(0, "No RTP session to destroy\n");
-	if (l->subs[SUB_THREEWAY]) {
-		if (unistimdebug)
-			ast_verb(0, "Cleaning other subchannels\n");
-		unalloc_sub(l, SUB_THREEWAY);
+	refresh_all_favorite(s); /* Update favicons in case of DND keys */
+	if (s->state == STATE_RINGING && sub->subtype == SUB_RING) {
+		send_no_ring(s);
+		if (!ast_test_flag(ast, AST_FLAG_ANSWERED_ELSEWHERE) && ast->hangupcause != AST_CAUSE_ANSWERED_ELSEWHERE) {
+			d->missed_call++;
+			write_history(s, 'i', 1);
+		}
+		if (!sub_real) {
+			show_main_page(s);
+		}
 	}
-	if (s->state == STATE_RINGING)
-		cancel_dial(s);
-	else if (s->state == STATE_CALL)
+	if (s->state == STATE_CALL && sub->subtype == SUB_REAL) {
 		close_call(s);
-
+	}
+	sub->softkey = -1;
+	unistim_hangup_clean(ast, sub);
+	unistim_unalloc_sub(d, sub);
 	return 0;
 }
 
@@ -3893,6 +4679,7 @@
 	int res = 0;
 	struct unistim_subchannel *sub;
 	struct unistim_line *l;
+	struct unistim_device *d;
 	struct unistimsession *s;
 
 	s = channel_to_session(ast);
@@ -3902,18 +4689,19 @@
 	}
 	sub = ast->tech_pvt;
 	l = sub->parent;
+	d = l->parent;
 
-	if ((!sub->rtp) && (!l->subs[SUB_THREEWAY]))
+	if ((!sub->rtp) && (!get_sub(d, SUB_THREEWAY)))
 		start_rtp(sub);
 	if (unistimdebug)
 		ast_verb(0, "unistim_answer(%s) on %s@%s-%d\n", ast->name, l->name,
-					l->parent->name, sub->subtype);
-	send_text(TEXT_LINE2, TEXT_NORMAL, l->parent->session, "is now on-line");
-	if (l->subs[SUB_THREEWAY])
-		send_text_status(l->parent->session, "Transf Cancel");
+					l->parent->name, sub->softkey);
+	send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("is now on-line", s));
+	if (get_sub(d, SUB_THREEWAY))
+		send_text_status(s, ustmtext("Transf Cancel", s));
 	else
-		send_text_status(l->parent->session, "Hangup Transf");
-	send_start_timer(l->parent->session);
+		send_text_status(s, ustmtext("       Transf        Hangup", s));
+	send_start_timer(s);
 	if (ast->_state != AST_STATE_UP)
 		ast_setstate(ast, AST_STATE_UP);
 	return res;
@@ -3928,11 +4716,13 @@
 	int found = 0;
 	int tmp = 0;
 	int dw_num_bytes_rcvd;
+	unsigned int size_addr_from;
 #ifdef DUMP_PACKET
 	int dw_num_bytes_rcvdd;
 	char iabuf[INET_ADDRSTRLEN];
 #endif
 
+	size_addr_from = sizeof(addr_from);
 	dw_num_bytes_rcvd =
 		recvfrom(unistimsock, buff, SIZE_PAGE, 0, (struct sockaddr *) &addr_from,
 				 &size_addr_from);
@@ -4009,10 +4799,11 @@
 	if (sub->owner) {
 		/* We already hold the channel lock */
 		if (f->frametype == AST_FRAME_VOICE) {
-			if (f->subclass.codec != sub->owner->nativeformats) {
+			if (sub->owner->nativeformats != f->subclass.codec) {
+				char tmp[256];
 				ast_debug(1,
 						"Oooh, format changed from %s to %s\n",
-						ast_getformatname(sub->owner->nativeformats),
+						ast_getformatname_multiple(tmp, sizeof(tmp), sub->owner->nativeformats),
 						ast_getformatname(f->subclass.codec));
 
 				sub->owner->nativeformats = f->subclass.codec;
@@ -4051,7 +4842,7 @@
 			return 0;
 		}
 	} else {
-		if (!(frame->subclass.codec & ast->nativeformats)) {
+		if (!(ast->nativeformats & frame->subclass.codec)) {
 			char tmp[256];
 			ast_log(LOG_WARNING,
 					"Asked to transmit frame type %s, while native formats is %s (read/write = (%s/%s)\n",
@@ -4127,6 +4918,12 @@
 		return "Key Radio";
 	case AST_CONTROL_RADIO_UNKEY:
 		return "Un-Key Radio";
+	case AST_CONTROL_CONNECTED_LINE:
+		return "Remote end changed";
+	case AST_CONTROL_SRCCHANGE:
+		return "RTP source updated";
+	case AST_CONTROL_SRCUPDATE:
+		return "Source of media changed";
 	case -1:
 		return "Stop tone";
 	}
@@ -4154,8 +4951,8 @@
 	struct unistimsession *s;
 
 	if (unistimdebug) {
-		ast_verb(3, "Asked to indicate '%s' condition on channel %s\n",
-					control2str(ind), ast->name);
+		ast_verb(3, "Asked to indicate '%s' (%d) condition on channel %s\n",
+					control2str(ind), ind, ast->name);
 	}
 
 	s = channel_to_session(ast);
@@ -4168,7 +4965,7 @@
 	switch (ind) {
 	case AST_CONTROL_RINGING:
 		if (ast->_state != AST_STATE_UP) {
-			send_text(TEXT_LINE2, TEXT_NORMAL, s, "Ringing...");
+			send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("Ringing...", s));
 			in_band_indication(ast, l->parent->tz, "ring");
 			s->device->missed_call = -1;
 			break;
@@ -4177,7 +4974,7 @@
 	case AST_CONTROL_BUSY:
 		if (ast->_state != AST_STATE_UP) {
 			sub->alreadygone = 1;
-			send_text(TEXT_LINE2, TEXT_NORMAL, s, "Busy");
+			send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("Busy", s));
 			in_band_indication(ast, l->parent->tz, "busy");
 			s->device->missed_call = -1;
 			break;
@@ -4190,7 +4987,7 @@
 	case AST_CONTROL_CONGESTION:
 		if (ast->_state != AST_STATE_UP) {
 			sub->alreadygone = 1;
-			send_text(TEXT_LINE2, TEXT_NORMAL, s, "Congestion");
+			send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("Congestion", s));
 			in_band_indication(ast, l->parent->tz, "congestion");
 			s->device->missed_call = -1;
 			break;
@@ -4204,12 +5001,22 @@
 		break;
 	case AST_CONTROL_PROGRESS:
 	case AST_CONTROL_SRCUPDATE:
+	case AST_CONTROL_PROCEEDING:
 		break;
 	case -1:
 		ast_playtones_stop(ast);
 		s->device->missed_call = 0;
 		break;
-	case AST_CONTROL_PROCEEDING:
+        case AST_CONTROL_CONNECTED_LINE:
+		ast_log(LOG_NOTICE, "Connected party is now %s <%s>\n",
+		S_COR(ast->connected.id.name.valid, ast->connected.id.name.str, ""),
+		S_COR(ast->connected.id.number.valid, ast->connected.id.number.str, ""));
+		if (sub->subtype == SUB_REAL) {
+			send_callerid_screen(s, sub);
+		}
+		break;
+	case AST_CONTROL_SRCCHANGE:
+		ast_rtp_instance_change_source(sub->rtp);
 		break;
 	default:
 		ast_log(LOG_WARNING, "Don't know how to indicate condition %d\n", ind);
@@ -4223,6 +5030,7 @@
 {
 	struct unistim_line *l;
 	struct unistim_device *d;
+	struct unistim_subchannel *sub = NULL;
 	char line[256];
 	char *at;
 	char *device;
@@ -4246,12 +5054,22 @@
 			if (unistimdebug)
 				ast_verb(0, "Found device: %s\n", d->name);
 			/* Found the device */
-			l = d->lines;
-			while (l) {
+			AST_LIST_LOCK(&d->lines);
+			AST_LIST_TRAVERSE(&d->lines, l, list) {
 				/* Search for the right line */
 				if (!strcasecmp(l->name, line)) {
-					l->subs[SUB_REAL]->ringvolume = -1;
-					l->subs[SUB_REAL]->ringstyle = -1;
+					if (unistimdebug)
+						ast_verb(0, "Found line: %s\n", l->name);
+					sub = get_sub(d, SUB_REAL);
+					if (!sub) {
+						sub = unistim_alloc_sub(d, SUB_REAL);
+					}
+					if (sub->owner) {
+						/* Allocate additional channel if asterisk channel already here */
+						sub = unistim_alloc_sub(d, SUB_ONHOLD);
+					}
+					sub->ringvolume = -1;
+					sub->ringstyle = -1;
 					if (at) {       /* Other options ? */
 						at++;   /* Skip slash */
 						if (*at == 'r') {       /* distinctive ring */
@@ -4267,15 +5085,19 @@
 								if (unistimdebug)
 									ast_verb(0, "Distinctive ring : style #%d volume %d\n",
 										 ring_style, ring_volume);
-								l->subs[SUB_REAL]->ringvolume = ring_volume;
-								l->subs[SUB_REAL]->ringstyle = ring_style;
+								sub->ringvolume = ring_volume;
+								sub->ringstyle = ring_style;
 							}
 						}
 					}
-					ast_mutex_unlock(&devicelock);
-					return l->subs[SUB_REAL];
+					sub->parent = l;
+					break;
 				}
-				l = l->next;
+			}
+			AST_LIST_UNLOCK(&d->lines);
+			if (sub) {
+				ast_mutex_unlock(&devicelock);
+				return sub;
 			}
 		}
 		d = d->next;
@@ -4302,9 +5124,9 @@
 	struct ast_frame f = { 0, };
 	struct unistim_subchannel *sub;
 
-	sub = pte->device->lines->subs[SUB_REAL];
+	sub = get_sub(pte->device, SUB_REAL);
 
-	if (!sub->owner || sub->alreadygone) {
+	if (!sub || !sub->owner || sub->alreadygone) {
 		ast_log(LOG_WARNING, "Unable to find subchannel in dtmf senddigit_end\n");
 		return -1;
 	}
@@ -4437,7 +5259,7 @@
 		if (pte->device->height == 1) {
 			send_text(TEXT_LINE0, TEXT_NORMAL, pte, text);
 		} else {
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Message :");
+			send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Message :", pte));
 			send_text(TEXT_LINE1, TEXT_NORMAL, pte, text);
 		}
 		if (size <= TEXT_LENGTH_MAX) {
@@ -4460,18 +5282,17 @@
 }
 
 /*--- unistim_send_mwi_to_peer: Send message waiting indication ---*/
-static int unistim_send_mwi_to_peer(struct unistimsession *s, unsigned int tick)
+static int unistim_send_mwi_to_peer(struct unistim_line *peer, unsigned int tick)
 {
 	struct ast_event *event;
 	int new;
 	char *mailbox, *context;
-	struct unistim_line *peer = s->device->lines;
-
+	
 	context = mailbox = ast_strdupa(peer->mailbox);
 	strsep(&context, "@");
 	if (ast_strlen_zero(context))
 		context = "default";
-
+	
 	event = ast_event_get_cached(AST_EVENT_MWI,
 		AST_EVENT_IE_MAILBOX, AST_EVENT_IE_PLTYPE_STR, mailbox,
 		AST_EVENT_IE_CONTEXT, AST_EVENT_IE_PLTYPE_STR, context,
@@ -4490,13 +5311,9 @@
 	if (new == peer->lastmsgssent) {
 		return 0;
 	}
-
+	
 	peer->lastmsgssent = new;
-	if (new == 0) {
-		send_led_update(s, 0);
-	} else {
-		send_led_update(s, 1);
-	}
+	send_led_update(peer->parent->session, (new > 0));
 
 	return 0;
 }
@@ -4519,9 +5336,9 @@
 	}
 	l = sub->parent;
 	tmp = ast_channel_alloc(1, state, l->cid_num, NULL, l->accountcode, l->exten,
-		l->context, linkedid, l->amaflags, "%s@%s-%d", l->name, l->parent->name, sub->subtype);
+		l->parent->context, linkedid, l->amaflags, "USTM/%s@%s-%p", l->name, l->parent->name, sub);
 	if (unistimdebug)
-		ast_verb(0, "unistim_new sub=%d (%p) chan=%p\n", sub->subtype, sub, tmp);
+		ast_verb(0, "unistim_new sub=%d (%p) chan=%p line=%s\n", sub->subtype, sub, tmp, l->name);
 	if (!tmp) {
 		ast_log(LOG_WARNING, "Unable to allocate channel structure\n");
 		return NULL;
@@ -4559,12 +5376,9 @@
 	tmp->rawreadformat = fmt;
 	tmp->tech_pvt = sub;
 	tmp->tech = &unistim_tech;
-	if (!ast_strlen_zero(l->language))
-		ast_string_field_set(tmp, language, l->language);
+	if (!ast_strlen_zero(l->parent->language))
+		ast_string_field_set(tmp, language, l->parent->language);
 	sub->owner = tmp;
-	ast_mutex_lock(&usecnt_lock);
-	usecnt++;
-	ast_mutex_unlock(&usecnt_lock);
 	ast_update_use_count();
 	tmp->callgroup = l->callgroup;
 	tmp->pickupgroup = l->pickupgroup;
@@ -4640,11 +5454,19 @@
 				dw_timeout = cur->timeout - tick;
 			/* Checking if the phone is logged on for a new MWI */
 			if (cur->device) {
-				if ((!ast_strlen_zero(cur->device->lines->mailbox)) &&
-					((tick >= cur->device->lines->nextmsgcheck))) {
-					DEBUG_TIMER("Checking mailbox for MWI\n");
-					unistim_send_mwi_to_peer(cur, tick);
-					break;
+				struct unistim_line *l;
+				AST_LIST_LOCK(&cur->device->lines);
+				AST_LIST_TRAVERSE(&cur->device->lines, l, list) {
+					if ((!ast_strlen_zero(l->mailbox)) && (tick >= l->nextmsgcheck)) {
+						DEBUG_TIMER("Checking mailbox for MWI\n");
+						unistim_send_mwi_to_peer(l, tick);
+						break;
+					}
+				}
+				AST_LIST_UNLOCK(&cur->device->lines);
+				if (cur->device->nextdial && tick >= cur->device->nextdial) {
+					handle_call_outgoing(cur);
+					cur->device->nextdial = 0;
 				}
 			}
 			cur = cur->next;
@@ -4711,18 +5533,14 @@
 										   int *cause)
 {
 	format_t oldformat;
-	struct unistim_subchannel *sub;
+	struct unistim_subchannel *sub, *sub_ring, *sub_trans;
+	struct unistim_device *d;
 	struct ast_channel *tmpc = NULL;
 	char tmp[256];
 	char *dest = data;
 
 	oldformat = format;
 	format &= CAPABILITY;
-	ast_log(LOG_NOTICE,
-			"Asked to get a channel of format %s while capability is %s result : %s\n",
-			ast_getformatname(oldformat),
-			ast_getformatname_multiple(tmp, sizeof(tmp), CAPABILITY),
-			ast_getformatname(format));
 	if (!format) {
 		ast_log(LOG_NOTICE,
 				"Asked to get a channel of unsupported format %s while capability is %s\n",
@@ -4735,22 +5553,37 @@
 		ast_log(LOG_NOTICE, "Unistim channels require a device\n");
 		return NULL;
 	}
-
 	sub = find_subchannel_by_name(tmp);
 	if (!sub) {
 		ast_log(LOG_NOTICE, "No available lines on: %s\n", dest);
 		*cause = AST_CAUSE_CONGESTION;
 		return NULL;
 	}
-
-	ast_verb(3, "unistim_request(%s)\n", tmp);
-	/* Busy ? */
-	if (sub->owner) {
+	d = sub->parent->parent;
+	sub_ring = get_sub(d, SUB_RING);
+	sub_trans = get_sub(d, SUB_THREEWAY);
+	/* Another request already in progress */
+	if (!d->session) {
+		unistim_unalloc_sub(d, sub);
+		*cause = AST_CAUSE_CONGESTION;
+		return NULL;
+	}
+	if (sub_ring || sub_trans) {
+		if (unistimdebug)
+			ast_verb(0, "Can't create channel, request already in progress: Busy!\n");
+		unistim_unalloc_sub(d, sub);
+		*cause = AST_CAUSE_BUSY;
+		return NULL;
+	}
+        if (get_avail_softkey(d->session, sub->parent->name) == -1) {
 		if (unistimdebug)
-			ast_verb(0, "Can't create channel : Busy !\n");
+			ast_verb(0, "Can't create channel for line %s, all lines busy\n", sub->parent->name);
+		unistim_unalloc_sub(d, sub);
 		*cause = AST_CAUSE_BUSY;
 		return NULL;
 	}
+	sub->subtype = SUB_RING;
+	sub->softkey = -1;
 	sub->parent->capability = format;
 	tmpc = unistim_new(sub, AST_STATE_DOWN, requestor ? requestor->linkedid : NULL);
 	if (!tmpc)
@@ -4763,13 +5596,12 @@
 	return tmpc;
 }
 
-static char *unistim_info(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
+static char *unistim_show_info(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
 {
 	struct unistim_device *device = devices;
 	struct unistim_line *line;
 	struct unistim_subchannel *sub;
 	struct unistimsession *s;
-	int i;
 	struct ast_channel *tmp;
 
 	switch (cmd) {
@@ -4777,7 +5609,11 @@
 		e->command = "unistim show info";
 		e->usage =
 			"Usage: unistim show info\n" 
-			"       Dump internal structures.\n";
+			"       Dump internal structures.\n\n"
+			"       device\n"
+			"       ->line\n"
+			"       -->sub\n"
+			"       ==>key\n";
 		return NULL;
 
 	case CLI_GENERATE:
@@ -4787,45 +5623,59 @@
 	if (a->argc != e->args)
 		return CLI_SHOWUSAGE;
 
-	ast_cli(a->fd, "Dumping internal structures :\ndevice\n->line\n-->sub\n");
+	ast_cli(a->fd, "Dumping internal structures:\n");
+	ast_mutex_lock(&devicelock);
 	while (device) {
-		ast_cli(a->fd, "\nname=%s id=%s line=%p ha=%p sess=%p device=%p\n",
-				device->name, device->id, device->lines, device->ha, device->session,
-				device);
-		line = device->lines;
-		while (line) {
+		int i;
+
+		ast_cli(a->fd, "\nname=%s id=%s ha=%p sess=%p device=%p selected=%d height=%d\n",
+				device->name, device->id, device->ha, device->session,
+				device, device->selected, device->height);
+		AST_LIST_LOCK(&device->lines);
+		AST_LIST_TRAVERSE(&device->lines,line,list) {
+			char tmp2[256];
 			ast_cli(a->fd,
-					"->name=%s fullname=%s exten=%s callid=%s cap=%" PRId64 " device=%p line=%p\n",
+					"->name=%s fullname=%s exten=%s callid=%s cap=%s line=%p\n",
 					line->name, line->fullname, line->exten, line->cid_num,
-					line->capability, line->parent, line);
-			for (i = 0; i < MAX_SUBS; i++) {
-				sub = line->subs[i];
-				if (!sub)
-					continue;
-				if (!sub->owner)
-					tmp = (void *) -42;
-				else
-					tmp = sub->owner->_bridge;
-				if (sub->subtype != i)
-					ast_cli(a->fd, "Warning ! subchannel->subs[%d] have a subtype=%d\n", i,
-							sub->subtype);
-				ast_cli(a->fd,
-						"-->subtype=%d chan=%p rtp=%p bridge=%p line=%p alreadygone=%d\n",
-						sub->subtype, sub->owner, sub->rtp, tmp, sub->parent,
-						sub->alreadygone);
+					ast_getformatname_multiple(tmp2, sizeof(tmp2), line->capability), line);
+		}
+		AST_LIST_UNLOCK(&device->lines);
+                
+		AST_LIST_LOCK(&device->subs);
+		AST_LIST_TRAVERSE(&device->subs, sub, list) {
+			if (!sub)
+				continue;
+			if (!sub->owner) {
+				tmp = (void *) -42;
+			} else {
+				tmp = sub->owner->_bridge;
+			}
+			ast_cli(a->fd,
+					"-->subtype=%s chan=%p rtp=%p bridge=%p line=%p alreadygone=%d softkey=%d\n",
+					subtype_tostr(sub->subtype), sub->owner, sub->rtp, tmp, sub->parent,
+					sub->alreadygone, sub->softkey);
+		}
+		AST_LIST_UNLOCK(&device->subs);
+
+		for (i = 0; i < FAVNUM; i++) {
+			if (!soft_key_visible(device, i)) {
+				continue;
 			}
-			line = line->next;
+			ast_cli(a->fd, "==> %d. dev=%s icon=%#-4x label=%-10s number=%-5s sub=%p line=%p\n",
+				i, device->softkeydevice[i], device->softkeyicon[i], device->softkeylabel[i], device->softkeynumber[i],
+				device->ssub[i], device->sline[i]);
 		}
 		device = device->next;
 	}
+	ast_mutex_unlock(&devicelock);
 	ast_cli(a->fd, "\nSessions:\n");
 	ast_mutex_lock(&sessionlock);
 	s = sessions;
 	while (s) {
 		ast_cli(a->fd,
-				"sin=%s timeout=%u state=%d macaddr=%s device=%p session=%p\n",
-				ast_inet_ntoa(s->sin.sin_addr), s->timeout, s->state, s->macaddr,
-				s->device, s);
+				"sin=%s timeout=%u state=%s macaddr=%s device=%s session=%p\n",
+				ast_inet_ntoa(s->sin.sin_addr), s->timeout, ptestate_tostr(s->state), s->macaddr,
+				s->device->name, s);
 		s = s->next;
 	}
 	ast_mutex_unlock(&sessionlock);
@@ -4833,6 +5683,38 @@
 	return CLI_SUCCESS;
 }
 
+static char *unistim_show_devices(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
+{
+	struct unistim_device *device = devices;
+
+	switch (cmd) {
+	case CLI_INIT:
+		e->command = "unistim show devices";
+		e->usage =
+			"Usage: unistim show devices\n"
+			"       Lists all known Unistim devices.\n";
+		return NULL;
+	case CLI_GENERATE:
+		return NULL;	/* no completion */
+	}
+
+	if (a->argc != e->args)
+		return CLI_SHOWUSAGE;
+
+	ast_cli(a->fd, "%-20.20s %-20.20s %-15.15s %s\n", "Name/username", "MAC", "Host", "Status");
+	ast_mutex_lock(&devicelock);
+	while (device) {
+		ast_cli(a->fd, "%-20.20s %-20.20s %-15.15s %s\n",
+			device->name, device->id, 
+			(!device->session) ? "(Unspecified)" : ast_inet_ntoa(device->session->sin.sin_addr),
+			(!device->session) ? "UNKNOWN" : "OK");
+		device = device->next;
+	}
+	ast_mutex_unlock(&devicelock);
+
+	return CLI_SUCCESS;
+}
+
 static char *unistim_sp(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
 {
 	BUFFSEND;
@@ -4943,22 +5825,15 @@
 	if (e && a && a->argc != e->args)
 		return CLI_SHOWUSAGE;
 
-	if (unistimdebug)
-		ast_verb(0, "reload unistim\n");
-
-	ast_mutex_lock(&unistim_reload_lock);
-	if (!unistim_reloading)
-		unistim_reloading = 1;
-	ast_mutex_unlock(&unistim_reload_lock);
-
-	restart_monitor();
+	reload();
 
 	return CLI_SUCCESS;
 }
 
 static struct ast_cli_entry unistim_cli[] = {
 	AST_CLI_DEFINE(unistim_reload, "Reload UNISTIM configuration"),
-	AST_CLI_DEFINE(unistim_info, "Show UNISTIM info"),
+	AST_CLI_DEFINE(unistim_show_info, "Show UNISTIM info"),
+	AST_CLI_DEFINE(unistim_show_devices, "Show UNISTIM devices"),
 	AST_CLI_DEFINE(unistim_sp, "Send packet (for reverse engineering)"),
 	AST_CLI_DEFINE(unistim_do_debug, "Toggle UNITSTIM debugging"),
 };
@@ -4982,7 +5857,7 @@
 	return;
 }
 
-static int ParseBookmark(const char *text, struct unistim_device *d)
+static int parse_bookmark(const char *text, struct unistim_device *d)
 {
 	char line[256];
 	char *at;
@@ -5057,7 +5932,7 @@
 	ast_copy_string(d->softkeylabel[p], line, sizeof(d->softkeylabel[p]));
 	ast_copy_string(d->softkeynumber[p], number, sizeof(d->softkeynumber[p]));
 	if (unistimdebug)
-		ast_verb(0, "New bookmark at pos %d label='%s' number='%s' icon=%x\n",
+		ast_verb(0, "New bookmark at pos %d label='%s' number='%s' icon=%#x\n",
 					p, d->softkeylabel[p], d->softkeynumber[p], d->softkeyicon[p]);
 	return 1;
 }
@@ -5067,6 +5942,7 @@
 {
 	struct unistim_device *d = devices;
 	int i;
+	ast_mutex_lock(&devicelock);
 	while (d) {
 		for (i = 0; i < 6; i++) {
 			if (d->softkeyicon[i] == 1) {   /* Something for us */
@@ -5086,17 +5962,31 @@
 		}
 		d = d->next;
 	}
+	ast_mutex_unlock(&devicelock);
+}
+
+static struct unistim_line *find_line_by_number(struct unistim_device *d, const char *val) {
+	struct unistim_line *l, *ret = NULL;
+
+	AST_LIST_LOCK(&d->lines);
+	AST_LIST_TRAVERSE(&d->lines, l, list) {
+		if (!strcmp(l->name, val)) {
+			ret = l;
+			break;
+		}
+	}
+	AST_LIST_UNLOCK(&d->lines);
+	return ret;
 }
 
 static struct unistim_device *build_device(const char *cat, const struct ast_variable *v)
 {
 	struct unistim_device *d;
-	struct unistim_line *l = NULL;
+	struct unistim_line *l = NULL, *lt = NULL;
 	int create = 1;
-	int nbsoftkey, dateformat, timeformat, callhistory;
+	int nbsoftkey, dateformat, timeformat, callhistory, sharpdial, linecnt;
 	char linelabel[AST_MAX_EXTENSION];
-	char context[AST_MAX_EXTENSION];
-	char ringvolume, ringstyle;
+	char ringvolume, ringstyle, cwvolume, cwstyle;
 
 	/* First, we need to know if we already have this name in our list */
 	/* Get a lock for the device chained list */
@@ -5113,36 +6003,58 @@
 			}
 			/* we're reloading right now */
 			create = 0;
-			l = d->lines;
 			break;
 		}
 		d = d->next;
 	}
+	if (!(lt = ast_calloc(1, sizeof(*lt)))) {
+		return NULL;
+	}
 	ast_mutex_unlock(&devicelock);
 	if (create) {
 		if (!(d = ast_calloc(1, sizeof(*d))))
 			return NULL;
-
-		if (!(l = ast_calloc(1, sizeof(*l)))) {
-			ast_free(d);
-			return NULL;
-		}
+		ast_mutex_init(&d->lock);
 		ast_copy_string(d->name, cat, sizeof(d->name));
-	}
-	ast_copy_string(context, DEFAULTCONTEXT, sizeof(context));
+	} else {
+		/* Delete existing line information */
+		AST_LIST_LOCK(&d->lines);
+		AST_LIST_TRAVERSE_SAFE_BEGIN(&d->lines, l, list){
+			AST_LIST_REMOVE_CURRENT(list);
+			unistim_line_destroy(l);
+		}
+		AST_LIST_TRAVERSE_SAFE_END
+		AST_LIST_UNLOCK(&d->lines);
+	}
+
+	/* reset bookmarks */
+	memset(d->softkeylabel, 0, sizeof(d->softkeylabel));
+	memset(d->softkeynumber, 0, sizeof(d->softkeynumber));
+	memset(d->softkeyicon, 0, sizeof(d->softkeyicon));
+	memset(d->softkeydevice, 0, sizeof(d->softkeydevice));
+	memset(d->ssub, 0, sizeof(d->ssub));
+	memset(d->sline, 0, sizeof(d->sline));
+	memset(d->sp, 0, sizeof(d->sp));
+        
+	ast_copy_string(d->context, DEFAULTCONTEXT, sizeof(d->context));
 	d->contrast = -1;
 	d->output = OUTPUT_HANDSET;
 	d->previous_output = OUTPUT_HANDSET;
 	d->volume = VOLUME_LOW;
 	d->mute = MUTE_OFF;
 	d->height = DEFAULTHEIGHT;
+	d->selected = -1;
 	linelabel[0] = '\0';
 	dateformat = 1;
 	timeformat = 1;
 	ringvolume = 2;
+	cwvolume = 1;
 	callhistory = 1;
+	sharpdial = 0;
 	ringstyle = 3;
+	cwstyle = 2;
 	nbsoftkey = 0;
+	linecnt = 0;
 	while (v) {
 		if (!strcasecmp(v->name, "rtp_port"))
 			d->rtp_port = atoi(v->value);
@@ -5157,7 +6069,7 @@
 		else if (!strcasecmp(v->name, "permit") || !strcasecmp(v->name, "deny"))
 			d->ha = ast_append_ha(v->name, v->value, d->ha, NULL);
 		else if (!strcasecmp(v->name, "context"))
-			ast_copy_string(context, v->value, sizeof(context));
+			ast_copy_string(d->context, v->value, sizeof(d->context));
 		else if (!strcasecmp(v->name, "maintext0"))
 			unquote(d->maintext0, v->value, sizeof(d->maintext0) - 1);
 		else if (!strcasecmp(v->name, "maintext1"))
@@ -5182,19 +6094,25 @@
 			ringvolume = atoi(v->value);
 		else if (!strcasecmp(v->name, "ringstyle"))
 			ringstyle = atoi(v->value);
+		else if (!strcasecmp(v->name, "cwvolume"))
+			cwvolume = atoi(v->value);
+		else if (!strcasecmp(v->name, "cwstyle"))
+			cwstyle = atoi(v->value);
 		else if (!strcasecmp(v->name, "callhistory"))
 			callhistory = atoi(v->value);
-		else if (!strcasecmp(v->name, "callerid")) {
+		else if (!strcasecmp(v->name, "sharpdial")) {
+			sharpdial = ast_true(v->value) ? 1 : 0;
+		} else if (!strcasecmp(v->name, "callerid")) {
 			if (!strcasecmp(v->value, "asreceived"))
-				l->cid_num[0] = '\0';
+				lt->cid_num[0] = '\0';
 			else
-				ast_copy_string(l->cid_num, v->value, sizeof(l->cid_num));
+				ast_copy_string(lt->cid_num, v->value, sizeof(lt->cid_num));
 		} else if (!strcasecmp(v->name, "language"))
-			ast_copy_string(l->language, v->value, sizeof(l->language));
+			ast_copy_string(d->language, v->value, sizeof(d->language));
 		else if (!strcasecmp(v->name, "country"))
 			ast_copy_string(d->country, v->value, sizeof(d->country));
 		else if (!strcasecmp(v->name, "accountcode"))
-			ast_copy_string(l->accountcode, v->value, sizeof(l->accountcode));
+			ast_copy_string(lt->accountcode, v->value, sizeof(lt->accountcode));
 		else if (!strcasecmp(v->name, "amaflags")) {
 			int y;
 			y = ast_cdr_amaflags2int(v->value);
@@ -5202,17 +6120,17 @@
 				ast_log(LOG_WARNING, "Invalid AMA flags: %s at line %d\n", v->value,
 						v->lineno);
 			else
-				l->amaflags = y;
+				lt->amaflags = y;
 		} else if (!strcasecmp(v->name, "musiconhold"))
-			ast_copy_string(l->musicclass, v->value, sizeof(l->musicclass));
+			ast_copy_string(lt->musicclass, v->value, sizeof(lt->musicclass));
 		else if (!strcasecmp(v->name, "callgroup"))
-			l->callgroup = ast_get_group(v->value);
+			lt->callgroup = ast_get_group(v->value);
 		else if (!strcasecmp(v->name, "pickupgroup"))
-			l->pickupgroup = ast_get_group(v->value);
+			lt->pickupgroup = ast_get_group(v->value);
 		else if (!strcasecmp(v->name, "mailbox"))
-			ast_copy_string(l->mailbox, v->value, sizeof(l->mailbox));
+			ast_copy_string(lt->mailbox, v->value, sizeof(lt->mailbox));
 		else if (!strcasecmp(v->name, "parkinglot"))
-			ast_copy_string(l->parkinglot, v->value, sizeof(l->parkinglot));
+			ast_copy_string(lt->parkinglot, v->value, sizeof(lt->parkinglot));
 		else if (!strcasecmp(v->name, "linelabel"))
 			unquote(linelabel, v->value, sizeof(linelabel) - 1);
 		else if (!strcasecmp(v->name, "extension")) {
@@ -5229,103 +6147,93 @@
 				ast_log(LOG_WARNING,
 						"More than 6 softkeys defined. Ignoring new entries.\n");
 			else {
-				if (ParseBookmark(v->value, d))
+				if (parse_bookmark(v->value, d))
 					nbsoftkey++;
 			}
 		} else if (!strcasecmp(v->name, "line")) {
 			int len = strlen(linelabel);
+			int create_line = 0;
 
-			if (nbsoftkey) {
-				ast_log(LOG_WARNING,
-						"You must use bookmark AFTER line=>. Only one line is supported in this version\n");
-				if (create) {
+			l = find_line_by_number(d, v->value);
+			if (!l) { /* If line still not exists */
+				if (!(l = unistim_line_alloc())) {
 					ast_free(d);
-					ast_free(l);
+					ast_free(lt);
+					return NULL;
 				}
-				return NULL;
-			}
-			if (create) {
+				lt->capability = l->capability;
+				memcpy(l, lt, sizeof(*l));
 				ast_mutex_init(&l->lock);
-			} else {
-				d->to_delete = 0;
-				/* reset bookmarks */
-				memset(d->softkeylabel, 0, sizeof(d->softkeylabel));
-				memset(d->softkeynumber, 0, sizeof(d->softkeynumber));
-				memset(d->softkeyicon, 0, sizeof(d->softkeyicon));
-				memset(d->softkeydevice, 0, sizeof(d->softkeydevice));
-				memset(d->sp, 0, sizeof(d->sp));
-			}
-			ast_copy_string(l->name, v->value, sizeof(l->name));
-			snprintf(l->fullname, sizeof(l->fullname), "USTM/%s@%s", l->name, d->name);
-			d->softkeyicon[0] = FAV_ICON_ONHOOK_BLACK;
+				create_line = 1;
+			}
+			d->to_delete = 0;
+
+			/* Set softkey info for new line*/
+			d->sline[nbsoftkey] = l;
+			d->softkeyicon[nbsoftkey] = FAV_LINE_ICON;
 			if (!len)		       /* label is undefined ? */
-				ast_copy_string(d->softkeylabel[0], v->value, sizeof(d->softkeylabel[0]));
+				ast_copy_string(d->softkeylabel[nbsoftkey], v->value, sizeof(d->softkeylabel[nbsoftkey]));
 			else {
+				int softkeylinepos = 0;
 				if ((len > 2) && (linelabel[1] == '@')) {
-					d->softkeylinepos = linelabel[0];
-					if ((d->softkeylinepos >= '0') && (d->softkeylinepos <= '5')) {
-						d->softkeylinepos -= '0';
-						d->softkeyicon[0] = 0;
+					softkeylinepos = linelabel[0];
+					if ((softkeylinepos >= '0') && (softkeylinepos <= '5')) {
+						softkeylinepos -= '0';
+						d->softkeyicon[nbsoftkey] = FAV_ICON_NONE;
 					} else {
 						ast_log(LOG_WARNING,
 								"Invalid position for linelabel : must be between 0 and 5\n");
-						d->softkeylinepos = 0;
 					}
-					ast_copy_string(d->softkeylabel[d->softkeylinepos], linelabel + 2,
-									sizeof(d->softkeylabel[d->softkeylinepos]));
-					d->softkeyicon[d->softkeylinepos] = FAV_ICON_ONHOOK_BLACK;
+					ast_copy_string(d->softkeylabel[softkeylinepos], linelabel + 2,
+									sizeof(d->softkeylabel[softkeylinepos]));
+					d->softkeyicon[softkeylinepos] = FAV_LINE_ICON;
 				} else
-					ast_copy_string(d->softkeylabel[0], linelabel,
-									sizeof(d->softkeylabel[0]));
+					ast_copy_string(d->softkeylabel[nbsoftkey], linelabel,
+									sizeof(d->softkeylabel[nbsoftkey]));
 			}
 			nbsoftkey++;
-			ast_copy_string(l->context, context, sizeof(l->context));
-			if (!ast_strlen_zero(l->mailbox)) {
-				if (unistimdebug)
-					ast_verb(3, "Setting mailbox '%s' on %s@%s\n", l->mailbox, d->name, l->name);
-			}
-
-			l->capability = CAPABILITY;
-			l->parent = d;
-
-			if (create) {
-				if (!alloc_sub(l, SUB_REAL)) {
-					ast_mutex_destroy(&l->lock);
-					ast_free(l);
-					ast_free(d);
-					return NULL;
+                        
+			if (create_line) {
+				ast_copy_string(l->name, v->value, sizeof(l->name));
+				snprintf(l->fullname, sizeof(l->fullname), "USTM/%s@%s", l->name, d->name);
+				if (!ast_strlen_zero(l->mailbox)) {
+					if (unistimdebug)
+						ast_verb(3, "Setting mailbox '%s' on %s@%s\n", l->mailbox, d->name, l->name);
 				}
-				l->next = d->lines;
-				d->lines = l;
+				l->capability = CAPABILITY;
+				l->parent = d;
+				linecnt++;
+				AST_LIST_LOCK(&d->lines);
+				AST_LIST_INSERT_TAIL(&d->lines, l, list);
+				AST_LIST_UNLOCK(&d->lines);
 			}
 		} else if (!strcasecmp(v->name, "height")) {
 			/* Allow the user to lower the expected display lines on the phone
-			 * For example the Nortal I2001 and I2002 only have one ! */
+			 * For example the Nortel i2001 and i2002 only have one ! */
 			d->height = atoi(v->value);
 		} else
 			ast_log(LOG_WARNING, "Don't know keyword '%s' at line %d\n", v->name,
 					v->lineno);
 		v = v->next;
 	}
+	ast_free(lt);
+	if (linecnt == 0) {
+		ast_log(LOG_ERROR, "An Unistim device must have at least one line!\n");
+		ast_free(d);
+		return NULL;
+	}
 	d->ringvolume = ringvolume;
 	d->ringstyle = ringstyle;
+	d->cwvolume = cwvolume;
+	d->cwstyle = cwstyle;
 	d->callhistory = callhistory;
+	d->sharp_dial = sharpdial;
 	d->tz = ast_get_indication_zone(d->country);
 	if ((d->tz == NULL) && !ast_strlen_zero(d->country))
 		ast_log(LOG_WARNING, "Country '%s' was not found in indications.conf\n",
 				d->country);
 	d->datetimeformat = 56 + (dateformat * 4);
 	d->datetimeformat += timeformat;
-	if (!d->lines) {
-		ast_log(LOG_ERROR, "An Unistim device must have at least one line!\n");
-		ast_mutex_destroy(&l->lock);
-		ast_free(l);
-		if (d->tz) {
-			d->tz = ast_tone_zone_unref(d->tz);
-		}
-		ast_free(d);
-		return NULL;
-	}
 	if ((autoprovisioning == AUTOPROVISIONING_TN) &&
 		(!ast_strlen_zero(d->extension_number))) {
 		d->extension = EXTENSION_TN;
@@ -5338,8 +6246,6 @@
 	} else if (ast_strlen_zero(d->id)) {
 		if (strcmp(d->name, "template")) {
 			ast_log(LOG_ERROR, "You must specify the mac address with device=\n");
-			ast_mutex_destroy(&l->lock);
-			ast_free(l);
 			if (d->tz) {
 				d->tz = ast_tone_zone_unref(d->tz);
 			}
@@ -5352,8 +6258,6 @@
 		d->rtp_port = 10000;
 	if (d->contrast == -1)
 		d->contrast = 8;
-	if (ast_strlen_zero(d->maintext0))
-		strcpy(d->maintext0, "Welcome");
 	if (ast_strlen_zero(d->maintext1))
 		strcpy(d->maintext1, d->name);
 	if (ast_strlen_zero(d->titledefault)) {
@@ -5361,7 +6265,6 @@
 		struct timeval cur_time = ast_tvnow();
 
 		if ((ast_localtime(&cur_time, &tm, 0)) == 0 || ast_strlen_zero(tm.tm_zone)) {
-			display_last_error("Error in ast_localtime()");
 			ast_copy_string(d->titledefault, "UNISTIM for*", 12);
 		} else {
 			if (strlen(tm.tm_zone) < 4) {
@@ -5439,13 +6342,16 @@
                 } else if (!strcasecmp(v->name, "cos_audio")) {
                         if (ast_str2cos(v->value, &qos.cos_audio))
                             ast_log(LOG_WARNING, "Invalid cos_audio value at line %d, refer to QoS documentation\n", v->lineno);
+		} else if (!strcasecmp(v->name, "debug")) {
+			if (!strcasecmp(v->value, "no"))
+				unistimdebug = 0;
+			else if (!strcasecmp(v->value, "yes"))
+				unistimdebug = 1;
 		} else if (!strcasecmp(v->name, "autoprovisioning")) {
 			if (!strcasecmp(v->value, "no"))
 				autoprovisioning = AUTOPROVISIONING_NO;
 			else if (!strcasecmp(v->value, "yes"))
 				autoprovisioning = AUTOPROVISIONING_YES;
-			else if (!strcasecmp(v->value, "db"))
-				autoprovisioning = AUTOPROVISIONING_DB;
 			else if (!strcasecmp(v->value, "tn"))
 				autoprovisioning = AUTOPROVISIONING_TN;
 			else
@@ -5498,44 +6404,52 @@
 	d = devices;
 	while (d) {
 		if (d->to_delete) {
-			int i;
+			struct unistim_line *l;
+			struct unistim_subchannel *sub;
 
 			if (unistimdebug)
 				ast_verb(0, "Removing device '%s'\n", d->name);
-			if (!d->lines) {
-				ast_log(LOG_ERROR, "Device '%s' without a line !, aborting\n", d->name);
-				ast_config_destroy(cfg);
-				return 0;
-			}
-			if (!d->lines->subs[0]) {
-				ast_log(LOG_ERROR, "Device '%s' without a subchannel !, aborting\n",
-						d->name);
-				ast_config_destroy(cfg);
-				return 0;
-			}
-			if (d->lines->subs[0]->owner) {
-				ast_log(LOG_WARNING,
-						"Device '%s' was not deleted : a call is in progress. Try again later.\n",
-						d->name);
-				d = d->next;
-				continue;
-			}
-			ast_mutex_destroy(&d->lines->subs[0]->lock);
-			ast_free(d->lines->subs[0]);
-			for (i = 1; i < MAX_SUBS; i++) {
-				if (d->lines->subs[i]) {
+
+			AST_LIST_LOCK(&d->subs);
+			AST_LIST_TRAVERSE_SAFE_BEGIN(&d->subs, sub, list){
+				if (sub->subtype == SUB_REAL) {
+					if (!sub) {
+						ast_log(LOG_ERROR, "Device '%s' without a subchannel !, aborting\n",
+								d->name);
+						ast_config_destroy(cfg);
+						return 0;
+					}
+					if (sub->owner) {
+						ast_log(LOG_WARNING,
+								"Device '%s' was not deleted : a call is in progress. Try again later.\n",
+								d->name);
+						d = d->next;
+						continue;
+					}
+				}
+				if (sub->subtype == SUB_THREEWAY) {
 					ast_log(LOG_WARNING,
 							"Device '%s' with threeway call subchannels allocated, aborting.\n",
 							d->name);
 					break;
 				}
+				AST_LIST_REMOVE_CURRENT(list);
+				ast_mutex_destroy(&sub->lock);
+				ast_free(sub);
+			}
+			AST_LIST_TRAVERSE_SAFE_END
+			AST_LIST_UNLOCK(&d->subs);
+
+
+			AST_LIST_LOCK(&d->lines);
+			AST_LIST_TRAVERSE_SAFE_BEGIN(&d->lines, l, list){
+				AST_LIST_REMOVE_CURRENT(list);
+				ast_mutex_destroy(&l->lock);
+				unistim_line_destroy(l);
 			}
-			if (i < MAX_SUBS) {
-				d = d->next;
-				continue;
-			}
-			ast_mutex_destroy(&d->lines->lock);
-			ast_free(d->lines);
+			AST_LIST_TRAVERSE_SAFE_END
+			AST_LIST_UNLOCK(&d->lines);
+
 			if (d->session) {
 				if (sessions == d->session)
 					sessions = d->session->next;
@@ -5567,6 +6481,7 @@
 			if (d->tz) {
 				d->tz = ast_tone_zone_unref(d->tz);
 			}
+			ast_mutex_destroy(&d->lock);
 			ast_free(d);
 			d = devices;
 			continue;
@@ -5579,8 +6494,14 @@
 	ast_mutex_lock(&sessionlock);
 	s = sessions;
 	while (s) {
-		if (s->device)
+		if (s->device) {
 			refresh_all_favorite(s);
+			if (ast_strlen_zero(s->device->language)) {
+				struct unistim_languages lang;
+				lang = options_languages[find_language(s->device->language)];
+				send_charset_update(s, lang.encoding);
+			}
+		}
 		s = s->next;
 	}
 	ast_mutex_unlock(&sessionlock);
@@ -5694,8 +6615,9 @@
 static int unload_module(void)
 {
 	/* First, take us out of the channel loop */
-	if (sched)
+	if (sched) {
 		sched_context_destroy(sched);
+	}
 
 	ast_cli_unregister_multiple(unistim_cli, ARRAY_LEN(unistim_cli));
 
@@ -5722,7 +6644,16 @@
 /*! reload: Part of Asterisk module interface ---*/
 int reload(void)
 {
-	unistim_reload(NULL, 0, NULL);
+	if (unistimdebug)
+		ast_verb(0, "reload unistim\n");
+
+	ast_mutex_lock(&unistim_reload_lock);
+	if (!unistim_reloading)
+		unistim_reloading = 1;
+	ast_mutex_unlock(&unistim_reload_lock);
+
+	restart_monitor();
+	
 	return 0;
 }
 
