Massive changes in chan_unistim channel driver. Include many fixes in channel driver...
authorIgor Goncharovskiy <igor.goncharovsky@gmail.com>
Mon, 12 Mar 2012 17:01:26 +0000 (17:01 +0000)
committerIgor Goncharovskiy <igor.goncharovsky@gmail.com>
Mon, 12 Mar 2012 17:01:26 +0000 (17:01 +0000)
 * Added ability to use multiple lines on phone, so for one device in configuration multiple lines can be defined, it allows to have multiple calls on one phone, callwaiting and switching between calls.
 * Added ability for translation on-screen menu to multiple languages. Tested on Russian languages.  Supported encodings: ISO 8859-1, ISO 8859-2, ISO 8859-4, ISO 8859-5, ISO 2022-JP. Language controlled by 'language' and on-screen menu of phone
 * Other described in CHANGES file

Testing done by issue tracker users: ibercom, scsiborg, idarwin, TeknoJuce, c0rnoTa.
Tested on production system by Jonn Taylor (jonnt) using phone models: Nortel i2004, 1120E and 1140E.

(closes issue ASTERISK-16890)

Review: https://reviewboard.asterisk.org/r/1243/

git-svn-id: https://origsvn.digium.com/svn/asterisk/trunk@358766 65c4cc65-6c06-0410-ace0-fbb531ad65f3

CHANGES
UPGRADE.txt
channels/chan_unistim.c
configs/unistim.conf.sample
contrib/unistimLang/en.po [new file with mode: 0644]
contrib/unistimLang/ru.po [new file with mode: 0644]
contrib/unistimLang/ru.po.utf8 [new file with mode: 0644]

diff --git a/CHANGES b/CHANGES
index dccfb5c..9299077 100644 (file)
--- a/CHANGES
+++ b/CHANGES
@@ -80,6 +80,23 @@ Chan_dahdi changes
  * Added dialtone_detect option for analog ports to disconnect incoming
    calls when dialtone is detected.
 
+Chan_unistim changes
+--------------------
+ * Added ability to use multiple lines on phone, so for one device in 
+   configuration multiple lines can be defined, it allows to have multiple calls
+   on one phone, callwaiting and switching between calls.
+ * Added option 'sharpdial' allowing end dialing by pressing # key
+ * Added options 'cwstyle', 'cwvolume' controlling callwaiting appearance
+ * Added global 'debug' option, that enables debug in channel driver
+ * Added ability for translation on-screen menu to multiple languages. Tested on
+   Russian languages.  Supported encodings: ISO 8859-1, ISO 8859-2, ISO 8859-4, 
+   ISO 8859-5, ISO 2022-JP. Language controlled by 'language' and on-screen 
+   menu of phone
+ * Reworked dialing number input: added dialing by timeout, immediate dial on 
+   on dialplan compare, phone number length now not limited by screen size
+ * Added ability for pickup a call using fetures.conf defined value and 
+   on-screen key
+
 Codec changes
 -------------
  * Codec lists may now be modified by the '!' character, to allow succinct
index 6c76841..c935d12 100644 (file)
@@ -83,6 +83,10 @@ SIP
    a call is terminated due to RTP stream inactivity or SIP session timer
    expiration.
 
+chan_unistim
+ - Due to massive update in chan_unistim phone keys functions and on-screen 
+   information changed.
+
 users.conf:
  - A defined user with hasvoicemail=yes now finally uses a Gosub to stdexten
    as documented in extensions.conf.sample since v1.6.0 instead of a Macro as
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 ? */
-