Massive changes in chan_unistim channel driver. Include many fixes in channel driver...
[asterisk/asterisk.git] / channels / chan_unistim.c
index fcde6e6..48fde42 100644 (file)
@@ -73,19 +73,24 @@ ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
 #include "asterisk/musiconhold.h"
 #include "asterisk/causes.h"
 #include "asterisk/indications.h"
+#include "asterisk/features.h"
+#include "asterisk/astobj2.h"
+#include "asterisk/astdb.h"
 
-/*! Beware, G729 and G723 are not supported by asterisk, except with the proper licence */
 
 #define DEFAULTCONTEXT   "default"
 #define DEFAULTCALLERID         "Unknown"
 #define DEFAULTCALLERNAME       " "
 #define DEFAULTHEIGHT   3
 #define USTM_LOG_DIR       "unistimHistory"
+#define USTM_LANG_DIR      "unistimLang"
 
 /*! Size of the transmit buffer */
 #define MAX_BUF_SIZE       64
 /*! Number of slots for the transmit queue */
 #define MAX_BUF_NUMBER   50
+/*! Number of digits displayed on screen */
+#define MAX_SCREEN_NUMBER   15
 /*! Try x times before removing the phone */
 #define NB_MAX_RETRANSMIT       8
 /*! Nb of milliseconds waited when no events are scheduled */
@@ -93,7 +98,9 @@ ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
 /*! Wait x milliseconds before resending a packet */
 #define RETRANSMIT_TIMER       2000
 /*! How often the mailbox is checked for new messages */
-#define TIMER_MWI             10000
+#define TIMER_MWI             5000
+/*! How often the mailbox is checked for new messages */
+#define TIMER_DIAL            4000
 /*! Not used */
 #define DEFAULT_CODEC     0x00
 #define SIZE_PAGE             4096
@@ -102,15 +109,15 @@ ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
 #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
 
 struct ast_format_cap *global_cap;
 
 enum autoprovision {
        AUTOPROVISIONING_NO = 0,
        AUTOPROVISIONING_YES,
-       AUTOPROVISIONING_DB,
        AUTOPROVISIONING_TN
 };
 
@@ -186,6 +193,9 @@ enum autoprov_extn {
 
 #define FAV_MAX_LENGTH           0x0A
 
+#define FAVNUM                    6
+#define FAV_LINE_ICON         FAV_ICON_ONHOOK_BLACK
+
 static void dummy(char *unused, ...)
 {
        return;
@@ -225,17 +235,9 @@ static struct {
 static struct io_context *io;
 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;
-/*! size of the sockaddr_in (in WSARecvFrom) */
-static unsigned int size_addr_from = sizeof(address_from);
-/*! Receive buffer address */
-static unsigned char *buff;
+static unsigned char *buff; /*! Receive buffer address */
 static int unistim_reloading = 0;
 AST_MUTEX_DEFINE_STATIC(unistim_reload_lock);
-AST_MUTEX_DEFINE_STATIC(usecnt_lock);
-static int usecnt = 0;
-/* extern char ast_config_AST_LOG_DIR[AST_CONFIG_MAX_PATH]; */
 
 /*! This is the thread for the monitor which checks for input on the channels
  * which are not currently in use.  */
@@ -257,7 +259,9 @@ enum phone_state {
        STATE_DIALPAGE,
        STATE_RINGING,
        STATE_CALL,
+       STATE_SELECTOPTION,
        STATE_SELECTCODEC,
+       STATE_SELECTLANGUAGE,
        STATE_CLEANING,
        STATE_HISTORY
 };
@@ -308,75 +312,36 @@ 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;
        unsigned char *buf;
 };
 
-struct systemtime {
-       unsigned short w_year;
-       unsigned short w_month;
-       unsigned short w_day_of_week;
-       unsigned short w_day;
-       unsigned short w_hour;
-       unsigned short w_minute;
-       unsigned short w_second;
-       unsigned short w_milliseconds;
-};
-
 struct unistim_subchannel {
        ast_mutex_t lock;
-       /*! SUBS_REAL or SUBS_THREEWAY */
-       unsigned int subtype;
-       /*! Asterisk channel used by the subchannel */
-       struct ast_channel *owner;
-       /*! Unistim line */
-       struct unistim_line *parent;
-       /*! RTP handle */
-       struct ast_rtp_instance *rtp;
+       unsigned int subtype;           /*! SUB_REAL, SUB_RING, SUB_THREEWAY or SUB_ONHOLD */
+       struct ast_channel *owner;      /*! Asterisk channel used by the subchannel */
+       struct unistim_line *parent;    /*! Unistim line */
+       struct ast_rtp_instance *rtp;   /*! RTP handle */
+       int softkey;                    /*! Softkey assigned */
+       pthread_t ss_thread;            /*! unistim_ss thread handle */
        int alreadygone;
        char ringvolume;
        char ringstyle;
+       int moh;                                        /*!< Music on hold in progress */
+       AST_LIST_ENTRY(unistim_subchannel) list;
 };
 
 /*!
@@ -384,62 +349,45 @@ struct unistim_subchannel {
  */
 struct unistim_line {
        ast_mutex_t lock;
-       /*! Like 200 */
-       char name[80];
-       /*! Like USTM/200\@black */
-       char fullname[80];
-       /*! pointer to our current connection, channel... */
-       struct unistim_subchannel *subs[MAX_SUBS];
-       /*! Extension where to start */
-       char exten[AST_MAX_EXTENSION];
-       /*! Context to start in */
-       char context[AST_MAX_EXTENSION];
-       /*! Language for asterisk sounds */
-       char language[MAX_LANGUAGE];
-       /*! CallerID Number */
-       char cid_num[AST_MAX_EXTENSION];
-       /*! Mailbox for MWI */
-       char mailbox[AST_MAX_EXTENSION];
-       /*! Used by MWI */
-       int lastmsgssent;
-       /*! Used by MWI */
-       time_t nextmsgcheck;
-       /*! MusicOnHold class */
-       char musicclass[MAX_MUSICCLASS];
-       /*! Call group */
-       unsigned int callgroup;
-       /*! Pickup group */
-       unsigned int pickupgroup;
-       /*! Account code (for billing) */
-       char accountcode[80];
-       /*! AMA flags (for billing) */
-       int amaflags;
-       /*! Codec supported */
-       struct ast_format_cap *cap;
-       /*! Parkinglot */
-       char parkinglot[AST_MAX_CONTEXT];
+       char name[80]; /*! Like 200 */
+       char fullname[80]; /*! Like USTM/200\@black */
+       char exten[AST_MAX_EXTENSION]; /*! Extension where to start */
+       char cid_num[AST_MAX_EXTENSION]; /*! CallerID Number */
+       char mailbox[AST_MAX_EXTENSION]; /*! Mailbox for MWI */
+       int lastmsgssent; /*! Used by MWI */
+       time_t nextmsgcheck; /*! Used by MWI */
+       char musicclass[MAX_MUSICCLASS]; /*! MusicOnHold class */
+       ast_group_t callgroup; /*! Call group */
+       ast_group_t pickupgroup; /*! Pickup group */
+       char accountcode[AST_MAX_ACCOUNT_CODE]; /*! Account code (for billing) */
+       int amaflags; /*! AMA flags (for billing) */
+       struct ast_format_cap *cap; /*! Codec supported */
+       char parkinglot[AST_MAX_CONTEXT]; /*! Parkinglot */
        struct unistim_line *next;
        struct unistim_device *parent;
+       AST_LIST_ENTRY(unistim_line) list;
 };
 
-/*! 
- * \brief A device containing one or more lines 
+/*!
+ * \brief A device containing one or more lines
  */
 static struct unistim_device {
+       ast_mutex_t lock;
        int receiver_state;           /*!< state of the receiver (see ReceiverState) */
        int size_phone_number;    /*!< size of the phone number */
-       char phone_number[16];    /*!< the phone number entered by the user */
-       char redial_number[16];  /*!< the last phone number entered by the user */
-       int phone_current;                    /*!< Number of the current phone */
-       int pos_fav;                        /*!< Position of the displayed favorites (used for scrolling) */
+       char context[AST_MAX_EXTENSION]; /*!< Context to start in */
+       char phone_number[AST_MAX_EXTENSION];     /*!< the phone number entered by the user */
+       char redial_number[AST_MAX_EXTENSION];   /*!< the last phone number entered by the user */
        char id[18];                        /*!< mac address of the current phone in ascii */
        char name[DEVICE_NAME_LEN];     /*!< name of the device */
-       int softkeylinepos;                  /*!< position of the line softkey (default 0) */
-       char softkeylabel[6][11];       /*!< soft key label */
-       char softkeynumber[6][16];      /*!< number dialed when the soft key is pressed */
-       char softkeyicon[6];        /*!< icon number */
-       char softkeydevice[6][16];      /*!< name of the device monitored */
-       struct unistim_device *sp[6];   /*!< pointer to the device monitored by this soft key */
+       char softkeylabel[FAVNUM][11];       /*!< soft key label */
+       char softkeynumber[FAVNUM][AST_MAX_EXTENSION];      /*!< number dialed when the soft key is pressed */
+       char softkeyicon[FAVNUM];           /*!< icon number */
+       char softkeydevice[FAVNUM][16];      /*!< name of the device monitored */
+       struct unistim_subchannel *ssub[FAVNUM];
+       struct unistim_line *sline[FAVNUM];
+       struct unistim_device *sp[FAVNUM];   /*!< pointer to the device monitored by this soft key */
+       char language[MAX_LANGUAGE];    /*!< Language for asterisk sounds */
        int height;                                                     /*!< The number of lines the phone can display */
        char maintext0[25];                  /*!< when the phone is idle, display this string on line 0 */
        char maintext1[25];                  /*!< when the phone is idle, display this string on line 1 */
@@ -451,27 +399,31 @@ 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 */
+       char cwvolume;                  /*!< Ring volume on call waiting */
+       char cwstyle;                    /*!< Ring melody on call waiting */
+       time_t nextdial;                /*!< Timer used for dial by timeout */
        int rtp_port;                      /*!< RTP port used by the phone */
        int rtp_method;                  /*!< Select the unistim data used to establish a RTP session */
        int status_method;                    /*!< Select the unistim packet used for sending status text */
        char codec_number;                    /*!< The current codec used to make calls */
        int missed_call;                        /*!< Number of call unanswered */
        int callhistory;                        /*!< Allowed to record call history */
+        int sharp_dial;                                /*!< Execute Dial on '#' or not */
        char lst_cid[TEXT_LENGTH_MAX];  /*!< Last callerID received */
        char lst_cnm[TEXT_LENGTH_MAX];  /*!< Last callername recevied */
        char call_forward[AST_MAX_EXTENSION];   /*!< Forward number */
        int output;                                  /*!< Handset, headphone or speaker */
        int previous_output;        /*!< Previous output */
        int volume;                                  /*!< Default volume */
+        int selected;                           /*!< softkey selected */
        int mute;                                      /*!< Mute mode */
-       int moh;                                        /*!< Music on hold in progress */
        int nat;                                        /*!< Used by the obscure ast_rtp_setnat */
        enum autoprov_extn extension;   /*!< See ifdef EXTENSION for valid values */
        char extension_number[11];      /*!< Extension number entered by the user */
        char to_delete;                  /*!< Used in reload */
-       time_t start_call_timestamp;    /*!< timestamp for the length calculation of the call */
        struct ast_silence_generator *silence_generator;
-       struct unistim_line *lines;
+       AST_LIST_HEAD(,unistim_subchannel) subs; /*!< pointer to our current connection, channel... */
+       AST_LIST_HEAD(,unistim_line) lines;
        struct ast_ha *ha;
        struct unistimsession *session;
        struct unistim_device *next;
@@ -498,6 +450,21 @@ static struct unistimsession {
        struct unistimsession *next;
 } *sessions = NULL;
 
+/*! Store on screen phone menu item (label and handler function) */
+struct unistim_menu_item {
+       char *label;
+       int state;
+       void (*handle_option)(struct unistimsession *);
+};
+
+/*! Language item for currently existed translations */
+struct unistim_languages {
+       char *label;
+       char *lang_short;
+       int encoding;
+       struct ao2_container *trans;
+};
+
 /*!
  * \page Unistim datagram formats
  *
@@ -516,6 +483,8 @@ static const unsigned char packet_send_discovery_ack[] =
 
 static const unsigned char packet_recv_firm_version[] =
        { 0x00, 0x00, 0x00, 0x13, 0x9a, 0x0a, 0x02 };
+static const unsigned char packet_recv_it_type[] =
+       { 0x00, 0x00, 0x00, 0x13, 0x9a, 0x04, 0x03 };
 static const unsigned char packet_recv_pressed_key[] =
        { 0x00, 0x00, 0x00, 0x13, 0x99, 0x04, 0x00 };
 static const unsigned char packet_recv_pick_up[] =
@@ -629,9 +598,9 @@ static const unsigned char packet_send_date_time2[] = { 0x17, 0x04, 0x17, 0x3d,
 };
 static const unsigned char packet_send_Contrast[] =
        { 0x17, 0x04, 0x24, /*Contrast */ 0x08 };
-static const unsigned char packet_send_StartTimer[] =
-       { 0x17, 0x05, 0x0b, 0x05, 0x00, 0x17, 0x08, 0x16, /* Text */ 0x44, 0x75, 0x72, 0xe9,
-0x65 };
+static const unsigned char packet_send_start_timer[] =
+       { 0x17, 0x05, 0x0b, /*Timer option*/0x05, /* Timer ID */0x00, 0x17, 0x08, 0x16,
+       /* Text */ 0x44, 0x75, 0x72, 0xe9, 0x65 };
 static const unsigned char packet_send_stop_timer[] = { 0x17, 0x05, 0x0b, 0x02, 0x00 };
 static const unsigned char packet_send_icon[] = { 0x17, 0x05, 0x14, /*pos */ 0x00, /*icon */ 0x25 };      /* display an icon in front of the text zone */
 static const unsigned char packet_send_S7[] = { 0x17, 0x06, 0x0f, 0x30, 0x07, 0x07 };
@@ -663,6 +632,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 };
@@ -685,7 +671,7 @@ static int reload(void);
 static int unload_module(void);
 static int reload_config(void);
 static void show_main_page(struct unistimsession *pte);
-static struct ast_channel *unistim_request(const char *type, struct ast_format_cap *cap, const struct ast_channel *requestor, 
+static struct ast_channel *unistim_request(const char *type, struct ast_format_cap *cap, const struct ast_channel *requestor,
        const char *dest, int *cause);
 static int unistim_call(struct ast_channel *ast, const char *dest, int timeout);
 static int unistim_hangup(struct ast_channel *ast);
@@ -722,15 +708,143 @@ static 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 *);
+static void key_favorite(struct unistimsession *, char);
+
+static void handle_select_codec(struct unistimsession *);
+static void handle_select_language(struct unistimsession *);
+static int find_language(const char*);
+
+static int unistim_free_sub(struct unistim_subchannel *);
+
+static struct unistim_menu_item options_menu[] =
+{
+       {"Change codec", STATE_SELECTCODEC, handle_select_codec},
+       {"Language", STATE_SELECTLANGUAGE, handle_select_language},
+       {NULL, 0, NULL}
+};
+
+static struct unistim_languages options_languages[] =
+{
+       {"English", "en", ISO_8859_1, NULL},
+       {"Russian", "ru", ISO_8859_5, NULL},
+       {NULL, NULL, 0, NULL}
+};
+
+static char ustm_strcopy[1024];
+
+struct ustm_lang_entry {
+       const char *str_orig;
+       const char *str_trans;
+};
+
+static int lang_hash_fn(const void *obj, const int flags)
+{
+       const struct ustm_lang_entry *entry = obj;
+       return ast_str_hash(entry->str_orig);
+}
+
+static int lang_cmp_fn(void *obj, void *arg, int flags)
 {
-       time_t cur_time;
+       struct ustm_lang_entry *entry1 = obj;
+       struct ustm_lang_entry *entry2 = arg;
+
+       return (!strcmp(entry1->str_orig, entry2->str_orig)) ? (CMP_MATCH | CMP_STOP) : 0;
+}
+
+static const char *ustmtext(const char *str, struct unistimsession *pte)
+{
+       struct ustm_lang_entry *lang_entry;
+       struct ustm_lang_entry le_search;
+       struct unistim_languages *lang = NULL;
+       int size;
        
-       time(&cur_time);
+       if (pte->device) {
+               lang = &options_languages[find_language(pte->device->language)];
+       }
+       if (!lang) {
+               return str;
+       }
+       /* Check if specified language exists */
+       if (!lang->trans) {
+               char tmp[1024], *p, *p_orig = NULL, *p_trans = NULL;
+               FILE *f;
+
+               if (!(lang->trans = ao2_container_alloc(8, lang_hash_fn, lang_cmp_fn))) {
+                       ast_log(LOG_ERROR, "Unable to allocate container for translation!\n");
+                       return str;
+               }
+               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) {
+                       ast_log(LOG_ERROR, "There is no translation file for '%s'\n", pte->device->language);
+                       return str;
+               }
+               while (fgets(tmp, sizeof(tmp), f)) {
+                       if (!(p = strchr(tmp, '\n'))) {
+                               ast_log(LOG_ERROR, "Too long line found in language file - truncated!\n");
+                               continue;
+                       }
+                       *p = '\0';
+                       if (!(p = strchr(tmp, '"'))) {
+                               continue;
+                       }
+                       if (tmp == strstr(tmp, "msgid")) {
+                               p_orig = ast_strdup(p + 1);
+                               p = strchr(p_orig, '"');
+                       } else if (tmp == strstr(tmp, "msgstr")) {
+                               p_trans = ast_strdup(p + 1);
+                               p = strchr(p_trans, '"');
+                       } else {
+                               continue;
+                       }
+                       *p = '\0';
+                       if (!p_trans || !p_orig) {
+                               continue;
+                       }
+                       if (ast_strlen_zero(p_trans)) {
+                               ast_free(p_trans);
+                               ast_free(p_orig);
+                               p_trans = NULL;
+                               p_orig = NULL;
+                               continue;
+                       }
+                       if (!(lang_entry = ao2_alloc(sizeof(*lang_entry), NULL))) {
+                               fclose(f);
+                               return str;
+                       }
+
+                       lang_entry->str_trans = p_trans;
+                       lang_entry->str_orig = p_orig;
+                       ao2_link(lang->trans, lang_entry);
+                       p_trans = NULL;
+                       p_orig = NULL;
+               }
+
+               fclose(f);
+       }
+
+       le_search.str_orig = str;
+       if ((lang_entry = ao2_find(lang->trans, &le_search, OBJ_POINTER))) {
+               size = strlen(lang_entry->str_trans)+1;
+               if (size > 1024) {
+                       size = 1024;
+               }
+               memcpy(ustm_strcopy, lang_entry->str_trans, size);
+               ao2_ref(lang_entry, -1);
+               return ustm_strcopy;
+       }
+
+       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)
@@ -789,8 +903,9 @@ static void send_raw_client(int size, const unsigned char *data, struct sockaddr
        }
 #endif
 
-       if (sendmsg(unistimsock, &msg, 0) == -1)
+       if (sendmsg(unistimsock, &msg, 0) == -1) {
                display_last_error("Error sending datas");
+       }
 #else
        if (sendto(unistimsock, data, size, 0, (struct sockaddr *) addr_to, sizeof(*addr_to))
                == -1)
@@ -820,8 +935,9 @@ static void send_client(int size, const unsigned char *data, struct unistimsessi
        pte->timeout = tick + RETRANSMIT_TIMER;
 
 /*#ifdef DUMP_PACKET */
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(6, "Sending datas with seq #0x%.4x Using slot #%d :\n", pte->seq_server, buf_pos);
+       }
 /*#endif */
        send_raw_client(pte->wsabufsend[buf_pos].len, pte->wsabufsend[buf_pos].buf, &(pte->sin),
                                  &(pte->sout));
@@ -832,8 +948,9 @@ static void send_client(int size, const unsigned char *data, struct unistimsessi
 static void send_ping(struct unistimsession *pte)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(6, "Sending ping\n");
+       }
        pte->tick_next_ping = get_tick_count() + unistim_keepalive;
        memcpy(buffsend + SIZE_HEADER, packet_send_ping, sizeof(packet_send_ping));
        send_client(SIZE_HEADER + sizeof(packet_send_ping), buffsend, pte);
@@ -860,8 +977,9 @@ static int get_to_address(int fd, struct sockaddr_in *toAddr)
        msg.msg_controllen = sizeof(ip_msg);
        /* Get info about the incoming packet */
        err = recvmsg(fd, &msg, MSG_PEEK);
-       if (err == -1)
+       if (err == -1) {
                ast_log(LOG_WARNING, "recvmsg returned an error: %s\n", strerror(errno));
+       }
        memcpy(&toAddr->sin_addr, &ip_msg.address, sizeof(struct in_addr));
        return err;
 #else
@@ -893,11 +1011,6 @@ static struct unistimsession *create_client(const struct sockaddr_in *addr_from)
        sessions = s;
 
        s->timeout = get_tick_count() + RETRANSMIT_TIMER;
-       s->seq_phone = (short) 0x0000;
-       s->seq_server = (short) 0x0000;
-       s->last_seq_ack = (short) 0x000;
-       s->last_buf_available = 0;
-       s->nb_retransmit = 0;
        s->state = STATE_INIT;
        s->tick_next_ping = get_tick_count() + unistim_keepalive;
        /* Initialize struct wsabuf  */
@@ -911,8 +1024,9 @@ static struct unistimsession *create_client(const struct sockaddr_in *addr_from)
 static void send_end_call(struct unistimsession *pte)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending end call\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_end_call, sizeof(packet_send_end_call));
        send_client(SIZE_HEADER + sizeof(packet_send_end_call), buffsend, pte);
 }
@@ -932,48 +1046,54 @@ static void check_send_queue(struct unistimsession *pte)
 {
        /* Check if our send queue contained only one element */
        if (pte->last_buf_available == 1) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(6, "Our single packet was ACKed.\n");
+               }
                pte->last_buf_available--;
                set_ping_timer(pte);
                return;
        }
        /* Check if this ACK catch up our latest packet */
        else if (pte->last_seq_ack + 1 == pte->seq_server + 1) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(6, "Our send queue is completely ACKed.\n");
+               }
                pte->last_buf_available = 0;    /* Purge the send queue */
                set_ping_timer(pte);
                return;
        }
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(6, "We still have packets in our send queue\n");
+       }
        return;
 }
 
 static void send_start_timer(struct unistimsession *pte)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending start timer\n");
-       memcpy(buffsend + SIZE_HEADER, packet_send_StartTimer, sizeof(packet_send_StartTimer));
-       send_client(SIZE_HEADER + sizeof(packet_send_StartTimer), buffsend, pte);
+       }
+       memcpy(buffsend + SIZE_HEADER, packet_send_start_timer, sizeof(packet_send_start_timer));
+       send_client(SIZE_HEADER + sizeof(packet_send_start_timer), buffsend, pte);
 }
 
 static void send_stop_timer(struct unistimsession *pte)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending stop timer\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_stop_timer, sizeof(packet_send_stop_timer));
        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)
+       if (unistimdebug) {
                ast_verb(0, "Sending icon pos %d with status 0x%.2x\n", pos, status);
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_icon, sizeof(packet_send_icon));
        buffsend[9] = pos;
        buffsend[10] = status;
@@ -984,8 +1104,9 @@ static void send_tone(struct unistimsession *pte, uint16_t tone1, uint16_t tone2
 {
        BUFFSEND;
        if (!tone1) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Sending Stream Based Tone Off\n");
+               }
                memcpy(buffsend + SIZE_HEADER, packet_send_stream_based_tone_off,
                           sizeof(packet_send_stream_based_tone_off));
                send_client(SIZE_HEADER + sizeof(packet_send_stream_based_tone_off), buffsend, pte);
@@ -996,8 +1117,9 @@ static void send_tone(struct unistimsession *pte, uint16_t tone1, uint16_t tone2
           ast_verb(0, "Sending Stream Based Tone Cadence Download\n");
           memcpy (buffsend + SIZE_HEADER, packet_send_StreamBasedToneCad, sizeof (packet_send_StreamBasedToneCad));
           send_client (SIZE_HEADER + sizeof (packet_send_StreamBasedToneCad), buffsend, pte); */
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending Stream Based Tone Frequency Component List Download %d %d\n", tone1, tone2);
+       }
        tone1 *= 8;
        if (!tone2) {
                memcpy(buffsend + SIZE_HEADER, packet_send_stream_based_tone_single_freq,
@@ -1018,8 +1140,9 @@ static void send_tone(struct unistimsession *pte, uint16_t tone1, uint16_t tone2
                                   pte);
        }
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending Stream Based Tone On\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_stream_based_tone_on,
                   sizeof(packet_send_stream_based_tone_on));
        send_client(SIZE_HEADER + sizeof(packet_send_stream_based_tone_on), buffsend, pte);
@@ -1027,9 +1150,9 @@ static void send_tone(struct unistimsession *pte, uint16_t tone1, uint16_t tone2
 
 /* 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 */
@@ -1040,55 +1163,153 @@ send_favorite(unsigned char pos, unsigned char status, struct unistimsession *pt
        BUFFSEND;
        int i;
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending favorite pos %d with status 0x%.2x\n", pos, status);
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_favorite, sizeof(packet_send_favorite));
        buffsend[10] = pos;
        buffsend[24] = pos;
        buffsend[25] = status;
-       i = strlen(text);
-       if (i > FAV_MAX_LENGTH)
+       i = strlen(ustmtext(text, pte));
+       if (i > FAV_MAX_LENGTH) {
                i = FAV_MAX_LENGTH;
-       memcpy(buffsend + FAV_MAX_LENGTH + 1, text, i);
+       }
+       memcpy(buffsend + FAV_MAX_LENGTH + 1, ustmtext(text, pte), i);
        send_client(SIZE_HEADER + sizeof(packet_send_favorite), buffsend, pte);
 }
 
+static void send_favorite_short(unsigned char pos, unsigned char status, struct unistimsession *pte) {
+       send_favorite(pos, status, pte, pte->device->softkeylabel[pos]);
+       return;
+}
+
+static void send_favorite_selected(unsigned char status, struct unistimsession *pte) {
+       if (pte->device->selected != -1) {
+               send_favorite(pte->device->selected, status, pte, pte->device->softkeylabel[pte->device->selected]);
+       }
+       return;
+}
+
+static int soft_key_visible(struct unistim_device* d, unsigned char num)
+{
+       if(d->height == 1 && num % 3 == 2) {
+               return 0;
+       }
+       return 1;
+}
+
 static void refresh_all_favorite(struct unistimsession *pte)
 {
-       int i = 0;
+       unsigned char i = 0;
+       char data[256];
+       struct unistim_line *line;
+       line = AST_LIST_FIRST(&pte->device->lines);
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Refreshing all favorite\n");
-       for (i = 0; i < 6; i++) {
-               if ((pte->device->softkeyicon[i] <= FAV_ICON_HEADPHONES_ONHOLD) &&
-                       (pte->device->softkeylinepos != i))
-                       send_favorite((unsigned char) i, pte->device->softkeyicon[i] + 1, pte,
-                                                pte->device->softkeylabel[i]);
-               else
-                       send_favorite((unsigned char) i, pte->device->softkeyicon[i], pte,
-                                                pte->device->softkeylabel[i]);
+       }
+       for (i = 0; i < FAVNUM; i++) {
+               unsigned char status = pte->device->softkeyicon[i];
 
+               if (!soft_key_visible(pte->device, i)) {
+                       continue;
+               }
+               if (!strcasecmp(pte->device->softkeylabel[i], "DND") && line) {
+                       if (!ast_db_get("DND", line->name, data, sizeof(data))) {
+                               status = FAV_ICON_SPEAKER_ONHOOK_WHITE;
+                       }
+               }
+
+               send_favorite_short(i, status, pte);
+       }
+}
+
+static int is_key_favorite(struct unistim_device *d, int fav)
+{
+       if ((fav < 0) && (fav > 5)) {
+               return 0;
+       }
+       if (d->sline[fav]) {
+               return 0;
+       }
+       if (d->softkeynumber[fav][0] == '\0') {
+               return 0;
+       }
+       return 1;
+}
+
+static int is_key_line(struct unistim_device *d, int fav)
+{
+       if ((fav < 0) && (fav > 5)) {
+               return 0;
+       }
+       if (!d->sline[fav]) {
+               return 0;
+       }
+       if (is_key_favorite(d, fav)) {
+               return 0;
+       }
+       return 1;
+}
+
+static int get_active_softkey(struct unistimsession *pte)
+{
+       return pte->device->selected;
+}
+
+static int get_avail_softkey(struct unistimsession *pte, const char* name)
+{
+       int i;
+
+       if (!is_key_line(pte->device, pte->device->selected)) {
+               pte->device->selected = -1;
+       }
+       for (i = 0; i < FAVNUM; i++) {
+               if (pte->device->selected != -1 && pte->device->selected != i) {
+                       continue;
+               }
+               if (!soft_key_visible(pte->device, i)) {
+                       continue;
+               }
+               if (pte->device->ssub[i]) {
+                       continue;
+               }
+               if (is_key_line(pte->device, i)) {
+                       if (name && strcmp(name, pte->device->sline[i]->name)) {
+                               continue;
+                       }
+                       if (unistimdebug) {
+                               ast_verb(0, "Found softkey %d for device %s\n", i, name);
+                       }
+                       return i;
+               }
        }
+       return -1;
 }
 
+
 /* Change the status for this phone (pte) and update for each phones where pte is bookmarked
  * use FAV_ICON_*_BLACK constant in status parameters */
 static void change_favorite_icon(struct unistimsession *pte, unsigned char status)
 {
        struct unistim_device *d = devices;
        int i;
-       /* Update the current phone */
-       if (pte->state != STATE_CLEANING)
-               send_favorite(pte->device->softkeylinepos, status, pte,
-                                        pte->device->softkeylabel[pte->device->softkeylinepos]);
+       /* Update the current phone line softkey icon */
+       if (pte->state != STATE_CLEANING) {
+               int softkeylinepos = get_active_softkey(pte);
+               if (softkeylinepos != -1) {
+                       send_favorite_short(softkeylinepos, status, pte);
+               }
+       }
        /* Notify other phones if we're in their bookmark */
        while (d) {
-               for (i = 0; i < 6; i++) {
+               for (i = 0; i < FAVNUM; i++) {
                        if (d->sp[i] == pte->device) {  /* It's us ? */
                                if (d->softkeyicon[i] != status) {      /* Avoid resending the same icon */
                                        d->softkeyicon[i] = status;
-                                       if (d->session)
+                                       if (d->session) {
                                                send_favorite(i, status + 1, d->session, d->softkeylabel[i]);
+                                       }
                                }
                        }
                }
@@ -1096,72 +1317,94 @@ static void change_favorite_icon(struct unistimsession *pte, unsigned char statu
        }
 }
 
-static int RegisterExtension(const struct unistimsession *pte)
+static int register_extension(const struct unistimsession *pte)
 {
-       if (unistimdebug)
+       struct unistim_line *line;
+       line = AST_LIST_FIRST(&pte->device->lines);
+       if (unistimdebug) {
                ast_verb(0, "Trying to register extension '%s' into context '%s' to %s\n",
-                                       pte->device->extension_number, pte->device->lines->context,
-                                       pte->device->lines->fullname);
-       return ast_add_extension(pte->device->lines->context, 0,
+                                       pte->device->extension_number, pte->device->context,
+                                       line->fullname);
+       }
+       return ast_add_extension(pte->device->context, 0,
                                                         pte->device->extension_number, 1, NULL, NULL, "Dial",
-                                                        pte->device->lines->fullname, 0, "Unistim");
+                                                        line->fullname, 0, "Unistim");
 }
 
-static int UnregisterExtension(const struct unistimsession *pte)
+static int unregister_extension(const struct unistimsession *pte)
 {
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Trying to unregister extension '%s' context '%s'\n",
-                                       pte->device->extension_number, pte->device->lines->context);
-       return ast_context_remove_extension(pte->device->lines->context,
+                                       pte->device->extension_number, pte->device->context);
+       }
+       return ast_context_remove_extension(pte->device->context,
                                                                                pte->device->extension_number, 1, "Unistim");
 }
 
 /* Free memory allocated for a phone */
 static void close_client(struct unistimsession *s)
 {
-       struct unistim_subchannel *sub;
+       struct unistim_subchannel *sub = NULL;
        struct unistimsession *cur, *prev = NULL;
        ast_mutex_lock(&sessionlock);
        cur = sessions;
        /* Looking for the session in the linked chain */
        while (cur) {
-               if (cur == s)
+               if (cur == s) {
                        break;
+               }
                prev = cur;
                cur = cur->next;
        }
        if (cur) {                                    /* Session found ? */
                if (cur->device) {            /* This session was registered ? */
                        s->state = STATE_CLEANING;
-                       if (unistimdebug)
-                               ast_verb(0, "close_client session %p device %p lines %p sub %p\n",
-                                                       s, s->device, s->device->lines,
-                                                       s->device->lines->subs[SUB_REAL]);
+                       if (unistimdebug) {
+                               ast_verb(0, "close_client session %p device %p\n", s, s->device);
+                       }
                        change_favorite_icon(s, FAV_ICON_NONE);
-                       sub = s->device->lines->subs[SUB_REAL];
-                       if (sub) {
+                       ast_mutex_lock(&s->device->lock);
+                       AST_LIST_LOCK(&s->device->subs);
+                       AST_LIST_TRAVERSE_SAFE_BEGIN(&s->device->subs, sub, list) {
+                               if (!sub) {
+                                       continue;
+                               }
                                if (sub->owner) {       /* Call in progress ? */
-                                       if (unistimdebug)
+                                       if (unistimdebug) {
                                                ast_verb(0, "Aborting call\n");
+                                       }
                                        ast_queue_hangup_with_cause(sub->owner, AST_CAUSE_NETWORK_OUT_OF_ORDER);
+                               } else {
+                                       if (unistimdebug) {
+                                               ast_debug(1, "Released sub %d of channel %s@%s\n", sub->subtype, sub->parent->name, s->device->name);
+                                       }
+                                       AST_LIST_REMOVE_CURRENT(list);
+                                       unistim_free_sub(sub);
                                }
-                       } else
-                               ast_log(LOG_WARNING, "Freeing a client with no subchannel !\n");
-                       if (!ast_strlen_zero(s->device->extension_number))
-                               UnregisterExtension(s);
+                       }
+                       AST_LIST_TRAVERSE_SAFE_END;
+                       AST_LIST_UNLOCK(&s->device->subs);
+
+                       if (!ast_strlen_zero(s->device->extension_number)) {
+                               unregister_extension(s);
+                       }
                        cur->device->session = NULL;
+                       ast_mutex_unlock(&s->device->lock);
                } else {
-                       if (unistimdebug)
+                       if (unistimdebug) {
                                ast_verb(0, "Freeing an unregistered client\n");
+                       }
                }
-               if (prev)
+               if (prev) {
                        prev->next = cur->next;
-               else
+               } else {
                        sessions = cur->next;
+               }
                ast_mutex_destroy(&s->lock);
                ast_free(s);
-       } else
+       } else {
                ast_log(LOG_WARNING, "Trying to delete non-existent session %p?\n", s);
+       }
        ast_mutex_unlock(&sessionlock);
        return;
 }
@@ -1173,8 +1416,9 @@ static int send_retransmit(struct unistimsession *pte)
 
        ast_mutex_lock(&pte->lock);
        if (++pte->nb_retransmit >= NB_MAX_RETRANSMIT) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Too many retransmit - freeing client\n");
+               }
                ast_mutex_unlock(&pte->lock);
                close_client(pte);
                return 1;
@@ -1212,14 +1456,23 @@ send_text(unsigned char pos, unsigned char inverse, struct unistimsession *pte,
 {
        int i;
        BUFFSEND;
-       if (unistimdebug)
+       if (!text) {
+               ast_log(LOG_ERROR, "Asked to display NULL text (pos %d, inverse flag %d)\n", pos, inverse);
+               return;
+       }
+       if (pte->device && pte->device->height == 1 && pos != TEXT_LINE0) {
+               return;
+       }
+       if (unistimdebug) {
                ast_verb(0, "Sending text at pos %d, inverse flag %d\n", pos, inverse);
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_text, sizeof(packet_send_text));
        buffsend[10] = pos;
        buffsend[11] = inverse;
        i = strlen(text);
-       if (i > TEXT_LENGTH_MAX)
+       if (i > TEXT_LENGTH_MAX) {
                i = TEXT_LENGTH_MAX;
+       }
        memcpy(buffsend + 12, text, i);
        send_client(SIZE_HEADER + sizeof(packet_send_text), buffsend, pte);
 }
@@ -1228,8 +1481,9 @@ static void send_text_status(struct unistimsession *pte, const char *text)
 {
        BUFFSEND;
        int i;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending status text\n");
+       }
        if (pte->device) {
                if (pte->device->status_method == 1) {  /* For new firmware and i2050 soft phone */
                        int n = strlen(text);
@@ -1251,8 +1505,9 @@ static void send_text_status(struct unistimsession *pte, const char *text)
 
        memcpy(buffsend + SIZE_HEADER, packet_send_status, sizeof(packet_send_status));
        i = strlen(text);
-       if (i > STATUS_LENGTH_MAX)
+       if (i > STATUS_LENGTH_MAX) {
                i = STATUS_LENGTH_MAX;
+       }
        memcpy(buffsend + 10, text, i);
        send_client(SIZE_HEADER + sizeof(packet_send_status), buffsend, pte);
 
@@ -1265,8 +1520,9 @@ static void send_text_status(struct unistimsession *pte, const char *text)
 static void send_led_update(struct unistimsession *pte, unsigned char led)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending led_update (%x)\n", led);
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_led_update, sizeof(packet_send_led_update));
        buffsend[9] = led;
        send_client(SIZE_HEADER + sizeof(packet_send_led_update), buffsend, pte);
@@ -1280,67 +1536,78 @@ send_select_output(struct unistimsession *pte, unsigned char output, unsigned ch
                                 unsigned char mute)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending select output packet output=%x volume=%x mute=%x\n", output,
                                        volume, mute);
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_select_output,
                   sizeof(packet_send_select_output));
        buffsend[9] = output;
-       if (output == OUTPUT_SPEAKER)
+       if (output == OUTPUT_SPEAKER) {
                volume = VOLUME_LOW_SPEAKER;
-       else
+       } else {
                volume = VOLUME_LOW;
+       }
        buffsend[10] = volume;
-       if (mute == MUTE_ON_DISCRET)
+       if (mute == MUTE_ON_DISCRET) {
                buffsend[11] = MUTE_ON;
-       else
+       } else {
                buffsend[11] = mute;
+       }
        send_client(SIZE_HEADER + sizeof(packet_send_select_output), buffsend, pte);
-       if (mute == MUTE_OFF)
+       if (mute == MUTE_OFF) {
                send_led_update(pte, 0x18);
-       else if (mute == MUTE_ON)
+       } else if (mute == MUTE_ON) {
                send_led_update(pte, 0x19);
+       }
        pte->device->mute = mute;
        if (output == OUTPUT_HANDSET) {
-               if (mute == MUTE_ON)
+               if (mute == MUTE_ON) {
                        change_favorite_icon(pte, FAV_ICON_ONHOLD_BLACK);
-               else
+               } else {
                        change_favorite_icon(pte, FAV_ICON_OFFHOOK_BLACK);
+               }
                send_led_update(pte, 0x08);
                send_led_update(pte, 0x10);
        } else if (output == OUTPUT_HEADPHONE) {
-               if (mute == MUTE_ON)
+               if (mute == MUTE_ON) {
                        change_favorite_icon(pte, FAV_ICON_HEADPHONES_ONHOLD);
-               else
+               } else {
                        change_favorite_icon(pte, FAV_ICON_HEADPHONES);
+               }
                send_led_update(pte, 0x08);
                send_led_update(pte, 0x11);
        } else if (output == OUTPUT_SPEAKER) {
                send_led_update(pte, 0x10);
                send_led_update(pte, 0x09);
                if (pte->device->receiver_state == STATE_OFFHOOK) {
-                       if (mute == MUTE_ON)
+                       if (mute == MUTE_ON) {
                                change_favorite_icon(pte, FAV_ICON_SPEAKER_ONHOLD_BLACK);
-                       else
+                       } else {
                                change_favorite_icon(pte, FAV_ICON_SPEAKER_ONHOOK_BLACK);
+                       }
                } else {
-                       if (mute == MUTE_ON)
+                       if (mute == MUTE_ON) {
                                change_favorite_icon(pte, FAV_ICON_SPEAKER_ONHOLD_BLACK);
-                       else
+                       } else {
                                change_favorite_icon(pte, FAV_ICON_SPEAKER_OFFHOOK_BLACK);
+                       }
                }
-       } else
+       } else {
                ast_log(LOG_WARNING, "Invalid output (%d)\n", output);
-       if (output != pte->device->output)
+       }
+       if (output != pte->device->output) {
                pte->device->previous_output = pte->device->output;
+       }
        pte->device->output = output;
 }
 
 static void send_ring(struct unistimsession *pte, char volume, char style)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending ring packet\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_ring, sizeof(packet_send_ring));
        buffsend[24] = style + 0x10;
        buffsend[29] = volume * 0x10;
@@ -1350,8 +1617,9 @@ static void send_ring(struct unistimsession *pte, char volume, char style)
 static void send_no_ring(struct unistimsession *pte)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending no ring packet\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_no_ring, sizeof(packet_send_no_ring));
        send_client(SIZE_HEADER + sizeof(packet_send_no_ring), buffsend, pte);
 }
@@ -1360,25 +1628,33 @@ static void send_texttitle(struct unistimsession *pte, const char *text)
 {
        BUFFSEND;
        int i;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending title text\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_title, sizeof(packet_send_title));
        i = strlen(text);
-       if (i > 12)
+       if (i > 12) {
                i = 12;
+       }
        memcpy(buffsend + 10, text, i);
        send_client(SIZE_HEADER + sizeof(packet_send_title), buffsend, pte);
 
 }
 
+static void send_idle_clock(struct unistimsession *pte)
+{
+       send_text(TEXT_LINE0, TEXT_NORMAL, pte, "");
+}
+
 static void send_date_time(struct unistimsession *pte)
 {
        BUFFSEND;
        struct timeval now = ast_tvnow();
        struct ast_tm atm = { 0, };
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending Time & Date\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_date_time, sizeof(packet_send_date_time));
        ast_localtime(&now, &atm, NULL);
        buffsend[10] = (unsigned char) atm.tm_mon + 1;
@@ -1394,14 +1670,16 @@ static void send_date_time2(struct unistimsession *pte)
        struct timeval now = ast_tvnow();
        struct ast_tm atm = { 0, };
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending Time & Date #2\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_date_time2, sizeof(packet_send_date_time2));
        ast_localtime(&now, &atm, NULL);
-       if (pte->device)
+       if (pte->device) {
                buffsend[9] = pte->device->datetimeformat;
-       else
+       } else {
                buffsend[9] = 61;
+       }
        buffsend[14] = (unsigned char) atm.tm_mon + 1;
        buffsend[15] = (unsigned char) atm.tm_mday;
        buffsend[16] = (unsigned char) atm.tm_hour;
@@ -1415,8 +1693,9 @@ static void send_date_time3(struct unistimsession *pte)
        struct timeval now = ast_tvnow();
        struct ast_tm atm = { 0, };
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending Time & Date #3\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_date_time3, sizeof(packet_send_date_time3));
        ast_localtime(&now, &atm, NULL);
        buffsend[10] = (unsigned char) atm.tm_mon + 1;
@@ -1429,8 +1708,9 @@ static void send_date_time3(struct unistimsession *pte)
 static void send_blink_cursor(struct unistimsession *pte)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending set blink\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_blink_cursor, sizeof(packet_send_blink_cursor));
        send_client(SIZE_HEADER + sizeof(packet_send_blink_cursor), buffsend, pte);
        return;
@@ -1440,8 +1720,9 @@ static void send_blink_cursor(struct unistimsession *pte)
 static void send_cursor_pos(struct unistimsession *pte, unsigned char pos)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending set cursor position\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_set_pos_cursor,
                   sizeof(packet_send_set_pos_cursor));
        buffsend[11] = pos;
@@ -1449,6 +1730,44 @@ static void send_cursor_pos(struct unistimsession *pte, unsigned char pos)
        return;
 }
 
+static void send_charset_update(struct unistimsession *pte, int charset)
+{
+       const unsigned char* packet_send_charset;
+       int packet_size;
+       BUFFSEND;
+       if (unistimdebug) {
+               ast_verb(0, "Sending set default charset\n");
+       }
+       if (charset == LANG_DEFAULT) {
+               charset = options_languages[find_language(pte->device->language)].encoding;
+       }
+       switch (charset) {
+       case ISO_8859_2:
+               packet_send_charset = packet_send_charset_iso_8859_2;
+               packet_size = sizeof(packet_send_charset_iso_8859_2);
+               break;
+       case ISO_8859_4:
+               packet_send_charset = packet_send_charset_iso_8859_4;
+               packet_size = sizeof(packet_send_charset_iso_8859_4);
+               break;
+       case ISO_8859_5:
+               packet_send_charset = packet_send_charset_iso_8859_5;
+               packet_size = sizeof(packet_send_charset_iso_8859_5);
+               break;
+       case ISO_2022_JP:
+               packet_send_charset = packet_send_charset_iso_2022_jp;
+               packet_size = sizeof(packet_send_charset_iso_2022_jp);
+               break;
+       case ISO_8859_1:
+       default:
+               packet_send_charset = packet_send_charset_iso_8859_1;
+               packet_size = sizeof(packet_send_charset_iso_8859_1);
+       }
+       memcpy(buffsend + SIZE_HEADER, packet_send_charset, packet_size);
+       send_client(SIZE_HEADER + packet_size, buffsend, pte);
+       return;
+}
+
 static void rcv_resume_connection_with_server(struct unistimsession *pte)
 {
        BUFFSEND;
@@ -1474,7 +1793,6 @@ static int unistim_register(struct unistimsession *s)
                        s->device = d;
                        d->session = s;
                        d->codec_number = DEFAULT_CODEC;
-                       d->pos_fav = 0;
                        d->missed_call = 0;
                        d->receiver_state = STATE_ONHOOK;
                        break;
@@ -1483,8 +1801,9 @@ static int unistim_register(struct unistimsession *s)
        }
        ast_mutex_unlock(&devicelock);
 
-       if (!d)
+       if (!d) {
                return 0;
+       }
 
        return 1;
 }
@@ -1521,37 +1840,100 @@ static struct unistim_line *unistim_line_alloc(void)
        return l;
 }
 
-static int alloc_sub(struct unistim_line *l, int x)
+static int unistim_free_sub(struct unistim_subchannel *sub) {
+       if (unistimdebug) {
+               ast_debug(1, "Released sub %d of channel %s@%s\n", sub->subtype, sub->parent->name, sub->parent->parent->name);
+       }
+       ast_mutex_destroy(&sub->lock);
+       ast_free(sub);
+       return 0;
+}
+
+static struct unistim_subchannel *unistim_alloc_sub(struct unistim_device *d, int x)
 {
        struct unistim_subchannel *sub;
-       if (!(sub = ast_calloc(1, sizeof(*sub))))
-               return 0;
+       if (!(sub = ast_calloc(1, sizeof(*sub)))) {
+               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;
+       if (unistimdebug) {
+               ast_verb(3, "Allocating UNISTIM subchannel #%d on %s ptr=%p\n", x, d->name, sub);
+       }
+       sub->ss_thread = AST_PTHREADT_NULL;
        sub->subtype = x;
-       l->subs[x] = sub;
+       AST_LIST_LOCK(&d->subs);
+       AST_LIST_INSERT_TAIL(&d->subs, sub, list);
+       AST_LIST_UNLOCK(&d->subs);
        ast_mutex_init(&sub->lock);
-       return 1;
+       return sub;
 }
 
-static int unalloc_sub(struct unistim_line *p, int x)
+static int unistim_unalloc_sub(struct unistim_device *d, struct unistim_subchannel *sub)
 {
-       if (!x) {
-               ast_log(LOG_WARNING, "Trying to unalloc the real channel %s@%s?!?\n", p->name,
-                               p->parent->name);
-               return -1;
+       struct unistim_subchannel *s;
+
+       AST_LIST_LOCK(&d->subs);
+       AST_LIST_TRAVERSE_SAFE_BEGIN(&d->subs, s, list) {
+               if (!s) {
+                       continue;
+               }
+               if (s != sub) {
+                       continue;
+               }
+               AST_LIST_REMOVE_CURRENT(list);
+               unistim_free_sub(sub);
        }
-       if (unistimdebug)
-               ast_debug(1, "Released sub %d of channel %s@%s\n", x, p->name,
-                               p->parent->name);
-       ast_mutex_destroy(&p->lock);
-       ast_free(p->subs[x]);
-       p->subs[x] = 0;
+       AST_LIST_TRAVERSE_SAFE_END;
+       AST_LIST_UNLOCK(&d->subs);
        return 0;
 }
 
+static const char *subtype_tostr(const int type)
+{
+       switch (type) {
+       case SUB_REAL:
+               return "REAL";
+       case SUB_ONHOLD:
+               return "ONHOLD";
+       case SUB_RING:
+               return "RINGING";
+       case SUB_THREEWAY:
+               return "THREEWAY";
+       }
+       return "UNKNOWN";
+}
+
+static const char *ptestate_tostr(const int type)
+{
+       switch (type) {
+       case STATE_INIT:
+               return "INIT";
+       case STATE_AUTHDENY:
+               return "AUTHDENY";
+       case STATE_MAINPAGE:
+               return "MAINPAGE";
+       case STATE_EXTENSION:
+               return "EXTENSION";
+       case STATE_DIALPAGE:
+               return "DIALPAGE";
+       case STATE_RINGING:
+               return "RINGING";
+       case STATE_CALL:
+               return "CALL";
+       case STATE_SELECTOPTION:
+               return "SELECTOPTION";
+       case STATE_SELECTCODEC:
+               return "SELECTCODEC";
+       case STATE_SELECTLANGUAGE:
+               return "SELECTLANGUAGE";
+       case STATE_CLEANING:
+               return "CLEARING";
+       case STATE_HISTORY:
+               return "HISTORY";
+       }
+       return "UNKNOWN";
+}
+
 static void rcv_mac_addr(struct unistimsession *pte, const unsigned char *buf)
 {
        BUFFSEND;
@@ -1563,9 +1945,8 @@ static void rcv_mac_addr(struct unistimsession *pte, const unsigned char *buf)
                i += 2;
        }
        if (unistimdebug) {
-               ast_verb(0, "Mac Address received : %s\n", addrmac);
+               ast_verb(0, "MAC Address received: %s\n", addrmac);
        }
-
        strcpy(pte->macaddr, addrmac);
        res = unistim_register(pte);
        if (!res) {
@@ -1576,61 +1957,71 @@ static void rcv_mac_addr(struct unistimsession *pte, const unsigned char *buf)
                        break;
                case AUTOPROVISIONING_YES:
                        {
-                               struct unistim_device *d, *newd;
-                               struct unistim_line *newl;
-                               if (unistimdebug)
+                               struct unistim_device *d = NULL, *newd = NULL;
+                               struct unistim_line *newl = NULL, *l = NULL;
+                               if (unistimdebug) {
                                        ast_verb(0, "New phone, autoprovisioning on\n");
+                               }
                                /* First : locate the [template] section */
                                ast_mutex_lock(&devicelock);
                                d = devices;
                                while (d) {
-                                       if (!strcasecmp(d->name, "template")) {
-                                               /* Found, cloning this entry */
-                                               if (!(newd = ast_malloc(sizeof(*newd)))) {
-                                                       ast_mutex_unlock(&devicelock);
-                                                       return;
-                                               }
-
-                                               memcpy(newd, d, sizeof(*newd));
+                                       if (strcasecmp(d->name, "template")) {
+                                               d = d->next;
+                                               continue;
+                                       }
+                                       /* Found, cloning this entry */
+                                       if (!(newd = ast_malloc(sizeof(*newd)))) {
+                                               ast_mutex_unlock(&devicelock);
+                                               return;
+                                       }
+                                       memcpy(newd, d, sizeof(*newd));
+                                       ast_mutex_init(&newd->lock);
+                                       newd->lines.first = NULL;
+                                       newd->lines.last = NULL;
+                                       AST_LIST_LOCK(&d->lines);
+                                       AST_LIST_TRAVERSE(&d->lines, l, list) {
                                                if (!(newl = unistim_line_alloc())) {
-                                                       ast_free(newd);
-                                                       ast_mutex_unlock(&devicelock);
-                                                       return;
-                                               }
-
-                                               unistim_line_copy(d->lines, newl);
-                                               if (!alloc_sub(newl, SUB_REAL)) {
-                                                       ast_free(newd);
-                                                       unistim_line_destroy(newl);
-                                                       ast_mutex_unlock(&devicelock);
-                                                       return;
+                                                       break;
                                                }
-                                               /* Ok, now updating some fields */
-                                               ast_copy_string(newd->id, addrmac, sizeof(newd->id));
-                                               ast_copy_string(newd->name, addrmac, sizeof(newd->name));
-                                               if (newd->extension == EXTENSION_NONE)
-                                                       newd->extension = EXTENSION_ASK;
-                                               newd->lines = newl;
-                                               newd->receiver_state = STATE_ONHOOK;
-                                               newd->session = pte;
-                                               newd->to_delete = -1;
-                                               pte->device = newd;
-                                               newd->next = NULL;
+                                               unistim_line_copy(l, newl);
                                                newl->parent = newd;
-                                               strcpy(newl->name, d->lines->name);
-                                               snprintf(d->lines->name, sizeof(d->lines->name), "%d",
-                                                                atoi(d->lines->name) + 1);
+                                               ast_copy_string(newl->name, l->name, sizeof(newl->name));
                                                snprintf(newl->fullname, sizeof(newl->fullname), "USTM/%s@%s",
                                                                 newl->name, newd->name);
-                                               /* Go to the end of the linked chain */
-                                               while (d->next) {
-                                                       d = d->next;
-                                               }
-                                               d->next = newd;
-                                               d = newd;
-                                               break;
+                                               snprintf(l->name, sizeof(l->name), "%d", atoi(l->name) + 1);
+
+                                               AST_LIST_LOCK(&newd->lines);
+                                               AST_LIST_INSERT_TAIL(&newd->lines, newl, list);
+                                               AST_LIST_UNLOCK(&newd->lines);
+                                       }
+                                       AST_LIST_UNLOCK(&d->lines);
+                                       if (!newl) {
+                                               ast_free(newd);
+                                               ast_mutex_unlock(&devicelock);
+                                       }
+
+                                       /* Ok, now updating some fields */
+                                       ast_copy_string(newd->id, addrmac, sizeof(newd->id));
+                                       ast_copy_string(newd->name, addrmac, sizeof(newd->name));
+                                       if (newd->extension == EXTENSION_NONE) {
+                                               newd->extension = EXTENSION_ASK;
+                                       }
+
+                                       newd->receiver_state = STATE_ONHOOK;
+                                       newd->session = pte;
+                                       newd->language[0] = '\0';
+                                       newd->to_delete = -1;
+                                       newd->next = NULL;
+                                       pte->device = newd;
+
+                                       /* Go to the end of the linked chain */
+                                       while (d->next) {
+                                               d = d->next;
                                        }
-                                       d = d->next;
+                                       d->next = newd;
+                                       d = newd;
+                                       break;
                                }
                                ast_mutex_unlock(&devicelock);
                                if (!d) {
@@ -1642,40 +2033,54 @@ static void rcv_mac_addr(struct unistimsession *pte, const unsigned char *buf)
                case AUTOPROVISIONING_TN:
                        pte->state = STATE_AUTHDENY;
                        break;
-               case AUTOPROVISIONING_DB:
-                       ast_log(LOG_WARNING,
-                                       "Autoprovisioning with database is not yet functional\n");
-                       break;
                default:
                        ast_log(LOG_WARNING, "Internal error : unknown autoprovisioning value = %d\n",
                                        autoprovisioning);
                }
        }
        if (pte->state != STATE_AUTHDENY) {
+               struct unistim_line *line;
+               struct unistim_subchannel *sub;
+
                ast_verb(3, "Device '%s' successfuly registered\n", pte->device->name);
+
+               AST_LIST_LOCK(&pte->device->subs);
+               AST_LIST_TRAVERSE_SAFE_BEGIN(&pte->device->subs, sub, list) {
+                       if (sub) {
+                               ast_log(LOG_ERROR, "Subchannel lost sice reboot. Hanged channel may apear!\n");
+                               AST_LIST_REMOVE_CURRENT(list);
+                               ast_free(sub);
+                       }
+               }
+               AST_LIST_TRAVERSE_SAFE_END;
+               AST_LIST_UNLOCK(&pte->device->subs);
+
                switch (pte->device->extension) {
                case EXTENSION_NONE:
                        pte->state = STATE_MAINPAGE;
                        break;
                case EXTENSION_ASK:
                        /* Checking if we already have an extension number */
-                       if (ast_strlen_zero(pte->device->extension_number))
+                       if (ast_strlen_zero(pte->device->extension_number)) {
                                pte->state = STATE_EXTENSION;
-                       else {
+                       } 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
+                               } else {
                                        pte->state = STATE_MAINPAGE;
+                               }
                        }
                        break;
                case EXTENSION_LINE:
-                       ast_copy_string(pte->device->extension_number, pte->device->lines->name,
+                       line = AST_LIST_FIRST(&pte->device->lines);
+                       ast_copy_string(pte->device->extension_number, line->name,
                                                        sizeof(pte->device->extension_number));
-                       if (RegisterExtension(pte))
+                       if (register_extension(pte)) {
                                pte->state = STATE_EXTENSION;
-                       else
+                       } else {
                                pte->state = STATE_MAINPAGE;
+                       }
                        break;
                case EXTENSION_TN:
                        /* If we are here, it's because of a phone reboot */
@@ -1689,23 +2094,27 @@ static void rcv_mac_addr(struct unistimsession *pte, const unsigned char *buf)
                }
        }
        if (pte->state == STATE_EXTENSION) {
-               if (pte->device->extension != EXTENSION_TN)
+               if (pte->device->extension != EXTENSION_TN) {
                        pte->device->extension = EXTENSION_ASK;
+               }
                pte->device->extension_number[0] = '\0';
        }
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "\nSending S1\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_S1, sizeof(packet_send_S1));
        send_client(SIZE_HEADER + sizeof(packet_send_S1), buffsend, pte);
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending query_basic_manager_04\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_query_basic_manager_04,
                   sizeof(packet_send_query_basic_manager_04));
        send_client(SIZE_HEADER + sizeof(packet_send_query_basic_manager_04), buffsend, pte);
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending query_basic_manager_10\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_query_basic_manager_10,
                   sizeof(packet_send_query_basic_manager_10));
        send_client(SIZE_HEADER + sizeof(packet_send_query_basic_manager_10), buffsend, pte);
@@ -1745,10 +2154,12 @@ static int write_history(struct unistimsession *pte, char way, char ismissed)
        struct timeval now = ast_tvnow();
        struct ast_tm atm = { 0, };
 
-       if (!pte->device)
+       if (!pte->device) {
                return -1;
-       if (!pte->device->callhistory)
+       }
+       if (!pte->device->callhistory) {
                return 0;
+       }
        if (strchr(pte->device->name, '/') || (pte->device->name[0] == '.')) {
                ast_log(LOG_WARNING, "Account code '%s' insecure for writing file\n",
                                pte->device->name);
@@ -1757,20 +2168,20 @@ static int write_history(struct unistimsession *pte, char way, char ismissed)
 
        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");
-               else
-                       strcpy(tmp2, "Fail");
-       } else
-               strcpy(tmp2, "Answ");
+               if (way == 'i') {
+                       ast_copy_string(tmp2, ustmtext("Miss", pte), sizeof(tmp2));
+               } else {
+                       ast_copy_string(tmp2, ustmtext("Fail", pte), sizeof(tmp2));
+               }
+       } else {
+               ast_copy_string(tmp2, ustmtext("Answ", pte), sizeof(tmp2));
+       }
        snprintf(line1, sizeof(line1), "%04d/%02d/%02d %02d:%02d:%02d %s",
                         atm.tm_year + 1900, atm.tm_mon + 1, atm.tm_mday, atm.tm_hour,
                         atm.tm_min, atm.tm_sec, tmp2);
@@ -1822,8 +2233,9 @@ static int write_history(struct unistimsession *pte, char way, char ismissed)
                                return -1;
                        }
                }
-               if (fclose(f))
+               if (fclose(f)) {
                        display_last_error("Unable to close history - creation.");
+               }
                return 0;
        }
        /* We can open the log file, we create a temporary one, we add our entry and copy the rest */
@@ -1846,15 +2258,14 @@ static int write_history(struct unistimsession *pte, char way, char ismissed)
                return -1;
        }
 
-       if (++count > MAX_ENTRY_LOG)
+       if (++count > MAX_ENTRY_LOG) {
                count = MAX_ENTRY_LOG;
-
+       }
        if (write_entry_history(pte, f2, count, line1)) {
                fclose(f);
                fclose(f2);
                return -1;
        }
-
        size = (MAX_ENTRY_LOG - 1) * TEXT_LENGTH_MAX * 3;
        if (!(histbuf = ast_malloc(size))) {
                fclose(f);
@@ -1877,52 +2288,30 @@ static int write_history(struct unistimsession *pte, char way, char ismissed)
                return -1;
        }
        ast_free(histbuf);
-       if (fclose(f))
+       if (fclose(f)) {
                display_last_error("Unable to close history log.");
-       if (fclose(f2))
+       }
+       if (fclose(f2)) {
                display_last_error("Unable to close temporary history log.");
-       if (unlink(tmp))
+       }
+       if (unlink(tmp)) {
                display_last_error("Unable to remove old history log.");
-       if (rename(tmp2, tmp))
+       }
+       if (rename(tmp2, tmp)) {
                display_last_error("Unable to rename new history log.");
+       }
        return 0;
 }
 
-static void cancel_dial(struct unistimsession *pte)
-{
-       send_no_ring(pte);
-       pte->device->missed_call++;
-       write_history(pte, 'i', 1);
-       show_main_page(pte);
-       return;
-}
-
-static void swap_subs(struct unistim_line *p, int a, int b)
+static void unistim_quiet_chan(struct ast_channel *chan)
 {
-/*  struct ast_channel *towner; */
-       struct ast_rtp_instance *rtp;
-       int fds;
-
-       if (unistimdebug)
-               ast_verb(0, "Swapping %d and %d\n", a, b);
-
-       if ((!p->subs[a]->owner) || (!p->subs[b]->owner)) {
-               ast_log(LOG_WARNING,
-                               "Attempted to swap subchannels with a null owner : sub #%d=%p sub #%d=%p\n",
-                               a, p->subs[a]->owner, b, p->subs[b]->owner);
-               return;
+       if (chan && ast_channel_state(chan) == AST_STATE_UP) {
+               if (ast_test_flag(chan, AST_FLAG_MOH)) {
+                       ast_moh_stop(chan);
+               } else if (ast_channel_generatordata(chan)) {
+                       ast_deactivate_generator(chan);
+               }
        }
-       rtp = p->subs[a]->rtp;
-       p->subs[a]->rtp = p->subs[b]->rtp;
-       p->subs[b]->rtp = rtp;
-
-       fds = ast_channel_fd(p->subs[a]->owner, 0);
-       ast_channel_internal_fd_set(p->subs[a]->owner, 0, ast_channel_fd(p->subs[b]->owner, 0));
-       ast_channel_internal_fd_set(p->subs[b]->owner, 0, fds);
-
-       fds = ast_channel_fd(p->subs[a]->owner, 1);
-       ast_channel_internal_fd_set(p->subs[a]->owner, 1, ast_channel_fd(p->subs[b]->owner, 1));
-       ast_channel_internal_fd_set(p->subs[b]->owner, 1, fds);
 }
 
 static int attempt_transfer(struct unistim_subchannel *p1, struct unistim_subchannel *p2)
@@ -1930,7 +2319,7 @@ static int attempt_transfer(struct unistim_subchannel *p1, struct unistim_subcha
        int res = 0;
        struct ast_channel
         *chana = NULL, *chanb = NULL, *bridgea = NULL, *bridgeb = NULL, *peera =
-               NULL, *peerb = NULL, *peerc = NULL;
+               NULL, *peerb = NULL, *peerc = NULL, *peerd = NULL;
 
        if (!p1->owner || !p2->owner) {
                ast_log(LOG_WARNING, "Transfer attempted without dual ownership?\n");
@@ -1945,17 +2334,21 @@ static int attempt_transfer(struct unistim_subchannel *p1, struct unistim_subcha
                peera = chana;
                peerb = chanb;
                peerc = bridgea;
+               peerd = bridgeb;
        } else if (bridgeb) {
                peera = chanb;
                peerb = chana;
                peerc = bridgeb;
+               peerd = bridgea;
        }
 
        if (peera && peerb && peerc && (peerb != peerc)) {
-               /*ast_quiet_chan(peera);
-                  ast_quiet_chan(peerb);
-                  ast_quiet_chan(peerc);
-                  ast_quiet_chan(peerd); */
+               unistim_quiet_chan(peera);
+               unistim_quiet_chan(peerb);
+               unistim_quiet_chan(peerc);
+               if (peerd) {
+                       unistim_quiet_chan(peerd);
+               }
 
                if (ast_channel_cdr(peera) && ast_channel_cdr(peerb)) {
                        ast_channel_cdr_set(peerb, ast_cdr_append(ast_channel_cdr(peerb), ast_channel_cdr(peera)));
@@ -1971,6 +2364,7 @@ static int attempt_transfer(struct unistim_subchannel *p1, struct unistim_subcha
                }
                ast_channel_cdr_set(peerc, NULL);
 
+               ast_log(LOG_NOTICE, "UNISTIM transfer: trying to masquerade %s into %s\n", ast_channel_name(peerc), ast_channel_name(peerb));
                if (ast_channel_masquerade(peerb, peerc)) {
                        ast_log(LOG_WARNING, "Failed to masquerade %s into %s\n", ast_channel_name(peerb),
                                        ast_channel_name(peerc));
@@ -1980,10 +2374,12 @@ static int attempt_transfer(struct unistim_subchannel *p1, struct unistim_subcha
        } else {
                ast_log(LOG_NOTICE,
                                "Transfer attempted with no appropriate bridged calls to transfer\n");
-               if (chana)
+               if (chana) {
                        ast_softhangup_nolock(chana, AST_SOFTHANGUP_DEV);
-               if (chanb)
+               }
+               if (chanb) {
                        ast_softhangup_nolock(chanb, AST_SOFTHANGUP_DEV);
+               }
                return -1;
        }
        return 0;
@@ -1994,44 +2390,144 @@ void change_callerid(struct unistimsession *pte, int type, char *callerid)
        char *data;
        int size;
 
-       if (type)
+       if (type) {
                data = pte->device->lst_cnm;
-       else
+       } else {
                data = pte->device->lst_cid;
+       }
 
        /* This is very nearly strncpy(), except that the remaining buffer
         * is padded with ' ', instead of '\0' */
        memset(data, ' ', TEXT_LENGTH_MAX);
        size = strlen(callerid);
-       if (size > TEXT_LENGTH_MAX)
+       if (size > TEXT_LENGTH_MAX) {
                size = TEXT_LENGTH_MAX;
+       }
        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_stop_timer(pte);
+       if (sub->owner) {
+               ast_queue_control_data(sub->owner, AST_CONTROL_HOLD, NULL, 0);
+               send_end_call(pte);
+       }
+       return;
+}
+
+static void sub_unhold(struct unistimsession *pte, struct unistim_subchannel *sub)
+{
+       struct unistim_subchannel *sub_real;
+
+       sub_real = get_sub(pte->device, SUB_REAL);
+       if (sub_real) {
+           sub_hold(pte, sub_real);
+       }
+
+       sub->moh = 0;
+       sub->subtype = SUB_REAL;
+       send_favorite_short(sub->softkey, FAV_ICON_OFFHOOK_BLACK, pte);
+       send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
+       send_start_timer(pte);
+       if (sub->owner) {
+               ast_queue_control_data(sub->owner, AST_CONTROL_UNHOLD, NULL, 0);
+               if (sub->rtp) {
+                       send_start_rtp(sub);
+               }
+       }
+       return;
+}
+
 static void close_call(struct unistimsession *pte)
 {
-       struct unistim_subchannel *sub;
-       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;
+                       if (attempt_transfer(sub, sub_transf) < 0) {
                                ast_verb(0, "attempt_transfer failed.\n");
-               } else
+                       }
+               } else {
                        ast_queue_hangup(sub->owner);
+               }
        } else {
-               if (l->subs[SUB_THREEWAY]) {
-                       if (l->subs[SUB_THREEWAY]->owner)
-                               ast_queue_hangup_with_cause(l->subs[SUB_THREEWAY]->owner, AST_CAUSE_NORMAL_CLEARING);
-                       else
+               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);
+                                               pte->device->name, sub->softkey);
+               }
        }
        change_callerid(pte, 0, pte->device->redial_number);
        change_callerid(pte, 1, "");
@@ -2041,12 +2537,24 @@ 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);
        return;
 }
 
+static void discard_call(struct unistimsession *pte)
+{
+       struct unistim_subchannel* sub;
+       sub = get_sub(pte->device, SUB_RING);
+       if (!sub) {
+           return;
+       }
+
+       ast_queue_hangup_with_cause(sub->owner, AST_CAUSE_NORMAL_CLEARING);
+       return;
+}
+
 static void *unistim_ss(void *data)
 {
        struct ast_channel *chan = data;
@@ -2055,7 +2563,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_channel_exten_set(chan, s->device->phone_number);
        ast_copy_string(s->device->redial_number, s->device->phone_number,
                                        sizeof(s->device->redial_number));
@@ -2063,90 +2571,59 @@ 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)
 {
-       BUFFSEND;
+       struct unistim_subchannel *sub = NULL;
+       int rtp_start = s->parent->parent->rtp_port;
+       struct ast_sockaddr us_tmp;
        struct sockaddr_in us = { 0, };
+
+       AST_LIST_LOCK(&s->parent->parent->subs);
+       AST_LIST_TRAVERSE(&s->parent->parent->subs, sub, list) {
+               if (!sub) {
+                       continue;
+               }
+               if (sub->rtp) {
+                       ast_rtp_instance_get_remote_address(sub->rtp, &us_tmp);
+                       ast_sockaddr_to_sin(&us_tmp, &us);
+                       if (htons(us.sin_port)) {
+                               rtp_start = htons(us.sin_port) + 1;
+                               break;
+                       }
+               }
+       }
+       AST_LIST_UNLOCK(&s->parent->parent->subs);
+       return rtp_start;
+}
+
+static void send_start_rtp(struct unistim_subchannel *sub)
+{
+       BUFFSEND;
+
+       int codec;
        struct sockaddr_in public = { 0, };
+       struct sockaddr_in us = { 0, };
        struct sockaddr_in sin = { 0, };
-       int 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_rtp_instance_get_local_address(sub->rtp, &us_tmp);
+       ast_sockaddr_to_sin(&us_tmp, &us);
+       ast_rtp_instance_get_remote_address(sub->rtp, &sin_tmp);
+       ast_sockaddr_to_sin(&sin_tmp, &sin);
 
-       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) {
-               ast_channel_internal_fd_set(sub->owner, 0, ast_rtp_instance_fd(sub->rtp, 0));
-               ast_channel_internal_fd_set(sub->owner, 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 (!(ast_format_cap_iscompatible(ast_channel_nativeformats(sub->owner), ast_channel_readformat(sub->owner)))) {
-               struct ast_format tmpfmt;
-               char tmp[256];
-               ast_best_codec(ast_channel_nativeformats(sub->owner), &tmpfmt);
-               ast_log(LOG_WARNING,
-                               "Our read/writeformat has been changed to something incompatible: %s, using %s best codec from %s\n",
-                               ast_getformatname(ast_channel_readformat(sub->owner)),
-                               ast_getformatname(&tmpfmt),
-                               ast_getformatname_multiple(tmp, sizeof(tmp), ast_channel_nativeformats(sub->owner)));
-               ast_format_copy(ast_channel_readformat(sub->owner), &tmpfmt);
-               ast_format_copy(ast_channel_writeformat(sub->owner), &tmpfmt);
-       }
-       codec = ast_rtp_codecs_payload_code(ast_rtp_instance_get_codecs(sub->rtp), 1, ast_channel_readformat(sub->owner), 0);
        /* Setting up RTP of the phone */
-       if (public_ip.sin_family == 0)  /* NAT IP override ?   */
+       if (public_ip.sin_family == 0) {  /* NAT IP override ?   */
                memcpy(&public, &us, sizeof(public));   /* No defined, using IP from recvmsg  */
-       else
+       } else {
                memcpy(&public, &public_ip, sizeof(public));    /* override  */
+       }
        if (unistimdebug) {
                ast_verb(0, "RTP started : Our IP/port is : %s:%hd with codec %s\n",
                         ast_inet_ntoa(us.sin_addr),
@@ -2154,35 +2631,39 @@ 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));
        }
+
+       pte = sub->parent->parent->session;
+       codec = ast_rtp_codecs_payload_code(ast_rtp_instance_get_codecs(sub->rtp), 1, ast_channel_readformat(sub->owner), 0);
        if ((ast_channel_readformat(sub->owner)->id == AST_FORMAT_ULAW) ||
                (ast_channel_readformat(sub->owner)->id == AST_FORMAT_ALAW)) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Sending packet_send_rtp_packet_size for codec %d\n", codec);
+               }
                memcpy(buffsend + SIZE_HEADER, packet_send_rtp_packet_size,
                           sizeof(packet_send_rtp_packet_size));
                buffsend[10] = (int) codec & 0xffffffffLL;
-               send_client(SIZE_HEADER + sizeof(packet_send_rtp_packet_size), buffsend,
-                                  sub->parent->parent->session);
+               send_client(SIZE_HEADER + sizeof(packet_send_rtp_packet_size), buffsend, pte);
        }
-       if (unistimdebug)
+       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)
+               if (unistimdebug) {
+                       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
+               } 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);
@@ -2201,18 +2682,19 @@ 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)
+               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
+               } 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);
@@ -2231,13 +2713,13 @@ 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 */
 
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Sending packet_send_call default method\n");
+               }
 
                memcpy(buffsend + SIZE_HEADER, packet_send_call, sizeof(packet_send_call));
                memcpy(buffsend + 53, &public.sin_addr, sizeof(public.sin_addr));
@@ -2250,300 +2732,492 @@ static void start_rtp(struct unistim_subchannel *sub)
                /* Codec */
                buffsend[40] = codec;
                buffsend[41] = codec;
-               if (ast_channel_readformat(sub->owner)->id == AST_FORMAT_ULAW)
+               if (ast_channel_readformat(sub->owner)->id == AST_FORMAT_ULAW) {
                        buffsend[42] = 1;       /* 1 = 20ms (160 bytes), 2 = 40ms (320 bytes) */
-               else if (ast_channel_readformat(sub->owner)->id == AST_FORMAT_ALAW)
+               } else if (ast_channel_readformat(sub->owner)->id == AST_FORMAT_ALAW) {
                        buffsend[42] = 1;       /* 1 = 20ms (160 bytes), 2 = 40ms (320 bytes) */
-               else if (ast_channel_readformat(sub->owner)->id == AST_FORMAT_G723_1)
+               } else if (ast_channel_readformat(sub->owner)->id == AST_FORMAT_G723_1) {
                        buffsend[42] = 2;       /* 1 = 30ms (24 bytes), 2 = 60 ms (48 bytes) */
-               else if (ast_channel_readformat(sub->owner)->id == AST_FORMAT_G729A)
+               } else if (ast_channel_readformat(sub->owner)->id == AST_FORMAT_G729A) {
                        buffsend[42] = 2;       /* 1 = 10ms (10 bytes), 2 = 20ms (20 bytes) */
-               else
+               } else {
                        ast_log(LOG_WARNING, "Unsupported codec %s!\n",
                                        ast_getformatname(ast_channel_readformat(sub->owner)));
+               }
                /* Source port for transmit RTP and Destination port for receiving RTP */
                buffsend[45] = (htons(sin.sin_port) & 0xff00) >> 8;
                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 (!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;
        }
-       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->owner) {
+               ast_log(LOG_WARNING, "start_rtp with a null asterisk channel!\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;
        }
-       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);
+       ast_rtp_instance_set_prop(sub->rtp, AST_RTP_PROPERTY_RTCP, 1);
+       ast_channel_internal_fd_set(sub->owner, 0, ast_rtp_instance_fd(sub->rtp, 0));
+       ast_channel_internal_fd_set(sub->owner, 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 (!ast_format_cap_iscompatible(ast_channel_nativeformats(sub->owner), ast_channel_readformat(sub->owner))) {
+               struct ast_format tmpfmt;
+               char tmp[256];
+               ast_best_codec(ast_channel_nativeformats(sub->owner), &tmpfmt);
+               ast_log(LOG_WARNING,
+                               "Our read/writeformat has been changed to something incompatible: %s, using %s best codec from %s\n",
+                               ast_getformatname(ast_channel_readformat(sub->owner)),
+                               ast_getformatname(&tmpfmt),
+                               ast_getformatname_multiple(tmp, sizeof(tmp), ast_channel_nativeformats(sub->owner)));
+
+                ast_format_copy(ast_channel_readformat(sub->owner), &tmpfmt);
+                ast_format_copy(ast_channel_writeformat(sub->owner), &tmpfmt);
+       }
+       send_start_rtp(sub);
+       ast_mutex_unlock(&sub->lock);
+}
+
+static void send_dial_tone(struct unistimsession *pte)
+{
+       struct ast_tone_zone_sound *ts = NULL;
+       struct ast_tone_zone_part tone_data;
+       char *s = NULL;
+       char *ind;
+
+       if ((ts = ast_get_indication_tone(pte->device->tz, "dial"))) {
+               ind = ast_strdupa(ts->data);
+               s = strsep(&ind, ",");
+               ast_tone_zone_part_parse(s, &tone_data);
+               if (tone_data.modulate) {
+                       tone_data.freq2 = 0;
                }
-               i++;
+               send_tone(pte, tone_data.freq1, tone_data.freq2);
+               if (unistimdebug) {
+                       ast_verb(0, "Country code found (%s), freq1=%d freq2=%d\n",
+                                                       pte->device->tz->country, tone_data.freq1, tone_data.freq2);
+               }
+               ts = ast_tone_zone_sound_unref(ts);
        }
 }
 
+static void show_phone_number(struct unistimsession *pte)
+{
+       char tmp[TEXT_LENGTH_MAX + 1];
+       const char *tmp_number = ustmtext("Number:", pte);
+       int line, tmp_copy, offset = 0, i;
+
+       pte->device->phone_number[pte->device->size_phone_number] = '\0';
+       if  (pte->device->size_phone_number > MAX_SCREEN_NUMBER) {
+               offset = pte->device->size_phone_number - MAX_SCREEN_NUMBER - 1;
+               if (offset > strlen(tmp_number)) {
+                       offset = strlen(tmp_number);
+               }
+               tmp_copy = strlen(tmp_number) - offset + 1;
+               if (tmp_copy > sizeof(tmp)) {
+                       tmp_copy = sizeof(tmp);
+               }
+               memcpy(tmp, tmp_number + offset, tmp_copy);
+       } else {
+               ast_copy_string(tmp, tmp_number, sizeof(tmp));
+       }
+
+       offset = (pte->device->size_phone_number >= TEXT_LENGTH_MAX) ? (pte->device->size_phone_number - TEXT_LENGTH_MAX +1) : 0;
+       if (pte->device->size_phone_number) {
+               memcpy(tmp + strlen(tmp), pte->device->phone_number + offset, pte->device->size_phone_number - offset + 1);
+       }
+       offset = strlen(tmp);
+
+       for (i = strlen(tmp); i < TEXT_LENGTH_MAX; i++) {
+               tmp[i] = '.';
+       }
+       tmp[i] = '\0';
+
+       line = (pte->device->height == 1) ? TEXT_LINE0 : TEXT_LINE2;
+       send_text(line, TEXT_NORMAL, pte, tmp);
+       send_blink_cursor(pte);
+       send_cursor_pos(pte, (unsigned char) (line + offset));
+       send_led_update(pte, 0);
+}
+
 static void handle_dial_page(struct unistimsession *pte)
 {
        pte->state = STATE_DIALPAGE;
        if (pte->device->call_forward[0] == -1) {
                send_text(TEXT_LINE0, TEXT_NORMAL, pte, "");
-               send_text(TEXT_LINE1, TEXT_NORMAL, pte, "Enter forward");
-               send_text_status(pte, "ForwardCancel BackSpcErase");
+               send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("Enter forward", pte));
+               send_text_status(pte, ustmtext("Fwd    Cancel BackSp Erase", pte));
                if (pte->device->call_forward[1] != 0) {
-                       char tmp[TEXT_LENGTH_MAX + 1];
-
                        ast_copy_string(pte->device->phone_number, pte->device->call_forward + 1,
                                                        sizeof(pte->device->phone_number));
-                       pte->device->size_phone_number = strlen(pte->device->phone_number);
-                       if (pte->device->size_phone_number > 15)
-                               pte->device->size_phone_number = 15;
-                       strcpy(tmp, "Number : ...............");
-                       memcpy(tmp + 9, pte->device->phone_number, pte->device->size_phone_number);
-
-                       if (pte->device->height == 1) {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmp);
-                               send_blink_cursor(pte);
-                               send_cursor_pos(pte,
-                                                 (unsigned char) (TEXT_LINE0 + 0x09 +
-                                                                                  pte->device->size_phone_number));
-                       } else {
-                               send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmp);
-                               send_blink_cursor(pte);
-                               send_cursor_pos(pte,
-                                                 (unsigned char) (TEXT_LINE2 + 0x09 +
-                                                                                  pte->device->size_phone_number));
-                       }
-
+                       show_phone_number(pte);
                        send_led_update(pte, 0);
                        return;
                }
        } else {
                if ((pte->device->output == OUTPUT_HANDSET) &&
-                       (pte->device->receiver_state == STATE_ONHOOK))
+                       (pte->device->receiver_state == STATE_ONHOOK)) {
                        send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
-               else
+               } else {
                        send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
-               SendDialTone(pte);
+               }
+               send_dial_tone(pte);
 
                if (pte->device->height > 1) {
-                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Enter the number to dial");
-                       send_text(TEXT_LINE1, TEXT_NORMAL, pte, "and press Call");
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Enter the number to dial", pte));
+                       send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("and press Call", pte));
                }
-               send_text_status(pte, "Call   Redial BackSpcErase");
+               send_text_status(pte, ustmtext("Call   Redial BackSp Erase", pte));
        }
 
-       if (pte->device->height == 1) {
-               send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Number : ...............");
-               send_blink_cursor(pte);
-               send_cursor_pos(pte, TEXT_LINE0 + 0x09);
-       } else {
-               send_text(TEXT_LINE2, TEXT_NORMAL, pte, "Number : ...............");
-               send_blink_cursor(pte);
-               send_cursor_pos(pte, TEXT_LINE2 + 0x09);
-       }
        pte->device->size_phone_number = 0;
        pte->device->phone_number[0] = 0;
+       show_phone_number(pte);
        change_favorite_icon(pte, FAV_ICON_PHONE_BLACK);
-       Sendicon(TEXT_LINE0, FAV_ICON_NONE, pte);
+       send_icon(TEXT_LINE0, FAV_ICON_NONE, pte);
        pte->device->missed_call = 0;
        send_led_update(pte, 0);
        return;
 }
 
+static void swap_subs(struct unistim_subchannel *a, struct unistim_subchannel *b)
+{
+       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 = ast_channel_fd(a->owner, 0);
+       ast_channel_internal_fd_set(a->owner, 0, ast_channel_fd(b->owner, 0));
+       ast_channel_internal_fd_set(b->owner, 0, fds);
+
+       fds = ast_channel_fd(a->owner, 1);
+       ast_channel_internal_fd_set(a->owner, 1, ast_channel_fd(b->owner, 1));
+       ast_channel_internal_fd_set(b->owner, 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 (unistimdebug)
-                       ast_verb(0, "Transfer canceled, hangup our threeway channel\n");
-               if (p->subs[SUB_THREEWAY]->owner)
-                       ast_queue_hangup_with_cause(p->subs[SUB_THREEWAY]->owner, AST_CAUSE_NORMAL_CLEARING);
-               else
-                       ast_log(LOG_WARNING, "Canceling a threeway channel without owner\n");
-               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;
+       } else {
+               if (ast_bridged_channel(sub->owner)) {
+                       ast_moh_start(ast_bridged_channel(sub->owner),
+                                                 sub->parent->musicclass, NULL);
+                       sub->moh = 1;
+                       sub->subtype = SUB_THREEWAY;
                } else {
                        ast_log(LOG_WARNING, "Unable to find peer subchannel for music on hold\n");
                        return;
                }
        }
-       /* Silence our channel */
-       if (!pte->device->silence_generator) {
-               pte->device->silence_generator =
-                       ast_channel_start_silence_generator(p->subs[SUB_REAL]->owner);
-               if (pte->device->silence_generator == NULL)
-                       ast_log(LOG_WARNING, "Unable to start a silence generator.\n");
-               else if (unistimdebug)
-                       ast_verb(0, "Starting silence generator\n");
-       }
+       sub_start_silence(pte, sub);
        handle_dial_page(pte);
 }
 
+static void transfer_cancel_step2(struct unistimsession *pte)
+{
+       struct unistim_subchannel *sub, *sub_trans;
+       struct unistim_device *d = pte->device;
+
+       sub = get_sub(d, SUB_REAL);
+       sub_trans = get_sub(d, SUB_THREEWAY);
+
+       if (!sub || !sub->owner) {
+               ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
+               return;
+       }
+       if (sub_trans) {
+               if (unistimdebug) {
+                       ast_verb(0, "Transfer canceled, hangup our threeway channel\n");
+               }
+               if (sub->owner) {
+                       swap_subs(sub, sub_trans);
+                       ast_moh_stop(ast_bridged_channel(sub_trans->owner));
+                       sub_trans->moh = 0;
+                       sub_trans->subtype = SUB_REAL;
+                       sub->subtype = SUB_THREEWAY;
+                       ast_queue_hangup_with_cause(sub->owner, AST_CAUSE_NORMAL_CLEARING);
+               } else {
+                       ast_log(LOG_WARNING, "Canceling a threeway channel without owner\n");
+               }
+               return;
+       }
+}
+
 /* From phone to PBX */
-static void HandleCallOutgoing(struct unistimsession *s)
+static void handle_call_outgoing(struct unistimsession *s)
 {
        struct ast_channel *c;
        struct unistim_subchannel *sub;
-       pthread_t t;
+       int softkey;
+
        s->state = STATE_CALL;
-       sub = s->device->lines->subs[SUB_REAL];
-       if (!sub) {
-               ast_log(LOG_NOTICE, "No available lines on: %s\n", s->device->name);
+
+       sub = get_sub(s->device, SUB_THREEWAY);
+       if (sub) {
+               /* If sub for threway call created than we use transfer behaviuor */
+               struct unistim_subchannel *sub_trans = NULL;
+               struct unistim_device *d = s->device;
+
+               sub_trans = get_sub(d, SUB_REAL);
+               if (sub_trans) {
+                       ast_log(LOG_WARNING, "Can't transfer while active subchannel exists!\n");
+                       return;
+               }
+               if (!sub->owner) {
+                       ast_log(LOG_WARNING, "Unable to find subchannel with music on hold\n");
+                       return;
+               }
+
+               sub_trans = unistim_alloc_sub(d, SUB_REAL);
+               if (!sub_trans) {
+                       ast_log(LOG_WARNING, "Unable to allocate three-way subchannel\n");
+                       return;
+               }
+               sub_trans->parent = sub->parent;
+               sub_stop_silence(s, sub);
+               send_tone(s, 0, 0);
+               /* Make new channel */
+               c = unistim_new(sub_trans, AST_STATE_DOWN, NULL);
+               if (!c) {
+                       ast_log(LOG_WARNING, "Cannot allocate new structure on channel %p\n", sub->parent);
+                       return;
+               }
+               /* Swap things around between the three-way and real call */
+               swap_subs(sub, sub_trans);
+               send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
+               if (s->device->height == 1) {
+                       send_text(TEXT_LINE0, TEXT_NORMAL, s, s->device->phone_number);
+               } else {
+                       send_text(TEXT_LINE0, TEXT_NORMAL, s, ustmtext("Calling (pre-transfer)", s));
+                       send_text(TEXT_LINE1, TEXT_NORMAL, s, s->device->phone_number);
+                       send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("Dialing...", s));
+               }
+               send_text_status(s, ustmtext("TransfrCancel", s));
+
+               if (ast_pthread_create(&sub->ss_thread, NULL, unistim_ss, c)) {
+                       ast_log(LOG_WARNING, "Unable to start simple switch on channel %p\n", c);
+                       sub->ss_thread = AST_PTHREADT_NULL;
+                       ast_hangup(c);
+                       return;
+               }
+               if (unistimdebug) {
+                       ast_verb(0, "Started three way call on channel %p (%s) subchan %d\n",
+                                sub_trans->owner, ast_channel_name(sub_trans->owner), 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 */
+       }
+       if (!(sub = unistim_alloc_sub(s->device, SUB_REAL))) {
+               ast_log(LOG_WARNING, "Unable to allocate subchannel!\n");
+               return;     
+       }
+       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);
+               if (!sub->rtp) { /* Need to start RTP before calling ast_pbx_run */
+                       start_rtp(sub);
+               }
+               if (c && !strcmp(s->device->phone_number, ast_pickup_ext())) {
+                       if (unistimdebug) {
+                               ast_verb(0, "Try to pickup in unistim_new\n");
+                       }
+                       send_text(TEXT_LINE0, TEXT_NORMAL, s, "");
+                       send_text_status(s, ustmtext("       Transf        Hangup", s));
+                       send_start_timer(s);
+                       if (ast_pickup_call(c)) {
+                               ast_log(LOG_NOTICE, "Nothing to pick up\n");
+                               ast_channel_hangupcause_set(c, AST_CAUSE_CALL_REJECTED);
+                       } else {
+                               ast_channel_hangupcause_set(c, AST_CAUSE_NORMAL_CLEARING);
+                       }
+                       ast_hangup(c);
+                       c = NULL;
+                } else if (c) {
                        send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
-
+                       send_tone(s, 0, 0); /* Dialing empty number should also stop dial tone */
                        if (s->device->height == 1) {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, s, s->device->phone_number);
+                               if (strlen(s->device->phone_number) > 0) {
+                                       send_text(TEXT_LINE0, TEXT_NORMAL, s, s->device->phone_number);
+                               } else {
+                                       send_text(TEXT_LINE0, TEXT_NORMAL, s, ustmtext("Calling...", s));
+                               }
                        } else {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, s, "Calling :");
+                               send_text(TEXT_LINE0, TEXT_NORMAL, s, ustmtext("Calling :", s));
                                send_text(TEXT_LINE1, TEXT_NORMAL, s, s->device->phone_number);
-                               send_text(TEXT_LINE2, TEXT_NORMAL, s, "Dialing...");
+                               send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("Dialing...", s));
                        }
-                       send_text_status(s, "Hangup");
+                       send_text_status(s, ustmtext("                     Hangup", s));
 
                        /* start switch */
-                       if (ast_pthread_create(&t, NULL, unistim_ss, c)) {
-                               display_last_error("Unable to create switch thread");
+                       if (ast_pthread_create(&sub->ss_thread, NULL, unistim_ss, c)) {
+                               ast_log(LOG_WARNING, "Unable to create switch thread\n");
+                               sub->ss_thread = AST_PTHREADT_NULL;
                                ast_queue_hangup_with_cause(c, AST_CAUSE_SWITCH_CONGESTION);
                        }
                } else
                        ast_log(LOG_WARNING, "Unable to create channel for %s@%s\n",
                                        sub->parent->name, s->device->name);
-       } else {                                        /* We already have a call, so we switch in a threeway call */
-
-               if (s->device->moh) {
-                       struct unistim_subchannel *subchannel;
-                       struct unistim_line *p = s->device->lines;
-                       subchannel = p->subs[SUB_REAL];
-
-                       if (!subchannel->owner) {
-                               ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
-                               return;
-                       }
-                       if (p->subs[SUB_THREEWAY]) {
-                               ast_log(LOG_WARNING,
-                                               "Can't transfer while an another transfer is taking place\n");
-                               return;
-                       }
-                       if (!alloc_sub(p, SUB_THREEWAY)) {
-                               ast_log(LOG_WARNING, "Unable to allocate three-way subchannel\n");
-                               return;
-                       }
-                       /* Stop the silence generator */
-                       if (s->device->silence_generator) {
-                               if (unistimdebug)
-                                       ast_verb(0, "Stopping silence generator\n");
-                               ast_channel_stop_silence_generator(subchannel->owner,
-                                                                                                  s->device->silence_generator);
-                               s->device->silence_generator = NULL;
-                       }
-                       send_tone(s, 0, 0);
-                       /* Make new channel */
-                       c = unistim_new(p->subs[SUB_THREEWAY], AST_STATE_DOWN, NULL);
-                       if (!c) {
-                               ast_log(LOG_WARNING, "Cannot allocate new structure on channel %p\n", p);
-                               return;
-                       }
-                       /* Swap things around between the three-way and real call */
-                       swap_subs(p, SUB_THREEWAY, SUB_REAL);
-                       send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
-
-                       if (s->device->height == 1) {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, s, s->device->phone_number);
-                       } else {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, s, "Calling (pre-transfer)");
-                               send_text(TEXT_LINE1, TEXT_NORMAL, s, s->device->phone_number);
-                               send_text(TEXT_LINE2, TEXT_NORMAL, s, "Dialing...");
-                       }
-                       send_text_status(s, "TransfrCancel");
-
-                       if (ast_pthread_create(&t, NULL, unistim_ss, p->subs[SUB_THREEWAY]->owner)) {
-                               ast_log(LOG_WARNING, "Unable to start simple switch on channel %p\n", p);
-                               ast_hangup(c);
-                               return;
-                       }
-                       if (unistimdebug)
-                               ast_verb(0, "Started three way call on channel %p (%s) subchan %d\n",
-                                        p->subs[SUB_THREEWAY]->owner, ast_channel_name(p->subs[SUB_THREEWAY]->owner),
-                                        p->subs[SUB_THREEWAY]->subtype);
-               } else
-                       ast_debug(1, "Current sub [%s] already has owner\n", ast_channel_name(sub->owner));
+       } else {
+               ast_debug(1, "Current sub [%s] already has owner\n", ast_channel_name(sub->owner));
        }
        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);
-       if (!sub->rtp)
+       if (!sub->rtp) {
                ast_log(LOG_WARNING, "Unable to create channel for %s@%s\n", sub->parent->name,
                                s->device->name);
-       ast_queue_control(sub->owner, AST_CONTROL_ANSWER);
-       send_text(TEXT_LINE2, TEXT_NORMAL, s, "is on-line");
-       send_text_status(s, "Hangup Transf");
+       }
+       if (sub->owner) {
+               ast_queue_control(sub->owner, AST_CONTROL_ANSWER);
+       }
+       send_text(TEXT_LINE2, TEXT_NORMAL, s, ustmtext("is on-line", s));
+       send_text_status(s, ustmtext("       Transf        Hangup", s));
        send_start_timer(s);
 
        if ((s->device->output == OUTPUT_HANDSET) &&
-               (s->device->receiver_state == STATE_ONHOOK))
+               (s->device->receiver_state == STATE_ONHOOK)) {
                send_select_output(s, OUTPUT_SPEAKER, s->device->volume, MUTE_OFF);
-       else
+       } else {
                send_select_output(s, s->device->output, s->device->volume, MUTE_OFF);
-       s->device->start_call_timestamp = time(0);
+       }
        write_history(s, 'i', 0);
        return;
 }
@@ -2552,8 +3226,10 @@ static int unistim_do_senddigit(struct unistimsession *pte, char digit)
 {
        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;
        }
@@ -2561,90 +3237,128 @@ static int unistim_do_senddigit(struct unistimsession *pte, char digit)
        /* Send DTMF indication _before_ playing sounds */
        ast_queue_frame(sub->owner, &f);
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Send Digit %c\n", digit);
-       switch (digit) {
-       case '0':
-               send_tone(pte, 941, 1336);
-               break;
-       case '1':
-               send_tone(pte, 697, 1209);
-               break;
-       case '2':
-               send_tone(pte, 697, 1336);
-               break;
-       case '3':
-               send_tone(pte, 697, 1477);
-               break;
-       case '4':
-               send_tone(pte, 770, 1209);
-               break;
-       case '5':
-               send_tone(pte, 770, 1336);
-               break;
-       case '6':
-               send_tone(pte, 770, 1477);
-               break;
-       case '7':
-               send_tone(pte, 852, 1209);
-               break;
-       case '8':
-               send_tone(pte, 852, 1336);
-               break;
-       case '9':
-               send_tone(pte, 852, 1477);
-               break;
-       case 'A':
-               send_tone(pte, 697, 1633);
-               break;
-       case 'B':
-               send_tone(pte, 770, 1633);
-               break;
-       case 'C':
-               send_tone(pte, 852, 1633);
-               break;
-       case 'D':
-               send_tone(pte, 941, 1633);
-               break;
-       case '*':
-               send_tone(pte, 941, 1209);
-               break;
-       case '#':
-               send_tone(pte, 941, 1477);
-               break;
-       default:
+       }
+       row = (digit - '1') % 3;
+       col = (digit - '1' - row) / 3;
+       if (digit >= '1' && digit <='9') {
+               send_tone(pte, dtmf_row[row], dtmf_col[col]);
+       } else if (digit >= 'A' && digit <= 'D') {
+               send_tone(pte, dtmf_row[digit-'A'], dtmf_col[3]);
+       } else if (digit == '*') {
+               send_tone(pte, dtmf_row[3], dtmf_col[0]);
+       } else if (digit == '0') {
+               send_tone(pte, dtmf_row[3], dtmf_col[1]);
+       } else if (digit == '#') {
+               send_tone(pte, dtmf_row[3], dtmf_col[2]);
+       } else {
                send_tone(pte, 500, 2000);
        }
-       usleep(150000);                  /* XXX Less than perfect, blocking an important thread is not a good idea */
-       send_tone(pte, 0, 0);
-       return 0;
+       usleep(150000);                  /* XXX Less than perfect, blocking an important thread is not a good idea */
+       send_tone(pte, 0, 0);
+       return 0;
+}
+
+static void handle_key_fav(struct unistimsession *pte, char keycode)
+{
+       int keynum = keycode - KEY_FAV0;
+       struct unistim_subchannel *sub;
+
+       sub = get_sub(pte->device, SUB_REAL);
+
+       /* Make an action on selected favorite key */
+       if (!pte->device->ssub[keynum]) { /* Key have no assigned call */
+               send_favorite_selected(FAV_LINE_ICON, pte);
+               if (is_key_line(pte->device, keynum)) {
+                       if (unistimdebug) {
+                               ast_verb(0, "Handle line w/o sub - dialpage\n");
+                       }
+                       pte->device->selected = keynum;
+                       sub_hold(pte, sub); /* Put active call on hold */
+                       send_stop_timer(pte);
+                       handle_dial_page(pte);
+               } else if (is_key_favorite(pte->device, keynum)) {
+                       /* Put active call on hold in handle_call_outgoing function, after preparation and
+                        checking if lines available for calling */
+                       if (unistimdebug) {
+                               ast_verb(0, "Handle favorite w/o sub - dialing\n");
+                       }
+                       if ((pte->device->output == OUTPUT_HANDSET) &&
+                               (pte->device->receiver_state == STATE_ONHOOK)) {
+                               send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
+                       } else {
+                               send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
+                       }
+                       key_favorite(pte, keycode);
+               }
+       } else {
+               sub = pte->device->ssub[keynum];
+               /* Favicon have assigned sub, activate it and put current on hold */
+               if (sub->subtype == SUB_REAL) {
+                       sub_hold(pte, sub);
+                       show_main_page(pte);
+               } else if (sub->subtype == SUB_RING) {
+                       sub->softkey = keynum;
+                       handle_call_incoming(pte);
+               } else if (sub->subtype == SUB_ONHOLD) {
+                       if (pte->state == STATE_DIALPAGE){
+                               send_tone(pte, 0, 0);
+                       }
+                       send_callerid_screen(pte, sub);
+                       sub_unhold(pte, sub);
+                       pte->state = STATE_CALL;
+               }
+       }
 }
 
 static void key_call(struct unistimsession *pte, char keycode)
 {
+       struct unistim_subchannel *sub = NULL;
        if ((keycode >= KEY_0) && (keycode <= KEY_SHARP)) {
-               if (keycode == KEY_SHARP)
+               if (keycode == KEY_SHARP) {
                        keycode = '#';
-               else if (keycode == KEY_STAR)
+               } else if (keycode == KEY_STAR) {
                        keycode = '*';
-               else
+               } else {
                        keycode -= 0x10;
+               }
                unistim_do_senddigit(pte, keycode);
                return;
        }
        switch (keycode) {
-       case KEY_HANGUP:
        case KEY_FUNC1:
-               close_call(pte);
+               if (get_sub(pte->device, SUB_THREEWAY)) {
+                       close_call(pte);
+               }
                break;
        case KEY_FUNC2:
-               TransferCallStep1(pte);
+               if (get_sub(pte->device, SUB_THREEWAY)) {
+                       transfer_cancel_step2(pte);
+               } else {
+                       transfer_call_step1(pte);
+               }
+               break;
+       case KEY_HANGUP:
+       case KEY_FUNC4:
+               if (!get_sub(pte->device, SUB_THREEWAY)) {
+                       close_call(pte);
+               }
+               break;
+       case KEY_FAV0:
+       case KEY_FAV1:
+       case KEY_FAV2:
+       case KEY_FAV3:
+       case KEY_FAV4:
+       case KEY_FAV5:
+               handle_key_fav(pte, keycode);
                break;
        case KEY_HEADPHN:
-               if (pte->device->output == OUTPUT_HEADPHONE)
+               if (pte->device->output == OUTPUT_HEADPHONE) {
                        send_select_output(pte, OUTPUT_HANDSET, pte->device->volume, MUTE_OFF);
-               else
+               } else {
                        send_select_output(pte, OUTPUT_HEADPHONE, pte->device->volume, MUTE_OFF);
+               }
                break;
        case KEY_LOUDSPK:
                if (pte->device->output != OUTPUT_SPEAKER)
@@ -2654,133 +3368,124 @@ static void key_call(struct unistimsession *pte, char keycode)
                                                         MUTE_OFF);
                break;
        case KEY_MUTE:
-               if (!pte->device->moh) {
-                       if (pte->device->mute == MUTE_ON)
+               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
+                       } else {
                                send_select_output(pte, pte->device->output, pte->device->volume, MUTE_ON);
+                       }
                        break;
                }
+               break;
        case KEY_ONHOLD:
-               {
-                       struct unistim_subchannel *sub;
-                       struct ast_channel *bridgepeer = NULL;
-                       sub = pte->device->lines->subs[SUB_REAL];
-                       if (!sub->owner) {
-                               ast_log(LOG_WARNING, "Unable to find subchannel for music on hold\n");
-                               return;
+               sub = get_sub(pte->device, SUB_REAL);
+               if (!sub) {
+                       if(pte->device->ssub[pte->device->selected]) {
+                               sub_hold(pte, pte->device->ssub[pte->device->selected]);
                        }
-                       if ((bridgepeer = ast_bridged_channel(sub->owner))) {
-                               if (pte->device->moh) {
-                                       ast_moh_stop(bridgepeer);
-                                       pte->device->moh = 0;
-                                       send_select_output(pte, pte->device->output, pte->device->volume,
-                                                                        MUTE_OFF);
-                               } else {
-                                       ast_moh_start(bridgepeer, pte->device->lines->musicclass, NULL);
-                                       pte->device->moh = 1;
-                                       send_select_output(pte, pte->device->output, pte->device->volume,
-                                                                        MUTE_ON);
-                               }
-                       } else
-                               ast_log(LOG_WARNING,
-                                               "Unable to find peer subchannel for music on hold\n");
-                       break;
+               } else {
+                       sub_hold(pte, sub);
                }
+               break;
        }
        return;
 }
 
 static void key_ringing(struct unistimsession *pte, char keycode)
 {
-       if (keycode == KEY_FAV0 + pte->device->softkeylinepos) {
-               HandleCallIncoming(pte);
-               return;
-       }
        switch (keycode) {
+       case KEY_FAV0:
+       case KEY_FAV1:
+       case KEY_FAV2:
+       case KEY_FAV3:
+       case KEY_FAV4:
+       case KEY_FAV5:
+               handle_key_fav(pte, keycode);
+               break;
+       case KEY_FUNC3:
+               ignore_call(pte);
+               break;
        case KEY_HANGUP:
        case KEY_FUNC4:
-               IgnoreCall(pte);
+               discard_call(pte);
+               break;
+       case KEY_LOUDSPK:
+               pte->device->output = OUTPUT_SPEAKER;
+               handle_call_incoming(pte);
+               break;
+       case KEY_HEADPHN:
+               pte->device->output = OUTPUT_HEADPHONE;
+               handle_call_incoming(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)) {
+       int fav = keycode - KEY_FAV0;
+       if (!is_key_favorite(pte->device, fav)) {
                ast_log(LOG_WARNING, "It's not a favorite key\n");
                return;
        }
-       if (keycode == KEY_FAV0)
-               return;
-       fav = keycode - KEY_FAV0;
-       if (pte->device->softkeyicon[fav] == 0)
-               return;
        ast_copy_string(pte->device->phone_number, pte->device->softkeynumber[fav],
                                        sizeof(pte->device->phone_number));
-       HandleCallOutgoing(pte);
+       handle_call_outgoing(pte);
        return;
 }
 
 static void key_dial_page(struct unistimsession *pte, char keycode)
 {
+       struct unistim_subchannel *sub = get_sub(pte->device, SUB_THREEWAY);
+
+       pte->device->nextdial = 0;
        if (keycode == KEY_FUNC3) {
-               if (pte->device->size_phone_number <= 1)
+               if (pte->device->size_phone_number <= 1) {
                        keycode = KEY_FUNC4;
-               else {
+               } else {
                        pte->device->size_phone_number -= 2;
                        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)
+               if (pte->device->size_phone_number == 0) {
                        send_tone(pte, 0, 0);
-               while (i < pte->device->size_phone_number) {
-                       tmpbuf[i + 9] = pte->device->phone_number[i];
-                       i++;
                }
-               if (keycode == KEY_SHARP)
+               if (keycode == KEY_SHARP) {
                        keycode = '#';
-               else if (keycode == KEY_STAR)
+               } else if (keycode == KEY_STAR) {
                        keycode = '*';
-               else
+               } else {
                        keycode -= 0x10;
-               tmpbuf[i + 9] = keycode;
+               }
                pte->device->phone_number[i] = keycode;
                pte->device->size_phone_number++;
                pte->device->phone_number[i + 1] = 0;
-               if (pte->device->height == 1) {
-                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmpbuf);
+               show_phone_number(pte);
+
+               if (ast_exists_extension(NULL, pte->device->context, pte->device->phone_number, 1, NULL) && 
+                       !ast_matchmore_extension(NULL, pte->device->context, pte->device->phone_number, 1, NULL)) {
+                   keycode = KEY_FUNC1;
                } else {
-                       send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmpbuf);
+                   pte->device->nextdial = get_tick_count() + TIMER_DIAL;
                }
-               send_blink_cursor(pte);
-               send_cursor_pos(pte, (unsigned char) (TEXT_LINE2 + 0x0a + i));
-               return;
        }
        if (keycode == KEY_FUNC4) {
-
                pte->device->size_phone_number = 0;
-               if (pte->device->height == 1) {
-                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Number : ...............");
-                       send_blink_cursor(pte);
-                       send_cursor_pos(pte, TEXT_LINE0 + 0x09);
-               } else {
-                       send_text(TEXT_LINE2, TEXT_NORMAL, pte, "Number : ...............");
-                       send_blink_cursor(pte);
-                       send_cursor_pos(pte, TEXT_LINE2 + 0x09);
-               }
+               show_phone_number(pte);
                return;
        }
 
@@ -2791,85 +3496,132 @@ static void key_dial_page(struct unistimsession *pte, char keycode)
                        show_main_page(pte);
                } else if ((keycode == KEY_FUNC2) || (keycode == KEY_HANGUP)) {
                        pte->device->call_forward[0] = '\0';
+                       send_led_update(pte, 0x08);
+                       send_led_update(pte, 0x10);
                        show_main_page(pte);
                }
                return;
        }
        switch (keycode) {
        case KEY_FUNC2:
-               if (ast_strlen_zero(pte->device->redial_number))
+               if (ast_strlen_zero(pte->device->redial_number)) {
                        break;
+               }
                ast_copy_string(pte->device->phone_number, pte->device->redial_number,
                                                sizeof(pte->device->phone_number));
        case KEY_FUNC1:
-               HandleCallOutgoing(pte);
+               handle_call_outgoing(pte);
                break;
        case KEY_HANGUP:
-               if (pte->device->lines->subs[SUB_REAL]->owner) {
-                       /* Stop the silence generator */
-                       if (pte->device->silence_generator) {
-                               if (unistimdebug)
-                                       ast_verb(0, "Stopping silence generator\n");
-                               ast_channel_stop_silence_generator(pte->device->lines->subs[SUB_REAL]->
-                                                                                                  owner, pte->device->silence_generator);
-                               pte->device->silence_generator = NULL;
-                       }
+               if (sub && sub->owner) {
+                       struct ast_channel *bridgepeer = NULL;
+
+                       sub_stop_silence(pte, sub);
                        send_tone(pte, 0, 0);
-                       ast_moh_stop(ast_bridged_channel(pte->device->lines->subs[SUB_REAL]->owner));
-                       pte->device->moh = 0;
+                       if ((bridgepeer = ast_bridged_channel(sub->owner))) {
+                               ast_moh_stop(bridgepeer);
+                       }
+                       sub->moh = 0;
+                       sub->subtype = SUB_REAL;
                        pte->state = STATE_CALL;
 
-                       if (pte->device->height == 1) {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Dial Cancel,back to priv. call.");
-                       } else {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Dialing canceled,");
-                               send_text(TEXT_LINE1, TEXT_NORMAL, pte, "switching back to");
-                               send_text(TEXT_LINE2, TEXT_NORMAL, pte, "previous call.");
-                       }
-                       send_text_status(pte, "Hangup Transf");
-               } else
+                       send_text_status(pte, ustmtext("       Transf        Hangup", pte));
+                       send_callerid_screen(pte, sub);
+               } else {
+                       send_led_update(pte, 0x08);
+                       send_led_update(pte, 0x10);
                        show_main_page(pte);
+                }
                break;
+       case KEY_FAV0:
        case KEY_FAV1:
        case KEY_FAV2:
        case KEY_FAV3:
        case KEY_FAV4:
        case KEY_FAV5:
-               Keyfavorite(pte, keycode);
+               send_favorite_selected(FAV_LINE_ICON, pte);
+               pte->device->selected = -1;
+               handle_key_fav(pte, keycode);
                break;
        case KEY_LOUDSPK:
                if (pte->device->output == OUTPUT_SPEAKER) {
-                       if (pte->device->receiver_state == STATE_OFFHOOK)
+                       if (pte->device->receiver_state == STATE_OFFHOOK) {
                                send_select_output(pte, pte->device->previous_output, pte->device->volume,
                                                                 MUTE_OFF);
-                       else
+                       } else {
                                show_main_page(pte);
-               } else
+                       }
+               } else {
                        send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
+               }
                break;
        case KEY_HEADPHN:
                if (pte->device->output == OUTPUT_HEADPHONE) {
-                       if (pte->device->receiver_state == STATE_OFFHOOK)
+                       if (pte->device->receiver_state == STATE_OFFHOOK) {
                                send_select_output(pte, OUTPUT_HANDSET, pte->device->volume, MUTE_OFF);
-                       else
+                       } else {
                                show_main_page(pte);
-               } else
+                       }
+               } else {
                        send_select_output(pte, OUTPUT_HEADPHONE, pte->device->volume, MUTE_OFF);
+               }
+               break;
+       }
+       return;
+}
+
+static void handle_select_option(struct unistimsession *pte)
+{
+       char tmp[128];
+
+       if (pte->state != STATE_SELECTOPTION) {
+               pte->state = STATE_SELECTOPTION;
+               pte->size_buff_entry = 1;
+               pte->buff_entry[0] = 0; /* Position in menu */
+       }
+       snprintf(tmp, sizeof(tmp), "%d. %s", pte->buff_entry[0] + 1, ustmtext(options_menu[(int)pte->buff_entry[0]].label, pte));
+       send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmp);
+       send_text_status(pte, ustmtext("Select               Cancel", pte));
+       return;
+}
+
+static void key_select_option(struct unistimsession *pte, char keycode)
+{
+       switch (keycode) {
+       case KEY_DOWN:
+               pte->buff_entry[0]++;
+               if (options_menu[(int)pte->buff_entry[0]].label == NULL) {
+                       pte->buff_entry[0]--;
+               }
+               break;
+       case KEY_UP:
+               if (pte->buff_entry[0] > 0) {
+                       pte->buff_entry[0]--;
+               }
                break;
+       case KEY_FUNC1:
+               options_menu[(int)pte->buff_entry[0]].handle_option(pte);
+               return;
+       case KEY_HANGUP:
+       case KEY_FUNC4:
+               show_main_page(pte);
+               return;
        }
+
+       handle_select_option(pte);
        return;
 }
 
 #define SELECTCODEC_START_ENTRY_POS 15
 #define SELECTCODEC_MAX_LENGTH 2
 #define SELECTCODEC_MSG "Codec number : .."
-static void HandleSelectCodec(struct unistimsession *pte)
+static void handle_select_codec(struct unistimsession *pte)
 {
        char buf[30], buf2[5];
 
        pte->state = STATE_SELECTCODEC;
-       strcpy(buf, "Using codec ");
-       sprintf(buf2, "%d", pte->device->codec_number);
+       ast_copy_string(buf, ustmtext("Using codec", pte), sizeof(buf));
+       snprintf(buf2, sizeof(buf2), " %d", pte->device->codec_number);
        strcat(buf, buf2);
        strcat(buf, " (G711u=0,");
 
@@ -2879,16 +3631,16 @@ static void HandleSelectCodec(struct unistimsession *pte)
        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;
 }
 
 static void key_select_codec(struct unistimsession *pte, char keycode)
 {
        if (keycode == KEY_FUNC2) {
-               if (pte->size_buff_entry <= 1)
+               if (pte->size_buff_entry <= 1) {
                        keycode = KEY_FUNC3;
-               else {
+               } else {
                        pte->size_buff_entry -= 2;
                        keycode = pte->buff_entry[pte->size_buff_entry] + 0x10;
                }
@@ -2897,9 +3649,9 @@ static void key_select_codec(struct unistimsession *pte, char keycode)
                char tmpbuf[] = SELECTCODEC_MSG;
                int i = 0;
 
-               if (pte->size_buff_entry >= SELECTCODEC_MAX_LENGTH)
+               if (pte->size_buff_entry >= SELECTCODEC_MAX_LENGTH) {
                        return;
-
+               }
                while (i < pte->size_buff_entry) {
                        tmpbuf[i + SELECTCODEC_START_ENTRY_POS] = pte->buff_entry[i];
                        i++;
@@ -2916,11 +3668,12 @@ static void key_select_codec(struct unistimsession *pte, char keycode)
 
        switch (keycode) {
        case KEY_FUNC1:
-               if (pte->size_buff_entry == 1)
+               if (pte->size_buff_entry == 1) {
                        pte->device->codec_number = pte->buff_entry[0] - 48;
-               else if (pte->size_buff_entry == 2)
+               } else if (pte->size_buff_entry == 2) {
                        pte->device->codec_number =
                                ((pte->buff_entry[0] - 48) * 10) + (pte->buff_entry[1] - 48);
+               }
                show_main_page(pte);
                break;
        case KEY_FUNC3:
@@ -2937,19 +3690,85 @@ static void key_select_codec(struct unistimsession *pte, char keycode)
        return;
 }
 
+static int find_language(const char* lang)
+{
+       int i = 0;
+       while (options_languages[i].lang_short != NULL) {
+               if(!strcmp(options_languages[i].lang_short, lang)) {
+                       return i;
+               }
+               i++;
+       }
+       return 0;
+}
+
+static void handle_select_language(struct unistimsession *pte)
+{
+       char tmp_language[40];
+       struct unistim_languages lang;
+
+       if (pte->state != STATE_SELECTLANGUAGE) {
+               pte->state = STATE_SELECTLANGUAGE;
+               pte->size_buff_entry = 1;
+               pte->buff_entry[0] = find_language(pte->device->language);
+       }
+       lang = options_languages[(int)pte->buff_entry[0]];
+       ast_copy_string(tmp_language, pte->device->language, sizeof(tmp_language));
+       ast_copy_string(pte->device->language, lang.lang_short, sizeof(pte->device->language));
+       send_charset_update(pte, lang.encoding);
+       send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext(lang.label, pte));
+
+       ast_copy_string(pte->device->language, tmp_language, sizeof(pte->device->language));
+       lang = options_languages[find_language(pte->device->language)];
+       send_charset_update(pte, lang.encoding);
+       send_text_status(pte, ustmtext("Select               Cancel", pte));
+       return;
+}
+
+static void key_select_language(struct unistimsession *pte, char keycode)
+{
+       switch (keycode) {
+       case KEY_DOWN:
+               pte->buff_entry[0]++;
+               if (options_languages[(int)pte->buff_entry[0]].label == NULL) {
+                       pte->buff_entry[0]--;
+               }
+               break;
+       case KEY_UP:
+               if (pte->buff_entry[0] > 0) {
+                       pte->buff_entry[0]--;
+               }
+               break;
+       case KEY_FUNC1:
+               ast_copy_string(pte->device->language, options_languages[(int)pte->buff_entry[0]].lang_short, sizeof(pte->device->language));
+               send_charset_update(pte, options_languages[(int)pte->buff_entry[0]].encoding);
+               refresh_all_favorite(pte);
+               show_main_page(pte);
+               return;
+       case KEY_HANGUP:
+       case KEY_FUNC4:
+               handle_select_option(pte);
+               return;
+       }
+
+       handle_select_language(pte);
+       return;
+}
+
+
 #define SELECTEXTENSION_START_ENTRY_POS 0
 #define SELECTEXTENSION_MAX_LENGTH 10
 #define SELECTEXTENSION_MSG ".........."
-static void ShowExtensionPage(struct unistimsession *pte)
+static void show_extension_page(struct unistimsession *pte)
 {
        pte->state = STATE_EXTENSION;
 
-       send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Please enter a Terminal");
-       send_text(TEXT_LINE1, TEXT_NORMAL, pte, "Number (TN) :");
+       send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Please enter a Terminal", pte));
+       send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("Number (TN) :", pte));
        send_text(TEXT_LINE2, TEXT_NORMAL, pte, SELECTEXTENSION_MSG);
        send_blink_cursor(pte);
        send_cursor_pos(pte, TEXT_LINE2 + SELECTEXTENSION_START_ENTRY_POS);
-       send_text_status(pte, "Enter  BackSpcErase");
+       send_text_status(pte, ustmtext("Enter  BackSpcErase", pte));
        pte->size_buff_entry = 0;
        return;
 }
@@ -2957,9 +3776,9 @@ static void ShowExtensionPage(struct unistimsession *pte)
 static void key_select_extension(struct unistimsession *pte, char keycode)
 {
        if (keycode == KEY_FUNC2) {
-               if (pte->size_buff_entry <= 1)
+               if (pte->size_buff_entry <= 1) {
                        keycode = KEY_FUNC3;
-               else {
+               } else {
                        pte->size_buff_entry -= 2;
                        keycode = pte->buff_entry[pte->size_buff_entry] + 0x10;
                }
@@ -2968,9 +3787,9 @@ static void key_select_extension(struct unistimsession *pte, char keycode)
                char tmpbuf[] = SELECTEXTENSION_MSG;
                int i = 0;
 
-               if (pte->size_buff_entry >= SELECTEXTENSION_MAX_LENGTH)
+               if (pte->size_buff_entry >= SELECTEXTENSION_MAX_LENGTH) {
                        return;
-
+               }
                while (i < pte->size_buff_entry) {
                        tmpbuf[i + SELECTEXTENSION_START_ENTRY_POS] = pte->buff_entry[i];
                        i++;
@@ -2980,16 +3799,15 @@ static void key_select_extension(struct unistimsession *pte, char keycode)
                pte->size_buff_entry++;
                send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmpbuf);
                send_blink_cursor(pte);
-               send_cursor_pos(pte,
-                                         (unsigned char) (TEXT_LINE2 + SELECTEXTENSION_START_ENTRY_POS + 1 +
-                                                                          i));
+               send_cursor_pos(pte, (unsigned char) (TEXT_LINE2 + SELECTEXTENSION_START_ENTRY_POS + 1 + i));
                return;
        }
 
        switch (keycode) {
        case KEY_FUNC1:
-               if (pte->size_buff_entry < 1)
+               if (pte->size_buff_entry < 1) {
                        return;
+               }
                if (autoprovisioning == AUTOPROVISIONING_TN) {
                        struct unistim_device *d;
 
@@ -3004,7 +3822,6 @@ static void key_select_extension(struct unistimsession *pte, char keycode)
                                                pte->device = d;
                                                d->session = pte;
                                                d->codec_number = DEFAULT_CODEC;
-                                               d->pos_fav = 0;
                                                d->missed_call = 0;
                                                d->receiver_state = STATE_ONHOOK;
                                                strcpy(d->id, pte->macaddr);
@@ -3021,22 +3838,20 @@ static void key_select_extension(struct unistimsession *pte, char keycode)
                                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_cursor_pos(pte,
-                                                 (unsigned char) (TEXT_LINE2 + SELECTEXTENSION_START_ENTRY_POS +
+                       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));
                        send_blink_cursor(pte);
                } 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 :");
-                               send_cursor_pos(pte,
-                                                         (unsigned char) (TEXT_LINE2 +
-                                                                                          SELECTEXTENSION_START_ENTRY_POS +
-                                                                                          pte->size_buff_entry));
+                       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 +
+                                                                                  pte->size_buff_entry));
                                send_blink_cursor(pte);
                        } else
                                show_main_page(pte);
@@ -3052,24 +3867,6 @@ static void key_select_extension(struct unistimsession *pte, char keycode)
        return;
 }
 
-static int ReformatNumber(char *number)
-{
-       int pos = 0, i = 0, size = strlen(number);
-
-       for (; i < size; i++) {
-               if ((number[i] >= '0') && (number[i] <= '9')) {
-                       if (i == pos) {
-                               pos++;
-                               continue;
-                       }
-                       number[pos] = number[i];
-                       pos++;
-               }
-       }
-       number[pos] = 0;
-       return pos;
-}
-
 static void show_entry_history(struct unistimsession *pte, FILE ** f)
 {
        char line[TEXT_LENGTH_MAX + 1], status[STATUS_LENGTH_MAX + 1], func1[10], func2[10],
@@ -3081,7 +3878,13 @@ static void show_entry_history(struct unistimsession *pte, FILE ** f)
                return;
        }
        line[sizeof(line) - 1] = '\0';
-       send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+       if (pte->device->height == 1) {
+               if (pte->buff_entry[3] == 1) {
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+               }
+       } else {
+               send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+       }
        if (fread(line, TEXT_LENGTH_MAX, 1, *f) != 1) {
                display_last_error("Can't read callerid entry");
                fclose(*f);
@@ -3089,37 +3892,52 @@ static void show_entry_history(struct unistimsession *pte, FILE ** f)
        }
        line[sizeof(line) - 1] = '\0';
        ast_copy_string(pte->device->lst_cid, line, sizeof(pte->device->lst_cid));
-       send_text(TEXT_LINE1, TEXT_NORMAL, pte, line);
+       if (pte->device->height == 1) {
+               if (pte->buff_entry[3] == 2) {
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+               }
+       } else {
+               send_text(TEXT_LINE1, TEXT_NORMAL, pte, line);
+       }
        if (fread(line, TEXT_LENGTH_MAX, 1, *f) != 1) {
                display_last_error("Can't read callername entry");
                fclose(*f);
                return;
        }
        line[sizeof(line) - 1] = '\0';
-       send_text(TEXT_LINE2, TEXT_NORMAL, pte, line);
+       if (pte->device->height == 1) {
+               if (pte->buff_entry[3] == 3) {
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, line);
+               }
+        } else {
+                send_text(TEXT_LINE2, TEXT_NORMAL, pte, line);
+        }
        fclose(*f);
 
-       snprintf(line, sizeof(line), "Call %03d/%03d", pte->buff_entry[2],
+       snprintf(line, sizeof(line), "%s %03d/%03d", ustmtext("Call", pte), pte->buff_entry[2],
                         pte->buff_entry[1]);
        send_texttitle(pte, line);
 
-       if (pte->buff_entry[2] == 1)
-               strcpy(func1, "       ");
-       else
-               strcpy(func1, "Prvious");
-       if (pte->buff_entry[2] >= pte->buff_entry[1])
-               strcpy(func2, "       ");
-       else
-               strcpy(func2, "Next   ");
-       if (ReformatNumber(pte->device->lst_cid))
-               strcpy(func3, "Redial ");
-       else
-               strcpy(func3, "       ");
-       snprintf(status, sizeof(status), "%s%s%sCancel", func1, func2, func3);
+       if (pte->buff_entry[2] == 1) {
+               ast_copy_string(func1, "       ", sizeof(func1));
+       } else {
+               ast_copy_string(func1, ustmtext("Prev   ", pte), sizeof(func1));
+       }
+       if (pte->buff_entry[2] >= pte->buff_entry[1]) {
+               ast_copy_string(func2, "       ", sizeof(func2));
+       } else {
+               ast_copy_string(func2, ustmtext("Next   ", pte), sizeof(func2));
+       }
+       if (strlen(pte->device->lst_cid)) {
+               ast_copy_string(func3, ustmtext("Redial ", pte), sizeof(func3));
+       } else {
+               ast_copy_string(func3, "       ", sizeof(func3));
+       }
+       snprintf(status, sizeof(status), "%s%s%s%s", func1, func2, func3, ustmtext("Cancel", pte));
        send_text_status(pte, status);
 }
 
-static char OpenHistory(struct unistimsession *pte, char way, FILE ** f)
+static char open_history(struct unistimsession *pte, char way, FILE ** f)
 {
        char tmp[AST_CONFIG_MAX_PATH];
        char count;
@@ -3150,16 +3968,20 @@ static void show_history(struct unistimsession *pte, char way)
        FILE *f;
        char count;
 
-       if (!pte->device)
+       if (!pte->device) {
                return;
-       if (!pte->device->callhistory)
+       }
+       if (!pte->device->callhistory) {
                return;
-       count = OpenHistory(pte, way, &f);
-       if (!count)
+       }
+       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;
 }
@@ -3167,60 +3989,76 @@ static void show_history(struct unistimsession *pte, char way)
 static void show_main_page(struct unistimsession *pte)
 {
        char tmpbuf[TEXT_LENGTH_MAX + 1];
-
+       const char *text;
 
        if ((pte->device->extension == EXTENSION_ASK) &&
                (ast_strlen_zero(pte->device->extension_number))) {
-               ShowExtensionPage(pte);
+               show_extension_page(pte);
                return;
        }
 
        pte->state = STATE_MAINPAGE;
 
        send_tone(pte, 0, 0);
+       send_stop_timer(pte); /* case of holding call */
        send_select_output(pte, pte->device->output, pte->device->volume, MUTE_ON_DISCRET);
-       pte->device->lines->lastmsgssent = 0;
-       send_favorite(pte->device->softkeylinepos, FAV_ICON_ONHOOK_BLACK, pte,
-                                pte->device->softkeylabel[pte->device->softkeylinepos]);
+       send_led_update(pte, 0x08);
+       send_led_update(pte, 0x10);
+
        if (!ast_strlen_zero(pte->device->call_forward)) {
                if (pte->device->height == 1) {
-                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Forwarding ON");
+                       char tmp_field[100];
+                       snprintf(tmp_field, sizeof(tmp_field), "%s %s", ustmtext("Fwd to:", pte), pte->device->call_forward);
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmp_field);
                } else {
-                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Call forwarded to :");
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Call forwarded to :", pte));
                        send_text(TEXT_LINE1, TEXT_NORMAL, pte, pte->device->call_forward);
                }
-               Sendicon(TEXT_LINE0, FAV_ICON_REFLECT + FAV_BLINK_SLOW, pte);
-               send_text_status(pte, "Dial   Redial NoForwd");
+               send_icon(TEXT_LINE0, FAV_ICON_REFLECT + FAV_BLINK_SLOW, pte);
+               send_text_status(pte, ustmtext("Dial   Redial NoFwd  ", pte));
        } else {
                if ((pte->device->extension == EXTENSION_ASK) ||
-                       (pte->device->extension == EXTENSION_TN))
-                       send_text_status(pte, "Dial   Redial ForwardUnregis");
-               else
-                       send_text_status(pte, "Dial   Redial Forward");
-
+                       (pte->device->extension == EXTENSION_TN)) {
+                       send_text_status(pte, ustmtext("Dial   Redial Fwd    Unregis", pte));
+               } else {
+                       send_text_status(pte, ustmtext("Dial   Redial Fwd    Pickup", pte));
+               }
                send_text(TEXT_LINE1, TEXT_NORMAL, pte, pte->device->maintext1);
-               if (pte->device->missed_call == 0)
-                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, pte->device->maintext0);
-               else {
-                       sprintf(tmpbuf, "%d unanswered call(s)", pte->device->missed_call);
+               if (pte->device->missed_call == 0) {
+                       send_date_time2(pte);
+                       send_idle_clock(pte);
+                       if (strlen(pte->device->maintext0)) {
+                               send_text(TEXT_LINE0, TEXT_NORMAL, pte, pte->device->maintext0);
+                       }
+               } else {
+                       if (pte->device->missed_call == 1) {
+                               text = ustmtext("unanswered call", pte);
+                       } else {
+                               text = ustmtext("unanswered calls", pte);
+                       }
+                       snprintf(tmpbuf, sizeof(tmpbuf), "%d %s", pte->device->missed_call, text);
                        send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmpbuf);
-                       Sendicon(TEXT_LINE0, FAV_ICON_CALL_CENTER + FAV_BLINK_SLOW, pte);
+                       send_icon(TEXT_LINE0, FAV_ICON_CALL_CENTER + FAV_BLINK_SLOW, pte);
                }
        }
-       if (ast_strlen_zero(pte->device->maintext2)) {
-               strcpy(tmpbuf, "IP : ");
-               strcat(tmpbuf, ast_inet_ntoa(pte->sin.sin_addr));
-               send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmpbuf);
-       } else
-               send_text(TEXT_LINE2, TEXT_NORMAL, pte, pte->device->maintext2);
+       if (pte->device->height > 1) {
+               if (ast_strlen_zero(pte->device->maintext2)) {
+                       strcpy(tmpbuf, "IP : ");
+                       strcat(tmpbuf, ast_inet_ntoa(pte->sin.sin_addr));
+                       send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmpbuf);
+               } else {
+                       send_text(TEXT_LINE2, TEXT_NORMAL, pte, pte->device->maintext2);
+               }
+       }
+
        send_texttitle(pte, pte->device->titledefault);
-       change_favorite_icon(pte, FAV_ICON_ONHOOK_BLACK);
+       change_favorite_icon(pte, FAV_LINE_ICON);
 }
 
 static void key_main_page(struct unistimsession *pte, char keycode)
 {
        if (pte->device->missed_call) {
-               Sendicon(TEXT_LINE0, FAV_ICON_NONE, pte);
+               send_icon(TEXT_LINE0, FAV_ICON_NONE, pte);
                pte->device->missed_call = 0;
        }
        if ((keycode >= KEY_0) && (keycode <= KEY_SHARP)) {
@@ -3230,20 +4068,22 @@ static void key_main_page(struct unistimsession *pte, char keycode)
        }
        switch (keycode) {
        case KEY_FUNC1:
+               pte->device->selected = get_avail_softkey(pte, NULL);
                handle_dial_page(pte);
                break;
        case KEY_FUNC2:
-               if (ast_strlen_zero(pte->device->redial_number))
+               if (ast_strlen_zero(pte->device->redial_number)) {
                        break;
+               }
                if ((pte->device->output == OUTPUT_HANDSET) &&
-                       (pte->device->receiver_state == STATE_ONHOOK))
+                       (pte->device->receiver_state == STATE_ONHOOK)) {
                        send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
-               else
+               } else {
                        send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
-
+               }
                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)) {
@@ -3251,7 +4091,7 @@ static void key_main_page(struct unistimsession *pte, char keycode)
                        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;
@@ -3261,9 +4101,9 @@ static void key_main_page(struct unistimsession *pte, char keycode)
                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);
@@ -3272,26 +4112,24 @@ static void key_main_page(struct unistimsession *pte, char keycode)
                        pte->device->session = NULL;
                        pte->device = NULL;
                        ast_mutex_unlock(&devicelock);
-                       ShowExtensionPage(pte);
-               }
+                       show_extension_page(pte);
+               } else { /* Pickup function */
+                       pte->device->selected = -1;
+                       ast_copy_string(pte->device->phone_number, ast_pickup_ext(),
+                                               sizeof(pte->device->phone_number));
+                       handle_call_outgoing(pte);
+                }
                break;
        case KEY_FAV0:
-               handle_dial_page(pte);
-               break;
        case KEY_FAV1:
        case KEY_FAV2:
        case KEY_FAV3:
        case KEY_FAV4:
        case KEY_FAV5:
-               if ((pte->device->output == OUTPUT_HANDSET) &&
-                       (pte->device->receiver_state == STATE_ONHOOK))
-                       send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
-               else
-                       send_select_output(pte, pte->device->output, pte->device->volume, MUTE_OFF);
-               Keyfavorite(pte, keycode);
+               handle_key_fav(pte, keycode);
                break;
        case KEY_CONF:
-               HandleSelectCodec(pte);
+               handle_select_option(pte);
                break;
        case KEY_LOUDSPK:
                send_select_output(pte, OUTPUT_SPEAKER, pte->device->volume, MUTE_OFF);
@@ -3316,45 +4154,47 @@ static void key_history(struct unistimsession *pte, char keycode)
        FILE *f;
        char count;
        long offset;
+       int flag = 0;
 
        switch (keycode) {
-       case KEY_UP:
        case KEY_LEFT:
+               if (pte->device->height == 1) {
+                       if (pte->buff_entry[3] <= 1) {
+                               return;
+                       }
+                       pte->buff_entry[3]--;
+                       flag = 1;
+                       break;
+               }
+       case KEY_UP:
        case KEY_FUNC1:
-               if (pte->buff_entry[2] <= 1)
+               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;
+               flag = 1;
+               break;
+       case KEY_RIGHT:
+               if (pte->device->height == 1) {
+                       if (pte->buff_entry[3] == 3) {
+                               return;
+                       }
+                       pte->buff_entry[3]++;
+                       flag = 1;
+                       break;
                }
-               show_entry_history(pte, &f);
-               break;
        case KEY_DOWN:
-       case KEY_RIGHT:
        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);
+               if (pte->buff_entry[2] >= pte->buff_entry[1]) {
                        return;
                }
-               show_entry_history(pte, &f);
+               pte->buff_entry[2]++;
+               flag = 1;
                break;
        case KEY_FUNC3:
-               if (!ReformatNumber(pte->device->lst_cid))
+               if (ast_strlen_zero(pte->device->lst_cid)) {
                        break;
+               }
                ast_copy_string(pte->device->redial_number, pte->device->lst_cid,
                                                sizeof(pte->device->redial_number));
                key_main_page(pte, KEY_FUNC2);
@@ -3364,65 +4204,95 @@ static void key_history(struct unistimsession *pte, char keycode)
                show_main_page(pte);
                break;
        case KEY_SNDHIST:
-               if (pte->buff_entry[0] == 'i')
+               if (pte->buff_entry[0] == 'i') {
                        show_history(pte, 'o');
-               else
+               } else {
                        show_main_page(pte);
+               }
                break;
        case KEY_RCVHIST:
-               if (pte->buff_entry[0] == 'i')
+               if (pte->buff_entry[0] == 'i') {
                        show_main_page(pte);
-               else
+               } else {
                        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;
 }
 
 static void init_phone_step2(struct unistimsession *pte)
 {
        BUFFSEND;
-       if (unistimdebug)
+       if (unistimdebug) {
                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_time3(pte);
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending S7\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_S7, sizeof(packet_send_S7));
        send_client(SIZE_HEADER + sizeof(packet_send_S7), buffsend, pte);
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending Contrast\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_Contrast, sizeof(packet_send_Contrast));
-       if (pte->device != NULL)
+       if (pte->device != NULL) {
                buffsend[9] = pte->device->contrast;
+       }
        send_client(SIZE_HEADER + sizeof(packet_send_Contrast), buffsend, pte);
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending S9\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_s9, sizeof(packet_send_s9));
        send_client(SIZE_HEADER + sizeof(packet_send_s9), buffsend, pte);
        send_no_ring(pte);
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending S7\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_S7, sizeof(packet_send_S7));
        send_client(SIZE_HEADER + sizeof(packet_send_S7), buffsend, pte);
        send_led_update(pte, 0);
        send_ping(pte);
+       if (unistimdebug) {
+               ast_verb(0, "Sending init language\n");
+       }
+       if (pte->device) {
+               send_charset_update(pte, options_languages[find_language(pte->device->language)].encoding);
+       }
        if (pte->state < STATE_MAINPAGE) {
                if (autoprovisioning == AUTOPROVISIONING_TN) {
-                       ShowExtensionPage(pte);
+                       show_extension_page(pte);
                        return;
                } else {
                        int i;
                        char tmp[30];
 
-                       for (i = 1; i < 6; i++)
+                       for (i = 1; i < FAVNUM; i++) {
                                send_favorite(i, 0, pte, "");
-                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, "Sorry, this phone is not");
-                       send_text(TEXT_LINE1, TEXT_NORMAL, pte, "registered in unistim.cfg");
+                       }
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, ustmtext("Phone is not registered", pte));
+                       send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext("in unistim.conf", pte));
                        strcpy(tmp, "MAC = ");
                        strcat(tmp, pte->macaddr);
                        send_text(TEXT_LINE2, TEXT_NORMAL, pte, tmp);
@@ -3433,8 +4303,9 @@ static void init_phone_step2(struct unistimsession *pte)
        }
        show_main_page(pte);
        refresh_all_favorite(pte);
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "Sending arrow\n");
+       }
        memcpy(buffsend + SIZE_HEADER, packet_send_arrow, sizeof(packet_send_arrow));
        send_client(SIZE_HEADER + sizeof(packet_send_arrow), buffsend, pte);
        return;
@@ -3449,44 +4320,62 @@ static void process_request(int size, unsigned char *buf, struct unistimsession
                rcv_resume_connection_with_server(pte);
                return;
        }
-       if (memcmp(buf + SIZE_HEADER, packet_recv_firm_version, sizeof(packet_recv_firm_version)) ==
-               0) {
+       if (memcmp(buf + SIZE_HEADER, packet_recv_firm_version, sizeof(packet_recv_firm_version)) == 0) {
                buf[size] = 0;
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Got the firmware version : '%s'\n", buf + 13);
+               }
                init_phone_step2(pte);
                return;
        }
+       if (memcmp(buf + SIZE_HEADER, packet_recv_it_type, sizeof(packet_recv_it_type)) == 0) {
+               char type = buf[13];
+               if (unistimdebug) {
+                       ast_verb(0, "Got the equipment type: '%d'\n", type);
+               }
+               switch (type) {
+               case 0x03: /* i2002 */
+                       if (pte->device) {
+                               pte->device->height = 1;
+                       }
+                       break;
+               }
+               return;
+       }
        if (memcmp(buf + SIZE_HEADER, packet_recv_mac_addr, sizeof(packet_recv_mac_addr)) == 0) {
                rcv_mac_addr(pte, buf);
                return;
        }
        if (memcmp(buf + SIZE_HEADER, packet_recv_r2, sizeof(packet_recv_r2)) == 0) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "R2 received\n");
+               }
                return;
        }
 
        if (pte->state < STATE_MAINPAGE) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Request not authorized in this state\n");
+               }
                return;
        }
        if (!memcmp(buf + SIZE_HEADER, packet_recv_pressed_key, sizeof(packet_recv_pressed_key))) {
                char keycode = buf[13];
 
-               if (unistimdebug)
-                       ast_verb(0, "Key pressed : keycode = 0x%.2x - current state : %d\n", keycode,
-                                               pte->state);
-
+               if (unistimdebug) {
+                       ast_verb(0, "Key pressed: keycode = 0x%.2x - current state: %s\n", keycode,
+                                               ptestate_tostr(pte->state));
+               }
                switch (pte->state) {
                case STATE_INIT:
-                       if (unistimdebug)
+                       if (unistimdebug) {
                                ast_verb(0, "No keys allowed in the init state\n");
+                       }
                        break;
                case STATE_AUTHDENY:
-                       if (unistimdebug)
+                       if (unistimdebug) {
                                ast_verb(0, "No keys allowed in authdeny state\n");
+                       }
                        break;
                case STATE_MAINPAGE:
                        key_main_page(pte, keycode);
@@ -3503,9 +4392,15 @@ static void process_request(int size, unsigned char *buf, struct unistimsession
                case STATE_EXTENSION:
                        key_select_extension(pte, keycode);
                        break;
+               case STATE_SELECTOPTION:
+                       key_select_option(pte, keycode);
+                       break;
                case STATE_SELECTCODEC:
                        key_select_codec(pte, keycode);
                        break;
+               case STATE_SELECTLANGUAGE:
+                       key_select_language(pte, keycode);
+                       break;
                case STATE_HISTORY:
                        key_history(pte, keycode);
                        break;
@@ -3515,47 +4410,55 @@ static void process_request(int size, unsigned char *buf, struct unistimsession
                return;
        }
        if (memcmp(buf + SIZE_HEADER, packet_recv_pick_up, sizeof(packet_recv_pick_up)) == 0) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Handset off hook\n");
-               if (!pte->device)              /* We are not yet registered (asking for a TN in AUTOPROVISIONING_TN) */
+               }
+               if (!pte->device) {            /* We are not yet registered (asking for a TN in AUTOPROVISIONING_TN) */
                        return;
+               }
                pte->device->receiver_state = STATE_OFFHOOK;
-               if (pte->device->output == OUTPUT_HEADPHONE)
+               if (pte->device->output == OUTPUT_HEADPHONE) {
                        send_select_output(pte, OUTPUT_HEADPHONE, pte->device->volume, MUTE_OFF);
-               else
+               } 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 */
+               } else if (pte->state == STATE_EXTENSION) { /* We must have a TN before calling */
                        return;
-               else {
+               } else {
+                       pte->device->selected = get_avail_softkey(pte, NULL);
                        send_select_output(pte, OUTPUT_HANDSET, pte->device->volume, MUTE_OFF);
                        handle_dial_page(pte);
                }
                return;
        }
        if (memcmp(buf + SIZE_HEADER, packet_recv_hangup, sizeof(packet_recv_hangup)) == 0) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Handset on hook\n");
-               if (!pte->device)
+               }
+               if (!pte->device) {
                        return;
+               }
                pte->device->receiver_state = STATE_ONHOOK;
-               if (pte->state == STATE_CALL)
-                       close_call(pte);
-               else if (pte->device->lines->subs[SUB_REAL]->owner)
-                       close_call(pte);
-               else if (pte->state == STATE_EXTENSION)
+               if (pte->state == STATE_CALL) {
+                       if (pte->device->output != OUTPUT_SPEAKER) {
+                               close_call(pte);
+                       }
+               } else if (pte->state == STATE_EXTENSION) {
                        return;
-               else
+               } else {
                        show_main_page(pte);
+               }
                return;
        }
        strcpy(tmpbuf, ast_inet_ntoa(pte->sin.sin_addr));
        strcat(tmpbuf, " Unknown request packet\n");
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_debug(1, "%s", tmpbuf);
+       }
        return;
 }
 
@@ -3581,26 +4484,30 @@ static void parsing(int size, unsigned char *buf, struct unistimsession *pte,
                        ast_log(LOG_NOTICE, "%s Invalid size of a discovery packet\n", tmpbuf);
                } else {
                        if (memcmp(buf, packet_rcv_discovery, sizeof(packet_rcv_discovery)) == 0) {
-                               if (unistimdebug)
+                               if (unistimdebug) {
                                        ast_verb(0, "Discovery packet received - Sending Discovery ACK\n");
+                               }
                                if (pte) {            /* A session was already active for this IP ? */
                                        if (pte->state == STATE_INIT) { /* Yes, but it's a dupe */
-                                               if (unistimdebug)
+                                               if (unistimdebug) {
                                                        ast_verb(1, "Duplicated Discovery packet\n");
+                                               }
                                                send_raw_client(sizeof(packet_send_discovery_ack),
                                                                          packet_send_discovery_ack, addr_from, &pte->sout);
                                                pte->seq_phone = (short) 0x0000;        /* reset sequence number */
                                        } else {        /* No, probably a reboot, phone side */
                                                close_client(pte);       /* Cleanup the previous session */
-                                               if (create_client(addr_from))
+                                               if (create_client(addr_from)) {
                                                        send_raw_client(sizeof(packet_send_discovery_ack),
                                                                                  packet_send_discovery_ack, addr_from, &pte->sout);
+                                               }
                                        }
                                } else {
                                        /* Creating new entry in our phone list */
-                                       if ((pte = create_client(addr_from)))
+                                       if ((pte = create_client(addr_from))) {
                                                send_raw_client(sizeof(packet_send_discovery_ack),
                                                                          packet_send_discovery_ack, addr_from, &pte->sout);
+                                       }
                                }
                                return;
                        }
@@ -3609,9 +4516,9 @@ static void parsing(int size, unsigned char *buf, struct unistimsession *pte,
                return;
        }
        if (!pte) {
-               if (unistimdebug)
-                       ast_verb(0, "%s Not a discovery packet from an unknown source : ignoring\n",
-                                               tmpbuf);
+               if (unistimdebug) {
+                       ast_verb(0, "%s Not a discovery packet from an unknown source : ignoring\n", tmpbuf);
+               }
                return;
        }
 
@@ -3627,8 +4534,9 @@ static void parsing(int size, unsigned char *buf, struct unistimsession *pte,
        seq = ntohs(sbuf[1]);
        if (buf[4] == 1) {
                ast_mutex_lock(&pte->lock);
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(6, "ACK received for packet #0x%.4x\n", seq);
+               }
                pte->nb_retransmit = 0;
 
                if ((pte->last_seq_ack) + 1 == seq) {
@@ -3641,10 +4549,11 @@ static void parsing(int size, unsigned char *buf, struct unistimsession *pte,
                        if (pte->last_seq_ack == 0xffff) {
                                ast_verb(0, "ACK at 0xffff, restarting counter.\n");
                                pte->last_seq_ack = 0;
-                       } else
+                       } else {
                                ast_log(LOG_NOTICE,
                                                "%s Warning : ACK received for an already ACKed packet : #0x%.4x we are at #0x%.4x\n",
                                                tmpbuf, seq, pte->last_seq_ack);
+                       }
                        ast_mutex_unlock(&pte->lock);
                        return;
                }
@@ -3655,17 +4564,19 @@ static void parsing(int size, unsigned char *buf, struct unistimsession *pte,
                        ast_mutex_unlock(&pte->lock);
                        return;
                }
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "%s ACK gap : Received ACK #0x%.4x, previous was #0x%.4x\n",
                                                tmpbuf, seq, pte->last_seq_ack);
+               }
                pte->last_seq_ack = seq;
                check_send_queue(pte);
                ast_mutex_unlock(&pte->lock);
                return;
        }
        if (buf[4] == 2) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Request received\n");
+               }
                if (pte->seq_phone == seq) {
                        /* Send ACK */
                        buf[4] = 1;
@@ -3735,187 +4646,230 @@ static struct unistimsession *channel_to_session(struct ast_channel *ast)
                ast_log(LOG_WARNING, "Unistim callback function called without a device\n");
                return NULL;
        }
+       ast_mutex_lock(&sub->parent->parent->lock);
        if (!sub->parent->parent->session) {
                ast_log(LOG_WARNING, "Unistim callback function called without a session\n");
                return NULL;
        }
+       ast_mutex_unlock(&sub->parent->parent->lock);
        return sub->parent->parent->session;
 }
 
+static void send_callerid_screen(struct unistimsession *pte, struct unistim_subchannel *sub)
+{
+       char *cidname_str;
+       char *cidnum_str;
+
+       if (!sub) {
+               return;
+       }
+       if (sub->owner) {
+               if (ast_channel_connected(sub->owner)->id.number.valid && ast_channel_connected(sub->owner)->id.number.str) {
+                       cidnum_str = ast_channel_connected(sub->owner)->id.number.str;
+               } else {
+                       cidnum_str = DEFAULTCALLERID;
+               }
+               change_callerid(pte, 0, cidnum_str);
+               if (strlen(cidnum_str) == 0) {
+                       cidnum_str = DEFAULTCALLERID;
+               }
+
+               if (ast_channel_connected(sub->owner)->id.name.valid && ast_channel_connected(sub->owner)->id.name.str) {
+                       cidname_str = ast_channel_connected(sub->owner)->id.name.str;
+               } else {
+                       cidname_str = DEFAULTCALLERNAME;
+               }
+               change_callerid(pte, 1, cidname_str);
+               if (strlen(cidname_str) == 0) {
+                       cidname_str = DEFAULTCALLERNAME;
+               }
+
+               if (pte->device->height == 1) {
+                       char tmpstr[256];
+                       snprintf(tmpstr, sizeof(tmpstr), "%s %s", cidnum_str, ustmtext(cidname_str, pte));
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, tmpstr);
+               } else {
+                       send_text(TEXT_LINE0, TEXT_NORMAL, pte, cidname_str);
+                       send_text(TEXT_LINE1, TEXT_NORMAL, pte, ustmtext(cidnum_str, pte));
+               }
+       }
+}
+
 /*--- unistim_call: Initiate UNISTIM call from PBX ---*/
 /*      used from the dial() application      */
 static int unistim_call(struct ast_channel *ast, const char *dest, int timeout)
 {
-       int res = 0;
-       struct unistim_subchannel *sub;
+       int res = 0, i;
+       struct unistim_subchannel *sub, *sub_real;
        struct unistimsession *session;
+       char ringstyle, ringvolume;
 
        session = channel_to_session(ast);
        if (!session) {
                ast_log(LOG_ERROR, "Device not registered, cannot call %s\n", dest);
                return -1;
        }
-
        sub = ast_channel_tech_pvt(ast);
+       sub_real = get_sub(session->device, SUB_REAL);
        if ((ast_channel_state(ast) != AST_STATE_DOWN) && (ast_channel_state(ast) != AST_STATE_RESERVED)) {
                ast_log(LOG_WARNING, "unistim_call called on %s, neither down nor reserved\n",
                                ast_channel_name(ast));
                return -1;
        }
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(3, "unistim_call(%s)\n", ast_channel_name(ast));
-
+       }
        session->state = STATE_RINGING;
-       Sendicon(TEXT_LINE0, FAV_ICON_NONE, session);
-
-       if (sub->owner) {
-               if (ast_channel_connected(sub->owner)->id.number.valid
-                       && ast_channel_connected(sub->owner)->id.number.str) {
-                       if (session->device->height == 1) {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, session, ast_channel_connected(sub->owner)->id.number.str);
-                       } else {
-                               send_text(TEXT_LINE1, TEXT_NORMAL, session, ast_channel_connected(sub->owner)->id.number.str);
-                       }
-                       change_callerid(session, 0, ast_channel_connected(sub->owner)->id.number.str);
+       send_callerid_screen(session, sub);
+       if (ast_strlen_zero(ast_channel_call_forward(ast))) { /* Send ring only if no call forward, otherwise short ring will apear */
+               send_text(TEXT_LINE2, TEXT_NORMAL, session, ustmtext("is calling you.", session));
+               send_text_status(session, ustmtext("Accept        Ignore Hangup", session));
+
+               if (sub_real) {
+                       ringstyle = session->device->cwstyle;
+                       ringvolume = session->device->cwvolume;
                } else {
-                       if (session->device->height == 1) {
-                               send_text(TEXT_LINE0, TEXT_NORMAL, session, DEFAULTCALLERID);
-                       } else {
-                               send_text(TEXT_LINE1, TEXT_NORMAL, session, DEFAULTCALLERID);
-                       }
-                       change_callerid(session, 0, DEFAULTCALLERID);
+                       ringstyle = (sub->ringstyle == -1) ? session->device->ringstyle : sub->ringstyle;
+                       ringvolume = (sub->ringvolume == -1) ? session->device->ringvolume : sub->ringvolume;
                }
-               if (ast_channel_connected(sub->owner)->id.name.valid
-                       && ast_channel_connected(sub->owner)->id.name.str) {
-                       send_text(TEXT_LINE0, TEXT_NORMAL, session, ast_channel_connected(sub->owner)->id.name.str);
-                       change_callerid(session, 1, ast_channel_connected(sub->owner)->id.name.str);
-               } else {
-                       send_text(TEXT_LINE0, TEXT_NORMAL, session, DEFAULTCALLERNAME);
-                       change_callerid(session, 1, DEFAULTCALLERNAME);
+               send_ring(session, ringvolume, ringstyle);
+               change_favorite_icon(session, FAV_ICON_SPEAKER_ONHOOK_BLACK + FAV_BLINK_FAST);
+               /* Send call identification to all */
+               for (i = 0; i < FAVNUM; i++) {
+                       if (!soft_key_visible(session->device, i)) {
+                               continue;
+                       }
+                       if (session->device->ssub[i]) {
+                               continue;
+                       }
+                       if (is_key_line(session->device, i) && !strcmp(sub->parent->name, session->device->sline[i]->name)) {
+                               if (unistimdebug) {
+                                       ast_verb(0, "Found softkey %d for line %s\n", i, sub->parent->name);
+                               }
+                               send_favorite_short(i, FAV_ICON_SPEAKER_ONHOOK_BLACK + FAV_BLINK_FAST, session);
+                               session->device->ssub[i] = sub;
+                       }
                }
        }
-       send_text(TEXT_LINE2, TEXT_NORMAL, session, "is calling you.");
-       send_text_status(session, "Accept              Ignore");
-
-       if (sub->ringstyle == -1)
-               send_ring(session, session->device->ringvolume, session->device->ringstyle);
-       else {
-               if (sub->ringvolume == -1)
-                       send_ring(session, session->device->ringvolume, sub->ringstyle);
-               else
-                       send_ring(session, sub->ringvolume, sub->ringstyle);
-       }
-       change_favorite_icon(session, FAV_ICON_SPEAKER_ONHOOK_BLACK + FAV_BLINK_FAST);
-
        ast_setstate(ast, AST_STATE_RINGING);
        ast_queue_control(ast, AST_CONTROL_RINGING);
        return res;
 }
 
+static int unistim_hangup_clean(struct ast_channel *ast, struct unistim_subchannel *sub) {
+       ast_mutex_lock(&sub->lock);
+       ast_channel_tech_pvt_set(ast, NULL);
+       sub->owner = NULL;
+       sub->alreadygone = 0;
+       ast_mutex_unlock(&sub->lock);
+       if (sub->rtp) {
+               if (unistimdebug) {
+                       ast_verb(0, "Destroying RTP session\n");
+               }
+               ast_rtp_instance_destroy(sub->rtp);
+               sub->rtp = NULL;
+       }
+       return 0;
+}
+
 /*--- unistim_hangup: Hangup UNISTIM call */
 static int unistim_hangup(struct ast_channel *ast)
 {
-       struct unistim_subchannel *sub;
+       struct unistim_subchannel *sub = NULL, *sub_real = NULL, *sub_trans = NULL;
        struct unistim_line *l;
+       struct unistim_device *d;
        struct unistimsession *s;
+       int i;
 
        s = channel_to_session(ast);
        sub = ast_channel_tech_pvt(ast);
+       l = sub->parent;
+       d = l->parent;
        if (!s) {
                ast_debug(1, "Asked to hangup channel not connected\n");
-               ast_mutex_lock(&sub->lock);
-               sub->owner = NULL;
-               ast_channel_tech_pvt_set(ast, NULL);
-               sub->alreadygone = 0;
-               ast_mutex_unlock(&sub->lock);
-               if (sub->rtp) {
-                       if (unistimdebug)
-                               ast_verb(0, "Destroying RTP session\n");
-                       ast_rtp_instance_destroy(sub->rtp);
-                       sub->rtp = NULL;
-               }
+               unistim_hangup_clean(ast, sub);
                return 0;
        }
-       l = sub->parent;
-       if (unistimdebug)
-               ast_verb(0, "unistim_hangup(%s) on %s@%s\n", ast_channel_name(ast), l->name, l->parent->name);
-
-       if ((l->subs[SUB_THREEWAY]) && (sub->subtype == SUB_REAL)) {
-               if (unistimdebug)
-                       ast_verb(0, "Real call disconnected while talking to threeway\n");
-               sub->owner = NULL;
-               ast_channel_tech_pvt_set(ast, NULL);
-               return 0;
+       if (unistimdebug) {
+               ast_verb(0, "unistim_hangup(%s) on %s@%s (STATE_%s)\n", ast_channel_name(ast), l->name, l->parent->name, ptestate_tostr(s->state));
        }
-       if ((l->subs[SUB_REAL]->owner) && (sub->subtype == SUB_THREEWAY) &&
+       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, "threeway call disconnected, switching to real call\n");
-               send_text(TEXT_LINE0, TEXT_NORMAL, s, "Three way call canceled,");
-               send_text(TEXT_LINE1, TEXT_NORMAL, s, "switching back to");
-               send_text(TEXT_LINE2, TEXT_NORMAL, s, "previous call.");
-               send_text_status(s, "Hangup Transf");
-               ast_moh_stop(ast_bridged_channel(l->subs[SUB_REAL]->owner));
-               swap_subs(l, SUB_THREEWAY, SUB_REAL);
-               l->parent->moh = 0;
-               ast_mutex_lock(&sub->lock);
-               sub->owner = NULL;
-               ast_channel_tech_pvt_set(ast, NULL);
-               ast_mutex_unlock(&sub->lock);
-               unalloc_sub(l, SUB_THREEWAY);
+               if (unistimdebug) {
+                       ast_verb(0, "Threeway call disconnected, switching to real call\n");
+               }
+               if (ast_bridged_channel(sub_trans->owner)) {
+                       ast_moh_stop(ast_bridged_channel(sub_trans->owner));
+               }
+               sub_trans->moh = 0;
+               sub_trans->subtype = SUB_REAL;
+               swap_subs(sub_trans, sub);
+
+               send_text_status(s, ustmtext("       Transf        Hangup", s));
+               send_callerid_screen(s, sub_trans);
+               unistim_hangup_clean(ast, sub);
+               unistim_unalloc_sub(d, sub);
                return 0;
        }
-       ast_mutex_lock(&sub->lock);
-       sub->owner = NULL;
-       ast_channel_tech_pvt_set(ast, 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;
+       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, "Real call disconnected, stay in call\n");
                }
+               send_text_status(s, ustmtext("       Transf        Hangup", s));
+               send_callerid_screen(s, sub_real);
+               unistim_hangup_clean(ast, sub);
+               unistim_unalloc_sub(d, sub);
                return 0;
        }
+
        if (sub->subtype == SUB_REAL) {
-               /* Stop the silence generator */
-               if (s->device->silence_generator) {
-                       if (unistimdebug)
-                               ast_verb(0, "Stopping silence generator\n");
-                       if (sub->owner)
-                               ast_channel_stop_silence_generator(sub->owner,
-                                                                                                  s->device->silence_generator);
-                       else
-                               ast_log(LOG_WARNING,
-                                               "Trying to stop silence generator on a null channel !\n");
-                       s->device->silence_generator = NULL;
+               sub_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;
+                       }
                }
        }
-       l->parent->moh = 0;
-       send_no_ring(s);
-       send_end_call(s);
-       if (sub->rtp) {
-               if (unistimdebug)
-                       ast_verb(0, "Destroying RTP session\n");
-               ast_rtp_instance_destroy(sub->rtp);
-               sub->rtp = NULL;
-       } else if (unistimdebug)
-               ast_verb(0, "No RTP session to destroy\n");
-       if (l->subs[SUB_THREEWAY]) {
-               if (unistimdebug)
-                       ast_verb(0, "Cleaning other subchannels\n");
-               unalloc_sub(l, SUB_THREEWAY);
-       }
-       if (s->state == STATE_RINGING)
-               cancel_dial(s);
-       else if (s->state == STATE_CALL)
+       sub->moh = 0;
+       if (sub->softkey >= 0) {
+               send_favorite_short(sub->softkey, FAV_LINE_ICON, s);
+       }
+       /* Delete assign sub to softkey */
+       for (i = 0; i < FAVNUM; i++) {
+               if (d->ssub[i] == sub) {
+                       d->ssub[i] = NULL;
+                       break;
+               }
+       }
+       refresh_all_favorite(s); /* Update favicons in case of DND keys */
+       if (s->state == STATE_RINGING && sub->subtype == SUB_RING) {
+               send_no_ring(s);
+               if (!ast_test_flag(ast, AST_FLAG_ANSWERED_ELSEWHERE) && ast_channel_hangupcause(ast) != AST_CAUSE_ANSWERED_ELSEWHERE) {
+                       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;
+       unistim_hangup_clean(ast, sub);
+       unistim_unalloc_sub(d, sub);
        return 0;
 }
 
@@ -3925,6 +4879,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);
@@ -3934,18 +4889,22 @@ static int unistim_answer(struct ast_channel *ast)
        }
        sub = ast_channel_tech_pvt(ast);
        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)
+       }
+       if (unistimdebug) {
                ast_verb(0, "unistim_answer(%s) on %s@%s-%d\n", ast_channel_name(ast), 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");
-       else
-               send_text_status(l->parent->session, "Hangup Transf");
-       send_start_timer(l->parent->session);
+                                       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(s, ustmtext("       Transf        Hangup", s));
+       }
+       send_start_timer(s);
        if (ast_channel_state(ast) != AST_STATE_UP)
                ast_setstate(ast, AST_STATE_UP);
        return res;
@@ -3960,19 +4919,22 @@ static int unistimsock_read(int *id, int fd, short events, void *ignore)
        int found = 0;
        int tmp = 0;
        int dw_num_bytes_rcvd;
+       unsigned int size_addr_from;
 #ifdef DUMP_PACKET
        int dw_num_bytes_rcvdd;
        char iabuf[INET_ADDRSTRLEN];
 #endif
 
+       size_addr_from = sizeof(addr_from);
        dw_num_bytes_rcvd =
                recvfrom(unistimsock, buff, SIZE_PAGE, 0, (struct sockaddr *) &addr_from,
                                 &size_addr_from);
        if (dw_num_bytes_rcvd == -1) {
-               if (errno == EAGAIN)
+               if (errno == EAGAIN) {
                        ast_log(LOG_NOTICE, "UNISTIM: Received packet with bad UDP checksum\n");
-               else if (errno != ECONNREFUSED)
+               } else if (errno != ECONNREFUSED) {
                        ast_log(LOG_WARNING, "Recv error %d (%s)\n", errno, strerror(errno));
+               }
                return 1;
        }
 
@@ -4000,13 +4962,14 @@ static int unistimsock_read(int *id, int fd, short events, void *ignore)
 #endif
 
        if (!found) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "Received a packet from an unknown source\n");
+               }
                parsing(dw_num_bytes_rcvd, buff, NULL, (struct sockaddr_in *) &addr_from);
 
-       } else
+       } else {
                parsing(dw_num_bytes_rcvd, buff, cur, (struct sockaddr_in *) &addr_from);
-
+       }
        return 1;
 }
 
@@ -4076,9 +5039,9 @@ static int unistim_write(struct ast_channel *ast, struct ast_frame *frame)
        int res = 0;
 
        if (frame->frametype != AST_FRAME_VOICE) {
-               if (frame->frametype == AST_FRAME_IMAGE)
+               if (frame->frametype == AST_FRAME_IMAGE) {
                        return 0;
-               else {
+               } else {
                        ast_log(LOG_WARNING, "Can't send %d type frames with unistim_write\n",
                                        frame->frametype);
                        return 0;
@@ -4160,6 +5123,12 @@ static char *control2str(int ind)
                return "Key Radio";
        case AST_CONTROL_RADIO_UNKEY:
                return "Un-Key Radio";
+       case AST_CONTROL_CONNECTED_LINE:
+               return "Remote end changed";
+       case AST_CONTROL_SRCCHANGE:
+               return "RTP source updated";
+       case AST_CONTROL_SRCUPDATE:
+               return "Source of media changed";
        case -1:
                return "Stop tone";
        }
@@ -4187,21 +5156,21 @@ static int unistim_indicate(struct ast_channel *ast, int ind, const void *data,
        struct unistimsession *s;
 
        if (unistimdebug) {
-               ast_verb(3, "Asked to indicate '%s' condition on channel %s\n",
-                                       control2str(ind), ast_channel_name(ast));
+               ast_verb(3, "Asked to indicate '%s' (%d) condition on channel %s\n",
+                                       control2str(ind), ind, ast_channel_name(ast));
        }
 
        s = channel_to_session(ast);
-       if (!s)
+       if (!s) {
                return -1;
-
+       }
        sub = ast_channel_tech_pvt(ast);
        l = sub->parent;
 
        switch (ind) {
        case AST_CONTROL_RINGING:
                if (ast_channel_state(ast) != 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;
@@ -4210,7 +5179,7 @@ static int unistim_indicate(struct ast_channel *ast, int ind, const void *data,
        case AST_CONTROL_BUSY:
                if (ast_channel_state(ast) != 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;
@@ -4223,7 +5192,7 @@ static int unistim_indicate(struct ast_channel *ast, int ind, const void *data,
        case AST_CONTROL_CONGESTION:
                if (ast_channel_state(ast) != 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;
@@ -4237,12 +5206,22 @@ static int unistim_indicate(struct ast_channel *ast, int ind, const void *data,
                break;
        case AST_CONTROL_PROGRESS:
        case AST_CONTROL_SRCUPDATE:
+       case AST_CONTROL_PROCEEDING:
                break;
        case -1:
                ast_playtones_stop(ast);
                s->device->missed_call = 0;
                break;
-       case AST_CONTROL_PROCEEDING:
+        case AST_CONTROL_CONNECTED_LINE:
+               ast_log(LOG_NOTICE, "Connected party is now %s <%s>\n",
+               S_COR(ast_channel_connected(ast)->id.name.valid, ast_channel_connected(ast)->id.name.str, ""),
+               S_COR(ast_channel_connected(ast)->id.number.valid, ast_channel_connected(ast)->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);
@@ -4256,6 +5235,7 @@ static struct unistim_subchannel *find_subchannel_by_name(const char *dest)
 {
        struct unistim_line *l;
        struct unistim_device *d;
+       struct unistim_subchannel *sub = NULL;
        char line[256];
        char *at;
        char *device;
@@ -4272,43 +5252,62 @@ static struct unistim_subchannel *find_subchannel_by_name(const char *dest)
        ast_mutex_lock(&devicelock);
        d = devices;
        at = strchr(device, '/');       /* Extra options ? */
-       if (at)
+       if (at) {
                *at = '\0';
+       }
        while (d) {
                if (!strcasecmp(d->name, device)) {
-                       if (unistimdebug)
+                       if (unistimdebug) {
                                ast_verb(0, "Found device: %s\n", d->name);
+                       }
                        /* Found the device */
-                       l = d->lines;
-                       while (l) {
+                       AST_LIST_LOCK(&d->lines);
+                       AST_LIST_TRAVERSE(&d->lines, l, list) {
                                /* Search for the right line */
                                if (!strcasecmp(l->name, line)) {
-                                       l->subs[SUB_REAL]->ringvolume = -1;
-                                       l->subs[SUB_REAL]->ringstyle = -1;
+                                       if (unistimdebug) {
+                                               ast_verb(0, "Found line: %s\n", l->name);
+                                       }
+                                       sub = get_sub(d, SUB_REAL);
+                                       if (!sub) {
+                                               sub = unistim_alloc_sub(d, SUB_REAL);
+                                       }
+                                       if (sub->owner) {
+                                               /* Allocate additional channel if asterisk channel already here */
+                                               sub = unistim_alloc_sub(d, SUB_ONHOLD);
+                                       }
+                                       sub->ringvolume = -1;
+                                       sub->ringstyle = -1;
                                        if (at) {       /* Other options ? */
                                                at++;   /* Skip slash */
                                                if (*at == 'r') {       /* distinctive ring */
                                                        at++;
-                                                       if ((*at < '0') || (*at > '7')) /* ring style */
+                                                       if ((*at < '0') || (*at > '7')) { /* ring style */
                                                                ast_log(LOG_WARNING, "Invalid ring selection (%s)", at);
-                                                       else {
+                                                       } else {
                                                                char ring_volume = -1;
                                                                char ring_style = *at - '0';
                                                                at++;
-                                                               if ((*at >= '0') && (*at <= '3'))       /* ring volume */
+                                                               if ((*at >= '0') && (*at <= '3')) {      /* ring volume */
                                                                        ring_volume = *at - '0';
-                                                               if (unistimdebug)
-                                                                       ast_verb(0, "Distinctive ring : style #%d volume %d\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;
@@ -4323,9 +5322,9 @@ static int unistim_senddigit_begin(struct ast_channel *ast, char digit)
 {
        struct unistimsession *pte = channel_to_session(ast);
 
-       if (!pte)
+       if (!pte) {
                return -1;
-
+       }
        return unistim_do_senddigit(pte, digit);
 }
 
@@ -4335,19 +5334,19 @@ static int unistim_senddigit_end(struct ast_channel *ast, char digit, unsigned i
        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;
        }
 
-       if (unistimdebug)
-               ast_verb(0, "Send Digit off %c\n", digit);
-
-       if (!pte)
+       if (unistimdebug) {
+               ast_verb(0, "Send Digit off %c\n", digit);
+       }
+       if (!pte) {
                return -1;
-
+       }
        send_tone(pte, 0, 0);
        f.frametype = AST_FRAME_DTMF;
        f.subclass.integer = digit;
@@ -4365,9 +5364,9 @@ static int unistim_sendtext(struct ast_channel *ast, const char *text)
        int size;
        char tmp[TEXT_LENGTH_MAX + 1];
 
-       if (unistimdebug)
+       if (unistimdebug) {
                ast_verb(0, "unistim_sendtext called\n");
-
+       }
        if (!text) {
                ast_log(LOG_WARNING, "unistim_sendtext called with a null text\n");
                return 1;
@@ -4434,8 +5433,9 @@ static int unistim_sendtext(struct ast_channel *ast, const char *text)
                                        sz = 0;
                                        continue;
                                }
-                               if (sz > 10)
+                               if (sz > 10) {
                                        continue;
+                               }
                                label[sz] = cur;
                                sz++;
                                continue;
@@ -4470,7 +5470,7 @@ static int unistim_sendtext(struct ast_channel *ast, const char *text)
                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) {
@@ -4493,18 +5493,17 @@ static int unistim_sendtext(struct ast_channel *ast, const char *text)
 }
 
 /*--- unistim_send_mwi_to_peer: Send message waiting indication ---*/
-static int unistim_send_mwi_to_peer(struct unistimsession *s, unsigned int tick)
+static int unistim_send_mwi_to_peer(struct unistim_line *peer, unsigned int tick)
 {
        struct ast_event *event;
        int new;
        char *mailbox, *context;
-       struct unistim_line *peer = s->device->lines;
 
        context = mailbox = ast_strdupa(peer->mailbox);
        strsep(&context, "@");
-       if (ast_strlen_zero(context))
+       if (ast_strlen_zero(context)) {
                context = "default";
-
+       }
        event = ast_event_get_cached(AST_EVENT_MWI,
                AST_EVENT_IE_MAILBOX, AST_EVENT_IE_PLTYPE_STR, mailbox,
                AST_EVENT_IE_CONTEXT, AST_EVENT_IE_PLTYPE_STR, context,
@@ -4525,11 +5524,7 @@ static int unistim_send_mwi_to_peer(struct unistimsession *s, unsigned int tick)
        }
 
        peer->lastmsgssent = new;
-       if (new == 0) {
-               send_led_update(s, 0);
-       } else {
-               send_led_update(s, 1);
-       }
+       send_led_update(peer->parent->session, (new > 0));
 
        return 0;
 }
@@ -4552,18 +5547,21 @@ static struct ast_channel *unistim_new(struct unistim_subchannel *sub, int state
        }
        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);
-       if (unistimdebug)
-               ast_verb(0, "unistim_new sub=%d (%p) chan=%p\n", sub->subtype, sub, tmp);
+               l->parent->context, linkedid, l->amaflags, "USTM/%s@%s-%p", l->name, l->parent->name, sub);
+       if (unistimdebug) {
+               ast_verb(0, "unistim_new sub=%d (%p) chan=%p line=%s\n", sub->subtype, sub, tmp, l->name);
+       }
        if (!tmp) {
                ast_log(LOG_WARNING, "Unable to allocate channel structure\n");
                return NULL;
        }
 
        ast_format_cap_copy(ast_channel_nativeformats(tmp), l->cap);
-       if (ast_format_cap_is_empty(ast_channel_nativeformats(tmp)))
+       if (ast_format_cap_is_empty(ast_channel_nativeformats(tmp))) {
                ast_format_cap_copy(ast_channel_nativeformats(tmp), global_cap);
+       }
        ast_best_codec(ast_channel_nativeformats(tmp), &tmpfmt);
+
        if (unistimdebug) {
                char tmp1[256], tmp2[256], tmp3[256];
                ast_verb(0, "Best codec = %s from nativeformats %s (line cap=%s global=%s)\n",
@@ -4573,18 +5571,20 @@ static struct ast_channel *unistim_new(struct unistim_subchannel *sub, int state
                        ast_getformatname_multiple(tmp3, sizeof(tmp3), global_cap));
        }
        if ((sub->rtp) && (sub->subtype == 0)) {
-               if (unistimdebug)
+               if (unistimdebug) {
                        ast_verb(0, "New unistim channel with a previous rtp handle ?\n");
+               }
                ast_channel_internal_fd_set(tmp, 0, ast_rtp_instance_fd(sub->rtp, 0));
                ast_channel_internal_fd_set(tmp, 1, ast_rtp_instance_fd(sub->rtp, 1));
        }
-       if (sub->rtp)
+       if (sub->rtp) {
                ast_jb_configure(tmp, &global_jbconf);
-               
+       }
 /*      tmp->type = type; */
        ast_setstate(tmp, state);
-       if (state == AST_STATE_RING)
+       if (state == AST_STATE_RING) {
                ast_channel_rings_set(tmp, 1);
+       }
        ast_channel_adsicpe_set(tmp, AST_ADSI_UNAVAILABLE);
        ast_format_copy(ast_channel_writeformat(tmp), &tmpfmt);
        ast_format_copy(ast_channel_rawwriteformat(tmp), &tmpfmt);
@@ -4592,12 +5592,11 @@ static struct ast_channel *unistim_new(struct unistim_subchannel *sub, int state
        ast_format_copy(ast_channel_rawreadformat(tmp), &tmpfmt);
        ast_channel_tech_pvt_set(tmp, sub);
        ast_channel_tech_set(tmp, &unistim_tech);
-       if (!ast_strlen_zero(l->language))
-               ast_channel_language_set(tmp, l->language);
+
+       if (!ast_strlen_zero(l->parent->language)) {
+               ast_channel_language_set(tmp, l->parent->language);
+       }
        sub->owner = tmp;
-       ast_mutex_lock(&usecnt_lock);
-       usecnt++;
-       ast_mutex_unlock(&usecnt_lock);
        ast_update_use_count();
        ast_channel_callgroup_set(tmp, l->callgroup);
        ast_channel_pickupgroup_set(tmp, l->pickupgroup);
@@ -4618,8 +5617,9 @@ static struct ast_channel *unistim_new(struct unistim_subchannel *sub, int state
        }
        ast_channel_priority_set(tmp, 1);
        if (state != AST_STATE_DOWN) {
-               if (unistimdebug)
+               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", ast_channel_name(tmp));
                        ast_hangup(tmp);
@@ -4639,9 +5639,9 @@ static void *do_monitor(void *data)
        int reloading;
 
        /* Add an I/O event to our UDP socket */
-       if (unistimsock > -1)
+       if (unistimsock > -1) {
                ast_io_add(io, unistimsock, unistimsock_read, AST_IO_IN, NULL);
-
+       }
        /* This thread monitors our UDP socket and timers */
        for (;;) {
                /* This loop is executed at least every IDLE_WAITus (1s) or every time a packet is received */
@@ -4658,9 +5658,9 @@ static void *do_monitor(void *data)
                        if (cur->timeout <= tick) {
                                DEBUG_TIMER("Event for session %p\n", cur);
                                /* If the queue is empty, send a ping */
-                               if (cur->last_buf_available == 0)
+                               if (cur->last_buf_available == 0) {
                                        send_ping(cur);
-                               else {
+                               } else {
                                        if (send_retransmit(cur)) {
                                                DEBUG_TIMER("The chained link was modified, restarting...\n");
                                                cur = sessions;
@@ -4669,15 +5669,24 @@ static void *do_monitor(void *data)
                                        }
                                }
                        }
-                       if (dw_timeout > cur->timeout - tick)
+                       if (dw_timeout > cur->timeout - tick) {
                                dw_timeout = cur->timeout - tick;
+                       }
                        /* Checking if the phone is logged on for a new MWI */
                        if (cur->device) {
-                               if ((!ast_strlen_zero(cur->device->lines->mailbox)) &&
-                                       ((tick >= cur->device->lines->nextmsgcheck))) {
-                                       DEBUG_TIMER("Checking mailbox for MWI\n");
-                                       unistim_send_mwi_to_peer(cur, tick);
-                                       break;
+                               struct unistim_line *l;
+                               AST_LIST_LOCK(&cur->device->lines);
+                               AST_LIST_TRAVERSE(&cur->device->lines, l, list) {
+                                       if ((!ast_strlen_zero(l->mailbox)) && (tick >= l->nextmsgcheck)) {
+                                               DEBUG_TIMER("Checking mailbox for MWI\n");
+                                               unistim_send_mwi_to_peer(l, tick);
+                                               break;
+                                       }
+                               }
+                               AST_LIST_UNLOCK(&cur->device->lines);
+                               if (cur->device->nextdial && tick >= cur->device->nextdial) {
+                                       handle_call_outgoing(cur);
+                                       cur->device->nextdial = 0;
                                }
                        }
                        cur = cur->next;
@@ -4686,8 +5695,9 @@ static void *do_monitor(void *data)
                DEBUG_TIMER("Waiting for %dus\n", dw_timeout);
                res = dw_timeout;
                /* We should not wait more than IDLE_WAIT */
-               if ((res < 0) || (res > IDLE_WAIT))
+               if ((res < 0) || (res > IDLE_WAIT)) {
                        res = IDLE_WAIT;
+               }
                /* Wait for UDP messages for a maximum of res us */
                res = ast_io_wait(io, res);     /* This function will call unistimsock_read if a packet is received */
                /* Check for a reload request */
@@ -4710,8 +5720,9 @@ static int restart_monitor(void)
 {
        pthread_attr_t attr;
        /* If we're supposed to be stopped -- stay stopped */
-       if (monitor_thread == AST_PTHREADT_STOP)
+       if (monitor_thread == AST_PTHREADT_STOP) {
                return 0;
+       }
        if (ast_mutex_lock(&monlock)) {
                ast_log(LOG_WARNING, "Unable to lock monitor\n");
                return -1;
@@ -4743,7 +5754,8 @@ static int restart_monitor(void)
 static struct ast_channel *unistim_request(const char *type, struct ast_format_cap *cap, const struct ast_channel *requestor, const char *dest,
                                                                                   int *cause)
 {
-       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 tmp2[256];
@@ -4760,98 +5772,134 @@ static struct ast_channel *unistim_request(const char *type, struct ast_format_c
                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) {
-               if (unistimdebug)
-                       ast_verb(0, "Can't create channel : Busy !\n");
+       d = sub->parent->parent;
+       sub_ring = get_sub(d, SUB_RING);
+       sub_trans = get_sub(d, SUB_THREEWAY);
+       /* Another request already in progress */
+       if (!d->session) {
+               unistim_unalloc_sub(d, sub);
+               *cause = AST_CAUSE_CONGESTION;
+               return NULL;
+       }
+       if (sub_ring || sub_trans) {
+               if (unistimdebug) {
+                       ast_verb(0, "Can't create channel, request already in progress: Busy!\n");
+               }
+               unistim_unalloc_sub(d, sub);
+               *cause = AST_CAUSE_BUSY;
+               return NULL;
+       }
+        if (get_avail_softkey(d->session, sub->parent->name) == -1) {
+               if (unistimdebug) {
+                       ast_verb(0, "Can't create channel for line %s, all lines busy\n", sub->parent->name);
+               }
+               unistim_unalloc_sub(d, sub);
                *cause = AST_CAUSE_BUSY;
                return NULL;
        }
+       sub->subtype = SUB_RING;
+       sub->softkey = -1;
        ast_format_cap_copy(sub->parent->cap, cap);
        tmpc = unistim_new(sub, AST_STATE_DOWN, requestor ? ast_channel_linkedid(requestor) : NULL);
-       if (!tmpc)
+       if (!tmpc) {
                ast_log(LOG_WARNING, "Unable to make channel for '%s'\n", tmp);
-       if (unistimdebug)
+       }
+       if (unistimdebug) {
                ast_verb(0, "unistim_request owner = %p\n", sub->owner);
+       }
        restart_monitor();
 
        /* and finish */
        return tmpc;
 }
 
-static char *unistim_info(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
+static char *unistim_show_info(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
 {
        struct unistim_device *device = devices;
        struct unistim_line *line;
        struct unistim_subchannel *sub;
        struct unistimsession *s;
-       int i;
        struct ast_channel *tmp;
 
        switch (cmd) {
        case CLI_INIT:
                e->command = "unistim show info";
                e->usage =
-                       "Usage: unistim show info\n" 
-                       "       Dump internal structures.\n";
+                       "Usage: unistim show info\n"
+                       "       Dump internal structures.\n\n"
+                       "       device\n"
+                       "       ->line\n"
+                       "       -->sub\n"
+                       "       ==>key\n";
                return NULL;
 
        case CLI_GENERATE:
                return NULL;    /* no completion */
        }
 
-       if (a->argc != e->args)
+       if (a->argc != e->args) {
                return CLI_SHOWUSAGE;
-
-       ast_cli(a->fd, "Dumping internal structures :\ndevice\n->line\n-->sub\n");
+       }
+       ast_cli(a->fd, "Dumping internal structures:\n");
+       ast_mutex_lock(&devicelock);
        while (device) {
-               ast_cli(a->fd, "\nname=%s id=%s line=%p ha=%p sess=%p device=%p\n",
-                               device->name, device->id, device->lines, device->ha, device->session,
-                               device);
-               line = device->lines;
-               while (line) {
+               int i;
+
+               ast_cli(a->fd, "\nname=%s id=%s ha=%p sess=%p device=%p selected=%d height=%d\n",
+                               device->name, device->id, device->ha, device->session,
+                               device, device->selected, device->height);
+               AST_LIST_LOCK(&device->lines);
+               AST_LIST_TRAVERSE(&device->lines,line,list) {
                        char tmp2[256];
                        ast_cli(a->fd,
-                                       "->name=%s fullname=%s exten=%s callid=%s cap=%s device=%p line=%p\n",
+                                       "->name=%s fullname=%s exten=%s callid=%s cap=%s line=%p\n",
                                        line->name, line->fullname, line->exten, line->cid_num,
-                                       ast_getformatname_multiple(tmp2, sizeof(tmp2), line->cap), line->parent, line);
-                       for (i = 0; i < MAX_SUBS; i++) {
-                               sub = line->subs[i];
-                               if (!sub)
-                                       continue;
-                               if (!sub->owner)
-                                       tmp = (void *) -42;
-                               else
-                                       tmp = sub->owner->_bridge;
-                               if (sub->subtype != i)
-                                       ast_cli(a->fd, "Warning ! subchannel->subs[%d] have a subtype=%d\n", i,
-                                                       sub->subtype);
-                               ast_cli(a->fd,
-                                               "-->subtype=%d chan=%p rtp=%p bridge=%p line=%p alreadygone=%d\n",
-                                               sub->subtype, sub->owner, sub->rtp, tmp, sub->parent,
-                                               sub->alreadygone);
+                                       ast_getformatname_multiple(tmp2, sizeof(tmp2), line->cap), line);
+               }
+               AST_LIST_UNLOCK(&device->lines);
+
+               AST_LIST_LOCK(&device->subs);
+               AST_LIST_TRAVERSE(&device->subs, sub, list) {
+                       if (!sub) {
+                               continue;
+                       }
+                       if (!sub->owner) {
+                               tmp = (void *) -42;
+                       } else {
+                               tmp = sub->owner->_bridge;
                        }
-                       line = line->next;
+                       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=%#-4x label=%-10s number=%-5s sub=%p line=%p\n",
+                               i, device->softkeydevice[i], device->softkeyicon[i], device->softkeylabel[i], device->softkeynumber[i],
+                               device->ssub[i], device->sline[i]);
                }
                device = device->next;
        }
+       ast_mutex_unlock(&devicelock);
        ast_cli(a->fd, "\nSessions:\n");
        ast_mutex_lock(&sessionlock);
        s = sessions;
        while (s) {
                ast_cli(a->fd,
-                               "sin=%s timeout=%u state=%d macaddr=%s device=%p session=%p\n",
-                               ast_inet_ntoa(s->sin.sin_addr), s->timeout, s->state, s->macaddr,
-                               s->device, s);
+                               "sin=%s timeout=%u state=%s macaddr=%s device=%s session=%p\n",
+                               ast_inet_ntoa(s->sin.sin_addr), s->timeout, ptestate_tostr(s->state), s->macaddr,
+                               s->device->name, s);
                s = s->next;
        }
        ast_mutex_unlock(&sessionlock);
@@ -4859,6 +5907,38 @@ static char *unistim_info(struct ast_cli_entry *e, int cmd, struct ast_cli_args
        return CLI_SUCCESS;
 }
 
+static char *unistim_show_devices(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
+{
+       struct unistim_device *device = devices;
+
+       switch (cmd) {
+       case CLI_INIT:
+               e->command = "unistim show devices";
+               e->usage =
+                       "Usage: unistim show devices\n"
+                       "       Lists all known Unistim devices.\n";
+               return NULL;
+       case CLI_GENERATE:
+               return NULL;    /* no completion */
+       }
+
+       if (a->argc != e->args)
+               return CLI_SHOWUSAGE;
+
+       ast_cli(a->fd, "%-20.20s %-20.20s %-15.15s %s\n", "Name/username", "MAC", "Host", "Status");
+       ast_mutex_lock(&devicelock);
+       while (device) {
+               ast_cli(a->fd, "%-20.20s %-20.20s %-15.15s %s\n",
+                       device->name, device->id,
+                       (!device->session) ? "(Unspecified)" : ast_inet_ntoa(device->session->sin.sin_addr),
+                       (!device->session) ? "UNKNOWN" : "OK");
+               device = device->next;
+       }
+       ast_mutex_unlock(&devicelock);
+
+       return CLI_SUCCESS;
+}
+
 static char *unistim_sp(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
 {
        BUFFSEND;
@@ -4878,17 +5958,17 @@ static char *unistim_sp(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a
        case CLI_GENERATE:
                return NULL;    /* no completion */
        }
-       
-       if (a->argc < 5)
-               return CLI_SHOWUSAGE;
 
-       if (strlen(a->argv[3]) < 9)
+       if (a->argc < 5) {
                return CLI_SHOWUSAGE;
-
+       }
+       if (strlen(a->argv[3]) < 9) {
+               return CLI_SHOWUSAGE;
+       }
        len = strlen(a->argv[4]);
-       if (len % 2)
+       if (len % 2) {
                return CLI_SHOWUSAGE;
-
+       }
        ast_copy_string(tmp, a->argv[3] + 5, sizeof(tmp));
        sub = find_subchannel_by_name(tmp);
        if (!sub) {
@@ -4902,16 +5982,18 @@ static char *unistim_sp(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a
        ast_cli(a->fd, "Sending '%s' to %s (%p)\n", a->argv[4], tmp, sub->parent->parent->session);
        for (i = 0; i < len; i++) {
                c = a->argv[4][i];
-               if (c >= 'a')
+               if (c >= 'a') {
                        c -= 'a' - 10;
-               else
+               } else {
                        c -= '0';
+               }
                i++;
                cc = a->argv[4][i];
-               if (cc >= 'a')
+               if (cc >= 'a') {
                        cc -= 'a' - 10;
-               else
+               } else {
                        cc -= '0';
+               }
                tmp[j++] = (c << 4) | cc;
        }
        memcpy(buffsend + SIZE_HEADER, tmp, j);
@@ -4933,18 +6015,18 @@ static char *unistim_do_debug(struct ast_cli_entry *e, int cmd, struct ast_cli_a
                return NULL;    /* no completion */
        }
 
-       if (a->argc != e->args)
+       if (a->argc != e->args) {
                return CLI_SHOWUSAGE;
-
+       }
        if (!strcasecmp(a->argv[3], "on")) {
                unistimdebug = 1;
                ast_cli(a->fd, "UNISTIM Debugging Enabled\n");
        } else if (!strcasecmp(a->argv[3], "off")) {
                unistimdebug = 0;
                ast_cli(a->fd, "UNISTIM Debugging Disabled\n");
-       } else
+       } else {
                return CLI_SHOWUSAGE;
-
+       }
        return CLI_SUCCESS;
 }
 
@@ -4966,25 +6048,18 @@ static char *unistim_reload(struct ast_cli_entry *e, int cmd, struct ast_cli_arg
                return NULL;    /* no completion */
        }
 
-       if (e && a && a->argc != e->args)
+       if (e && a && a->argc != e->args) {
                return CLI_SHOWUSAGE;
-
-       if (unistimdebug)
-               ast_verb(0, "reload unistim\n");
-
-       ast_mutex_lock(&unistim_reload_lock);
-       if (!unistim_reloading)
-               unistim_reloading = 1;
-       ast_mutex_unlock(&unistim_reload_lock);
-
-       restart_monitor();
+       }
+       reload();
 
        return CLI_SUCCESS;
 }
 
 static struct ast_cli_entry unistim_cli[] = {
        AST_CLI_DEFINE(unistim_reload, "Reload UNISTIM configuration"),
-       AST_CLI_DEFINE(unistim_info, "Show UNISTIM info"),
+       AST_CLI_DEFINE(unistim_show_info, "Show UNISTIM info"),
+       AST_CLI_DEFINE(unistim_show_devices, "Show UNISTIM devices"),
        AST_CLI_DEFINE(unistim_sp, "Send packet (for reverse engineering)"),
        AST_CLI_DEFINE(unistim_do_debug, "Toggle UNITSTIM debugging"),
 };
@@ -4992,23 +6067,26 @@ static struct ast_cli_entry unistim_cli[] = {
 static void unquote(char *out, const char *src, int maxlen)
 {
        int len = strlen(src);
-       if (!len)
+       if (!len) {
                return;
+       }
        if ((len > 1) && src[0] == '\"') {
                /* This is a quoted string */
                src++;
                /* Don't take more than what's there */
                len--;
-               if (maxlen > len - 1)
+               if (maxlen > len - 1) {
                        maxlen = len - 1;
+               }
                memcpy(out, src, maxlen);
                ((char *) out)[maxlen] = '\0';
-       } else
+       } else {
                memcpy(out, src, maxlen);
+       }
        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;
@@ -5021,9 +6099,9 @@ static int ParseBookmark(const char *text, struct unistim_device *d)
        /* Position specified ? */
        if ((len > 2) && (line[1] == '@')) {
                p = line[0];
-               if ((p >= '0') && (p <= '5'))
+               if ((p >= '0') && (p <= '5')) {
                        p -= '0';
-               else {
+               } else {
                        ast_log(LOG_WARNING,
                                        "Invalid position for bookmark : must be between 0 and 5\n");
                        return 0;
@@ -5036,8 +6114,9 @@ static int ParseBookmark(const char *text, struct unistim_device *d)
        } else {
                /* No position specified, looking for a free slot */
                for (p = 0; p <= 5; p++) {
-                       if (!d->softkeyicon[p])
+                       if (!d->softkeyicon[p]) {
                                break;
+                       }
                }
                if (p > 5) {
                        ast_log(LOG_WARNING, "No more free bookmark position\n");
@@ -5063,9 +6142,9 @@ static int ParseBookmark(const char *text, struct unistim_device *d)
        }
 
        at = strchr(number, '@');
-       if (!at)
+       if (!at) {
                d->softkeyicon[p] = FAV_ICON_SHARP;     /* default icon */
-       else {
+       } else {
                *at = '\0';
                at++;
                icon = at;
@@ -5073,18 +6152,19 @@ static int ParseBookmark(const char *text, struct unistim_device *d)
                        ast_log(LOG_NOTICE, "Bookmark entry '%s' has no icon value\n", text);
                        return 0;
                }
-               if (strncmp(icon, "USTM/", 5))
+               if (strncmp(icon, "USTM/", 5)) {
                        d->softkeyicon[p] = atoi(icon);
-               else {
+               } else {
                        d->softkeyicon[p] = 1;
                        ast_copy_string(d->softkeydevice[p], icon + 5, sizeof(d->softkeydevice[p]));
                }
        }
        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",
+       if (unistimdebug) {
+               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;
 }
 
@@ -5093,6 +6173,7 @@ static void finish_bookmark(void)
 {
        struct unistim_device *d = devices;
        int i;
+       ast_mutex_lock(&devicelock);
        while (d) {
                for (i = 0; i < 6; i++) {
                        if (d->softkeyicon[i] == 1) {   /* Something for us */
@@ -5105,25 +6186,39 @@ static void finish_bookmark(void)
                                        }
                                        d2 = d2->next;
                                }
-                               if (d->sp[i] == NULL)
+                               if (d->sp[i] == NULL) {
                                        ast_log(LOG_NOTICE, "Bookmark entry with device %s not found\n",
                                                        d->softkeydevice[i]);
+                               }
                        }
                }
                d = d->next;
        }
+       ast_mutex_unlock(&devicelock);
 }
 
+static struct unistim_line *find_line_by_number(struct unistim_device *d, const char *val) {
+       struct unistim_line *l, *ret = NULL;
+
+       AST_LIST_LOCK(&d->lines);
+       AST_LIST_TRAVERSE(&d->lines, l, list) {
+               if (!strcmp(l->name, val)) {
+                       ret = l;
+                       break;
+               }
+       }
+       AST_LIST_UNLOCK(&d->lines);
+       return ret;
+}
 
 static struct unistim_device *build_device(const char *cat, const struct ast_variable *v)
 {
        struct unistim_device *d;
-       struct unistim_line *l = NULL;
+       struct unistim_line *l = NULL, *lt = NULL;
        int create = 1;
-       int nbsoftkey, dateformat, timeformat, callhistory;
+       int nbsoftkey, dateformat, timeformat, callhistory, sharpdial, linecnt;
        char linelabel[AST_MAX_EXTENSION];
-       char context[AST_MAX_EXTENSION];
-       char ringvolume, ringstyle;
+       char ringvolume, ringstyle, cwvolume, cwstyle;
 
        /* First, we need to know if we already have this name in our list */
        /* Get a lock for the device chained list */
@@ -5140,255 +6235,282 @@ static struct unistim_device *build_device(const char *cat, const struct ast_var
                        }
                        /* we're reloading right now */
                        create = 0;
-                       l = d->lines;
                        break;
                }
                d = d->next;
        }
+       if (!(lt = ast_calloc(1, sizeof(*lt)))) {
+               return NULL;
+       }
        ast_mutex_unlock(&devicelock);
        if (create) {
-               if (!(d = ast_calloc(1, sizeof(*d))))
-                       return NULL;
-
-               if (!(l = unistim_line_alloc())) {
-                       ast_free(d);
+               if (!(d = ast_calloc(1, sizeof(*d)))) {
                        return NULL;
                }
+               ast_mutex_init(&d->lock);
                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);
+                       unistim_line_destroy(l);
+               }
+               AST_LIST_TRAVERSE_SAFE_END
+               AST_LIST_UNLOCK(&d->lines);
+
+               /* reset bookmarks */
+               memset(d->softkeylabel, 0, sizeof(d->softkeylabel));
+               memset(d->softkeynumber, 0, sizeof(d->softkeynumber));
+               memset(d->softkeyicon, 0, sizeof(d->softkeyicon));
+               memset(d->softkeydevice, 0, sizeof(d->softkeydevice));
+               memset(d->ssub, 0, sizeof(d->ssub));
+               memset(d->sline, 0, sizeof(d->sline));
+               memset(d->sp, 0, sizeof(d->sp));
        }
-       ast_copy_string(context, DEFAULTCONTEXT, sizeof(context));
+
+       ast_copy_string(d->context, DEFAULTCONTEXT, sizeof(d->context));
        d->contrast = -1;
        d->output = OUTPUT_HANDSET;
        d->previous_output = OUTPUT_HANDSET;
        d->volume = VOLUME_LOW;
        d->mute = MUTE_OFF;
        d->height = DEFAULTHEIGHT;
+       d->selected = -1;
        linelabel[0] = '\0';
        dateformat = 1;
        timeformat = 1;
        ringvolume = 2;
+       cwvolume = 1;
        callhistory = 1;
+       sharpdial = 0;
        ringstyle = 3;
+       cwstyle = 2;
        nbsoftkey = 0;
+       linecnt = 0;
        while (v) {
-               if (!strcasecmp(v->name, "rtp_port"))
+               if (!strcasecmp(v->name, "rtp_port")) {
                        d->rtp_port = atoi(v->value);
-               else if (!strcasecmp(v->name, "rtp_method"))
+               } else if (!strcasecmp(v->name, "rtp_method")) {
                        d->rtp_method = atoi(v->value);
-               else if (!strcasecmp(v->name, "status_method"))
+               } else if (!strcasecmp(v->name, "status_method")) {
                        d->status_method = atoi(v->value);
-               else if (!strcasecmp(v->name, "device"))
+               } else if (!strcasecmp(v->name, "device")) {
                        ast_copy_string(d->id, v->value, sizeof(d->id));
-               else if (!strcasecmp(v->name, "tn"))
+               } else if (!strcasecmp(v->name, "tn")) {
                        ast_copy_string(d->extension_number, v->value, sizeof(d->extension_number));
-               else if (!strcasecmp(v->name, "permit") || !strcasecmp(v->name, "deny"))
+               } else if (!strcasecmp(v->name, "permit") || !strcasecmp(v->name, "deny")) {
                        d->ha = ast_append_ha(v->name, v->value, d->ha, NULL);
-               else if (!strcasecmp(v->name, "context"))
-                       ast_copy_string(context, v->value, sizeof(context));
-               else if (!strcasecmp(v->name, "maintext0"))
+               } else if (!strcasecmp(v->name, "context")) {
+                       ast_copy_string(d->context, v->value, sizeof(d->context));
+               } else if (!strcasecmp(v->name, "maintext0")) {
                        unquote(d->maintext0, v->value, sizeof(d->maintext0) - 1);
-               else if (!strcasecmp(v->name, "maintext1"))
+               } else if (!strcasecmp(v->name, "maintext1")) {
                        unquote(d->maintext1, v->value, sizeof(d->maintext1) - 1);
-               else if (!strcasecmp(v->name, "maintext2"))
+               } else if (!strcasecmp(v->name, "maintext2")) {
                        unquote(d->maintext2, v->value, sizeof(d->maintext2) - 1);
-               else if (!strcasecmp(v->name, "titledefault"))
+               } else if (!strcasecmp(v->name, "titledefault")) {
                        unquote(d->titledefault, v->value, sizeof(d->titledefault) - 1);
-               else if (!strcasecmp(v->name, "dateformat"))
+               } else if (!strcasecmp(v->name, "dateformat")) {
                        dateformat = atoi(v->value);
-               else if (!strcasecmp(v->name, "timeformat"))
+               } else if (!strcasecmp(v->name, "timeformat")) {
                        timeformat = atoi(v->value);
-               else if (!strcasecmp(v->name, "contrast")) {
+               } else if (!strcasecmp(v->name, "contrast")) {
                        d->contrast = atoi(v->value);
                        if ((d->contrast < 0) || (d->contrast > 15)) {
                                ast_log(LOG_WARNING, "constrast must be beetween 0 and 15");
                                d->contrast = 8;
                        }
-               } else if (!strcasecmp(v->name, "nat"))
+               } else if (!strcasecmp(v->name, "nat")) {
                        d->nat = ast_true(v->value);
-               else if (!strcasecmp(v->name, "ringvolume"))
+               } else if (!strcasecmp(v->name, "ringvolume")) {
                        ringvolume = atoi(v->value);
-               else if (!strcasecmp(v->name, "ringstyle"))
+               } 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")) {
-                       if (!strcasecmp(v->value, "asreceived"))
-                               l->cid_num[0] = '\0';
-                       else
-                               ast_copy_string(l->cid_num, v->value, sizeof(l->cid_num));
-               } else if (!strcasecmp(v->name, "language"))
-                       ast_copy_string(l->language, v->value, sizeof(l->language));
-               else if (!strcasecmp(v->name, "country"))
+               } else if (!strcasecmp(v->name, "sharpdial")) {
+                       sharpdial = ast_true(v->value) ? 1 : 0;
+               } else if (!strcasecmp(v->name, "callerid")) {
+                       if (!strcasecmp(v->value, "asreceived")) {
+                               lt->cid_num[0] = '\0';
+                       } else {
+                               ast_copy_string(lt->cid_num, v->value, sizeof(lt->cid_num));
+                       }
+               } else if (!strcasecmp(v->name, "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));
-               else if (!strcasecmp(v->name, "amaflags")) {
+               } else if (!strcasecmp(v->name, "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);
-                       if (y < 0)
+                       if (y < 0) {
                                ast_log(LOG_WARNING, "Invalid AMA flags: %s at line %d\n", v->value,
                                                v->lineno);
-                       else
-                               l->amaflags = y;
-               } else if (!strcasecmp(v->name, "musiconhold"))
-                       ast_copy_string(l->musicclass, v->value, sizeof(l->musicclass));
-               else if (!strcasecmp(v->name, "callgroup"))
-                       l->callgroup = ast_get_group(v->value);
-               else if (!strcasecmp(v->name, "pickupgroup"))
-                       l->pickupgroup = ast_get_group(v->value);
-               else if (!strcasecmp(v->name, "mailbox"))
-                       ast_copy_string(l->mailbox, v->value, sizeof(l->mailbox));
-               else if (!strcasecmp(v->name, "parkinglot"))
-                       ast_copy_string(l->parkinglot, v->value, sizeof(l->parkinglot));
-               else if (!strcasecmp(v->name, "linelabel"))
+                       } else {
+                               lt->amaflags = y;
+                       }
+               } else if (!strcasecmp(v->name, "musiconhold")) {
+                       ast_copy_string(lt->musicclass, v->value, sizeof(lt->musicclass));
+               } else if (!strcasecmp(v->name, "callgroup")) {
+                       lt->callgroup = ast_get_group(v->value);
+               } else if (!strcasecmp(v->name, "pickupgroup")) {
+                       lt->pickupgroup = ast_get_group(v->value);
+               } else if (!strcasecmp(v->name, "mailbox")) {
+                       ast_copy_string(lt->mailbox, v->value, sizeof(lt->mailbox));
+               } else if (!strcasecmp(v->name, "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")) {
-                       if (!strcasecmp(v->value, "none"))
+               } else if (!strcasecmp(v->name, "extension")) {
+                       if (!strcasecmp(v->value, "none")) {
                                d->extension = EXTENSION_NONE;
-                       else if (!strcasecmp(v->value, "ask"))
+                       } else if (!strcasecmp(v->value, "ask")) {
                                d->extension = EXTENSION_ASK;
-                       else if (!strcasecmp(v->value, "line"))
+                       } else if (!strcasecmp(v->value, "line")) {
                                d->extension = EXTENSION_LINE;
-                       else
+                       } else {
                                ast_log(LOG_WARNING, "Unknown extension option.\n");
+                       }
                } else if (!strcasecmp(v->name, "bookmark")) {
-                       if (nbsoftkey > 5)
+                       if (nbsoftkey > 5) {
                                ast_log(LOG_WARNING,
                                                "More than 6 softkeys defined. Ignoring new entries.\n");
-                       else {
-                               if (ParseBookmark(v->value, d))
+                       } else {
+                               if (parse_bookmark(v->value, d)) {
                                        nbsoftkey++;
+                               }
                        }
                } else if (!strcasecmp(v->name, "line")) {
                        int len = strlen(linelabel);
+                       int create_line = 0;
 
-                       if (nbsoftkey) {
-                               ast_log(LOG_WARNING,
-                                               "You must use bookmark AFTER line=>. Only one line is supported in this version\n");
-                               if (create) {
+                       l = find_line_by_number(d, v->value);
+                       if (!l) { /* If line still not exists */
+                               if (!(l = unistim_line_alloc())) {
                                        ast_free(d);
-                                       unistim_line_destroy(l);
+                                       ast_free(lt);
+                                       return NULL;
                                }
-                               return NULL;
-                       }
-                       if (create) {
+                               lt->cap = l->cap;
+                               memcpy(l, lt, sizeof(*l));
                                ast_mutex_init(&l->lock);
-                       } else {
-                               d->to_delete = 0;
-                               /* reset bookmarks */
-                               memset(d->softkeylabel, 0, sizeof(d->softkeylabel));
-                               memset(d->softkeynumber, 0, sizeof(d->softkeynumber));
-                               memset(d->softkeyicon, 0, sizeof(d->softkeyicon));
-                               memset(d->softkeydevice, 0, sizeof(d->softkeydevice));
-                               memset(d->sp, 0, sizeof(d->sp));
+                               create_line = 1;
                        }
-                       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;
-                       if (!len)                      /* label is undefined ? */
-                               ast_copy_string(d->softkeylabel[0], v->value, sizeof(d->softkeylabel[0]));
-                       else {
+                       d->to_delete = 0;
+
+                       /* Set softkey info for new line*/
+                       d->sline[nbsoftkey] = l;
+                       d->softkeyicon[nbsoftkey] = FAV_LINE_ICON;
+                       if (!len) {       /* label is undefined ? */
+                               ast_copy_string(d->softkeylabel[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;
                                        }
-                               &n