$OpenBSD: patch-channels_chan_unistim_c,v 1.6 2011/12/16 14:50:09 sthen Exp $

Patch is from https://issues.asterisk.org/jira/browse/18229
https://issues.asterisk.org/jira/secure/attachment/38296/chan_unistim.c.1.8.2.2-9.diff

--- channels/chan_unistim.c.orig	Fri Sep  9 17:09:09 2011
+++ channels/chan_unistim.c	Sat Dec 10 14:34:33 2011
@@ -73,6 +73,8 @@ ASTERISK_FILE_VERSION(__FILE__, "$Revision: 1.6 $")
 #include "asterisk/musiconhold.h"
 #include "asterisk/causes.h"
 #include "asterisk/indications.h"
+#include "asterisk/features.h"
+#include "asterisk/astobj2.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 */
@@ -82,11 +84,14 @@ ASTERISK_FILE_VERSION(__FILE__, "$Revision: 1.6 $")
 #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 */
@@ -103,8 +108,9 @@ ASTERISK_FILE_VERSION(__FILE__, "$Revision: 1.6 $")
 #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,
@@ -185,6 +191,14 @@ enum autoprov_extn {
 
 #define FAV_MAX_LENGTH		  0x0A
 
+#define FAVNUM                    6
+#define FAV_LINE_ICON         FAV_ICON_ONHOOK_BLACK
+
+#define CALL_TIMER_START            0x01
+#define CALL_TIMER_RESET            0x02
+#define CALL_TIMER_DISPLAY          0x04
+#define CALL_TIMER_DELAY            0x08
+
 static void dummy(char *unused, ...)
 {
 	return;
@@ -222,7 +236,7 @@ static struct {
 } qos = { 0, 0, 0, 0 };
 
 static struct io_context *io;
-static struct sched_context *sched;
+static struct ast_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;
@@ -256,7 +270,10 @@ enum phone_state {
 	STATE_DIALPAGE,
 	STATE_RINGING,
 	STATE_CALL,
+        STATE_SELECTOPTION,
 	STATE_SELECTCODEC,
+        STATE_SELECTCONTRAST,
+        STATE_SELECTLANGUAGE,
 	STATE_CLEANING,
 	STATE_HISTORY
 };
@@ -307,45 +324,17 @@ enum phone_key {
 	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;
@@ -365,7 +354,7 @@ struct systemtime {
 
 struct unistim_subchannel {
 	ast_mutex_t lock;
-	/*! SUBS_REAL or SUBS_THREEWAY */
+	/*! SUB_REAL, SUB_THREEWAY or SUB_ONHOLD */
 	unsigned int subtype;
 	/*! Asterisk channel used by the subchannel */
 	struct ast_channel *owner;
@@ -373,9 +362,14 @@ struct unistim_subchannel {
 	struct unistim_line *parent;
 	/*! RTP handle */
 	struct ast_rtp_instance *rtp;
+        /*! Softkey assigned */
+        int softkey;
+	time_t start_call_timestamp;    /*!< timestamp for the length calculation of the call */
 	int alreadygone;
 	char ringvolume;
 	char ringstyle;
+	int moh;					/*!< Music on hold in progress */
+        AST_LIST_ENTRY(unistim_subchannel) list;
 };
 
 /*!
@@ -387,14 +381,10 @@ struct unistim_line {
 	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 */
@@ -419,6 +409,7 @@ struct unistim_line {
 	char parkinglot[AST_MAX_CONTEXT];
 	struct unistim_line *next;
 	struct unistim_device *parent;
+        AST_LIST_ENTRY(unistim_line) list;
 };
 
 /*! 
@@ -427,19 +418,21 @@ struct unistim_line {
 static struct unistim_device {
 	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 */
+	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 */
 	int phone_current;		      /*!< Number of the current phone */
 	int pos_fav;			    /*!< Position of the displayed favorites (used for scrolling) */
 	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 */
-	int height;							/*!< The number of lines the phone can display */
+	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 */
 	char maintext2[25];		     /*!< when the phone is idle, display this string on line 2 */
@@ -450,12 +443,16 @@ static struct unistim_device {
 	struct ast_tone_zone *tz;	       /*!< Tone zone for res_indications (ring, busy, congestion) */
 	char ringvolume;			/*!< Ring volume */
 	char ringstyle;			 /*!< Ring melody */
-	int rtp_port;			   /*!< RTP port used by the phone */
+	char cwvolume;			/*!< Ring volume on call waiting */
+	char cwstyle;			 /*!< Ring melody on call waiting */
+        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 selected;                           /*!< softkey selected */
+        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 */
@@ -463,15 +460,14 @@ static struct unistim_device {
 	int previous_output;	    /*!< Previous output */
 	int volume;				     /*!< Default volume */
 	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;
-	struct ast_ha *ha;
+	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;
 } *devices = NULL;
@@ -494,9 +490,23 @@ static struct unistimsession {
 	struct wsabuf wsabufsend[MAX_BUF_NUMBER];      /*!< Size of each paquet stored in the buffer array & pointer to this buffer */
 	unsigned char buf[MAX_BUF_NUMBER][MAX_BUF_SIZE];	/*!< Buffer array used to keep the lastest non-acked paquets */
 	struct unistim_device *device;
-	struct unistimsession *next;
+        struct unistim_subchannel *active_sub;                  /*!< Active subchannel for detect active softkey */
+        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
  *
@@ -628,10 +638,14 @@ static const unsigned char packet_send_date_time2[] = 
 };
 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_stop_timer[] = { 0x17, 0x05, 0x0b, 0x02, 0x00 };
+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_delay_timer[] = { 0x17, 0x04, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+static const unsigned char packet_send_hide_timer[] = { 0x17, 0x05, 0x0b, CALL_TIMER_RESET, 0x00 };
+static const unsigned char packet_send_show_timer[] = { 0x17, 0x05, 0x0b, 0x05 & CALL_TIMER_DELAY, 0x00 };
+static const unsigned char packet_send_stop_timer[] = { 0x17, 0x05, 0x0b, CALL_TIMER_RESET, 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 };
 static const unsigned char packet_send_set_pos_cursor[] =
@@ -662,6 +676,23 @@ static const unsigned char packet_send_status2[] =
 	{ 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 };
@@ -722,15 +753,125 @@ static const struct ast_channel_tech unistim_tech = {
 	.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 *sub);
+static void key_favorite(struct unistimsession *, char);
+
+static void handle_select_codec(struct unistimsession *);
+static void handle_select_contrast(struct unistimsession *);
+static void handle_select_language(struct unistimsession *);
+static int find_language(const char*);
+
+struct unistim_menu_item options_menu[] =
 {
-	time_t cur_time;
-	
-	time(&cur_time);
+    {"Change codec", STATE_SELECTCODEC, handle_select_codec},
+    {"Contrast", STATE_SELECTCONTRAST, handle_select_contrast},
+    {"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 void lang_entry_destructor(void *obj)
+{
+        return;
+}
+
+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;
+
+        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 (tmp == strstr(tmp, "msgid")) {
+                             p_orig = ast_strdup(strchr(tmp, '"') + 1);
+                             p = strchr(p_orig, '"');
+                             *p = '\0';
+                     } else if (tmp == strstr(tmp, "msgstr")) {
+                             p_trans = ast_strdup(strchr(tmp, '"') + 1);
+                             p = strchr(p_trans, '"');
+                             *p = '\0';
+                     } else {
+                            continue;
+                     }
+                     if (!p_trans || !p_orig)
+                            continue;
+                     if (!strlen(p_trans)) {
+                            p_trans = NULL;
+                            p_orig = NULL;
+                            continue;
+                     }
+                     if (!(lang_entry = ao2_alloc(sizeof(*lang_entry), lang_entry_destructor))) {
+                            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 = ast_strdup(str);
+        lang_entry = ao2_find(lang->trans, &le_search, OBJ_POINTER);
+        //lang_entry = NULL;
+        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)
@@ -955,10 +1096,44 @@ static void send_start_timer(struct unistimsession *pt
 	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_start_timer_id(struct unistimsession *pte, const int id)
+{
+	BUFFSEND;
+	if (unistimdebug)
+		ast_verb(0, "Sending start timer\n");
+	memcpy(buffsend + SIZE_HEADER, packet_send_start_timer, sizeof(packet_send_start_timer));
+	//buffsend[10] = id;
+        send_client(SIZE_HEADER + sizeof(packet_send_start_timer), buffsend, pte);
+}
+
+static void send_hide_timer_id(struct unistimsession *pte, const int id)
+{
+	BUFFSEND;
+	if (unistimdebug)
+		ast_verb(0, "Sending start timer\n");
+	memcpy(buffsend + SIZE_HEADER, packet_send_hide_timer, sizeof(packet_send_hide_timer));
+        //buffsend[10] = id;
+	send_client(SIZE_HEADER + sizeof(packet_send_hide_timer), buffsend, pte);
+}
+
+static void send_show_timer_id(struct unistimsession *pte, const int id)
+{
+	BUFFSEND;
+	if (unistimdebug)
+		ast_verb(0, "Sending start timer\n");
+	memcpy(buffsend + SIZE_HEADER, packet_send_delay_timer, sizeof(packet_send_delay_timer));
+        //buffsend[10] = id;
+	send_client(SIZE_HEADER + sizeof(packet_send_delay_timer), buffsend, pte);
+
+        memset(buffsend + SIZE_HEADER, 0, sizeof(packet_send_delay_timer));
+	memcpy(buffsend + SIZE_HEADER, packet_send_show_timer, sizeof(packet_send_show_timer));
+	send_client(SIZE_HEADER + sizeof(packet_send_show_timer), buffsend, pte);
+}
+
 static void send_stop_timer(struct unistimsession *pte)
 {
 	BUFFSEND;
@@ -968,7 +1143,7 @@ static void send_stop_timer(struct unistimsession *pte
 	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)
@@ -1026,9 +1201,9 @@ static void send_tone(struct unistimsession *pte, uint
 
 /* 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 */
@@ -1045,44 +1220,122 @@ send_favorite(unsigned char pos, unsigned char status,
 	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, int num)
+{
+        if(d->height == 1 && num%3 == 2) {
+            return 0;
+        }
+        return 1;
+}
+
 static void refresh_all_favorite(struct unistimsession *pte)
 {
 	int i = 0;
 
 	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]);
+	for (i = 0; i < FAVNUM; i++) {
+                if (!soft_key_visible(pte->device, i))
+                        continue;
+                /* XXX Here icon for the lines displayed */
+		if ((pte->device->softkeyicon[i] <= FAV_ICON_HEADPHONES_ONHOLD)/* &&
+			(pte->device->softkeylinepos != i)*/)
+			send_favorite_short((unsigned char) i, pte->device->softkeyicon[i]/* + 1*/, pte);
 		else
-			send_favorite((unsigned char) i, pte->device->softkeyicon[i], pte,
-						 pte->device->softkeylabel[i]);
+			send_favorite_short((unsigned char) i, pte->device->softkeyicon[i], 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;
@@ -1095,30 +1348,40 @@ static void change_favorite_icon(struct unistimsession
 	}
 }
 
-static int RegisterExtension(const struct unistimsession *pte)
+/* XXX Check all places where get_main_line used, as it is a hack and may be better usage possible */
+static struct unistim_line* get_main_line(const struct unistim_device *d)
 {
+    return AST_LIST_FIRST(&d->lines);
+}
+
+static int register_extension(const struct unistimsession *pte)
+{
+        struct unistim_line *line;
+        line = get_main_line(pte->device);
 	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, line->context,
+					line->fullname);
+	return ast_add_extension(line->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)
 {
+        struct unistim_line *line;
+        line = get_main_line(pte->device);
 	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, line->context);
+	return ast_context_remove_extension(line->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;
@@ -1132,12 +1395,11 @@ static void close_client(struct unistimsession *s)
 	if (cur) {				      /* Session found ? */
 		if (cur->device) {	      /* This session was registered ? */
 			s->state = STATE_CLEANING;
-			if (unistimdebug)
+                        /* !!! sub seems to be never used and uninitialized */
+                        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]);
+							s, s->device, get_main_line(s->device), sub);
 			change_favorite_icon(s, FAV_ICON_NONE);
-			sub = s->device->lines->subs[SUB_REAL];
 			if (sub) {
 				if (sub->owner) {       /* Call in progress ? */
 					if (unistimdebug)
@@ -1147,7 +1409,7 @@ static void close_client(struct unistimsession *s)
 			} else
 				ast_log(LOG_WARNING, "Freeing a client with no subchannel !\n");
 			if (!ast_strlen_zero(s->device->extension_number))
-				UnregisterExtension(s);
+				unregister_extension(s);
 			cur->device->session = NULL;
 		} else {
 			if (unistimdebug)
@@ -1211,6 +1473,12 @@ send_text(unsigned char pos, unsigned char inverse, st
 {
 	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->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));
@@ -1370,12 +1638,21 @@ static void send_texttitle(struct unistimsession *pte,
 
 }
 
+static void send_idle_clock(struct unistimsession *pte)
+{
+	char text[2];
+
+        memset(text, 0x00, 2);
+        send_text(TEXT_LINE0, TEXT_NORMAL, pte, text);
+}
+
 static void send_date_time(struct unistimsession *pte)
 {
 	BUFFSEND;
 	struct timeval now = ast_tvnow();
 	struct ast_tm atm = { 0, };
 
+        ast_log(LOG_WARNING, "Time Received\n");
 	if (unistimdebug)
 		ast_verb(0, "Sending Time & Date\n");
 	memcpy(buffsend + SIZE_HEADER, packet_send_date_time, sizeof(packet_send_date_time));
@@ -1448,6 +1725,43 @@ static void send_cursor_pos(struct unistimsession *pte
 	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;
@@ -1488,37 +1802,91 @@ static int unistim_register(struct unistimsession *s)
 	return 1;
 }
 
-static int alloc_sub(struct unistim_line *l, int x)
+static struct unistim_subchannel *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->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 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;
-	}
-	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;
-	return 0;
+        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);
+                if (unistimdebug)
+                	ast_debug(1, "Released sub %d of channel %s@%s\n", sub->subtype, sub->parent->name, d->name);
+
+                ast_free(sub);
+        }
+        AST_LIST_TRAVERSE_SAFE_END;
+        AST_LIST_UNLOCK(&d->subs);
+	//ast_mutex_destroy(&p->lock); TODO
+        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_SELECTCONTRAST:
+                        return "SELECTCONTRAST";
+                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;
@@ -1552,6 +1920,7 @@ static void rcv_mac_addr(struct unistimsession *pte, c
 				d = devices;
 				while (d) {
 					if (!strcasecmp(d->name, "template")) {
+                                                struct unistim_line *l;
 						/* Found, cloning this entry */
 						if (!(newd = ast_malloc(sizeof(*newd)))) {
 							ast_mutex_unlock(&devicelock);
@@ -1565,8 +1934,9 @@ static void rcv_mac_addr(struct unistimsession *pte, c
 							return;
 						}
 
-						memcpy(newl, d->lines, sizeof(*newl));
-						if (!alloc_sub(newl, SUB_REAL)) {
+                                                l = get_main_line(d);
+						memcpy(newl, l, sizeof(*newl));
+						if (!alloc_sub(newd, SUB_REAL)) {
 							ast_free(newd);
 							ast_free(newl);
 							ast_mutex_unlock(&devicelock);
@@ -1577,16 +1947,16 @@ static void rcv_mac_addr(struct unistimsession *pte, c
 						ast_copy_string(newd->name, addrmac, sizeof(newd->name));
 						if (newd->extension == EXTENSION_NONE)
 							newd->extension = EXTENSION_ASK;
-						newd->lines = newl;
+						//newd->lines = newl; TODO
 						newd->receiver_state = STATE_ONHOOK;
 						newd->session = pte;
 						newd->to_delete = -1;
 						pte->device = newd;
 						newd->next = NULL;
 						newl->parent = newd;
-						strcpy(newl->name, d->lines->name);
-						snprintf(d->lines->name, sizeof(d->lines->name), "%d",
-								 atoi(d->lines->name) + 1);
+						strcpy(newl->name, l->name);
+						snprintf(l->name, sizeof(l->name), "%d",
+								 atoi(l->name) + 1);
 						snprintf(newl->fullname, sizeof(newl->fullname), "USTM/%s@%s",
 								 newl->name, newd->name);
 						/* Go to the end of the linked chain */
@@ -1630,16 +2000,16 @@ static void rcv_mac_addr(struct unistimsession *pte, c
 				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,
+			ast_copy_string(pte->device->extension_number, get_main_line(pte->device)->name,
 							sizeof(pte->device->extension_number));
-			if (RegisterExtension(pte))
+			if (register_extension(pte))
 				pte->state = STATE_EXTENSION;
 			else
 				pte->state = STATE_MAINPAGE;
@@ -1724,20 +2094,18 @@ static int write_history(struct unistimsession *pte, c
 
 	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");
+			strcpy(tmp2, ustmtext("Miss", pte));
 		else
-			strcpy(tmp2, "Fail");
+			strcpy(tmp2, ustmtext("Fail", pte));
 	} else
-		strcpy(tmp2, "Answ");
+		strcpy(tmp2, ustmtext("Answ", pte));
 	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);
@@ -1855,41 +2223,14 @@ static int write_history(struct unistimsession *pte, c
 	return 0;
 }
 
-static void cancel_dial(struct unistimsession *pte)
+static void unistim_quiet_chan(struct ast_channel *chan)
 {
-	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)
-{
-/*  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)
@@ -1919,25 +2260,13 @@ static int attempt_transfer(struct unistim_subchannel 
 	}
 
 	if (peera && peerb && peerc && (peerb != peerc)) {
-		/*ast_quiet_chan(peera);
-		   ast_quiet_chan(peerb);
-		   ast_quiet_chan(peerc);
-		   ast_quiet_chan(peerd); */
+		unistim_quiet_chan(peera);
+		unistim_quiet_chan(peerb);
+		unistim_quiet_chan(peerc);
+                /* if (peerd)
+                        unistim_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;
-
+                ast_debug(4, "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);
@@ -1975,32 +2304,122 @@ void change_callerid(struct unistimsession *pte, int t
 	memcpy(data, callerid, size);
 }
 
+static struct unistim_subchannel* get_sub(struct unistim_device *device, int type) {
+        struct unistim_subchannel *sub = NULL;
+
+        AST_LIST_LOCK(&device->subs);
+        AST_LIST_TRAVERSE(&device->subs, sub, list) {
+                if (!sub)
+                        continue;
+                if (sub->subtype == type) {
+                        break;
+                }
+        }
+        AST_LIST_UNLOCK(&device->subs);
+
+        return sub;
+}
+
+static void sub_start_silence(struct unistimsession *pte, struct unistim_subchannel *sub) {
+	/* 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_hide_timer_id(pte, sub->softkey);
+        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_show_timer_id(pte, sub->softkey);
+        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;
-	struct unistim_line *l = pte->device->lines;
+	struct unistim_subchannel *sub, *sub_transf;
 
-	sub = pte->device->lines->subs[SUB_REAL];
+	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 (l->subs[SUB_THREEWAY]) {
-			l->subs[SUB_THREEWAY]->alreadygone = 1;
-			if (attempt_transfer(sub, l->subs[SUB_THREEWAY]) < 0)
+		if (sub_transf) {
+			sub_transf->alreadygone = 1;
+                        ast_moh_stop(ast_bridged_channel(sub_transf->owner));
+                        sub_transf->moh = 0;
+			if (attempt_transfer(sub, sub_transf) < 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);
+		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
+		} else {
 			ast_verb(0, "USTM(%s@%s-%d) channel already destroyed\n", sub->parent->name,
-						sub->parent->parent->name, sub->subtype);
-	}
-	change_callerid(pte, 0, pte->device->redial_number);
+						pte->device->name, sub->softkey);
+                    /* XXX Crash because of outputing strings in log? probably sub->parent->name going wrong
+                     * when buttons very fast pressed
+                     */
+                }
+	}        
+        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;
@@ -2008,9 +2427,10 @@ static void close_call(struct unistimsession *pte)
 	return;
 }
 
-static void IgnoreCall(struct unistimsession *pte)
+static void ignore_call(struct unistimsession *pte)
 {
 	send_no_ring(pte);
+        /* XXX Add option to hangup incoming call or forward it to voicemail/exten */
 	return;
 }
 
@@ -2022,7 +2442,7 @@ static void *unistim_ss(void *data)
 	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);
+	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));
@@ -2030,85 +2450,50 @@ static void *unistim_ss(void *data)
 	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) {
+        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;
-	struct sockaddr_in us = { 0, };
+
+	format_t 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  */
@@ -2121,35 +2506,35 @@ static void start_rtp(struct unistim_subchannel *sub)
 		ast_verb(0, "Starting phone RTP stack. Our public IP is %s\n",
 					ast_inet_ntoa(public.sin_addr));
 	}
-	if ((sub->owner->readformat == AST_FORMAT_ULAW) ||
+
+        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));
 		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);
@@ -2168,18 +2553,17 @@ static void start_rtp(struct unistim_subchannel *sub)
 			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);
@@ -2198,8 +2582,7 @@ static void start_rtp(struct unistim_subchannel *sub)
 			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 */
 
@@ -2233,73 +2616,152 @@ static void start_rtp(struct unistim_subchannel *sub)
 		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);
-		}
-		i++;
+	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;
+
+                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, 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);
+                memcpy(tmp, tmp_number + offset, strlen(tmp_number) - offset + 1);
+        } else {
+                strcpy(tmp, tmp_number);
+        }
+
+        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;
 		}
@@ -2309,191 +2771,268 @@ static void handle_dial_page(struct unistimsession *pt
 			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;
-	change_favorite_icon(pte, FAV_ICON_PHONE_BLACK);
-	Sendicon(TEXT_LINE0, FAV_ICON_NONE, pte);
+        show_phone_number(pte);
+        change_favorite_icon(pte, FAV_ICON_PHONE_BLACK);
+	send_icon(TEXT_LINE0, FAV_ICON_NONE, pte);
 	pte->device->missed_call = 0;
 	send_led_update(pte, 0);
 	return;
 }
 
+static void swap_subs(struct unistim_subchannel *a, struct unistim_subchannel *b)
+{
+/*  struct ast_channel *towner; */
+	struct ast_rtp_instance *rtp;
+	int fds;
+
+	if (unistimdebug)
+		ast_verb(0, "Swapping %p and %p\n", a, b);
+
+	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;
+	}
+	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 TransferCallStep1(struct unistimsession *pte)
+static void transfer_call_step1(struct unistimsession *pte)
 {
-	struct unistim_subchannel *sub;
-	struct unistim_line *p = pte->device->lines;
+	struct unistim_subchannel *sub, *sub_trans;
+	struct unistim_device *d = pte->device;
 
-	sub = p->subs[SUB_REAL];
+	sub = get_sub(d, SUB_REAL);
+	sub_trans = get_sub(d, SUB_THREEWAY);
 
-	if (!sub->owner) {
+	if (!sub || !sub->owner) {
 		ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
 		return;
 	}
-	if (p->subs[SUB_THREEWAY]) {
+	if (sub_trans) {
 		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
+		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;
+			ast_queue_hangup_with_cause(sub->owner, AST_CAUSE_NORMAL_CLEARING);
+                } else
 			ast_log(LOG_WARNING, "Canceling a threeway channel without owner\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);
 }
 
 /* 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;
+        int softkey;
 	pthread_t t;
+
 	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);
-		return;
-	}
+        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 = alloc_sub(d, SUB_REAL);
+                sub_trans->parent = sub->parent;
+                if (!sub_trans) {
+                        ast_log(LOG_WARNING, "Unable to allocate three-way subchannel\n");
+                        return;
+                }
+                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(&t, NULL, unistim_ss, c)) {
+                        ast_log(LOG_WARNING, "Unable to start simple switch on channel %p\n", c);
+                        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 = 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("Hangup Transf", s));
+                        send_start_timer_id(s, sub->softkey);
+                        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");
+				ast_log(LOG_WARNING, "Unable to create switch thread\n");
 				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;
-	} else if (unistimdebug)
+	}
+        /* 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;
+        }
+        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);
@@ -2501,16 +3040,16 @@ static void HandleCallIncoming(struct unistimsession *
 		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");
-	send_start_timer(s);
+	send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("is on-line", s));
+	send_text_status(s, ustmtext("Hangup Transf", s));
+	send_start_timer_id(s, sub->softkey);
 
 	if ((s->device->output == OUTPUT_HANDSET) &&
 		(s->device->receiver_state == STATE_ONHOOK))
 		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);
+	sub->start_call_timestamp = time(0);
 	write_history(s, 'i', 0);
 	return;
 }
@@ -2519,8 +3058,10 @@ static int unistim_do_senddigit(struct unistimsession 
 {
 	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;
 	}
@@ -2530,65 +3071,80 @@ static int unistim_do_senddigit(struct unistimsession 
 
 	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:
-		send_tone(pte, 500, 2000);
-	}
+
+        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);
+                        //send_start_timer_id(pte, sub->softkey);
+                        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 = '#';
@@ -2605,9 +3161,17 @@ static void key_call(struct unistimsession *pte, char 
 		close_call(pte);
 		break;
 	case KEY_FUNC2:
-		TransferCallStep1(pte);
+		transfer_call_step1(pte);
 		break;
-	case KEY_HEADPHN:
+        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)
 			send_select_output(pte, OUTPUT_HANDSET, pte->device->volume, MUTE_OFF);
 		else
@@ -2621,82 +3185,71 @@ static void key_call(struct unistimsession *pte, char 
 							 MUTE_OFF);
 		break;
 	case KEY_MUTE:
-		if (!pte->device->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;
-		}
+                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;
-			}
-			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;
-		}
+                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]);
+                        }
+                } 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_HANGUP:
 	case KEY_FUNC4:
-		IgnoreCall(pte);
+		ignore_call(pte);
 		break;
 	case KEY_FUNC1:
-		HandleCallIncoming(pte);
+		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)) {
-		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;
+        int fav = keycode - KEY_FAV0;
+        if (!is_key_favorite(pte->device, fav)) {
+                ast_log(LOG_WARNING, "It's not a favorite key\n");
+                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_REAL);
+
 	if (keycode == KEY_FUNC3) {
 		if (pte->device->size_phone_number <= 1)
 			keycode = KEY_FUNC4;
@@ -2705,50 +3258,32 @@ static void key_dial_page(struct unistimsession *pte, 
 			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)
-			keycode = '#';
+
+		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);
-		} else {
-			send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmpbuf);
-		}
-		send_blink_cursor(pte);
-		send_cursor_pos(pte, (unsigned char) (TEXT_LINE2 + 0x0a + i));
+                
+		show_phone_number(pte);
 		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);
-		}
-		return;
+                show_phone_number(pte);
+                return;
 	}
 
 	if (pte->device->call_forward[0] == -1) {
@@ -2758,7 +3293,9 @@ static void key_dial_page(struct unistimsession *pte, 
 			show_main_page(pte);
 		} else if ((keycode == KEY_FUNC2) || (keycode == KEY_HANGUP)) {
 			pte->device->call_forward[0] = '\0';
-			show_main_page(pte);
+			send_led_update(pte, 0x08);
+                        send_led_update(pte, 0x10);
+                        show_main_page(pte);
 		}
 		return;
 	}
@@ -2769,40 +3306,43 @@ static void key_dial_page(struct unistimsession *pte, 
 		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;
-			}
-			send_tone(pte, 0, 0);
-			ast_moh_stop(ast_bridged_channel(pte->device->lines->subs[SUB_REAL]->owner));
-			pte->device->moh = 0;
+		if (sub && sub->owner) {
+                        struct ast_channel *bridgepeer = NULL;
+                        
+                        sub_stop_silence(pte, sub);
+                        send_tone(pte, 0, 0);
+                        if ((bridgepeer = ast_bridged_channel(sub->owner))) {
+                                ast_moh_stop(bridgepeer);
+                        }
+			get_sub(pte->device, SUB_REAL)->moh = 0;
 			pte->state = STATE_CALL;
 
 			if (pte->device->height == 1) {
-				send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Dial Cancel,back to priv. call.");
+				send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Dial Cancel,back to priv. call.", pte));
 			} 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(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Dialing canceled,", pte));
+				send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("switching back to", pte));
+				send_text(TEXT_LINE2, TEXT_NORMAL, pte, ustmtext("previous call.", pte));
 			}
-			send_text_status(pte, "Hangup Transf");
-		} else
+			send_text_status(pte, ustmtext("Hangup Transf", pte));
+		} else {                        
+			send_led_update(pte, 0x08);
+                        send_led_update(pte, 0x10);
 			show_main_page(pte);
+                }
 		break;
-	case KEY_FAV1:
+	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) {
@@ -2827,16 +3367,56 @@ static void key_dial_page(struct unistimsession *pte, 
 	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 */
+        }
+        sprintf(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_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);
+	strcpy(buf, ustmtext("Using codec", pte));
+	sprintf(buf2, " %d", pte->device->codec_number);
 	strcat(buf, buf2);
 	strcat(buf, " (G711u=0,");
 
@@ -2846,7 +3426,7 @@ static void HandleSelectCodec(struct unistimsession *p
 	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;
 }
 
@@ -2904,19 +3484,110 @@ static void key_select_codec(struct unistimsession *pt
 	return;
 }
 
+static void handle_select_contrast(struct unistimsession *pte)
+{
+        return;
+}
+
+static void key_select_contrast(struct unistimsession *pte, char keycode)
+{
+	switch (keycode) {
+        case KEY_DOWN:
+                pte->buff_entry[0]++;
+                break;
+        case KEY_UP:
+                if (pte->buff_entry[0] > 0)
+                        pte->buff_entry[0]--;
+                break;
+	case KEY_FUNC1:
+                show_main_page(pte);
+		return;
+	case KEY_HANGUP:
+	case KEY_FUNC4:
+		handle_select_option(pte);
+		return;
+        }
+
+        handle_select_contrast(pte);
+	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]];
+        strcpy(tmp_language, pte->device->language);
+        strcpy(pte->device->language, lang.lang_short);
+        send_charset_update(pte, lang.encoding);
+	send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext(lang.label, pte));
+
+	strcpy(pte->device->language, tmp_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;
 }
@@ -2988,8 +3659,8 @@ static void key_select_extension(struct unistimsession
 				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));
@@ -2997,9 +3668,9 @@ static void key_select_extension(struct unistimsession
 		} 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 +
@@ -3019,12 +3690,13 @@ static void key_select_extension(struct unistimsession
 	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] == '#')) {
 			if (i == pos) {
 				pos++;
 				continue;
@@ -3048,22 +3720,37 @@ static void show_entry_history(struct unistimsession *
 		return;
 	}
 	line[sizeof(line) - 1] = '\0';
-	send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
-	if (fread(line, TEXT_LENGTH_MAX, 1, *f) != 1) {
+        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);
 		return;
 	}
 	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 (fread(line, TEXT_LENGTH_MAX, 1, *f) != 1) {
+        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],
@@ -3073,20 +3760,20 @@ static void show_entry_history(struct unistimsession *
 	if (pte->buff_entry[2] == 1)
 		strcpy(func1, "       ");
 	else
-		strcpy(func1, "Prvious");
+		strcpy(func1, ustmtext("Prev   ", pte));
 	if (pte->buff_entry[2] >= pte->buff_entry[1])
 		strcpy(func2, "       ");
 	else
-		strcpy(func2, "Next   ");
-	if (ReformatNumber(pte->device->lst_cid))
-		strcpy(func3, "Redial ");
+		strcpy(func2, ustmtext("Next   ", pte));
+	if (reformat_number(pte->device->lst_cid))
+		strcpy(func3, ustmtext("Redial ", pte));
 	else
 		strcpy(func3, "       ");
-	snprintf(status, sizeof(status), "%s%s%sCancel", func1, func2, func3);
+	snprintf(status, sizeof(status), ustmtext("%s%s%sCancel", pte), func1, func2, func3);
 	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;
@@ -3121,12 +3808,13 @@ static void show_history(struct unistimsession *pte, c
 		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;
 }
@@ -3134,60 +3822,73 @@ static void show_history(struct unistimsession *pte, c
 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);
+	get_main_line(pte->device)->lastmsgssent = 0;
 	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];
+                        sprintf(tmp_field, ustmtext("Fwd to: %s", 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   Recal  Fwd    Unregis", pte));
 		else
-			send_text_status(pte, "Dial   Redial Forward");
+			send_text_status(pte, ustmtext("Dial   Recal  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);
-			send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmpbuf);
-			Sendicon(TEXT_LINE0, FAV_ICON_CALL_CENTER + FAV_BLINK_SLOW, pte);
+		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("%d unanswered call", pte);
+                        else
+                                text = ustmtext("%d unanswered calls", pte);
+                        sprintf(tmpbuf, text, pte->device->missed_call);
+                        send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmpbuf);
+			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)) {
@@ -3197,6 +3898,7 @@ static void key_main_page(struct unistimsession *pte, 
 	}
 	switch (keycode) {
 	case KEY_FUNC1:
+                pte->device->selected = get_avail_softkey(pte, NULL);
 		handle_dial_page(pte);
 		break;
 	case KEY_FUNC2:
@@ -3210,7 +3912,7 @@ static void key_main_page(struct unistimsession *pte, 
 
 		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)) {
@@ -3218,7 +3920,7 @@ static void key_main_page(struct unistimsession *pte, 
 			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;
@@ -3228,9 +3930,9 @@ static void key_main_page(struct unistimsession *pte, 
 		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);
@@ -3239,26 +3941,25 @@ static void key_main_page(struct unistimsession *pte, 
 			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_FAV0: 
 	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);
@@ -3283,44 +3984,41 @@ static void key_history(struct unistimsession *pte, ch
 	FILE *f;
 	char count;
 	long offset;
+        int flag = 0;
 
 	switch (keycode) {
+        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_LEFT:
 	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:
+        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 (!reformat_number(pte->device->lst_cid))
 			break;
 		ast_copy_string(pte->device->redial_number, pte->device->lst_cid,
 						sizeof(pte->device->redial_number));
@@ -3343,6 +4041,20 @@ static void key_history(struct unistimsession *pte, ch
 			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;
 }
 
@@ -3353,7 +4065,7 @@ static void init_phone_step2(struct unistimsession *pt
 		ast_verb(0, "Sending S4\n");
 	memcpy(buffsend + SIZE_HEADER, packet_send_s4, sizeof(packet_send_s4));
 	send_client(SIZE_HEADER + sizeof(packet_send_s4), buffsend, pte);
-	send_date_time2(pte);
+        send_date_time2(pte);
 	send_date_time3(pte);
 	if (unistimdebug)
 		ast_verb(0, "Sending S7\n");
@@ -3378,9 +4090,13 @@ static void init_phone_step2(struct unistimsession *pt
 	send_client(SIZE_HEADER + sizeof(packet_send_S7), buffsend, pte);
 	send_led_update(pte, 0);
 	send_ping(pte);
-	if (pte->state < STATE_MAINPAGE) {
+	if (unistimdebug)
+		ast_verb(0, "Sending init language\n");
+        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;
@@ -3388,8 +4104,8 @@ static void init_phone_step2(struct unistimsession *pt
 
 			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("Sorry, this phone is not", pte));
+			send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("registered in unistim.cfg", pte));
 			strcpy(tmp, "MAC = ");
 			strcat(tmp, pte->macaddr);
 			send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmp);
@@ -3443,8 +4159,8 @@ static void process_request(int size, unsigned char *b
 		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:
@@ -3470,10 +4186,19 @@ static void process_request(int size, unsigned char *b
 		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_HISTORY:
+                case STATE_SELECTCONTRAST:
+                        key_select_contrast(pte, keycode);
+                        break;
+                case STATE_SELECTLANGUAGE:
+                        key_select_language(pte, keycode);
+                        break;
+                case STATE_HISTORY:
 			key_history(pte, keycode);
 			break;
 		default:
@@ -3491,13 +4216,15 @@ static void process_request(int size, unsigned char *b
 			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);
 		}
@@ -3511,7 +4238,7 @@ static void process_request(int size, unsigned char *b
 		pte->device->receiver_state = STATE_ONHOOK;
 		if (pte->state == STATE_CALL)
 			close_call(pte);
-		else if (pte->device->lines->subs[SUB_REAL]->owner)
+                else if (get_sub(pte->device, SUB_REAL) && get_sub(pte->device, SUB_REAL)->owner)
 			close_call(pte);
 		else if (pte->state == STATE_EXTENSION)
 			return;
@@ -3709,13 +4436,52 @@ static struct unistimsession *channel_to_session(struc
 	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];
+                        sprintf(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));
+                }
+	}
+}
+
 /*--- 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) {
@@ -3724,6 +4490,7 @@ static int unistim_call(struct ast_channel *ast, char 
 	}
 
 	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);
@@ -3734,58 +4501,49 @@ static int unistim_call(struct ast_channel *ast, char 
 		ast_verb(3, "unistim_call(%s)\n", ast->name);
 
 	session->state = STATE_RINGING;
-	Sendicon(TEXT_LINE0, FAV_ICON_NONE, session);
+	//send_icon(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);
-		} 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);
-		}
-		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_text(TEXT_LINE2, TEXT_NORMAL, session, "is calling you.");
-	send_text_status(session, "Accept              Ignore");
+        send_callerid_screen(session, sub);
+        if (!strlen(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", session));
 
-	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;
+                if (sub_real) {
+                        ringstyle = session->device->cwstyle;
+                        ringvolume = session->device->cwvolume;
+                } else {
+                        ringstyle = (sub->ringstyle == -1) ? session->device->ringstyle : sub->ringstyle;
+                        ringvolume = (sub->ringvolume == -1) ? session->device->ringvolume : sub->ringvolume;
+                }
+                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;
+                        }
+                }
+        }
+        ast_setstate(ast, AST_STATE_RINGING);
+        ast_queue_control(ast, AST_CONTROL_RINGING);
+        return res;
 }
 
 /*--- 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 unistimsession *s;
+	struct unistim_device *d;
+        struct unistimsession *s;
+        int i;
 
 	s = channel_to_session(ast);
 	sub = ast->tech_pvt;
@@ -3805,32 +4563,43 @@ static int unistim_hangup(struct ast_channel *ast)
 		return 0;
 	}
 	l = sub->parent;
+        d = s->device;
 	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)) {
+        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");
+			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);
+
+                ast_mutex_lock(&sub->lock);
 		sub->owner = NULL;
 		ast->tech_pvt = NULL;
+                ast_mutex_unlock(&sub->lock);
+                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_verb(0, "Real call disconnected, stay in call\n");
+                
+		send_text(TEXT_LINE0, TEXT_NORMAL, s, ustmtext("Three way call canceled", s));
+		send_text(TEXT_LINE1, TEXT_NORMAL, s, ustmtext("switching back", s));
+		send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("to previous call.", s));
+		send_text_status(s, ustmtext("Hangup Transf", s));
+		
 		ast_mutex_lock(&sub->lock);
 		sub->owner = NULL;
 		ast->tech_pvt = NULL;
 		ast_mutex_unlock(&sub->lock);
-		unalloc_sub(l, SUB_THREEWAY);
+		unalloc_sub(d, sub);
 		return 0;
 	}
 	ast_mutex_lock(&sub->lock);
@@ -3838,34 +4607,26 @@ static int unistim_hangup(struct ast_channel *ast)
 	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;
-		}
-		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;
-		}
-	}
-	l->parent->moh = 0;
-	send_no_ring(s);
-	send_end_call(s);
+                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;
+                        }
+                }
+        }
+        sub->moh = 0;
+        if (sub->softkey >= 0)
+                send_favorite_short(sub->softkey, FAV_LINE_ICON, s);
 	if (sub->rtp) {
 		if (unistimdebug)
 			ast_verb(0, "Destroying RTP session\n");
@@ -3873,16 +4634,27 @@ static int unistim_hangup(struct ast_channel *ast)
 		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);
-	}
-	if (s->state == STATE_RINGING)
-		cancel_dial(s);
-	else if (s->state == STATE_CALL)
-		close_call(s);
-
+        /* Delete assign sub to softkey */
+        for (i = 0; i < FAVNUM; i++) {
+                if (d->ssub[i] == sub) {
+                        d->ssub[i] = NULL;
+                        break;
+                }
+        }
+        if (unistimdebug)
+                ast_verb(0, "Closing call while state STATE_%s\n", ptestate_tostr(s->state));
+        if (s->state == STATE_RINGING && sub->subtype == SUB_RING) {
+                send_no_ring(s);
+                d->missed_call++;
+                write_history(s, 'i', 1);
+                if (!sub_real)
+                        show_main_page(s);
+        }
+        if (s->state == STATE_CALL && sub->subtype == SUB_REAL) {
+                close_call(s);
+        }
+        sub->softkey = -1;
+        unalloc_sub(d, sub);
 	return 0;
 }
 
@@ -3892,6 +4664,7 @@ static int unistim_answer(struct ast_channel *ast)
 	int res = 0;
 	struct unistim_subchannel *sub;
 	struct unistim_line *l;
+        struct unistim_device *d;
 	struct unistimsession *s;
 
 	s = channel_to_session(ast);
@@ -3901,18 +4674,19 @@ static int unistim_answer(struct ast_channel *ast)
 	}
 	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("Hangup Transf", s));
+	send_start_timer(s);
 	if (ast->_state != AST_STATE_UP)
 		ast_setstate(ast, AST_STATE_UP);
 	return res;
@@ -4050,14 +4824,16 @@ static int unistim_write(struct ast_channel *ast, stru
 			return 0;
 		}
 	} else {
-		if (!(frame->subclass.codec & ast->nativeformats)) {
+                if (!(frame->subclass.codec & ast->nativeformats)) {
 			char tmp[256];
 			ast_log(LOG_WARNING,
-					"Asked to transmit frame type %s, while native formats is %s (read/write = (%s/%s)\n",
+					"Asked to transmit frame type %s, while native formats is %s (read/write = (%s/%s)) (raw read/write = (%s/%s))\n",
 					ast_getformatname(frame->subclass.codec),
 					ast_getformatname_multiple(tmp, sizeof(tmp), ast->nativeformats),
 					ast_getformatname(ast->readformat),
-					ast_getformatname(ast->writeformat));
+					ast_getformatname(ast->writeformat),
+                                        ast_getformatname(ast->rawwriteformat),
+                                        ast_getformatname(ast->rawreadformat));
 			return -1;
 		}
 	}
@@ -4126,7 +4902,13 @@ static char *control2str(int ind)
 		return "Key Radio";
 	case AST_CONTROL_RADIO_UNKEY:
 		return "Un-Key Radio";
-	case -1:
+        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";
 	}
 	return "UNKNOWN";
@@ -4153,8 +4935,8 @@ static int unistim_indicate(struct ast_channel *ast, i
 	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);
@@ -4167,7 +4949,7 @@ static int unistim_indicate(struct ast_channel *ast, i
 	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;
@@ -4176,7 +4958,7 @@ static int unistim_indicate(struct ast_channel *ast, i
 	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;
@@ -4189,7 +4971,7 @@ static int unistim_indicate(struct ast_channel *ast, i
 	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;
@@ -4203,13 +4985,23 @@ static int unistim_indicate(struct ast_channel *ast, i
 		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:
-		break;
+        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);
 		return -1;
@@ -4222,6 +5014,7 @@ static struct unistim_subchannel *find_subchannel_by_n
 {
 	struct unistim_line *l;
 	struct unistim_device *d;
+        struct unistim_subchannel *sub = NULL;
 	char line[256];
 	char *at;
 	char *device;
@@ -4245,12 +5038,22 @@ static struct unistim_subchannel *find_subchannel_by_n
 			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 = alloc_sub(d, SUB_REAL);
+                                        }
+                                        if (sub->owner) {
+                                                /* Allocate additional channel if asterisk channel already here */
+                                                sub = alloc_sub(d, SUB_ONHOLD);
+                                        }
+					sub->ringvolume = -1;
+					sub->ringstyle = -1;
 					if (at) {       /* Other options ? */
 						at++;   /* Skip slash */
 						if (*at == 'r') {       /* distinctive ring */
@@ -4266,16 +5069,20 @@ static struct unistim_subchannel *find_subchannel_by_n
 								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;
 	}
@@ -4301,9 +5108,9 @@ static int unistim_senddigit_end(struct ast_channel *a
 	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;
 	}
@@ -4436,7 +5243,7 @@ static int unistim_sendtext(struct ast_channel *ast, c
 		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) {
@@ -4464,7 +5271,7 @@ static int unistim_send_mwi_to_peer(struct unistimsess
 	struct ast_event *event;
 	int new;
 	char *mailbox, *context;
-	struct unistim_line *peer = s->device->lines;
+	struct unistim_line *peer = get_main_line(s->device);
 
 	context = mailbox = ast_strdupa(peer->mailbox);
 	strsep(&context, "@");
@@ -4518,9 +5325,9 @@ static struct ast_channel *unistim_new(struct unistim_
 	}
 	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->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;
@@ -4558,8 +5365,8 @@ static struct ast_channel *unistim_new(struct unistim_
 	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++;
@@ -4583,14 +5390,14 @@ static struct ast_channel *unistim_new(struct unistim_
 		}
 	}
 	tmp->priority = 1;
-	if (state != AST_STATE_DOWN) {
-		if (unistimdebug)
-			ast_verb(0, "Starting pbx in unistim_new\n");
-		if (ast_pbx_start(tmp)) {
-			ast_log(LOG_WARNING, "Unable to start PBX on %s\n", tmp->name);
-			ast_hangup(tmp);
-			tmp = NULL;
-		}
+        if (state != AST_STATE_DOWN) {
+                if (unistimdebug)
+                        ast_verb(0, "Starting pbx in unistim_new\n");
+                if (ast_pbx_start(tmp)) {
+                        ast_log(LOG_WARNING, "Unable to start PBX on %s\n", tmp->name);
+                        ast_hangup(tmp);
+                        tmp = NULL;
+                }
 	}
 
 	return tmp;
@@ -4639,8 +5446,8 @@ static void *do_monitor(void *data)
 				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))) {
+				if ((!ast_strlen_zero(get_main_line(cur->device)->mailbox)) &&
+					((tick >= get_main_line(cur->device)->nextmsgcheck))) {
 					DEBUG_TIMER("Checking mailbox for MWI\n");
 					unistim_send_mwi_to_peer(cur, tick);
 					break;
@@ -4710,7 +5517,8 @@ static struct ast_channel *unistim_request(const char 
 										   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;
@@ -4734,22 +5542,39 @@ static struct ast_channel *unistim_request(const char 
 		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) {
+                unalloc_sub(d, sub);
+		*cause = AST_CAUSE_CONGESTION;
+		return NULL;
+        }
+	if (sub_ring || sub_trans) {
 		if (unistimdebug)
-			ast_verb(0, "Can't create channel : Busy !\n");
+			ast_verb(0, "Can't create channel, request already in progress: Busy!\n");
+                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 for line %s, all lines busy\n", sub->parent->name);
+                unalloc_sub(d, sub);
+		*cause = AST_CAUSE_BUSY;
+		return NULL;
+        }
+
+        ast_verb(3, "unistim_request(%s)\n", tmp);
+        sub->subtype = SUB_RING;
+        sub->softkey = -1;
 	sub->parent->capability = format;
 	tmpc = unistim_new(sub, AST_STATE_DOWN, requestor ? requestor->linkedid : NULL);
 	if (!tmpc)
@@ -4768,7 +5593,6 @@ static char *unistim_info(struct ast_cli_entry *e, int
 	struct unistim_line *line;
 	struct unistim_subchannel *sub;
 	struct unistimsession *s;
-	int i;
 	struct ast_channel *tmp;
 
 	switch (cmd) {
@@ -4786,44 +5610,55 @@ static char *unistim_info(struct ast_cli_entry *e, int
 	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 :\ndevice\n->line\n-->sub\n==>key\n");
 	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) {
 			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=%" PRId64 " 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);
-			}
-			line = line->next;
+					line->capability, line);
 		}
-		device = device->next;
+		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;
+                        }
+                        ast_cli(a->fd, "==> %d. dev=%s icon=%#x label=%-8s 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_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,
+				"sin=%s timeout=%u state=%s macaddr=%s device=%p session=%p\n",
+				ast_inet_ntoa(s->sin.sin_addr), s->timeout, ptestate_tostr(s->state), s->macaddr,
 				s->device, s);
 		s = s->next;
 	}
@@ -4981,7 +5816,7 @@ static void unquote(char *out, const char *src, int ma
 	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;
@@ -5056,7 +5891,7 @@ static int ParseBookmark(const char *text, struct unis
 	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;
 }
@@ -5087,15 +5922,29 @@ static void finish_bookmark(void)
 	}
 }
 
+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;
 	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 */
@@ -5112,22 +5961,39 @@ static struct unistim_device *build_device(const char 
 			}
 			/* we're reloading right now */
 			create = 0;
-			l = d->lines;
+                        /* 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;
+                ast_copy_string(d->name, cat, sizeof(d->name));
+	} else {
+            /* Delete existing line information */
+            AST_LIST_LOCK(&d->lines);
+            AST_LIST_TRAVERSE_SAFE_BEGIN(&d->lines, l, list){
+                AST_LIST_REMOVE_CURRENT(list);
+                ast_free(l);
+            }
+            AST_LIST_TRAVERSE_SAFE_END
+            AST_LIST_UNLOCK(&d->lines);
+        }
 
-		if (!(l = ast_calloc(1, sizeof(*l)))) {
-			ast_free(d);
-			return NULL;
-		}
-		ast_copy_string(d->name, cat, sizeof(d->name));
-	}
+        /* 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(context, DEFAULTCONTEXT, sizeof(context));
 	d->contrast = -1;
 	d->output = OUTPUT_HANDSET;
@@ -5135,12 +6001,16 @@ static struct unistim_device *build_device(const char 
 	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;
 	while (v) {
 		if (!strcasecmp(v->name, "rtp_port"))
@@ -5181,19 +6051,25 @@ static struct unistim_device *build_device(const char 
 			ringvolume = atoi(v->value);
 		else if (!strcasecmp(v->name, "ringstyle"))
 			ringstyle = atoi(v->value);
-		else if (!strcasecmp(v->name, "callhistory"))
+		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);
@@ -5201,17 +6077,17 @@ static struct unistim_device *build_device(const char 
 				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")) {
@@ -5228,103 +6104,93 @@ static struct unistim_device *build_device(const char 
 				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 len = strlen(linelabel);
+                        int create_line = 0;
+                        
+                        l = find_line_by_number(d, v->value);
+                        if (!l) { /* If line still not exists */
+                                if (!(l = ast_calloc(1, sizeof(*l)))) {
+                                        ast_free(d);
+                                        return NULL;
+                                }
+                                memcpy(l, lt, sizeof(*l));
+                                ast_mutex_init(&l->lock);
+                                create_line = 1;
+                        }
+                        d->to_delete = 0;
 
-			if (nbsoftkey) {
-				ast_log(LOG_WARNING,
-						"You must use bookmark AFTER line=>. Only one line is supported in this version\n");
-				if (create) {
-					ast_free(d);
-					ast_free(l);
-				}
-				return NULL;
-			}
-			if (create) {
-				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;
+                        /* 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);
-			}
+                        
+                        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);
+                                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;
+                                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;
-				}
-				l->next = d->lines;
-				d->lines = l;
-			}
-		} else if (!strcasecmp(v->name, "height")) {
+                                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;
 	}
-	d->ringvolume = ringvolume;
+	if (!get_main_line(d)) {
+		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->callhistory = callhistory;
+        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;
@@ -5337,8 +6203,6 @@ static struct unistim_device *build_device(const char 
 	} 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);
 			}
@@ -5351,8 +6215,6 @@ static struct unistim_device *build_device(const char 
 		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)) {
@@ -5360,7 +6222,7 @@ static struct unistim_device *build_device(const char 
 		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_log(LOG_WARNING, "Error in ast_localtime()");
 			ast_copy_string(d->titledefault, "UNISTIM for*", 12);
 		} else {
 			if (strlen(tm.tm_zone) < 4) {
@@ -5438,8 +6300,13 @@ static int reload_config(void)
                 } 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, "autoprovisioning")) {
+		} 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;
@@ -5497,44 +6364,57 @@ static int reload_config(void)
 	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) {
+			if (!get_main_line(d)) {
 				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_log(LOG_WARNING,
-							"Device '%s' with threeway call subchannels allocated, aborting.\n",
-							d->name);
-					break;
-				}
-			}
-			if (i < MAX_SUBS) {
-				d = d->next;
-				continue;
-			}
-			ast_mutex_destroy(&d->lines->lock);
-			ast_free(d->lines);
+
+                        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);
+                            ast_free(l);
+                        }
+                        AST_LIST_TRAVERSE_SAFE_END
+                        AST_LIST_UNLOCK(&d->lines);
+
 			if (d->session) {
 				if (sessions == d->session)
 					sessions = d->session->next;
@@ -5578,9 +6458,15 @@ static int reload_config(void)
 	ast_mutex_lock(&sessionlock);
 	s = sessions;
 	while (s) {
-		if (s->device)
+		if (s->device) {
 			refresh_all_favorite(s);
-		s = s->next;
+                        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);
 	/* We don't recreate a socket when reloading (locks would be necessary). */
@@ -5693,8 +6579,9 @@ buff_failed:
 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));
 
