Merge tag 'android-4.2.2_r1' of https://android.googlesource.com/platform/external/bluetooth/bluedroid into 1.1

Android 4.2.2 release 1

25 files changed, 537 insertions, 1353 deletions

diff --git a/bta/av/bta_av_int.h b/bta/av/bta_av_int.h
index 05eb114..9ce7b2a 100644
--- a/bta/av/bta_av_int.h
+++ b/bta/av/bta_av_int.h
@@ -136,7 +136,7 @@ enum
 #define BTA_AV_SECURITY_MAX_LEN     400
 
 /* check number of buffers queued at L2CAP when this amount of buffers are queued to L2CAP */
-#define BTA_AV_QUEUE_DATA_CHK_NUM   5
+#define BTA_AV_QUEUE_DATA_CHK_NUM   L2CAP_HIGH_PRI_MIN_XMIT_QUOTA
 
 /* the number of ACL links with AVDT */
 #define BTA_AV_NUM_LINKS            AVDT_NUM_LINKS
diff --git a/bta/include/bta_jv_api.h b/bta/include/bta_jv_api.h
index 73a9164..7533a61 100644
--- a/bta/include/bta_jv_api.h
+++ b/bta/include/bta_jv_api.h
@@ -58,7 +58,7 @@ typedef UINT8 tBTA_JV_STATUS;
 
 /* */
 #ifndef BTA_JV_MAX_RFC_SR_SESSION
-#define BTA_JV_MAX_RFC_SR_SESSION   3
+#define BTA_JV_MAX_RFC_SR_SESSION   MAX_BD_CONNECTIONS
 #endif
 
 /* BTA_JV_MAX_RFC_SR_SESSION can not be bigger than MAX_BD_CONNECTIONS */
diff --git a/bta/jv/bta_jv_act.c b/bta/jv/bta_jv_act.c
index 77d18d9..51382af 100644
--- a/bta/jv/bta_jv_act.c
+++ b/bta/jv/bta_jv_act.c
@@ -44,15 +44,6 @@
 #include "avct_api.h"
 #include "avdt_api.h"
 
-
-#include <cutils/log.h>
-#define info(fmt, ...)  ALOGI ("%s: " fmt,__FUNCTION__,  ## __VA_ARGS__)
-#define debug(fmt, ...) ALOGD ("%s: " fmt,__FUNCTION__,  ## __VA_ARGS__)
-#define error(fmt, ...) ALOGE ("## ERROR : %s: " fmt "##",__FUNCTION__,  ## __VA_ARGS__)
-#define asrt(s) if(!(s)) ALOGE ("## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
-
-
-
 #define HDL2CB(handle) \
     UINT32  __hi = ((handle) & BTA_JV_RFC_HDL_MASK) - 1; \
     UINT32  __si = BTA_JV_RFC_HDL_TO_SIDX(handle); \
@@ -64,7 +55,7 @@ static inline void logu(const char* title, const uint8_t * p_uuid)
 {
     char uuids[128];
     uuid_to_string((bt_uuid_t*)p_uuid, uuids);
-    ALOGD("%s: %s", title, uuids);
+    APPL_TRACE_DEBUG2("%s: %s", title, uuids);
 }
 
 
@@ -295,41 +286,42 @@ tBTA_JV_STATUS bta_jv_free_rfc_cb(tBTA_JV_RFC_CB *p_cb)
     {
         APPL_TRACE_DEBUG2( "[%d]: port=%d", i, p_cb->rfc_hdl[i]);
         if (p_cb->rfc_hdl[i])
+        {
             bta_jv_free_rfc_pcb (&bta_jv_cb.port_cb[p_cb->rfc_hdl[i] - 1]);
+            p_cb->rfc_hdl[i] = 0;
+        }
     }
 
     p_cb->scn = 0;
     bta_jv_free_sec_id(&p_cb->sec_id);
     p_cb->p_cback = NULL;
     p_cb->handle = 0;
-
     return status;
 }
 static tBTA_JV_STATUS bta_jv_free_rfc_listen_cb(tBTA_JV_RFC_CB *p_cb)
 {
     tBTA_JV_STATUS status = BTA_JV_SUCCESS;
     UINT8           i;
-    debug( "max_sess:%d", p_cb->max_sess);
+    APPL_TRACE_DEBUG1("max_sess:%d", p_cb->max_sess);
     for (i=0; i<p_cb->max_sess; i++)
     {
-        APPL_TRACE_DEBUG2( "[%d]: port=%d", i, p_cb->rfc_hdl[i]);
+        APPL_TRACE_DEBUG2( "bta_jv_free_rfc_listen_cb: [%d]: port=%d", i, p_cb->rfc_hdl[i]);
         if (p_cb->rfc_hdl[i])
         {
             tBTA_JV_PCB *p_pcb = &bta_jv_cb.port_cb[p_cb->rfc_hdl[i] - 1];
             if(p_pcb->state == BTA_JV_ST_SR_LISTEN)
             {
-                debug( "free listen pcb: scn:%d, ueser_data:%d", p_cb->scn, (int)p_pcb->user_data);
+                APPL_TRACE_DEBUG2("free listen pcb: scn:%d, ueser_data:%d", p_cb->scn, (int)p_pcb->user_data);
                 p_pcb->user_data = 0;
                 bta_jv_free_rfc_pcb (p_pcb);
                 p_cb->max_sess = 1;
+                p_cb->rfc_hdl[i] = 0;
                 break;
             }
         }
     }
-    //p_cb->scn = 0;
+
     bta_jv_free_sec_id(&p_cb->sec_id);
-    //p_cb->p_cback = NULL;
-    //p_cb->handle = 0;
     return status;
 }
 
@@ -763,7 +755,7 @@ static inline tBT_UUID shorten_sdp_uuid(const tBT_UUID* u)
        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB };
 
     logu("in, uuid:", u);
-    debug("uuid len:%d", u->len);
+    APPL_TRACE_DEBUG1("uuid len:%d", u->len);
     if(u->len == 16)
     {
         if(memcmp(&u->uu.uuid128[4], &bt_base_uuid[4], 12) == 0)
@@ -776,7 +768,7 @@ static inline tBT_UUID shorten_sdp_uuid(const tBT_UUID* u)
                 uint16_t u16;
                 memcpy(&u16, &u->uu.uuid128[2], sizeof(u16));
                 su.uu.uuid16 = ntohs(u16);
-                debug("shorten to 16 bits uuid: %x", su.uu.uuid16);
+                APPL_TRACE_DEBUG1("shorten to 16 bits uuid: %x", su.uu.uuid16);
             }
             else
             {
@@ -784,12 +776,12 @@ static inline tBT_UUID shorten_sdp_uuid(const tBT_UUID* u)
                 uint32_t u32;
                 memcpy(&u32, &u->uu.uuid128[0], sizeof(u32));
                 su.uu.uuid32 = ntohl(u32);
-                debug("shorten to 32 bits uuid: %x", su.uu.uuid32);
+                APPL_TRACE_DEBUG1("shorten to 32 bits uuid: %x", su.uu.uuid32);
             }
             return su;
         }
     }
-    debug("cannot shorten none-reserved 128 bits uuid");
+    APPL_TRACE_DEBUG0("cannot shorten none-reserved 128 bits uuid");
     return *u;
 }
 
@@ -807,14 +799,14 @@ static void bta_jv_start_discovery_cback(UINT16 result, void * user_data)
     tBTA_JV_STATUS status;
     UINT8          old_sdp_act = bta_jv_cb.sdp_active;
 
-    debug( "bta_jv_start_discovery_cback res: 0x%x", result);
+    APPL_TRACE_DEBUG1("bta_jv_start_discovery_cback res: 0x%x", result);
 
     bta_jv_cb.sdp_active = BTA_JV_SDP_ACT_NONE;
     if(bta_jv_cb.p_dm_cback)
     {
         if (old_sdp_act == BTA_JV_SDP_ACT_CANCEL)
         {
-            debug("BTA_JV_SDP_ACT_CANCEL");
+            APPL_TRACE_DEBUG0("BTA_JV_SDP_ACT_CANCEL");
             status = BTA_JV_SUCCESS;
             bta_jv_cb.p_dm_cback(BTA_JV_CANCEL_DISCVRY_EVT, (tBTA_JV *)&status, user_data);
         }
@@ -831,7 +823,7 @@ static void bta_jv_start_discovery_cback(UINT16 result, void * user_data)
                 tBT_UUID su = shorten_sdp_uuid(&bta_jv_cb.uuid);
                 logu("shorten uuid:", su.uu.uuid128);
                 p_sdp_rec = SDP_FindServiceUUIDInDb(p_bta_jv_cfg->p_sdp_db, &su, p_sdp_rec);
-                debug("p_sdp_rec:%p", p_sdp_rec);
+                APPL_TRACE_DEBUG1("p_sdp_rec:%p", p_sdp_rec);
                 if(p_sdp_rec && SDP_FindProtocolListElemInRec(p_sdp_rec, UUID_PROTOCOL_RFCOMM, &pe))
                 {
                     dcomp.scn = (UINT8) pe.params[0];
@@ -859,7 +851,7 @@ static void bta_jv_start_discovery_cback(UINT16 result, void * user_data)
 void bta_jv_start_discovery(tBTA_JV_MSG *p_data)
 {
     tBTA_JV_STATUS status = BTA_JV_FAILURE;
-    debug("in, sdp_active:%d", bta_jv_cb.sdp_active);
+    APPL_TRACE_DEBUG1("bta_jv_start_discovery in, sdp_active:%d", bta_jv_cb.sdp_active);
     if (bta_jv_cb.sdp_active != BTA_JV_SDP_ACT_NONE)
     {
         /* SDP is still in progress */
@@ -877,7 +869,7 @@ void bta_jv_start_discovery(tBTA_JV_MSG *p_data)
     }
 */
     /* init the database/set up the filter */
-    debug("call SDP_InitDiscoveryDb, p_data->start_discovery.num_uuid:%d",
+    APPL_TRACE_DEBUG1("call SDP_InitDiscoveryDb, p_data->start_discovery.num_uuid:%d",
         p_data->start_discovery.num_uuid);
     SDP_InitDiscoveryDb (p_bta_jv_cfg->p_sdp_db, p_bta_jv_cfg->sdp_db_size,
                     p_data->start_discovery.num_uuid, p_data->start_discovery.uuid_list, 0, NULL);
@@ -1221,15 +1213,11 @@ void bta_jv_delete_attribute(tBTA_JV_MSG *p_data)
 void bta_jv_delete_record(tBTA_JV_MSG *p_data)
 {
     tBTA_JV_API_ADD_ATTRIBUTE *dr = &(p_data->add_attr);
-    UINT32 handle;
-
-    handle = bta_jv_get_sdp_handle(dr->handle);
 
-    if(handle)
+    if(dr->handle)
     {
-        /* this is a record created by JV */
-        SDP_DeleteRecord(handle);
-        bta_jv_free_sdp_id(dr->handle);
+        /* this is a record created by btif layer*/
+        SDP_DeleteRecord(dr->handle);
     }
 }
 
@@ -1861,11 +1849,12 @@ void bta_jv_rfcomm_connect(tBTA_JV_MSG *p_data)
     UINT16 handle = 0;
     UINT32 event_mask = (PORT_EV_RXCHAR | PORT_EV_FC | PORT_EV_FCS);
     tPORT_STATE port_state;
-    UINT8   sec_id;
+    UINT8   sec_id = 0;
     tBTA_JV_RFC_CB  *p_cb = NULL;
     tBTA_JV_PCB     *p_pcb;
     tBTA_JV_API_RFCOMM_CONNECT *cc = &(p_data->rfcomm_connect);
     tBTA_JV_RFCOMM_CL_INIT      evt_data;
+    memset(&evt_data, 0, sizeof(evt_data));
 
     /* TODO DM role manager
     L2CA_SetDesireRole(cc->role);
@@ -1879,13 +1868,14 @@ void bta_jv_rfcomm_connect(tBTA_JV_MSG *p_data)
                 BTM_SEC_PROTO_RFCOMM, cc->remote_scn) == FALSE)
     {
         evt_data.status = BTA_JV_FAILURE;
-        error("sec_id:%d is zero or BTM_SetSecurityLevel failed, remote_scn:%d", sec_id, cc->remote_scn);
+        APPL_TRACE_ERROR2("sec_id:%d is zero or BTM_SetSecurityLevel failed, remote_scn:%d", sec_id, cc->remote_scn);
     }
 
     if (evt_data.status == BTA_JV_SUCCESS &&
         RFCOMM_CreateConnection(UUID_SERVCLASS_SERIAL_PORT, cc->remote_scn, FALSE,
         BTA_JV_DEF_RFC_MTU, cc->peer_bd_addr, &handle, bta_jv_port_mgmt_cl_cback) != PORT_SUCCESS)
     {
+        APPL_TRACE_ERROR0("bta_jv_rfcomm_connect, RFCOMM_CreateConnection failed");
         evt_data.status = BTA_JV_FAILURE;
     }
     if (evt_data.status == BTA_JV_SUCCESS)
@@ -1921,6 +1911,13 @@ void bta_jv_rfcomm_connect(tBTA_JV_MSG *p_data)
         }
     }
     cc->p_cback(BTA_JV_RFCOMM_CL_INIT_EVT, (tBTA_JV *)&evt_data, cc->user_data);
+    if(evt_data.status == BTA_JV_FAILURE)
+    {
+        if(sec_id)
+            bta_jv_free_sec_id(&sec_id);
+        if(handle)
+            RFCOMM_RemoveConnection(handle);
+    }
  }
 
 /*******************************************************************************
@@ -1936,10 +1933,15 @@ void bta_jv_rfcomm_close(tBTA_JV_MSG *p_data)
 {
     tBTA_JV_RFCOMM_CLOSE     evt_data;
     tBTA_JV_API_RFCOMM_CLOSE *cc = &(p_data->rfcomm_close);
+    if(!cc->handle)
+    {
+        APPL_TRACE_ERROR0("bta_jv_rfcomm_close, rfc handle is null");
+        return;
+    }
+
     tBTA_JV_RFC_CB           *p_cb = cc->p_cb;
     tBTA_JV_PCB              *p_pcb = cc->p_pcb;
     tBTA_JV_RFCOMM_CBACK     *p_cback = p_cb->p_cback;
-
     evt_data.handle = p_cb->handle;
     evt_data.status = BTA_JV_FAILURE;
 
@@ -1960,6 +1962,7 @@ void bta_jv_rfcomm_close(tBTA_JV_MSG *p_data)
     else if(BTA_JV_ST_SR_OPEN == p_pcb->state)
     {
         /* server is connected */
+
         if(PORT_SUCCESS == RFCOMM_RemoveConnection(p_pcb->port_handle))
         {
             p_pcb->state = BTA_JV_ST_SR_CLOSING;
@@ -1972,7 +1975,7 @@ void bta_jv_rfcomm_close(tBTA_JV_MSG *p_data)
     if (p_cback)
         p_cback(BTA_JV_RFCOMM_CLOSE_EVT, (tBTA_JV *)&evt_data, user_data);
     else
-        error("### NO CALLBACK SET !!! ###");
+        APPL_TRACE_ERROR0("bta_jv_rfcomm_close, p_cback is null");
 }
 
 /*******************************************************************************
@@ -2026,7 +2029,6 @@ static void bta_jv_port_mgmt_sr_cback(UINT32 code, UINT16 port_handle)
     BD_ADDR rem_bda;
     UINT16 lcid;
     UINT8  num;
-    tBTA_JV_RFCOMM_CBACK    *p_cback;
     UINT32  si;
 
     if(NULL == p_cb || NULL == p_cb->p_cback)
@@ -2048,18 +2050,24 @@ static void bta_jv_port_mgmt_sr_cback(UINT32 code, UINT16 port_handle)
         {
             evt_data.rfc_srv_open.new_listen_handle = p_pcb_new_listen->handle;
             p_pcb_new_listen->user_data = p_cb->p_cback(BTA_JV_RFCOMM_SRV_OPEN_EVT, &evt_data, user_data);
+            APPL_TRACE_DEBUG1("PORT_SUCCESS: max_sess:%d", p_cb->max_sess);
             failed = FALSE;
         }
-        else error("bta_jv_add_rfc_port failed to create new listen port");
+        else APPL_TRACE_ERROR0("bta_jv_add_rfc_port failed to create new listen port");
     }
     if(failed)
     {
         evt_data.rfc_close.handle = p_cb->handle;
-        evt_data.rfc_close.status = BTA_JV_SUCCESS;
+        evt_data.rfc_close.status = BTA_JV_FAILURE;
         evt_data.rfc_close.async = TRUE;
+        evt_data.rfc_close.port_status = code;
+        p_pcb->cong = FALSE;
+
+        tBTA_JV_RFCOMM_CBACK    *p_cback = p_cb->p_cback;
         if(BTA_JV_ST_SR_CLOSING == p_pcb->state)
         {
             evt_data.rfc_close.async = FALSE;
+            evt_data.rfc_close.status = BTA_JV_SUCCESS;
         }
         p_pcb->cong = FALSE;
         p_cback = p_cb->p_cback;
@@ -2071,7 +2079,9 @@ static void bta_jv_port_mgmt_sr_cback(UINT32 code, UINT16 port_handle)
         RFCOMM_RemoveServer(port_handle);
         evt_data.rfc_close.port_status = code;
         p_cback(BTA_JV_RFCOMM_CLOSE_EVT, &evt_data, user_data);
-    }
+        APPL_TRACE_DEBUG1("PORT_CLOSED after BTA_JV_RFCOMM_CLOSE_EVT: max_sess:%d",
+                          p_cb->max_sess);
+     }
 }
 
 /*******************************************************************************
@@ -2146,8 +2156,9 @@ static tBTA_JV_PCB * bta_jv_add_rfc_port(tBTA_JV_RFC_CB *p_cb)
             }
         }
 
-        debug("bta_jv_add_rfc_port max_sess=%d used:%d listen:%d si:%d",
+        APPL_TRACE_DEBUG4("bta_jv_add_rfc_port max_sess=%d used:%d listen:%d si:%d",
                     p_cb->max_sess, used, listen, si);
+
         if (used <p_cb->max_sess && listen==0 && si)
         {
             si--;
@@ -2190,12 +2201,12 @@ void bta_jv_rfcomm_start_server(tBTA_JV_MSG *p_data)
     UINT16 handle = 0;
     UINT32 event_mask = (PORT_EV_RXCHAR | PORT_EV_FC | PORT_EV_FCS);
     tPORT_STATE port_state;
-    UINT8   sec_id;
+    UINT8   sec_id = 0;
     tBTA_JV_RFC_CB  *p_cb = NULL;
     tBTA_JV_PCB     *p_pcb;
     tBTA_JV_API_RFCOMM_SERVER *rs = &(p_data->rfcomm_server);
     tBTA_JV_RFCOMM_START        evt_data;
-
+    memset(&evt_data, 0, sizeof(evt_data));
     /* TODO DM role manager
     L2CA_SetDesireRole(rs->role);
     */
@@ -2208,19 +2219,21 @@ void bta_jv_rfcomm_start_server(tBTA_JV_MSG *p_data)
             BTM_SetSecurityLevel(FALSE, "JV PORT", sec_id,  rs->sec_mask,
                 BT_PSM_RFCOMM, BTM_SEC_PROTO_RFCOMM, rs->local_scn) == FALSE)
         {
+            APPL_TRACE_ERROR0("bta_jv_rfcomm_start_server, run out of sec_id");
             break;
         }
 
         if (RFCOMM_CreateConnection(sec_id, rs->local_scn, TRUE,
             BTA_JV_DEF_RFC_MTU, (UINT8 *) bd_addr_any, &handle, bta_jv_port_mgmt_sr_cback) != PORT_SUCCESS)
         {
+            APPL_TRACE_ERROR0("bta_jv_rfcomm_start_server, RFCOMM_CreateConnection failed");
             break;
         }
 
         p_cb = bta_jv_alloc_rfc_cb(handle, &p_pcb);
         if(!p_cb)
         {
-            APPL_TRACE_ERROR0("run out of rfc control block");
+            APPL_TRACE_ERROR0("bta_jv_rfcomm_start_server, run out of rfc control block");
             break;
         }
 
@@ -2245,12 +2258,18 @@ void bta_jv_rfcomm_start_server(tBTA_JV_MSG *p_data)
 FALSE-POSITIVE: port_state is initialized at PORT_GetState() */
         PORT_SetState(handle, &port_state);
     } while (0);
-
     rs->p_cback(BTA_JV_RFCOMM_START_EVT, (tBTA_JV *)&evt_data, rs->user_data);
     if(evt_data.status == BTA_JV_SUCCESS)
     {
         PORT_SetDataCOCallback (handle, bta_jv_port_data_co_cback);
     }
+    else
+    {
+        if(sec_id)
+            bta_jv_free_sec_id(&sec_id);
+        if(handle)
+            RFCOMM_RemoveConnection(handle);
+    }
 }
 
 /*******************************************************************************
@@ -2268,10 +2287,16 @@ void bta_jv_rfcomm_stop_server(tBTA_JV_MSG *p_data)
     tBTA_JV_RFCOMM_CLOSE  evt_data;
     int i;
     tBTA_JV_API_RFCOMM_SERVER *ls = &(p_data->rfcomm_server);
+    if(!ls->rfc_handle)
+    {
+        APPL_TRACE_ERROR0("bta_jv_rfcomm_stop_server, rfc handle is null");
+        return;
+    }
     HDL2CB(ls->rfc_handle);
     evt_data.status = BTA_JV_FAILURE;
     if(p_cb && p_pcb)
     {
+        tBTA_JV_RFCOMM_CBACK     *p_cback = p_cb->p_cback;
         evt_data.handle = p_cb->handle;
         void* user_data = p_pcb->user_data;
         evt_data.status = bta_jv_free_rfc_listen_cb(p_cb);
@@ -2280,15 +2305,15 @@ void bta_jv_rfcomm_stop_server(tBTA_JV_MSG *p_data)
         /* occasionally when shutting down stack the callback is already
            freed, hence make sure we check for this condition (pending investigatation
            of rootcause) */
-        debug("send BTA_JV_RFCOMM_CLOSE_EVT");
-        if( p_cb->p_cback)
-             p_cb->p_cback(BTA_JV_RFCOMM_CLOSE_EVT, (tBTA_JV *)&evt_data, user_data);
-    }
-    else
-    {
-        debug("warning, no jv callback set");
+        APPL_TRACE_DEBUG2("send BTA_JV_RFCOMM_CLOSE_EVT, p_cback:%p, user_data:%d", p_cback, (int)user_data);
+        if(p_cback)
+             p_cback(BTA_JV_RFCOMM_CLOSE_EVT, (tBTA_JV *)&evt_data, user_data);
+        else
+        {
+            APPL_TRACE_DEBUG0("bta_jv_rfcomm_stop_server, p_cback is null");
+        }
     }
-}
+ }
 
 /*******************************************************************************
 **
diff --git a/bta/sys/bta_sys_main.c b/bta/sys/bta_sys_main.c
index c1554bb..59fbdd7 100644
--- a/bta/sys/bta_sys_main.c
+++ b/bta/sys/bta_sys_main.c
@@ -57,8 +57,8 @@ tBTA_SYS_CB bta_sys_cb;
 
 /* trace level */
 /* TODO Bluedroid - Hard-coded trace levels -  Needs to be configurable */
-UINT8 appl_trace_level = BT_TRACE_LEVEL_DEBUG; //APPL_INITIAL_TRACE_LEVEL;
-UINT8 btif_trace_level = BT_TRACE_LEVEL_DEBUG;
+UINT8 appl_trace_level = BT_TRACE_LEVEL_WARNING; //APPL_INITIAL_TRACE_LEVEL;
+UINT8 btif_trace_level = BT_TRACE_LEVEL_WARNING;
 
 static const tBTA_SYS_REG bta_sys_hw_reg =
 {
diff --git a/btif/co/bta_hh_co.c b/btif/co/bta_hh_co.c
index 81414d2..b4ea4c2 100644
--- a/btif/co/bta_hh_co.c
+++ b/btif/co/bta_hh_co.c
@@ -20,6 +20,8 @@
 
 #include <ctype.h>
 #include <fcntl.h>
+#include <sys/poll.h>
+#include <pthread.h>
 #include <stdio.h>
 #include <string.h>
 #include <stdint.h>
@@ -41,7 +43,7 @@ static int uhid_write(int fd, const struct uhid_event *ev)
     ret = write(fd, ev, sizeof(*ev));
     if (ret < 0){
         int rtn = -errno;
-        APPL_TRACE_ERROR2("%s: Cannot write to uhid:%s",__FUNCTION__,strerror(errno));
+        APPL_TRACE_ERROR2("%s: Cannot write to uhid:%s", __FUNCTION__, strerror(errno));
         return rtn;
     } else if (ret != sizeof(*ev)) {
         APPL_TRACE_ERROR3("%s: Wrong size written to uhid: %ld != %lu",
@@ -52,6 +54,144 @@ static int uhid_write(int fd, const struct uhid_event *ev)
     }
 }
 
+/* Internal function to parse the events received from UHID driver*/
+static int uhid_event(btif_hh_device_t *p_dev)
+{
+    struct uhid_event ev;
+    ssize_t ret;
+    memset(&ev, 0, sizeof(ev));
+    if(!p_dev)
+        APPL_TRACE_ERROR1("%s: Device not found",__FUNCTION__)
+    ret = read(p_dev->fd, &ev, sizeof(ev));
+    if (ret == 0) {
+        APPL_TRACE_ERROR2("%s: Read HUP on uhid-cdev %s", __FUNCTION__,
+                                                 strerror(errno));
+        return -EFAULT;
+    } else if (ret < 0) {
+        APPL_TRACE_ERROR2("%s:Cannot read uhid-cdev: %s", __FUNCTION__,
+                                                strerror(errno));
+        return -errno;
+    } else if (ret != sizeof(ev)) {
+        APPL_TRACE_ERROR3("%s:Invalid size read from uhid-dev: %ld != %lu",
+                            __FUNCTION__, ret, sizeof(ev));
+        return -EFAULT;
+    }
+
+    switch (ev.type) {
+    case UHID_START:
+        APPL_TRACE_DEBUG0("UHID_START from uhid-dev\n");
+        break;
+    case UHID_STOP:
+        APPL_TRACE_DEBUG0("UHID_STOP from uhid-dev\n");
+        break;
+    case UHID_OPEN:
+        APPL_TRACE_DEBUG0("UHID_OPEN from uhid-dev\n");
+        break;
+    case UHID_CLOSE:
+        APPL_TRACE_DEBUG0("UHID_CLOSE from uhid-dev\n");
+        break;
+    case UHID_OUTPUT:
+        APPL_TRACE_DEBUG0("UHID_OUTPUT from uhid-dev\n");
+        APPL_TRACE_DEBUG2("UHID_OUTPUT: Report type = %d, report_size = %d"
+                            ,ev.u.output.rtype, ev.u.output.size);
+        //Send SET_REPORT with feature report if the report type in output event is FEATURE
+        if(ev.u.output.rtype == UHID_FEATURE_REPORT)
+            btif_hh_setreport(p_dev,BTHH_FEATURE_REPORT,ev.u.output.size,ev.u.output.data);
+        else if(ev.u.output.rtype == UHID_OUTPUT_REPORT)
+            btif_hh_setreport(p_dev,BTHH_OUTPUT_REPORT,ev.u.output.size,ev.u.output.data);
+        else
+            btif_hh_setreport(p_dev,BTHH_INPUT_REPORT,ev.u.output.size,ev.u.output.data);
+           break;
+    case UHID_OUTPUT_EV:
+        APPL_TRACE_DEBUG0("UHID_OUTPUT_EV from uhid-dev\n");
+        break;
+    case UHID_FEATURE:
+        APPL_TRACE_DEBUG0("UHID_FEATURE from uhid-dev\n");
+        break;
+    case UHID_FEATURE_ANSWER:
+        APPL_TRACE_DEBUG0("UHID_FEATURE_ANSWER from uhid-dev\n");
+        break;
+
+    default:
+        APPL_TRACE_DEBUG1("Invalid event from uhid-dev: %u\n", ev.type);
+    }
+
+    return 0;
+}
+
+/*******************************************************************************
+**
+** Function create_thread
+**
+** Description creat a select loop
+**
+** Returns pthread_t
+**
+*******************************************************************************/
+static inline pthread_t create_thread(void *(*start_routine)(void *), void * arg){
+    APPL_TRACE_DEBUG0("create_thread: entered");
+    pthread_attr_t thread_attr;
+
+    pthread_attr_init(&thread_attr);
+    pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_JOINABLE);
+    pthread_t thread_id = -1;
+    if ( pthread_create(&thread_id, &thread_attr, start_routine, arg)!=0 )
+    {
+        APPL_TRACE_ERROR1("pthread_create : %s", strerror(errno));
+        return -1;
+    }
+    APPL_TRACE_DEBUG0("create_thread: thread created successfully");
+    return thread_id;
+}
+
+/*******************************************************************************
+**
+** Function btif_hh_poll_event_thread
+**
+** Description the polling thread which polls for event from UHID driver
+**
+** Returns void
+**
+*******************************************************************************/
+static void *btif_hh_poll_event_thread(void *arg)
+{
+
+    btif_hh_device_t *p_dev = arg;
+    APPL_TRACE_DEBUG2("%s: Thread created fd = %d", __FUNCTION__, p_dev->fd);
+    struct pollfd pfds[1];
+    int ret;
+    pfds[0].fd = p_dev->fd;
+    pfds[0].events = POLLIN;
+
+    while(p_dev->hh_keep_polling){
+        ret = poll(pfds, 1, 500);
+        if (ret < 0) {
+            APPL_TRACE_ERROR2("%s: Cannot poll for fds: %s\n", __FUNCTION__, strerror(errno));
+            break;
+        }
+        if (pfds[0].revents & POLLIN) {
+            APPL_TRACE_DEBUG0("btif_hh_poll_event_thread: POLLIN");
+            ret = uhid_event(p_dev);
+            if (ret){
+                break;
+            }
+        }
+    }
+
+    p_dev->hh_poll_thread_id = -1;
+    return 0;
+}
+
+static inline void btif_hh_close_poll_thread(btif_hh_device_t *p_dev)
+{
+    APPL_TRACE_DEBUG1("%s", __FUNCTION__);
+    p_dev->hh_keep_polling = 0;
+    if(p_dev->hh_poll_thread_id > 0)
+        pthread_join(p_dev->hh_poll_thread_id,NULL);
+
+    return;
+}
+
 void bta_hh_co_destroy(int fd)
 {
     struct uhid_event ev;
@@ -119,7 +259,8 @@ void bta_hh_co_open(UINT8 dev_handle, UINT8 sub_class, tBTA_HH_ATTR_MASK attr_ma
                 }else
                     APPL_TRACE_DEBUG2("%s: uhid fd = %d", __FUNCTION__, p_dev->fd);
             }
-
+            p_dev->hh_keep_polling = 1;
+            p_dev->hh_poll_thread_id = create_thread(btif_hh_poll_event_thread, p_dev);
             break;
         }
         p_dev = NULL;
@@ -141,8 +282,11 @@ void bta_hh_co_open(UINT8 dev_handle, UINT8 sub_class, tBTA_HH_ATTR_MASK attr_ma
                 if (p_dev->fd < 0){
                     APPL_TRACE_ERROR2("%s: Error: failed to open uhid, err:%s",
                                                                     __FUNCTION__,strerror(errno));
-                }else
+                }else{
                     APPL_TRACE_DEBUG2("%s: uhid fd = %d", __FUNCTION__, p_dev->fd);
+                    p_dev->hh_keep_polling = 1;
+                    p_dev->hh_poll_thread_id = create_thread(btif_hh_poll_event_thread, p_dev);
+                }
 
 
                 break;
@@ -174,7 +318,26 @@ void bta_hh_co_open(UINT8 dev_handle, UINT8 sub_class, tBTA_HH_ATTR_MASK attr_ma
 *******************************************************************************/
 void bta_hh_co_close(UINT8 dev_handle, UINT8 app_id)
 {
+    UINT32 i;
+    btif_hh_device_t *p_dev = NULL;
+
     APPL_TRACE_WARNING3("%s: dev_handle = %d, app_id = %d", __FUNCTION__, dev_handle, app_id);
+    if (dev_handle == BTA_HH_INVALID_HANDLE) {
+        APPL_TRACE_WARNING2("%s: Oops, dev_handle (%d) is invalid...", __FUNCTION__, dev_handle);
+        return;
+    }
+
+    for (i = 0; i < BTIF_HH_MAX_HID; i++) {
+        p_dev = &btif_hh_cb.devices[i];
+        if (p_dev->dev_status != BTHH_CONN_STATE_UNKNOWN && p_dev->dev_handle == dev_handle) {
+            APPL_TRACE_WARNING3("%s: Found an existing device with the same handle "
+                                                                "dev_status = %d, dev_handle =%d",__FUNCTION__,
+                                                                p_dev->dev_status,p_dev->dev_handle);
+            btif_hh_close_poll_thread(p_dev);
+            break;
+        }
+     }
+
 }
 
 
diff --git a/btif/include/btif_common.h b/btif/include/btif_common.h
index 7f3b039..6bf885f 100644
--- a/btif/include/btif_common.h
+++ b/btif/include/btif_common.h
@@ -54,7 +54,7 @@ extern bt_callbacks_t *bt_hal_cbacks;
 
 #define HAL_CBACK(P_CB, P_CBACK, ...)\
     if (P_CB && P_CB->P_CBACK) {            \
-        ALOGD("HAL %s->%s", #P_CB, #P_CBACK); \
+        BTIF_TRACE_API2("HAL %s->%s", #P_CB, #P_CBACK); \
         P_CB->P_CBACK(__VA_ARGS__);         \
     }                                       \
     else {                                  \
diff --git a/btif/include/btif_hh.h b/btif/include/btif_hh.h
index 4f31002..25063eb 100644
--- a/btif/include/btif_hh.h
+++ b/btif/include/btif_hh.h
@@ -62,6 +62,8 @@ typedef struct
     UINT8                         app_id;
     int                           fd;
     BT_HDR                        *p_buf;
+    UINT32                        hh_poll_thread_id;
+    UINT8                         hh_keep_polling;
 } btif_hh_device_t;
 
 /* Control block to maintain properties of devices */
@@ -96,6 +98,8 @@ extern btif_hh_device_t *btif_hh_find_connected_dev_by_handle(UINT8 handle);
 extern void btif_hh_remove_device(bt_bdaddr_t bd_addr);
 extern bt_status_t btif_hh_virtual_unplug(bt_bdaddr_t *bd_addr);
 extern void btif_hh_disconnect(bt_bdaddr_t *bd_addr);
+extern void btif_hh_setreport(btif_hh_device_t *p_dev, bthh_report_type_t r_type,
+                    UINT16 size, UINT8* report);
 
 BOOLEAN btif_hh_add_added_dev(bt_bdaddr_t bd_addr, tBTA_HH_ATTR_MASK attr_mask);
 
diff --git a/btif/src/btif_config.c b/btif/src/btif_config.c
index 00e40bd..356cee0 100644
--- a/btif/src/btif_config.c
+++ b/btif/src/btif_config.c
@@ -41,20 +41,16 @@
 #include <stdlib.h>
 #include <private/android_filesystem_config.h>
 
-#define LOG_TAG "btif_config.c"
+#define LOG_TAG "btif_config"
 
 #include <hardware/bluetooth.h>
+#include "btif_api.h"
 #include "btif_config.h"
 #include "btif_config_util.h"
 #include "btif_sock_thread.h"
 #include "btif_sock_util.h"
 
-#include <cutils/log.h>
-#define info(fmt, ...)  ALOGI ("%s(L%d): " fmt,__FUNCTION__, __LINE__,  ## __VA_ARGS__)
-#define debug(fmt, ...) ALOGD ("%s(L%d): " fmt,__FUNCTION__, __LINE__,  ## __VA_ARGS__)
-#define warn(fmt, ...) ALOGW ("## WARNING : %s(L%d): " fmt "##",__FUNCTION__, __LINE__, ## __VA_ARGS__)
-#define error(fmt, ...) ALOGE ("## ERROR : %s(L%d): " fmt "##",__FUNCTION__, __LINE__, ## __VA_ARGS__)
-#define asrt(s) if(!(s)) ALOGE ("## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
+#define asrt(s) if(!(s)) BTIF_TRACE_ERROR3 ("## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
 //#define UNIT_TEST
 #define CFG_PATH "/data/misc/bluedroid/"
 #define CFG_FILE_NAME "bt_config"
@@ -112,22 +108,24 @@ static void cfg_test_read();
 #endif
 static inline void dump_node(const char* title, const cfg_node* p)
 {
-    if(p)
-        debug("%s, p->name:%s, child/value:%p, bytes:%d, p->used:%d, type:%x, p->flag:%d",
-            title, p->name, p->child, p->bytes, p->used, p->type, p->flag);
-    else debug("%s is NULL", title);
+    if(p) {
+        BTIF_TRACE_DEBUG4("%s, p->name:%s, child/value:%p, bytes:%d",
+                          title, p->name, p->child, p->bytes);
+        BTIF_TRACE_DEBUG3("p->used:%d, type:%x, p->flag:%d",
+                          p->used, p->type, p->flag);
+    } else BTIF_TRACE_DEBUG1("%s is NULL", title);
 }
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 int btif_config_init()
 {
     static int initialized;
-    debug("in initialized:%d", initialized);
+    BTIF_TRACE_DEBUG1("in initialized:%d", initialized);
     if(!initialized)
     {
         initialized = 1;
         struct stat st;
         if(stat(CFG_PATH, &st) != 0)
-            error("%s does not exist, need provision", CFG_PATH);
+            BTIF_TRACE_ERROR1("%s does not exist, need provision", CFG_PATH);
         btsock_thread_init();
         init_slot_lock(&slot_lock);
         lock_slot(&slot_lock);
@@ -180,8 +178,6 @@ int btif_config_exist(const char* section, const char* key, const char* name)
 }
 int btif_config_get(const char* section, const char* key, const char* name, char* value, int* bytes, int* type)
 {
-    //debug("in");
-
     int ret = FALSE;
     asrt(section && *section && key && *key && name && *name && bytes && type);
     //debug("section:%s, key:%s, name:%s, value:%p, bytes:%d, type:%d",
@@ -204,9 +200,11 @@ int btif_config_get(const char* section, const char* key, const char* name, char
             if(ret != TRUE)
             {
                 if(*type != node->type)
-                    error("value:%s, wrong type:%d, need to be type: %d", name, *type, node->type);
+                    BTIF_TRACE_ERROR3("value:%s, wrong type:%d, need to be type: %d",
+                                      name, *type, node->type);
                 if(value && *bytes < node->used)
-                    error("value:%s, not enough size: %d bytes, need %d bytes", name, node->used, *bytes);
+                    BTIF_TRACE_ERROR3("value:%s, not enough size: %d bytes, need %d bytes",
+                                      name, node->used, *bytes);
             }
         }
         unlock_slot(&slot_lock);
@@ -354,7 +352,7 @@ static inline short alloc_node(cfg_node* p, short grow)
             //debug("out");
             return old_bytes;//return the previous size
         }
-        else error("realloc failed, old_bytes:%d, grow:%d, total:%d", p->bytes, grow,  p->bytes + grow);
+        else BTIF_TRACE_ERROR3("realloc failed, old_bytes:%d, grow:%d, total:%d", p->bytes, grow,  p->bytes + grow);
     }
     //debug("out, alloc failed");
     return -1;
@@ -462,7 +460,7 @@ static int set_node(const char* section, const char* key, const char* name,
                         value_node->bytes = bytes;
                     else
                     {
-                        error("not enough memory!");
+                        BTIF_TRACE_ERROR0("not enough memory!");
                         value_node->bytes = 0;
                         return FALSE;
                     }
@@ -575,7 +573,6 @@ static int remove_node(const char* section, const char* key, const char* name)
 }
 static int save_cfg()
 {
-    debug("in");
     const char* file_name = CFG_PATH CFG_FILE_NAME CFG_FILE_EXT;
     const char* file_name_new = CFG_PATH CFG_FILE_NAME CFG_FILE_EXT_NEW;
     const char* file_name_old = CFG_PATH CFG_FILE_NAME CFG_FILE_EXT_OLD;
@@ -593,8 +590,7 @@ static int save_cfg()
         rename(file_name_new, file_name);
         ret = TRUE;
     }
-    else error("btif_config_save_file failed");
-    debug("out");
+    else BTIF_TRACE_ERROR0("btif_config_save_file failed");
     return ret;
 }
 
@@ -612,6 +608,45 @@ static void remove_bluez_cfg()
 {
     rename(BLUEZ_PATH, BLUEZ_PATH_BAK);
 }
+static void clean_newline_char()
+{
+    char kname[128], vname[128];
+    short kpos = 0;
+    int kname_size, vname_size;
+    vname[0] = 0;
+    vname_size = sizeof(vname);
+    //BTIF_TRACE_DEBUG0("removing newline at the end of the adapter and device name");
+    if(btif_config_get_str("Local", "Adapter", "Name", vname, &vname_size) &&
+        vname_size > 2)
+    {
+        if(vname[vname_size - 2] == '\n')
+        {
+            BTIF_TRACE_DEBUG1("remove newline at the end of the adapter name:%s", vname);
+            vname[vname_size - 2] = 0;
+            btif_config_set_str("Local", "Adapter", "Name", vname);
+        }
+    }
+    do
+    {
+        kname_size = sizeof(kname);
+        kname[0] = 0;
+        kpos = btif_config_next_key(kpos, "Remote", kname, &kname_size);
+        //BTIF_TRACE_DEBUG2("Remote device:%s, size:%d", kname, kname_size);
+        vname_size = sizeof(vname);
+        vname[0] = 0;
+        if(btif_config_get_str("Remote", kname, "Name", vname, &vname_size) &&
+            vname_size > 2)
+        {
+            BTIF_TRACE_DEBUG1("remote device name:%s", vname);
+            if(vname[vname_size - 2] == '\n')
+            {
+                BTIF_TRACE_DEBUG1("remove newline at the end of the device name:%s", vname);
+                vname[vname_size - 2] = 0;
+                btif_config_set_str("Remote", kname, "Name", vname);
+            }
+        }
+     } while(kpos != -1);
+}
 static void load_cfg()
 {
     const char* file_name = CFG_PATH CFG_FILE_NAME CFG_FILE_EXT;
@@ -627,10 +662,19 @@ static void load_cfg()
                 remove_bluez_cfg();
         }
     }
+    int bluez_migration_done = 0;
+    btif_config_get_int("Local", "Adapter", "BluezMigrationDone", &bluez_migration_done);
+    if(!bluez_migration_done)
+    {
+        //clean the new line char at the end of the device name. Caused by bluez config import bug
+        clean_newline_char();
+        btif_config_set_int("Local", "Adapter", "BluezMigrationDone", 1);
+        btif_config_save();
+    }
 }
 static void cfg_cmd_callback(int cmd_fd, int type, int size, uint32_t user_id)
 {
-    debug("cmd type:%d, size:%d", type, size);
+  //BTIF_TRACE_DEBUG2("cmd type:%d, size:%d", type, size);
     switch(type)
     {
         case CFG_CMD_SAVE:
@@ -647,15 +691,14 @@ static void cfg_test_load()
     char kname[128], vname[128];
     short kpos, vpos;
     int kname_size, vname_size;
-    debug("in");
-    debug("list all remote devices values:");
+    BTIF_TRACE_DEBUG0("list all remote devices values:");
     kname_size = sizeof(kname);
     kname[0] = 0;
     kpos = 0;
     do
     {
         kpos = btif_config_next_key(kpos, "Remote Devices", kname, &kname_size);
-        debug("Remote devices:%s, size:%d", kname, kname_size);
+        BTIF_TRACE_DEBUG2("Remote devices:%s, size:%d", kname, kname_size);
         vpos = 0;
         vname[0] = 0;
         vname_size = sizeof(vname);
@@ -665,8 +708,8 @@ static void cfg_test_load()
             int vtype = BTIF_CFG_TYPE_STR;
             int vsize = sizeof(v);
             int ret = btif_config_get("Remote Devices", kname, vname, v, &vsize, &vtype);
-            debug("btif_config_get return:%d, Remote devices:%s, value name:%s, value:%s, value size:%d, type:0x%x",
-                    ret, kname, vname, v, vsize, vtype);
+            BTIF_TRACE_DEBUG6("btif_config_get return:%d, Remote devices:%s, value name:%s, value:%s, value size:%d, type:0x%x",
+                              ret, kname, vname, v, vsize, vtype);
 
             vname[0] = 0;
             vname_size = sizeof(vname);
@@ -674,11 +717,9 @@ static void cfg_test_load()
         kname[0] = 0;
         kname_size = sizeof(kname);
     } while(kpos != -1);
-    debug("out");
 }
 static void cfg_test_write()
 {
-    debug("in");
     int i;
 
     char key[128];
@@ -696,11 +737,10 @@ static void cfg_test_write()
         btif_config_set_int(section, key, "connect time out", i);
     }
     btif_config_save();
-    debug("out");
 }
 static void cfg_test_read()
 {
-    debug("in");
+    //debug("in");
     char class[128] = {0};
     char link_key[128] = {0};
     int size, type;
@@ -713,32 +753,32 @@ static void cfg_test_read()
         section = "Remote Devices";
         size = sizeof(class);
         ret = btif_config_get_str(section, key, "class", class, &size);
-        debug("btif_config_get_str return:%d, Remote devices:%s, class:%s", ret, key, class);
+        BTIF_TRACE_DEBUG3("btif_config_get_str return:%d, Remote devices:%s, class:%s", ret, key, class);
 
         size = sizeof(link_key);
         type = BTIF_CFG_TYPE_BIN;
         ret = btif_config_get(section, key, "link keys", link_key, &size, &type);
-        debug("btif_config_get return:%d, Remote devices:%s, link key:%x, %x",
-                    ret, key, *(int *)link_key, *((int *)link_key + 1));
+        //debug("btif_config_get return:%d, Remote devices:%s, link key:%x, %x",
+        //            ret, key, *(int *)link_key, *((int *)link_key + 1));
 
         int timeout;
         ret = btif_config_get_int(section, key, "connect time out", &timeout);
-        debug("btif_config_get_int return:%d, Remote devices:%s, connect time out:%d", ret, key, timeout);
+        //debug("btif_config_get_int return:%d, Remote devices:%s, connect time out:%d", ret, key, timeout);
     }
 
-    debug("testing btif_config_remove");
+    // debug("testing btif_config_remove");
     size = sizeof(class);
     type = BTIF_CFG_TYPE_STR;
     btif_config_set("Remote Devices", "00:22:5F:97:56:04", "Class Delete", class, strlen(class) + 1, BTIF_CFG_TYPE_STR);
 
     btif_config_get("Remote Devices", "00:22:5F:97:56:04", "Class Delete", class, &size, &type);
-    debug("Remote devices, 00:22:5F:97:56:04 Class Delete:%s", class);
+    // debug("Remote devices, 00:22:5F:97:56:04 Class Delete:%s", class);
     btif_config_remove("Remote Devices", "00:22:5F:97:56:04", "Class Delete");
 
     size = sizeof(class);
     type = BTIF_CFG_TYPE_STR;
     ret = btif_config_get("Remote Devices", "00:22:5F:97:56:04", "Class Delete", class, &size, &type);
-    debug("after removed, btif_config_get ret:%d, Remote devices, 00:22:5F:97:56:04 Class Delete:%s", ret, class);
-    debug("out");
+    // debug("after removed, btif_config_get ret:%d, Remote devices, 00:22:5F:97:56:04 Class Delete:%s", ret, class);
+    // debug("out");
 }
 #endif
diff --git a/btif/src/btif_config_util.cpp b/btif/src/btif_config_util.cpp
index 19fa30b..885f5c2 100644
--- a/btif/src/btif_config_util.cpp
+++ b/btif/src/btif_config_util.cpp
@@ -391,9 +391,9 @@ static int read_file_line(const char* map, int start_pos, int size, int* line_si
     int i;
     for(i = start_pos; i < size; i++)
     {
-         ++*line_size;
         if(map[i] == '\r' || map[i] == '\n')
             break;
+         ++*line_size;
     }
     //debug("out, ret:%d, start pos:%d, size:%d, line_size:%d", i, start_pos, size, *line_size);
     return i + 1;
diff --git a/btif/src/btif_hh.c b/btif/src/btif_hh.c
index 61432b8..004f2ef 100644
--- a/btif/src/btif_hh.c
+++ b/btif/src/btif_hh.c
@@ -487,6 +487,9 @@ void btif_hh_remove_device(bt_bdaddr_t bd_addr)
         GKI_freebuf(p_dev->p_buf);
         p_dev->p_buf = NULL;
     }
+
+    p_dev->hh_keep_polling = 0;
+    p_dev->hh_poll_thread_id = -1;
     BTIF_TRACE_DEBUG2("%s: uhid fd = %d", __FUNCTION__, p_dev->fd);
     if (p_dev->fd >= 0) {
         bta_hh_co_destroy(p_dev->fd);
@@ -648,6 +651,47 @@ void btif_hh_disconnect(bt_bdaddr_t *bd_addr)
         BTIF_TRACE_DEBUG1("%s-- Error: device not connected:",__FUNCTION__);
 }
 
+
+/*******************************************************************************
+**
+** Function         btif_btif_hh_setreport
+**
+** Description      setreport initiated from the BTIF thread context
+**
+** Returns          void
+**
+*******************************************************************************/
+
+void btif_hh_setreport(btif_hh_device_t *p_dev, bthh_report_type_t r_type, UINT16 size,
+                            UINT8* report)
+{
+    UINT8  hexbuf[20];
+    UINT16 len = size;
+    int i = 0;
+    if (p_dev->p_buf != NULL) {
+        GKI_freebuf(p_dev->p_buf);
+    }
+    p_dev->p_buf = GKI_getbuf((UINT16) (len + BTA_HH_MIN_OFFSET + sizeof(BT_HDR)));
+    if (p_dev->p_buf == NULL) {
+        APPL_TRACE_ERROR2("%s: Error, failed to allocate RPT buffer, len = %d", __FUNCTION__, len);
+        return;
+    }
+
+    p_dev->p_buf->len = len;
+    p_dev->p_buf->offset = BTA_HH_MIN_OFFSET;
+
+    //Build a SetReport data buffer
+    memset(hexbuf, 0, 20);
+    for(i=0; i<len; i++)
+        hexbuf[i] = report[i];
+
+    UINT8* pbuf_data;
+    pbuf_data = (UINT8*) (p_dev->p_buf + 1) + p_dev->p_buf->offset;
+    memcpy(pbuf_data, hexbuf, len);
+    BTA_HhSetReport(p_dev->dev_handle, r_type, p_dev->p_buf);
+
+}
+
 /*****************************************************************************
 **   Section name (Group of functions)
 *****************************************************************************/
@@ -731,7 +775,10 @@ static void btif_hh_upstreams_evt(UINT16 event, char* p_param)
                     BTIF_TRACE_WARNING1("BTA_HH_OPEN_EVT: Found device...Getting dscp info for handle ... %d",p_data->conn.handle);
                     memcpy(&(p_dev->bd_addr), p_data->conn.bda, BD_ADDR_LEN);
                     btif_hh_cb.status = BTIF_HH_DEV_CONNECTED;
-                    BTA_HhSetIdle(p_data->conn.handle, 0);
+                    // Send set_idle if the peer_device is a keyboard
+                    if (check_cod((bt_bdaddr_t*)p_data->conn.bda, COD_HID_KEYBOARD )||
+                                check_cod((bt_bdaddr_t*)p_data->conn.bda, COD_HID_COMBO))
+                        BTA_HhSetIdle(p_data->conn.handle, 0);
                     btif_hh_cb.p_curr_dev = btif_hh_find_connected_dev_by_handle(p_data->conn.handle);
                     BTA_HhGetDscpInfo(p_data->conn.handle);
                     p_dev->dev_status = BTHH_CONN_STATE_CONNECTED;
@@ -1563,6 +1610,8 @@ static void  cleanup( void )
              BTIF_TRACE_DEBUG2("%s: Closing uhid fd = %d", __FUNCTION__, p_dev->fd);
              bta_hh_co_destroy(p_dev->fd);
              p_dev->fd = -1;
+             p_dev->hh_keep_polling = 0;
+             p_dev->hh_poll_thread_id = -1;
          }
      }
 
diff --git a/btif/src/btif_pan.c b/btif/src/btif_pan.c
index 2f6c932..633f6ef 100644
--- a/btif/src/btif_pan.c
+++ b/btif/src/btif_pan.c
@@ -69,16 +69,7 @@
 #define BTPAN_LOCAL_ROLE (BTPAN_ROLE_PANU | BTPAN_ROLE_PANNAP)
 #endif
 
-
-
-#include <cutils/log.h>
-#define info(fmt, ...)  ALOGI ("btif_pan: %s(L%d): " fmt,__FUNCTION__, __LINE__,  ## __VA_ARGS__)
-#define debug(fmt, ...) ALOGD ("btif_pan: %s(L%d): " fmt,__FUNCTION__, __LINE__,  ## __VA_ARGS__)
-#define warn(fmt, ...) ALOGW ("btif_pan: ## WARNING : %s(L%d): " fmt "##",__FUNCTION__, __LINE__, ## __VA_ARGS__)
-#define error(fmt, ...) ALOGE ("btif_pan: ## ERROR : %s(L%d): " fmt "##",__FUNCTION__, __LINE__, ## __VA_ARGS__)
-#define asrt(s) if(!(s)) ALOGE ("btif_pan: ## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
-
-
+#define asrt(s) if(!(s)) BTIF_TRACE_ERROR3("btif_pan: ## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
 
 btpan_cb_t btpan_cb;
 
@@ -118,11 +109,11 @@ btpan_interface_t *btif_pan_get_interface()
 }
 void btif_pan_init()
 {
-    debug("jni_initialized = %d, btpan_cb.enabled:%d", jni_initialized, btpan_cb.enabled);
+    BTIF_TRACE_DEBUG2("jni_initialized = %d, btpan_cb.enabled:%d", jni_initialized, btpan_cb.enabled);
     stack_initialized = TRUE;
     if (jni_initialized && !btpan_cb.enabled)
     {
-        debug("Enabling PAN....");
+        BTIF_TRACE_DEBUG0("Enabling PAN....");
         memset(&btpan_cb, 0, sizeof(btpan_cb));
         btpan_cb.tap_fd = -1;
         int i;
@@ -132,7 +123,6 @@ void btif_pan_init()
         btpan_cb.enabled = 1;
         btpan_enable(BTPAN_LOCAL_ROLE);
     }
-    debug("leaving");
 }
 static void pan_disable()
 {
@@ -157,7 +147,6 @@ void btif_pan_cleanup()
         for(i = 0; i < MAX_PAN_CONNS; i++)
             btpan_cleanup_conn(&btpan_cb.conns[i]);
         pan_disable();
-        debug("leaving");
     }
     stack_initialized = FALSE;
 }
@@ -165,12 +154,11 @@ void btif_pan_cleanup()
 static btpan_callbacks_t callback;
 static bt_status_t btpan_jni_init(const btpan_callbacks_t* callbacks)
 {
-    debug("stack_initialized = %d, btpan_cb.enabled:%d", stack_initialized, btpan_cb.enabled);
+    BTIF_TRACE_DEBUG2("stack_initialized = %d, btpan_cb.enabled:%d", stack_initialized, btpan_cb.enabled);
     jni_initialized = TRUE;
     if(stack_initialized && !btpan_cb.enabled)
         btif_pan_init();
     callback = *callbacks;
-    debug(" leaving");
     return BT_STATUS_SUCCESS;
 }
 
@@ -178,20 +166,17 @@ static void btpan_jni_cleanup()
 {
     pan_disable();
     jni_initialized = FALSE;
-    debug("leaving");
 }
 static inline int bta_role_to_btpan(int bta_pan_role)
 {
     int btpan_role = 0;
-    debug("bta_pan_role:0x%x", bta_pan_role);
+    BTIF_TRACE_DEBUG1("bta_pan_role:0x%x", bta_pan_role);
     if(bta_pan_role & PAN_ROLE_NAP_SERVER)
     {
-        debug("BTPAN_ROLE_PANNAP");
         btpan_role |= BTPAN_ROLE_PANNAP;
     }
     if(bta_pan_role & PAN_ROLE_CLIENT)
     {
-        debug("BTPAN_ROLE_PANU");
         btpan_role |= BTPAN_ROLE_PANU;
     }
     return btpan_role;
@@ -199,15 +184,13 @@ static inline int bta_role_to_btpan(int bta_pan_role)
 static inline int btpan_role_to_bta(int btpan_role)
 {
     int bta_pan_role = PAN_ROLE_INACTIVE;
-    debug("btpan_role:0x%x", btpan_role);
+    BTIF_TRACE_DEBUG1("btpan_role:0x%x", btpan_role);
     if(btpan_role & BTPAN_ROLE_PANNAP)
     {
-        debug("BTPAN_ROLE_PANNAP");
         bta_pan_role |= PAN_ROLE_NAP_SERVER;
     }
     if(btpan_role & BTPAN_ROLE_PANU)
     {
-        debug("BTPAN_ROLE_CLIENT");
         bta_pan_role |= PAN_ROLE_CLIENT;
     }
     return bta_pan_role;
@@ -219,7 +202,7 @@ static tBTA_PAN_ROLE_INFO bta_pan_nap_info = {PAN_NAP_SERVICE_NAME, 0, PAN_SECUR
 static bt_status_t btpan_enable(int local_role)
 {
     int bta_pan_role;
-    debug("local_role:%d", local_role);
+    BTIF_TRACE_DEBUG1("local_role:%d", local_role);
     bta_pan_role = btpan_role_to_bta(local_role);
     BTA_PanSetRole(bta_pan_role, &bta_panu_info, NULL, &bta_pan_nap_info);
     btpan_dev_local_role = local_role;
@@ -227,12 +210,12 @@ static bt_status_t btpan_enable(int local_role)
 }
 static int btpan_get_local_role()
 {
-    debug("btpan_dev_local_role:%d", btpan_dev_local_role);
+    BTIF_TRACE_DEBUG1("btpan_dev_local_role:%d", btpan_dev_local_role);
     return btpan_dev_local_role;
 }
 static bt_status_t btpan_connect(const bt_bdaddr_t *bd_addr, int local_role, int remote_role)
 {
-    debug("local_role:%d, remote_role:%d", local_role, remote_role);
+    BTIF_TRACE_DEBUG2("local_role:%d, remote_role:%d", local_role, remote_role);
     int bta_local_role = btpan_role_to_bta(local_role);
     int bta_remote_role = btpan_role_to_bta(remote_role);
     btpan_new_conn(-1, bd_addr->address, bta_local_role, bta_remote_role);
@@ -266,7 +249,6 @@ static void btif_in_pan_generic_evt(UINT16 event, char *p_param)
 }
 static bt_status_t btpan_disconnect(const bt_bdaddr_t *bd_addr)
 {
-    debug("in");
     btpan_conn_t* conn = btpan_find_conn_addr(bd_addr->address);
     if(conn && conn->handle >= 0)
     {
@@ -281,7 +263,6 @@ static bt_status_t btpan_disconnect(const bt_bdaddr_t *bd_addr)
 static int pth = -1;
 void create_tap_read_thread(int tap_fd)
 {
-    debug("in");
     if(pth < 0)
     {
         pth = btsock_thread_create(btpan_tap_fd_signaled, NULL);
@@ -310,23 +291,23 @@ static int tap_if_up(const char *devname, BD_ADDR addr)
     err = ioctl(sk, SIOCGIFHWADDR, &ifr);
     if(err < 0)
     {
-        error("Could not get network hardware for interface:%s, errno:%s", devname, strerror(errno));
+        BTIF_TRACE_ERROR2("Could not get network hardware for interface:%s, errno:%s", devname, strerror(errno));
         close(sk);
         return -1;
     }
-    debug("found mac address for interface:%s = %02x:%02x:%02x:%02x:%02x:%02x", devname,
-            ifr.ifr_hwaddr.sa_data[0], ifr.ifr_hwaddr.sa_data[1], ifr.ifr_hwaddr.sa_data[2],
-            ifr.ifr_hwaddr.sa_data[3], ifr.ifr_hwaddr.sa_data[4], ifr.ifr_hwaddr.sa_data[5]);
+    /* debug("found mac address for interface:%s = %02x:%02x:%02x:%02x:%02x:%02x", devname, */
+    /*         ifr.ifr_hwaddr.sa_data[0], ifr.ifr_hwaddr.sa_data[1], ifr.ifr_hwaddr.sa_data[2], */
+    /*         ifr.ifr_hwaddr.sa_data[3], ifr.ifr_hwaddr.sa_data[4], ifr.ifr_hwaddr.sa_data[5]); */
     strncpy(ifr.ifr_name, devname, IFNAMSIZ - 1);
     memcpy(ifr.ifr_hwaddr.sa_data, addr, 6);
-    debug("setting bt address for interface:%s = %02x:%02x:%02x:%02x:%02x:%02x", devname,
-            ifr.ifr_hwaddr.sa_data[0], ifr.ifr_hwaddr.sa_data[1], ifr.ifr_hwaddr.sa_data[2],
-            ifr.ifr_hwaddr.sa_data[3], ifr.ifr_hwaddr.sa_data[4], ifr.ifr_hwaddr.sa_data[5]);
+    /* debug("setting bt address for interface:%s = %02x:%02x:%02x:%02x:%02x:%02x", devname, */
+    /*         ifr.ifr_hwaddr.sa_data[0], ifr.ifr_hwaddr.sa_data[1], ifr.ifr_hwaddr.sa_data[2], */
+    /*         ifr.ifr_hwaddr.sa_data[3], ifr.ifr_hwaddr.sa_data[4], ifr.ifr_hwaddr.sa_data[5]); */
 
     err = ioctl(sk, SIOCSIFHWADDR, (caddr_t)&ifr);
 
     if (err < 0) {
-        error("Could not set bt address for interface:%s, errno:%s", devname, strerror(errno));
+        BTIF_TRACE_ERROR2("Could not set bt address for interface:%s, errno:%s", devname, strerror(errno));
         close(sk);
         return -1;
     }
@@ -342,12 +323,12 @@ static int tap_if_up(const char *devname, BD_ADDR addr)
 
 
     if (err < 0) {
-        error("Could not bring up network interface:%s, errno:%d", devname, errno);
+        BTIF_TRACE_ERROR2("Could not bring up network interface:%s, errno:%d", devname, errno);
         close(sk);
         return -1;
     }
     close(sk);
-    debug("network interface: %s is up", devname);
+    BTIF_TRACE_DEBUG1("network interface: %s is up", devname);
     return 0;
 }
 
@@ -371,7 +352,6 @@ static int tap_if_down(const char *devname)
 }
 int btpan_tap_open()
 {
-    debug("in");
     struct ifreq ifr;
     int fd, err;
     const char *clonedev = "/dev/tun";
@@ -381,7 +361,7 @@ int btpan_tap_open()
     //system("insmod /system/lib/modules/tun.ko");
     if( (fd = open(clonedev, O_RDWR)) < 0 ) {
 
-        debug("could not open %s, err:%d", clonedev, errno);
+        BTIF_TRACE_DEBUG2("could not open %s, err:%d", clonedev, errno);
         return fd;
     }
 
@@ -393,7 +373,7 @@ int btpan_tap_open()
     /* try to create the device */
     if( (err = ioctl(fd, TUNSETIFF, (void *) &ifr)) < 0 )//|| tap_setup_ip(TAP_IF_NAME) == FALSE)
     {
-        debug("ioctl error:%d, errno:%s", err, strerror(errno));
+        BTIF_TRACE_DEBUG2("ioctl error:%d, errno:%s", err, strerror(errno));
         close(fd);
         return err;
     }
@@ -402,25 +382,13 @@ int btpan_tap_open()
     {
         return fd;
     }
-    error("can not bring up tap interface:%s", TAP_IF_NAME);
+    BTIF_TRACE_ERROR1("can not bring up tap interface:%s", TAP_IF_NAME);
     close(fd);
     return -1;
 }
 int btpan_tap_send(int tap_fd, const BD_ADDR src, const BD_ADDR dst, UINT16 proto, const char* buf,
                     UINT16 len, BOOLEAN ext, BOOLEAN forward)
 {
-    debug("in");
-    debug("SRC ADDR = %02x:%02x:%02x:%02x:%02x:%02x",
-            src[0], src[1], src[2], src[3],
-            src[4], src[5]);
-    debug("DST ADDR = %02x:%02x:%02x:%02x:%02x:%02x",
-            dst[0], dst[1], dst[2], dst[3],
-            dst[4], dst[5]);
-
-    debug("Protocol = 0x%x", proto);
-    debug("Ext = 0x%x", ext);
-    debug("Forward = 0x%x", forward);
-    debug("Len = %d", len);
     if(tap_fd != -1)
     {
         tETH_HDR eth_hdr;
@@ -443,7 +411,7 @@ int btpan_tap_send(int tap_fd, const BD_ADDR src, const BD_ADDR dst, UINT16 prot
         //btnet_send(btpan_cb.conn[i].sock.sock, &buffer, (len + sizeof(tETH_HDR)));
         //dump_bin("packet to network", packet, len + sizeof(tETH_HDR));
         int ret = write(tap_fd, packet, len + sizeof(tETH_HDR));
-        debug("ret:%d", ret);
+        BTIF_TRACE_DEBUG1("ret:%d", ret);
         return ret;
     }
     return -1;
@@ -451,7 +419,6 @@ int btpan_tap_send(int tap_fd, const BD_ADDR src, const BD_ADDR dst, UINT16 prot
 }
 int btpan_tap_close(int fd)
 {
-    debug("in");
     tap_if_down(TAP_IF_NAME);
     close(fd);
     return 0;
@@ -486,13 +453,12 @@ static void btpan_cleanup_conn(btpan_conn_t* conn)
 btpan_conn_t* btpan_new_conn(int handle, const BD_ADDR addr, int local_role, int remote_role )
 {
     int i;
-    debug("in");
     for(i = 0; i < MAX_PAN_CONNS; i++)
     {
-        debug("conns[%d]:%d", i, btpan_cb.conns[i].handle);
+        BTIF_TRACE_DEBUG2("conns[%d]:%d", i, btpan_cb.conns[i].handle);
         if(btpan_cb.conns[i].handle == -1)
         {
-            debug("handle:%d, local_role:%d, remote_role:%d", handle, local_role, remote_role);
+            BTIF_TRACE_DEBUG3("handle:%d, local_role:%d, remote_role:%d", handle, local_role, remote_role);
 
             btpan_cb.conns[i].handle = handle;
             bdcpy(btpan_cb.conns[i].peer, addr);
@@ -501,13 +467,13 @@ btpan_conn_t* btpan_new_conn(int handle, const BD_ADDR addr, int local_role, int
             return &btpan_cb.conns[i];
         }
     }
-    debug("MAX_PAN_CONNS:%d exceeded, return NULL as failed", MAX_PAN_CONNS);
+    BTIF_TRACE_DEBUG1("MAX_PAN_CONNS:%d exceeded, return NULL as failed", MAX_PAN_CONNS);
     return NULL;
 }
 
 void btpan_close_handle(btpan_conn_t *p)
 {
-    debug("btpan_close_handle : close handle %d", p->handle);
+    BTIF_TRACE_DEBUG1("btpan_close_handle : close handle %d", p->handle);
     p->handle = -1;
     p->local_role = -1;
     p->remote_role = -1;
@@ -517,7 +483,7 @@ static inline int should_forward(tETH_HDR* hdr)
 {
     if(ntohs(hdr->h_proto) == ETH_P_IP || ntohs(hdr->h_proto) == ETH_P_ARP)
         return TRUE;
-    debug("unknown proto:%x", ntohs(hdr->h_proto));
+    BTIF_TRACE_DEBUG1("unknown proto:%x", ntohs(hdr->h_proto));
     return FALSE;
 }
 extern void bta_pan_ci_rx_write(UINT16 handle, BD_ADDR dst, BD_ADDR src, UINT16 protocol,
@@ -533,7 +499,7 @@ static void forward_bnep(tETH_HDR* eth_hdr, char * packet, int size)
                 (broadcast || memcmp(btpan_cb.conns[i].eth_addr, eth_hdr->h_dest, sizeof(BD_ADDR)) == 0
                  || memcmp(btpan_cb.conns[i].peer, eth_hdr->h_dest, sizeof(BD_ADDR)) == 0))
         {
-            debug("calling bta_pan_ci_rx_write, handle:%d", handle);
+            BTIF_TRACE_DEBUG1("calling bta_pan_ci_rx_write, handle:%d", handle);
             bta_pan_ci_rx_write(handle, eth_hdr->h_dest, eth_hdr->h_src,
                     ntohs(eth_hdr->h_proto), (UINT8*)packet, size, 0);
             break;
@@ -547,7 +513,7 @@ static void bta_pan_callback_transfer(UINT16 event, char *p_param)
     switch(event)
     {
         case BTA_PAN_ENABLE_EVT:
-            debug("BTA_PAN_ENABLE_EVT");
+            BTIF_TRACE_DEBUG0("BTA_PAN_ENABLE_EVT");
             break;
         case BTA_PAN_SET_ROLE_EVT:
             {
@@ -562,7 +528,7 @@ static void bta_pan_callback_transfer(UINT16 event, char *p_param)
                 btpan_conn_t* conn;
                 bdstr_t bds;
                 bd2str((bt_bdaddr_t*)p_data->opening.bd_addr, &bds);
-                debug("BTA_PAN_OPENING_EVT handle %d, addr: %s", p_data->opening.handle, bds);
+                BTIF_TRACE_DEBUG2("BTA_PAN_OPENING_EVT handle %d, addr: %s", p_data->opening.handle, bds);
                 conn = btpan_find_conn_addr(p_data->opening.bd_addr);
 
                 asrt(conn != NULL);
@@ -575,15 +541,15 @@ static void bta_pan_callback_transfer(UINT16 event, char *p_param)
                             (const bt_bdaddr_t*)p_data->opening.bd_addr, btpan_conn_local_role, btpan_remote_role);
                 }
                 else
-                    error("connection not found");
+                    BTIF_TRACE_ERROR0("connection not found");
                 break;
             }
         case BTA_PAN_OPEN_EVT:
             {
-                debug("BTA_PAN_OPEN_EVT, open status:%d, bd_addr = [%02X:%02X:%02X:%02X:%02X:%02X]",
-                        p_data->open.status,
-                        p_data->open.bd_addr[0], p_data->open.bd_addr[1], p_data->open.bd_addr[2],
-                        p_data->open.bd_addr[3], p_data->open.bd_addr[4], p_data->open.bd_addr[5]);
+                /* debug("BTA_PAN_OPEN_EVT, open status:%d, bd_addr = [%02X:%02X:%02X:%02X:%02X:%02X]", */
+                /*         p_data->open.status, */
+                /*         p_data->open.bd_addr[0], p_data->open.bd_addr[1], p_data->open.bd_addr[2], */
+                /*         p_data->open.bd_addr[3], p_data->open.bd_addr[4], p_data->open.bd_addr[5]); */
                 btpan_connection_state_t state;
                 bt_status_t status;
                 if(p_data->open.status == BTA_PAN_SUCCESS)
@@ -597,10 +563,10 @@ static void bta_pan_callback_transfer(UINT16 event, char *p_param)
                     status = BT_STATUS_FAIL;
                 }
                 btpan_conn_t* conn = btpan_find_conn_handle(p_data->open.handle);
-                debug("BTA_PAN_OPEN_EVT handle:%d, conn:%p",  p_data->open.handle, conn);
-                debug("conn bta local_role:%d, bta remote role:%d", conn->local_role, conn->remote_role);
+                /* debug("BTA_PAN_OPEN_EVT handle:%d, conn:%p",  p_data->open.handle, conn); */
+                /* debug("conn bta local_role:%d, bta remote role:%d", conn->local_role, conn->remote_role); */
                 int btpan_conn_local_role = bta_role_to_btpan(p_data->open.local_role);
-                debug("bta local_role:%d, bta remote role:%d", p_data->open.local_role, p_data->open.peer_role);
+                /* debug("bta local_role:%d, bta remote role:%d", p_data->open.local_role, p_data->open.peer_role); */
                 int btpan_remote_role = bta_role_to_btpan(p_data->open.peer_role);
                 callback.connection_state_cb(state, status, (const bt_bdaddr_t*)p_data->open.bd_addr,
                         btpan_conn_local_role, btpan_remote_role);
@@ -614,7 +580,7 @@ static void bta_pan_callback_transfer(UINT16 event, char *p_param)
 
                 if(conn && conn->handle >= 0)
                 {
-                    debug("BTA_PAN_CLOSE_EVT, conn local_role:%d, remote_role:%d", conn->local_role, conn->remote_role);
+                    /* debug("BTA_PAN_CLOSE_EVT, conn local_role:%d, remote_role:%d", conn->local_role, conn->remote_role); */
                     int btpan_conn_local_role = bta_role_to_btpan(conn->local_role);
                     int btpan_remote_role = bta_role_to_btpan(conn->remote_role);
                     callback.connection_state_cb(BTPAN_STATE_DISCONNECTED, 0, (const bt_bdaddr_t*)conn->peer,
@@ -622,11 +588,11 @@ static void bta_pan_callback_transfer(UINT16 event, char *p_param)
                     btpan_cleanup_conn(conn);
                 }
                 else
-                    error("pan handle not found (%d)", p_data->close.handle);
+                    BTIF_TRACE_ERROR1("pan handle not found (%d)", p_data->close.handle);
                 break;
             }
         default:
-            debug("Unknown pan event %d", event);
+            BTIF_TRACE_WARNING1("Unknown pan event %d", event);
             break;
     }
 }
@@ -642,20 +608,20 @@ static void btpan_tap_fd_signaled(int fd, int type, int flags, uint32_t user_id)
     tETH_HDR eth_hdr;
     if(flags & SOCK_THREAD_FD_EXCEPTION)
     {
-        error("pan tap fd:%d exception", fd);
+        BTIF_TRACE_ERROR1("pan tap fd:%d exception", fd);
     }
     else if(flags & SOCK_THREAD_FD_RD)
     {
-        debug("tab fd read trigged,  data");
+        /* debug("tab fd read trigged,  data"); */
         int size = read(fd, packet, MAX_PACKET_SIZE);
-        debug("tap fd read trigged, read size:%d", size);
+        /* debug("tap fd read trigged, read size:%d", size); */
         memcpy(&eth_hdr, &packet, sizeof(tETH_HDR));
-        debug("eth src = %02x:%02x:%02x:%02x:%02x:%02x",
-                eth_hdr.h_src[0],  eth_hdr.h_src[1], eth_hdr.h_src[2], eth_hdr.h_src[3],
-                eth_hdr.h_src[4], eth_hdr.h_src[5]);
-        debug("eth dest = %02x:%02x:%02x:%02x:%02x:%02x",
-                eth_hdr.h_dest[0], eth_hdr.h_dest[1], eth_hdr.h_dest[2], eth_hdr.h_dest[3],
-                eth_hdr.h_dest[4], eth_hdr.h_dest[5]);
+        /* debug("eth src = %02x:%02x:%02x:%02x:%02x:%02x", */
+        /*         eth_hdr.h_src[0],  eth_hdr.h_src[1], eth_hdr.h_src[2], eth_hdr.h_src[3], */
+        /*         eth_hdr.h_src[4], eth_hdr.h_src[5]); */
+        /* debug("eth dest = %02x:%02x:%02x:%02x:%02x:%02x", */
+        /*         eth_hdr.h_dest[0], eth_hdr.h_dest[1], eth_hdr.h_dest[2], eth_hdr.h_dest[3], */
+        /*         eth_hdr.h_dest[4], eth_hdr.h_dest[5]); */
         //dump_bin("eth packet received", packet, size);
         if(should_forward(&eth_hdr))
         {
diff --git a/btif/src/btif_sock.c b/btif/src/btif_sock.c
index a2c799a..621a91e 100644
--- a/btif/src/btif_sock.c
+++ b/btif/src/btif_sock.c
@@ -38,11 +38,6 @@
 #include "bta_api.h"
 #include "btif_sock_thread.h"
 #include "btif_sock_rfc.h"
-#include <cutils/log.h>
-#define info(fmt, ...)  ALOGI ("btif_sock: %s: " fmt,__FUNCTION__,  ## __VA_ARGS__)
-#define debug(fmt, ...) ALOGD ("btif_sock: %s: " fmt,__FUNCTION__,  ## __VA_ARGS__)
-#define error(fmt, ...) ALOGE ("btif_sock: ## ERROR : %s: " fmt "##",__FUNCTION__,  ## __VA_ARGS__)
-#define asrt(s) if(!(s)) ALOGE ("btif_sock: ## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
 
 static bt_status_t btsock_listen(btsock_type_t type, const char* service_name,
                                 const uint8_t* uuid, int channel, int* sock_fd, int flags);
@@ -71,31 +66,27 @@ btsock_interface_t *btif_sock_get_interface()
 }
 bt_status_t btif_sock_init()
 {
-    debug("");
-
-
     static volatile int binit;
     if(!binit)
     {
         //fix me, the process doesn't exit right now. don't set the init flag for now
         //binit = 1;
-        debug("btsock initializing...");
+        BTIF_TRACE_DEBUG0("btsock initializing...");
         btsock_thread_init();
         int handle = btsock_thread_create(btsock_signaled, NULL);
         if(handle >= 0 && btsock_rfc_init(handle) == BT_STATUS_SUCCESS)
         {
-            debug("btsock successfully initialized");
+            BTIF_TRACE_DEBUG0("btsock successfully initialized");
             return BT_STATUS_SUCCESS;
         }
     }
-    else error("btsock interface already initialized");
+    else BTIF_TRACE_ERROR0("btsock interface already initialized");
     return BT_STATUS_FAIL;
 }
 void btif_sock_cleanup()
 {
-    debug("");
     btsock_rfc_cleanup();
-    debug("leaving");
+    BTIF_TRACE_DEBUG0("leaving");
 }
 
 static bt_status_t btsock_listen(btsock_type_t type, const char* service_name,
@@ -103,7 +94,7 @@ static bt_status_t btsock_listen(btsock_type_t type, const char* service_name,
 {
     if((service_uuid == NULL && channel <= 0) || sock_fd == NULL)
     {
-        error("invalid parameters, uuid:%p, channel:%d, sock_fd:%p", service_uuid, channel, sock_fd);
+        BTIF_TRACE_ERROR3("invalid parameters, uuid:%p, channel:%d, sock_fd:%p", service_uuid, channel, sock_fd);
         return BT_STATUS_PARM_INVALID;
     }
     *sock_fd = -1;
@@ -114,15 +105,15 @@ static bt_status_t btsock_listen(btsock_type_t type, const char* service_name,
             status = btsock_rfc_listen(service_name, service_uuid, channel, sock_fd, flags);
             break;
         case BTSOCK_L2CAP:
-            error("bt l2cap socket type not supported, type:%d", type);
+            BTIF_TRACE_ERROR1("bt l2cap socket type not supported, type:%d", type);
             status = BT_STATUS_UNSUPPORTED;
             break;
         case BTSOCK_SCO:
-            error("bt sco socket not supported, type:%d", type);
+            BTIF_TRACE_ERROR1("bt sco socket not supported, type:%d", type);
             status = BT_STATUS_UNSUPPORTED;
             break;
         default:
-            error("unknown bt socket type:%d", type);
+            BTIF_TRACE_ERROR1("unknown bt socket type:%d", type);
             status = BT_STATUS_UNSUPPORTED;
             break;
     }
@@ -133,7 +124,7 @@ static bt_status_t btsock_connect(const bt_bdaddr_t *bd_addr, btsock_type_t type
 {
     if((uuid == NULL && channel <= 0) || bd_addr == NULL || sock_fd == NULL)
     {
-        error("invalid parameters, bd_addr:%p, uuid:%p, channel:%d, sock_fd:%p",
+        BTIF_TRACE_ERROR4("invalid parameters, bd_addr:%p, uuid:%p, channel:%d, sock_fd:%p",
                 bd_addr, uuid, channel, sock_fd);
         return BT_STATUS_PARM_INVALID;
     }
@@ -145,15 +136,15 @@ static bt_status_t btsock_connect(const bt_bdaddr_t *bd_addr, btsock_type_t type
             status = btsock_rfc_connect(bd_addr, uuid, channel, sock_fd, flags);
             break;
         case BTSOCK_L2CAP:
-            error("bt l2cap socket type not supported, type:%d", type);
+            BTIF_TRACE_ERROR1("bt l2cap socket type not supported, type:%d", type);
             status = BT_STATUS_UNSUPPORTED;
             break;
         case BTSOCK_SCO:
-            error("bt sco socket not supported, type:%d", type);
+            BTIF_TRACE_ERROR1("bt sco socket not supported, type:%d", type);
             status = BT_STATUS_UNSUPPORTED;
             break;
         default:
-            error("unknown bt socket type:%d", type);
+            BTIF_TRACE_ERROR1("unknown bt socket type:%d", type);
             status = BT_STATUS_UNSUPPORTED;
             break;
     }
@@ -167,13 +158,13 @@ static void btsock_signaled(int fd, int type, int flags, uint32_t user_id)
             btsock_rfc_signaled(fd, flags, user_id);
             break;
         case BTSOCK_L2CAP:
-            error("bt l2cap socket type not supported, fd:%d, flags:%d", fd, flags);
+            BTIF_TRACE_ERROR2("bt l2cap socket type not supported, fd:%d, flags:%d", fd, flags);
             break;
         case BTSOCK_SCO:
-            error("bt sco socket type not supported, fd:%d, flags:%d", fd, flags);
+            BTIF_TRACE_ERROR2("bt sco socket type not supported, fd:%d, flags:%d", fd, flags);
             break;
         default:
-            error("unknown socket type:%d, fd:%d, flags:%d", type, fd, flags);
+            BTIF_TRACE_ERROR3("unknown socket type:%d, fd:%d, flags:%d", type, fd, flags);
             break;
     }
 }
diff --git a/btif/src/btif_sock_rfc.c b/btif/src/btif_sock_rfc.c
index 650549f..0e52c1b 100644
--- a/btif/src/btif_sock_rfc.c
+++ b/btif/src/btif_sock_rfc.c
@@ -305,6 +305,8 @@ static inline rfc_slot_t* create_srv_accept_rfc_slot(rfc_slot_t* srv_rs, const b
 bt_status_t btsock_rfc_listen(const char* service_name, const uint8_t* service_uuid, int channel,
                             int* sock_fd, int flags)
 {
+
+    APPL_TRACE_DEBUG1("btsock_rfc_listen, service_name:%s", service_name);
     if(sock_fd == NULL || (service_uuid == NULL && (channel < 1 || channel > 30)))
     {
         APPL_TRACE_ERROR3("invalid rfc channel:%d or sock_fd:%p, uuid:%p", channel, sock_fd, service_uuid);
@@ -329,6 +331,7 @@ bt_status_t btsock_rfc_listen(const char* service_name, const uint8_t* service_u
     rfc_slot_t* rs = alloc_rfc_slot(NULL, service_name, service_uuid, channel, flags, TRUE);
     if(rs)
     {
+        APPL_TRACE_DEBUG1("BTA_JvCreateRecordByUser:%s", service_name);
         BTA_JvCreateRecordByUser((void *)rs->id);
         *sock_fd = rs->app_fd;
         rs->app_fd = -1; //the fd ownership is transferred to app
@@ -466,18 +469,19 @@ static inline void free_rfc_slot_scn(rfc_slot_t* rs)
 {
     if(rs->scn > 0)
     {
-        if(rs->f.server && !rs->f.closing)
+        if(rs->f.server && !rs->f.closing && rs->rfc_handle)
         {
             BTA_JvRfcommStopServer(rs->rfc_handle);
             rs->rfc_handle = 0;
         }
-        BTM_FreeSCN(rs->scn);
+        if(rs->f.server)
+            BTM_FreeSCN(rs->scn);
         rs->scn = 0;
     }
 }
 static void cleanup_rfc_slot(rfc_slot_t* rs)
 {
-    APPL_TRACE_DEBUG3("cleanup slot:%d, fd:%d, scn:%d", rs->id, rs->fd, rs->scn);
+    APPL_TRACE_DEBUG4("cleanup slot:%d, fd:%d, scn:%d, sdp_handle:0x%x", rs->id, rs->fd, rs->scn, rs->sdp_handle);
     if(rs->fd != -1)
     {
         shutdown(rs->fd, 2);
@@ -695,6 +699,7 @@ static void *rfcomm_cback(tBTA_JV_EVT event, tBTA_JV *p_data, void *user_data)
         break;
 
     case BTA_JV_RFCOMM_CLOSE_EVT:
+        APPL_TRACE_DEBUG1("BTA_JV_RFCOMM_CLOSE_EVT: user_data:%d", (uint32_t)user_data);
         on_rfc_close(&p_data->rfc_close, (uint32_t)user_data);
         break;
 
@@ -724,7 +729,7 @@ static void *rfcomm_cback(tBTA_JV_EVT event, tBTA_JV *p_data, void *user_data)
 static void jv_dm_cback(tBTA_JV_EVT event, tBTA_JV *p_data, void *user_data)
 {
     uint32_t id = (uint32_t)user_data;
-    APPL_TRACE_DEBUG2("event:%d, slot id:%d", event, id);
+    APPL_TRACE_DEBUG2("jv_dm_cback: event:%d, slot id:%d", event, id);
     switch(event)
     {
         case BTA_JV_CREATE_RECORD_EVT:
@@ -737,6 +742,11 @@ static void jv_dm_cback(tBTA_JV_EVT event, tBTA_JV *p_data, void *user_data)
                     BTA_JvRfcommStartServer(rs->security, rs->role, rs->scn, MAX_RFC_SESSION, rfcomm_cback,
                                             (void*)rs->id);
                 }
+                else if(rs)
+                {
+                    APPL_TRACE_ERROR1("jv_dm_cback: cannot start server, slot found:%p", rs);
+                    cleanup_rfc_slot(rs);
+                }
                 unlock_slot(&slot_lock);
                 break;
             }
@@ -896,7 +906,7 @@ void btsock_rfc_signaled(int fd, int flags, uint32_t user_id)
             APPL_TRACE_DEBUG1("SOCK_THREAD_FD_EXCEPTION, flags:%x", flags);
             rs->f.closing = TRUE;
             if(rs->f.server)
-               BTA_JvRfcommStopServer(rs->rfc_handle);
+                BTA_JvRfcommStopServer(rs->rfc_handle);
             else
                 BTA_JvRfcommClose(rs->rfc_handle);
         }
diff --git a/btif/src/btif_sock_sdp.c b/btif/src/btif_sock_sdp.c
index cf55e8a..a1ec6df 100644
--- a/btif/src/btif_sock_sdp.c
+++ b/btif/src/btif_sock_sdp.c
@@ -51,6 +51,7 @@
 #include "utl.h"
 #include "../bta/pb/bta_pbs_int.h"
 #include "../include/bta_op_api.h"
+#include "bta_jv_api.h"
 #include <cutils/log.h>
 
 #define RESERVED_SCN_PBS 19
@@ -113,7 +114,7 @@ static int add_sdp_by_uuid(const char *name,  const uint8_t *service_uuid, UINT1
             }
         }
     }
-
+    else APPL_TRACE_ERROR1("failed to create sdp record, service_name:%s", name);
     return 0;
 }
 
@@ -142,7 +143,7 @@ static int add_pbap_sdp(const char* p_service_name, int scn)
     UINT32              sdp_handle = 0;
     tBTA_PBS_CFG *p_bta_pbs_cfg = (tBTA_PBS_CFG *)&bta_pbs_cfg;
 
-    APPL_TRACE_DEBUG2("scn %d, service name %s", scn, p_service_name);
+    APPL_TRACE_DEBUG2("add_pbap_sdd:scn %d, service name %s", scn, p_service_name);
 
     if ((sdp_handle = SDP_CreateRecord()) == 0)
     {
@@ -437,7 +438,8 @@ int add_rfc_sdp_rec(const char* name, const uint8_t* uuid, int scn)
 
 void del_rfc_sdp_rec(int handle)
 {
+    APPL_TRACE_DEBUG1("del_rfc_sdp_rec: handle:0x%x", handle);
     if(handle != -1 && handle != 0)
-        SDP_DeleteRecord( handle );
+        BTA_JvDeleteRecord( handle );
 }
 
diff --git a/btif/src/btif_sock_util.c b/btif/src/btif_sock_util.c
index 09df03e..5b5de69 100644
--- a/btif/src/btif_sock_util.c
+++ b/btif/src/btif_sock_util.c
@@ -66,12 +66,7 @@
 #include "bta_jv_co.h"
 #include "port_api.h"
 
-#include <cutils/log.h>
-
-#define info(fmt, ...)  ALOGI ("%s: " fmt,__FUNCTION__,  ## __VA_ARGS__)
-#define debug(fmt, ...) ALOGD ("%s: " fmt,__FUNCTION__,  ## __VA_ARGS__)
-#define error(fmt, ...) ALOGE ("## ERROR : %s: " fmt "##",__FUNCTION__,  ## __VA_ARGS__)
-#define asrt(s) if(!(s)) ALOGE ("## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
+#define asrt(s) if(!(s)) BTIF_TRACE_ERROR3("## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
 
 
 int sock_send_all(int sock_fd, const uint8_t* buf, int len)
@@ -84,7 +79,7 @@ int sock_send_all(int sock_fd, const uint8_t* buf, int len)
         while(ret < 0 && errno == EINTR);
         if(ret <= 0)
         {
-            error("sock fd:%d send errno:%d, ret:%d", sock_fd, errno, ret);
+            BTIF_TRACE_ERROR3("sock fd:%d send errno:%d, ret:%d", sock_fd, errno, ret);
             return -1;
         }
         buf += ret;
@@ -102,7 +97,7 @@ int sock_recv_all(int sock_fd, uint8_t* buf, int len)
         while(ret < 0 && errno == EINTR);
         if(ret <= 0)
         {
-            error("sock fd:%d recv errno:%d, ret:%d", sock_fd, errno, ret);
+            BTIF_TRACE_ERROR3("sock fd:%d recv errno:%d, ret:%d", sock_fd, errno, ret);
             return -1;
         }
         buf += ret;
@@ -150,8 +145,8 @@ int sock_send_fd(int sock_fd, const uint8_t* buf, int len, int send_fd)
         } while (ret < 0 && errno == EINTR);
 
         if (ret < 0) {
-            error("fd:%d, send_fd:%d, sendmsg ret:%d, errno:%d, %s",
-                     sock_fd, send_fd, (int)ret, errno, strerror(errno));
+            BTIF_TRACE_ERROR5("fd:%d, send_fd:%d, sendmsg ret:%d, errno:%d, %s",
+                              sock_fd, send_fd, (int)ret, errno, strerror(errno));
             ret_len = -1;
             break;
         }
@@ -162,7 +157,7 @@ int sock_send_fd(int sock_fd, const uint8_t* buf, int len, int send_fd)
         // Wipes out any msg_control too
         memset(&msg, 0, sizeof(msg));
     }
-    debug("close fd:%d after sent", send_fd);
+    BTIF_TRACE_DEBUG1("close fd:%d after sent", send_fd);
     close(send_fd);
     return ret_len;
 }
diff --git a/btif/src/btif_storage.c b/btif/src/btif_storage.c
index 6d702a7..2190b1c 100755
--- a/btif/src/btif_storage.c
+++ b/btif/src/btif_storage.c
@@ -46,11 +46,6 @@
 #include "btif_hh.h"
 
 #include <cutils/log.h>
-#define info(fmt, ...)  ALOGI ("%s(L%d): " fmt,__FUNCTION__, __LINE__,  ## __VA_ARGS__)
-#define debug(fmt, ...) ALOGD ("%s(L%d): " fmt,__FUNCTION__, __LINE__,  ## __VA_ARGS__)
-#define warn(fmt, ...) ALOGW ("## WARNING : %s(L%d): " fmt "##",__FUNCTION__, __LINE__, ## __VA_ARGS__)
-#define error(fmt, ...) ALOGE ("## ERROR : %s(L%d): " fmt "##",__FUNCTION__, __LINE__, ## __VA_ARGS__)
-#define asrt(s) if(!(s)) ALOGE ("## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
 
 /************************************************************************************
 **  Constants & Macros
@@ -236,11 +231,11 @@ static int prop2cfg(bt_bdaddr_t *remote_bd_addr, bt_property_t *prop)
     bdstr_t bdstr = {0};
     if(remote_bd_addr)
         bd2str(remote_bd_addr, &bdstr);
-    debug("in, bd addr:%s, prop type:%d, len:%d", bdstr, prop->type, prop->len);
+    BTIF_TRACE_DEBUG3("in, bd addr:%s, prop type:%d, len:%d", bdstr, prop->type, prop->len);
     char value[1024];
     if(prop->len <= 0 || prop->len > (int)sizeof(value) - 1)
     {
-        error("property type:%d, len:%d is invalid", prop->type, prop->len);
+        BTIF_TRACE_ERROR2("property type:%d, len:%d is invalid", prop->type, prop->len);
         return FALSE;
     }
     switch(prop->type)
@@ -298,7 +293,7 @@ static int prop2cfg(bt_bdaddr_t *remote_bd_addr, bt_property_t *prop)
             break;
         }
         default:
-             error("Unknow prop type:%d", prop->type);
+             BTIF_TRACE_ERROR1("Unknow prop type:%d", prop->type);
              return FALSE;
     }
     return TRUE;
@@ -308,10 +303,10 @@ static int cfg2prop(bt_bdaddr_t *remote_bd_addr, bt_property_t *prop)
     bdstr_t bdstr = {0};
     if(remote_bd_addr)
         bd2str(remote_bd_addr, &bdstr);
-    debug("in, bd addr:%s, prop type:%d, len:%d", bdstr, prop->type, prop->len);
+    BTIF_TRACE_DEBUG3("in, bd addr:%s, prop type:%d, len:%d", bdstr, prop->type, prop->len);
     if(prop->len <= 0)
     {
-        error("property type:%d, len:%d is invalid", prop->type, prop->len);
+        BTIF_TRACE_ERROR2("property type:%d, len:%d is invalid", prop->type, prop->len);
         return FALSE;
     }
     int ret = FALSE;
@@ -389,7 +384,7 @@ static int cfg2prop(bt_bdaddr_t *remote_bd_addr, bt_property_t *prop)
             break;
         }
         default:
-            error("Unknow prop type:%d", prop->type);
+            BTIF_TRACE_ERROR1("Unknow prop type:%d", prop->type);
             return FALSE;
     }
     return ret;
@@ -408,7 +403,7 @@ static int cfg2prop(bt_bdaddr_t *remote_bd_addr, bt_property_t *prop)
 *******************************************************************************/
 static bt_status_t btif_in_fetch_bonded_devices(btif_bonded_devices_t *p_bonded_devices, int add)
 {
-    debug("in add:%d", add);
+    BTIF_TRACE_DEBUG1("in add:%d", add);
     memset(p_bonded_devices, 0, sizeof(btif_bonded_devices_t));
 
     char kname[128], vname[128];
@@ -420,7 +415,7 @@ static bt_status_t btif_in_fetch_bonded_devices(btif_bonded_devices_t *p_bonded_
     do
     {
         kpos = btif_config_next_key(kpos, "Remote", kname, &kname_size);
-        debug("Remote device:%s, size:%d", kname, kname_size);
+        BTIF_TRACE_DEBUG2("Remote device:%s, size:%d", kname, kname_size);
         int type = BTIF_CFG_TYPE_BIN;
         LINK_KEY link_key;
         int size = sizeof(link_key);
@@ -443,13 +438,12 @@ static bt_status_t btif_in_fetch_bonded_devices(btif_bonded_devices_t *p_bonded_
                 }
                 memcpy(&p_bonded_devices->devices[p_bonded_devices->num_devices++], &bd_addr, sizeof(bt_bdaddr_t));
             }
-            else error("bounded device:%s, LinkKeyType or PinLength is invalid", kname);
+            else BTIF_TRACE_ERROR1("bounded device:%s, LinkKeyType or PinLength is invalid", kname);
         }
-        else debug("Remote device:%s, no link key", kname);
+        else BTIF_TRACE_DEBUG1("Remote device:%s, no link key", kname);
         kname_size = sizeof(kname);
         kname[0] = 0;
     } while(kpos != -1);
-    debug("out");
     return BT_STATUS_SUCCESS;
 }
 
@@ -717,7 +711,7 @@ bt_status_t btif_storage_remove_bonded_device(bt_bdaddr_t *remote_bd_addr)
 {
     bdstr_t bdstr;
     bd2str(remote_bd_addr, &bdstr);
-    debug("in bd addr:%s", bdstr);
+    BTIF_TRACE_DEBUG1("in bd addr:%s", bdstr);
     int ret = btif_config_remove("Remote", bdstr, "LinkKeyType");
     ret &= btif_config_remove("Remote", bdstr, "PinLength");
     ret &= btif_config_remove("Remote", bdstr, "LinkKey");
@@ -895,7 +889,6 @@ bt_status_t btif_storage_add_hid_device_info(bt_bdaddr_t *remote_bd_addr,
 *******************************************************************************/
 bt_status_t btif_storage_load_bonded_hid_info(void)
 {
-    debug("in");
     bt_bdaddr_t bd_addr;
     tBTA_HH_DEV_DSCP_INFO dscp_info;
     uint32_t i;
@@ -913,7 +906,7 @@ bt_status_t btif_storage_load_bonded_hid_info(void)
     do
     {
         kpos = btif_config_next_key(kpos, "Remote", kname, &kname_size);
-        debug("Remote device:%s, size:%d", kname, kname_size);
+        BTIF_TRACE_DEBUG2("Remote device:%s, size:%d", kname, kname_size);
         int value;
         if(btif_config_get_int("Remote", kname, "HidAttrMask", &value))
         {
diff --git a/conf/bt_stack.conf b/conf/bt_stack.conf
index a956c14..2cd2f62 100644
--- a/conf/bt_stack.conf
+++ b/conf/bt_stack.conf
@@ -31,3 +31,4 @@ TRC_SDP=2
 TRC_GATT=2
 TRC_SMP=2
 TRC_BTAPP=2
+TRC_BTIF=2
diff --git a/include/bt_target.h b/include/bt_target.h
index db2a126..836868e 100644..100755
--- a/include/bt_target.h
+++ b/include/bt_target.h
@@ -1218,6 +1218,11 @@ and USER_HW_DISABLE_API macros */
 #define BTM_BLE_CONFORMANCE_TESTING           FALSE
 #endif
 
+/* Maximum number of consecutive HCI commands  that can time out
+* before  it gets treated as H/w error*/
+#ifndef BTM_MAX_HCI_CMD_TOUT_BEFORE_RESTART
+#define BTM_MAX_HCI_CMD_TOUT_BEFORE_RESTART 2
+#endif
 
 /******************************************************************************
 **
@@ -1305,7 +1310,7 @@ and USER_HW_DISABLE_API macros */
 
 /* Minimum number of ACL credit for high priority link */
 #ifndef L2CAP_HIGH_PRI_MIN_XMIT_QUOTA
-#define L2CAP_HIGH_PRI_MIN_XMIT_QUOTA       4
+#define L2CAP_HIGH_PRI_MIN_XMIT_QUOTA       5
 #endif
 
 /* used for monitoring HCI ACL credit management */
diff --git a/main/bte_logmsg.c b/main/bte_logmsg.c
index 52502ae..7035a57 100644
--- a/main/bte_logmsg.c
+++ b/main/bte_logmsg.c
@@ -311,6 +311,14 @@ BT_API UINT8 BTAPP_SetTraceLevel( UINT8 new_level )
     return (appl_trace_level);
 }
 
+BT_API UINT8 BTIF_SetTraceLevel( UINT8 new_level )
+{
+    if (new_level != 0xFF)
+        btif_trace_level = new_level;
+
+    return (btif_trace_level);
+}
+
 BTU_API UINT8 BTU_SetTraceLevel( UINT8 new_level )
 {
     if (new_level != 0xFF)
@@ -471,6 +479,10 @@ tBTTRC_FUNC_MAP bttrc_set_level_map[] = {
     {BTTRC_ID_BTA_ACC, BTTRC_ID_BTAPP, BTAPP_SetTraceLevel, BTL_GLOBAL_PROP_TRC_FLAG, DEFAULT_CONF_TRACE_LEVEL},
 #endif
 
+#if (BT_TRACE_BTIF == TRUE)
+    {BTTRC_ID_BTA_ACC, BTTRC_ID_BTAPP, BTIF_SetTraceLevel, "TRC_BTIF", DEFAULT_CONF_TRACE_LEVEL},
+#endif
+
     {0, 0, NULL, NULL, DEFAULT_CONF_TRACE_LEVEL}
 };
 
diff --git a/stack/Android.mk b/stack/Android.mk
index 0c24805..dcbe961 100644
--- a/stack/Android.mk
+++ b/stack/Android.mk
@@ -101,7 +101,6 @@ LOCAL_SRC_FILES:= \
     ./smp/smp_act.c \
     ./smp/smp_keys.c \
     ./smp/smp_api.c \
-    ./smp/aes.c \
     ./avdt/avdt_ccb.c \
     ./avdt/avdt_scb_act.c \
     ./avdt/avdt_msg.c \
diff --git a/stack/btu/btu_hcif.c b/stack/btu/btu_hcif.c
index 8ca9d35..63803bd 100644
--- a/stack/btu/btu_hcif.c
+++ b/stack/btu/btu_hcif.c
@@ -54,6 +54,9 @@
 #endif
 // btla-specific --
 
+//Counter to track number of HCI command timeout
+static int num_hci_cmds_timed_out;
+
 /********************************************************************************/
 /*              L O C A L    F U N C T I O N     P R O T O T Y P E S            */
 /********************************************************************************/
@@ -418,6 +421,8 @@ void btu_hcif_process_event (UINT8 controller_id, BT_HDR *p_msg)
                 btm_vendor_specific_evt (p, hci_evt_len);
             break;
     }
+    // reset the  num_hci_cmds_timed_out upon receving any event from controller.
+    num_hci_cmds_timed_out = 0;
 }
 
 
@@ -1591,9 +1596,26 @@ void btu_hcif_cmd_timeout (UINT8 controller_id)
     /* free stored command */
     GKI_freebuf(p_cmd);
 
-    /* If anyone wants device status notifications, give him one */
-    btm_report_device_status (BTM_DEV_STATUS_CMD_TOUT);
+    num_hci_cmds_timed_out++;
+    /* When we receive consecutive HCI cmd timeouts for >=BTM_MAX_HCI_CMD_TOUT_BEFORE_RESTART
+     times, Bluetooth process will be killed and restarted */
+    if (num_hci_cmds_timed_out >= BTM_MAX_HCI_CMD_TOUT_BEFORE_RESTART)
+    {
+        BT_TRACE_1(TRACE_LAYER_HCI, TRACE_TYPE_ERROR,
+                  "Num consecutive HCI Cmd tout =%d Restarting BT process",num_hci_cmds_timed_out);
 
+        usleep(10000); /* 10 milliseconds */
+        /* Killing the process to force a restart as part of fault tolerance */
+        kill(getpid(), SIGKILL);
+    }
+    else
+    {
+        BT_TRACE_1 (TRACE_LAYER_HCI, TRACE_TYPE_WARNING, "HCI Cmd timeout counter %d",
+                    num_hci_cmds_timed_out);
+
+        /* If anyone wants device status notifications, give him one */
+        btm_report_device_status (BTM_DEV_STATUS_CMD_TOUT);
+    }
     /* See if we can forward any more commands */
     btu_hcif_send_cmd (controller_id, NULL);
 }
diff --git a/stack/rfcomm/port_utils.c b/stack/rfcomm/port_utils.c
index ae41682..fe2eb60 100644
--- a/stack/rfcomm/port_utils.c
+++ b/stack/rfcomm/port_utils.c
@@ -33,13 +33,6 @@
 #include "btm_int.h"
 #include "btu.h"
 
-#include <cutils/log.h>
-#define info(fmt, ...)  ALOGI ("%s: " fmt,__FUNCTION__,  ## __VA_ARGS__)
-#define debug(fmt, ...) ALOGD ("%s: " fmt,__FUNCTION__,  ## __VA_ARGS__)
-#define error(fmt, ...) ALOGE ("## ERROR : %s: " fmt "##",__FUNCTION__,  ## __VA_ARGS__)
-#define asrt(s) if(!(s)) ALOGE ("## %s assert %s failed at line:%d ##",__FUNCTION__, #s, __LINE__)
-
-
 static const tPORT_STATE default_port_pars =
 {
     PORT_BAUD_RATE_9600,
@@ -92,7 +85,7 @@ tPORT *port_allocate_port (UINT8 dlci, BD_ADDR bd_addr)
             port_set_defaults (p_port);
 
             rfc_cb.rfc.last_port = yy;
-            debug("rfc_cb.port.port[%d] allocated, last_port:%d", yy, rfc_cb.rfc.last_port);
+            RFCOMM_TRACE_DEBUG2("rfc_cb.port.port[%d] allocated, last_port:%d", yy, rfc_cb.rfc.last_port);
             return (p_port);
         }
     }
@@ -217,7 +210,7 @@ void port_release_port (tPORT *p_port)
     tPORT_STATE user_port_pars;
 
     PORT_SCHEDULE_LOCK;
-    debug("port_release_port, p_port:%p", p_port);
+    RFCOMM_TRACE_DEBUG1("port_release_port, p_port:%p", p_port);
     while ((p_buf = (BT_HDR *)GKI_dequeue (&p_port->rx.queue)) != NULL)
         GKI_freebuf (p_buf);
 
diff --git a/stack/sdp/sdp_db.c b/stack/sdp/sdp_db.c
index 45af000..b5ab31a 100644
--- a/stack/sdp/sdp_db.c
+++ b/stack/sdp/sdp_db.c
@@ -300,7 +300,7 @@ UINT32 SDP_CreateRecord (void)
         p_db->record[p_db->num_records].record_handle = handle;
 
         p_db->num_records++;
-
+        SDP_TRACE_DEBUG1("SDP_CreateRecord ok, num_records:%d", p_db->num_records);
         /* Add the first attribute (the handle) automatically */
         UINT32_TO_BE_FIELD (buf, handle);
         SDP_AddAttribute (handle, ATTR_ID_SERVICE_RECORD_HDL, UINT_DESC_TYPE,
@@ -308,6 +308,7 @@ UINT32 SDP_CreateRecord (void)
 
         return (p_db->record[p_db->num_records - 1].record_handle);
     }
+    else SDP_TRACE_ERROR1("SDP_CreateRecord fail, exceed maximum records:%d", SDP_MAX_RECORDS);
 #endif
         return (0);
 }
@@ -362,6 +363,7 @@ BOOLEAN SDP_DeleteRecord (UINT32 handle)
 
                 sdp_cb.server_db.num_records--;
 
+                SDP_TRACE_DEBUG1("SDP_DeleteRecord ok, num_records:%d", sdp_cb.server_db.num_records);
                 /* if we're deleting the primary DI record, clear the */
                 /* value in the control block */
                 if( sdp_cb.server_db.di_primary_handle == handle )
diff --git a/stack/smp/aes.c b/stack/smp/aes.c
deleted file mode 100644
index 1028d5b..0000000
--- a/stack/smp/aes.c
+++ /dev/null
@@ -1,926 +0,0 @@
-/*
- ---------------------------------------------------------------------------
- Copyright (c) 1998-2008, Brian Gladman, Worcester, UK. All rights reserved.
-
- LICENSE TERMS
-
- The redistribution and use of this software (with or without changes)
- is allowed without the payment of fees or royalties provided that:
-
-  1. source code distributions include the above copyright notice, this
-     list of conditions and the following disclaimer;
-
-  2. binary distributions include the above copyright notice, this list
-     of conditions and the following disclaimer in their documentation;
-
-  3. the name of the copyright holder is not used to endorse products
-     built using this software without specific written permission.
-
- DISCLAIMER
-
- This software is provided 'as is' with no explicit or implied warranties
- in respect of its properties, including, but not limited to, correctness
- and/or fitness for purpose.
- ---------------------------------------------------------------------------
- Issue 09/09/2006
-
- This is an AES implementation that uses only 8-bit byte operations on the
- cipher state (there are options to use 32-bit types if available).
-
- The combination of mix columns and byte substitution used here is based on
- that developed by Karl Malbrain. His contribution is acknowledged.
- */
-
-/* define if you have a fast memcpy function on your system */
-#if 1
-#  define HAVE_MEMCPY
-#  include <string.h>
-#if 0
-#  if defined( _MSC_VER )
-#    include <intrin.h>
-#    pragma intrinsic( memcpy )
-#  endif
-#endif
-#endif
-
-#include <stdlib.h>
-
-/* define if you have fast 32-bit types on your system */
-#if 1
-#  define HAVE_UINT_32T
-#endif
-
-/* define if you don't want any tables */
-#if 1
-#  define USE_TABLES
-#endif
-
-/*  On Intel Core 2 duo VERSION_1 is faster */
-
-/* alternative versions (test for performance on your system) */
-#if 1
-#  define VERSION_1
-#endif
-
-#include "aes.h"
-
-#if defined( HAVE_UINT_32T )
-  typedef unsigned long uint_32t;
-#endif
-
-/* functions for finite field multiplication in the AES Galois field    */
-
-#define WPOLY   0x011b
-#define BPOLY     0x1b
-#define DPOLY   0x008d
-
-#define f1(x)   (x)
-#define f2(x)   ((x << 1) ^ (((x >> 7) & 1) * WPOLY))
-#define f4(x)   ((x << 2) ^ (((x >> 6) & 1) * WPOLY) ^ (((x >> 6) & 2) * WPOLY))
-#define f8(x)   ((x << 3) ^ (((x >> 5) & 1) * WPOLY) ^ (((x >> 5) & 2) * WPOLY) \
-                          ^ (((x >> 5) & 4) * WPOLY))
-#define d2(x)   (((x) >> 1) ^ ((x) & 1 ? DPOLY : 0))
-
-#define f3(x)   (f2(x) ^ x)
-#define f9(x)   (f8(x) ^ x)
-#define fb(x)   (f8(x) ^ f2(x) ^ x)
-#define fd(x)   (f8(x) ^ f4(x) ^ x)
-#define fe(x)   (f8(x) ^ f4(x) ^ f2(x))
-
-#if defined( USE_TABLES )
-
-#define sb_data(w) {    /* S Box data values */                            \
-    w(0x63), w(0x7c), w(0x77), w(0x7b), w(0xf2), w(0x6b), w(0x6f), w(0xc5),\
-    w(0x30), w(0x01), w(0x67), w(0x2b), w(0xfe), w(0xd7), w(0xab), w(0x76),\
-    w(0xca), w(0x82), w(0xc9), w(0x7d), w(0xfa), w(0x59), w(0x47), w(0xf0),\
-    w(0xad), w(0xd4), w(0xa2), w(0xaf), w(0x9c), w(0xa4), w(0x72), w(0xc0),\
-    w(0xb7), w(0xfd), w(0x93), w(0x26), w(0x36), w(0x3f), w(0xf7), w(0xcc),\
-    w(0x34), w(0xa5), w(0xe5), w(0xf1), w(0x71), w(0xd8), w(0x31), w(0x15),\
-    w(0x04), w(0xc7), w(0x23), w(0xc3), w(0x18), w(0x96), w(0x05), w(0x9a),\
-    w(0x07), w(0x12), w(0x80), w(0xe2), w(0xeb), w(0x27), w(0xb2), w(0x75),\
-    w(0x09), w(0x83), w(0x2c), w(0x1a), w(0x1b), w(0x6e), w(0x5a), w(0xa0),\
-    w(0x52), w(0x3b), w(0xd6), w(0xb3), w(0x29), w(0xe3), w(0x2f), w(0x84),\
-    w(0x53), w(0xd1), w(0x00), w(0xed), w(0x20), w(0xfc), w(0xb1), w(0x5b),\
-    w(0x6a), w(0xcb), w(0xbe), w(0x39), w(0x4a), w(0x4c), w(0x58), w(0xcf),\
-    w(0xd0), w(0xef), w(0xaa), w(0xfb), w(0x43), w(0x4d), w(0x33), w(0x85),\
-    w(0x45), w(0xf9), w(0x02), w(0x7f), w(0x50), w(0x3c), w(0x9f), w(0xa8),\
-    w(0x51), w(0xa3), w(0x40), w(0x8f), w(0x92), w(0x9d), w(0x38), w(0xf5),\
-    w(0xbc), w(0xb6), w(0xda), w(0x21), w(0x10), w(0xff), w(0xf3), w(0xd2),\
-    w(0xcd), w(0x0c), w(0x13), w(0xec), w(0x5f), w(0x97), w(0x44), w(0x17),\
-    w(0xc4), w(0xa7), w(0x7e), w(0x3d), w(0x64), w(0x5d), w(0x19), w(0x73),\
-    w(0x60), w(0x81), w(0x4f), w(0xdc), w(0x22), w(0x2a), w(0x90), w(0x88),\
-    w(0x46), w(0xee), w(0xb8), w(0x14), w(0xde), w(0x5e), w(0x0b), w(0xdb),\
-    w(0xe0), w(0x32), w(0x3a), w(0x0a), w(0x49), w(0x06), w(0x24), w(0x5c),\
-    w(0xc2), w(0xd3), w(0xac), w(0x62), w(0x91), w(0x95), w(0xe4), w(0x79),\
-    w(0xe7), w(0xc8), w(0x37), w(0x6d), w(0x8d), w(0xd5), w(0x4e), w(0xa9),\
-    w(0x6c), w(0x56), w(0xf4), w(0xea), w(0x65), w(0x7a), w(0xae), w(0x08),\
-    w(0xba), w(0x78), w(0x25), w(0x2e), w(0x1c), w(0xa6), w(0xb4), w(0xc6),\
-    w(0xe8), w(0xdd), w(0x74), w(0x1f), w(0x4b), w(0xbd), w(0x8b), w(0x8a),\
-    w(0x70), w(0x3e), w(0xb5), w(0x66), w(0x48), w(0x03), w(0xf6), w(0x0e),\
-    w(0x61), w(0x35), w(0x57), w(0xb9), w(0x86), w(0xc1), w(0x1d), w(0x9e),\
-    w(0xe1), w(0xf8), w(0x98), w(0x11), w(0x69), w(0xd9), w(0x8e), w(0x94),\
-    w(0x9b), w(0x1e), w(0x87), w(0xe9), w(0xce), w(0x55), w(0x28), w(0xdf),\
-    w(0x8c), w(0xa1), w(0x89), w(0x0d), w(0xbf), w(0xe6), w(0x42), w(0x68),\
-    w(0x41), w(0x99), w(0x2d), w(0x0f), w(0xb0), w(0x54), w(0xbb), w(0x16) }
-
-#define isb_data(w) {   /* inverse S Box data values */                    \
-    w(0x52), w(0x09), w(0x6a), w(0xd5), w(0x30), w(0x36), w(0xa5), w(0x38),\
-    w(0xbf), w(0x40), w(0xa3), w(0x9e), w(0x81), w(0xf3), w(0xd7), w(0xfb),\
-    w(0x7c), w(0xe3), w(0x39), w(0x82), w(0x9b), w(0x2f), w(0xff), w(0x87),\
-    w(0x34), w(0x8e), w(0x43), w(0x44), w(0xc4), w(0xde), w(0xe9), w(0xcb),\
-    w(0x54), w(0x7b), w(0x94), w(0x32), w(0xa6), w(0xc2), w(0x23), w(0x3d),\
-    w(0xee), w(0x4c), w(0x95), w(0x0b), w(0x42), w(0xfa), w(0xc3), w(0x4e),\
-    w(0x08), w(0x2e), w(0xa1), w(0x66), w(0x28), w(0xd9), w(0x24), w(0xb2),\
-    w(0x76), w(0x5b), w(0xa2), w(0x49), w(0x6d), w(0x8b), w(0xd1), w(0x25),\
-    w(0x72), w(0xf8), w(0xf6), w(0x64), w(0x86), w(0x68), w(0x98), w(0x16),\
-    w(0xd4), w(0xa4), w(0x5c), w(0xcc), w(0x5d), w(0x65), w(0xb6), w(0x92),\
-    w(0x6c), w(0x70), w(0x48), w(0x50), w(0xfd), w(0xed), w(0xb9), w(0xda),\
-    w(0x5e), w(0x15), w(0x46), w(0x57), w(0xa7), w(0x8d), w(0x9d), w(0x84),\
-    w(0x90), w(0xd8), w(0xab), w(0x00), w(0x8c), w(0xbc), w(0xd3), w(0x0a),\
-    w(0xf7), w(0xe4), w(0x58), w(0x05), w(0xb8), w(0xb3), w(0x45), w(0x06),\
-    w(0xd0), w(0x2c), w(0x1e), w(0x8f), w(0xca), w(0x3f), w(0x0f), w(0x02),\
-    w(0xc1), w(0xaf), w(0xbd), w(0x03), w(0x01), w(0x13), w(0x8a), w(0x6b),\
-    w(0x3a), w(0x91), w(0x11), w(0x41), w(0x4f), w(0x67), w(0xdc), w(0xea),\
-    w(0x97), w(0xf2), w(0xcf), w(0xce), w(0xf0), w(0xb4), w(0xe6), w(0x73),\
-    w(0x96), w(0xac), w(0x74), w(0x22), w(0xe7), w(0xad), w(0x35), w(0x85),\
-    w(0xe2), w(0xf9), w(0x37), w(0xe8), w(0x1c), w(0x75), w(0xdf), w(0x6e),\
-    w(0x47), w(0xf1), w(0x1a), w(0x71), w(0x1d), w(0x29), w(0xc5), w(0x89),\
-    w(0x6f), w(0xb7), w(0x62), w(0x0e), w(0xaa), w(0x18), w(0xbe), w(0x1b),\
-    w(0xfc), w(0x56), w(0x3e), w(0x4b), w(0xc6), w(0xd2), w(0x79), w(0x20),\
-    w(0x9a), w(0xdb), w(0xc0), w(0xfe), w(0x78), w(0xcd), w(0x5a), w(0xf4),\
-    w(0x1f), w(0xdd), w(0xa8), w(0x33), w(0x88), w(0x07), w(0xc7), w(0x31),\
-    w(0xb1), w(0x12), w(0x10), w(0x59), w(0x27), w(0x80), w(0xec), w(0x5f),\
-    w(0x60), w(0x51), w(0x7f), w(0xa9), w(0x19), w(0xb5), w(0x4a), w(0x0d),\
-    w(0x2d), w(0xe5), w(0x7a), w(0x9f), w(0x93), w(0xc9), w(0x9c), w(0xef),\
-    w(0xa0), w(0xe0), w(0x3b), w(0x4d), w(0xae), w(0x2a), w(0xf5), w(0xb0),\
-    w(0xc8), w(0xeb), w(0xbb), w(0x3c), w(0x83), w(0x53), w(0x99), w(0x61),\
-    w(0x17), w(0x2b), w(0x04), w(0x7e), w(0xba), w(0x77), w(0xd6), w(0x26),\
-    w(0xe1), w(0x69), w(0x14), w(0x63), w(0x55), w(0x21), w(0x0c), w(0x7d) }
-
-#define mm_data(w) {    /* basic data for forming finite field tables */   \
-    w(0x00), w(0x01), w(0x02), w(0x03), w(0x04), w(0x05), w(0x06), w(0x07),\
-    w(0x08), w(0x09), w(0x0a), w(0x0b), w(0x0c), w(0x0d), w(0x0e), w(0x0f),\
-    w(0x10), w(0x11), w(0x12), w(0x13), w(0x14), w(0x15), w(0x16), w(0x17),\
-    w(0x18), w(0x19), w(0x1a), w(0x1b), w(0x1c), w(0x1d), w(0x1e), w(0x1f),\
-    w(0x20), w(0x21), w(0x22), w(0x23), w(0x24), w(0x25), w(0x26), w(0x27),\
-    w(0x28), w(0x29), w(0x2a), w(0x2b), w(0x2c), w(0x2d), w(0x2e), w(0x2f),\
-    w(0x30), w(0x31), w(0x32), w(0x33), w(0x34), w(0x35), w(0x36), w(0x37),\
-    w(0x38), w(0x39), w(0x3a), w(0x3b), w(0x3c), w(0x3d), w(0x3e), w(0x3f),\
-    w(0x40), w(0x41), w(0x42), w(0x43), w(0x44), w(0x45), w(0x46), w(0x47),\
-    w(0x48), w(0x49), w(0x4a), w(0x4b), w(0x4c), w(0x4d), w(0x4e), w(0x4f),\
-    w(0x50), w(0x51), w(0x52), w(0x53), w(0x54), w(0x55), w(0x56), w(0x57),\
-    w(0x58), w(0x59), w(0x5a), w(0x5b), w(0x5c), w(0x5d), w(0x5e), w(0x5f),\
-    w(0x60), w(0x61), w(0x62), w(0x63), w(0x64), w(0x65), w(0x66), w(0x67),\
-    w(0x68), w(0x69), w(0x6a), w(0x6b), w(0x6c), w(0x6d), w(0x6e), w(0x6f),\
-    w(0x70), w(0x71), w(0x72), w(0x73), w(0x74), w(0x75), w(0x76), w(0x77),\
-    w(0x78), w(0x79), w(0x7a), w(0x7b), w(0x7c), w(0x7d), w(0x7e), w(0x7f),\
-    w(0x80), w(0x81), w(0x82), w(0x83), w(0x84), w(0x85), w(0x86), w(0x87),\
-    w(0x88), w(0x89), w(0x8a), w(0x8b), w(0x8c), w(0x8d), w(0x8e), w(0x8f),\
-    w(0x90), w(0x91), w(0x92), w(0x93), w(0x94), w(0x95), w(0x96), w(0x97),\
-    w(0x98), w(0x99), w(0x9a), w(0x9b), w(0x9c), w(0x9d), w(0x9e), w(0x9f),\
-    w(0xa0), w(0xa1), w(0xa2), w(0xa3), w(0xa4), w(0xa5), w(0xa6), w(0xa7),\
-    w(0xa8), w(0xa9), w(0xaa), w(0xab), w(0xac), w(0xad), w(0xae), w(0xaf),\
-    w(0xb0), w(0xb1), w(0xb2), w(0xb3), w(0xb4), w(0xb5), w(0xb6), w(0xb7),\
-    w(0xb8), w(0xb9), w(0xba), w(0xbb), w(0xbc), w(0xbd), w(0xbe), w(0xbf),\
-    w(0xc0), w(0xc1), w(0xc2), w(0xc3), w(0xc4), w(0xc5), w(0xc6), w(0xc7),\
-    w(0xc8), w(0xc9), w(0xca), w(0xcb), w(0xcc), w(0xcd), w(0xce), w(0xcf),\
-    w(0xd0), w(0xd1), w(0xd2), w(0xd3), w(0xd4), w(0xd5), w(0xd6), w(0xd7),\
-    w(0xd8), w(0xd9), w(0xda), w(0xdb), w(0xdc), w(0xdd), w(0xde), w(0xdf),\
-    w(0xe0), w(0xe1), w(0xe2), w(0xe3), w(0xe4), w(0xe5), w(0xe6), w(0xe7),\
-    w(0xe8), w(0xe9), w(0xea), w(0xeb), w(0xec), w(0xed), w(0xee), w(0xef),\
-    w(0xf0), w(0xf1), w(0xf2), w(0xf3), w(0xf4), w(0xf5), w(0xf6), w(0xf7),\
-    w(0xf8), w(0xf9), w(0xfa), w(0xfb), w(0xfc), w(0xfd), w(0xfe), w(0xff) }
-
-static const uint_8t sbox[256]  =  sb_data(f1);
-static const uint_8t isbox[256] = isb_data(f1);
-
-static const uint_8t gfm2_sbox[256] = sb_data(f2);
-static const uint_8t gfm3_sbox[256] = sb_data(f3);
-
-static const uint_8t gfmul_9[256] = mm_data(f9);
-static const uint_8t gfmul_b[256] = mm_data(fb);
-static const uint_8t gfmul_d[256] = mm_data(fd);
-static const uint_8t gfmul_e[256] = mm_data(fe);
-
-#define s_box(x)     sbox[(x)]
-#define is_box(x)    isbox[(x)]
-#define gfm2_sb(x)   gfm2_sbox[(x)]
-#define gfm3_sb(x)   gfm3_sbox[(x)]
-#define gfm_9(x)     gfmul_9[(x)]
-#define gfm_b(x)     gfmul_b[(x)]
-#define gfm_d(x)     gfmul_d[(x)]
-#define gfm_e(x)     gfmul_e[(x)]
-
-#else
-
-/* this is the high bit of x right shifted by 1 */
-/* position. Since the starting polynomial has  */
-/* 9 bits (0x11b), this right shift keeps the   */
-/* values of all top bits within a byte         */
-
-static uint_8t hibit(const uint_8t x)
-{   uint_8t r = (uint_8t)((x >> 1) | (x >> 2));
-
-    r |= (r >> 2);
-    r |= (r >> 4);
-    return (r + 1) >> 1;
-}
-
-/* return the inverse of the finite field element x */
-
-static uint_8t gf_inv(const uint_8t x)
-{   uint_8t p1 = x, p2 = BPOLY, n1 = hibit(x), n2 = 0x80, v1 = 1, v2 = 0;
-
-    if(x < 2)
-        return x;
-
-    for( ; ; )
-    {
-        if(n1)
-            while(n2 >= n1)             /* divide polynomial p2 by p1    */
-            {
-                n2 /= n1;               /* shift smaller polynomial left */
-                p2 ^= (p1 * n2) & 0xff; /* and remove from larger one    */
-                v2 ^= (v1 * n2);        /* shift accumulated value and   */
-                n2 = hibit(p2);         /* add into result               */
-            }
-        else
-            return v1;
-
-        if(n2)                          /* repeat with values swapped    */
-            while(n1 >= n2)
-            {
-                n1 /= n2;
-                p1 ^= p2 * n1;
-                v1 ^= v2 * n1;
-                n1 = hibit(p1);
-            }
-        else
-            return v2;
-    }
-}
-
-/* The forward and inverse affine transformations used in the S-box */
-uint_8t fwd_affine(const uint_8t x)
-{
-#if defined( HAVE_UINT_32T )
-    uint_32t w = x;
-    w ^= (w << 1) ^ (w << 2) ^ (w << 3) ^ (w << 4);
-    return 0x63 ^ ((w ^ (w >> 8)) & 0xff);
-#else
-    return 0x63 ^ x ^ (x << 1) ^ (x << 2) ^ (x << 3) ^ (x << 4)
-                    ^ (x >> 7) ^ (x >> 6) ^ (x >> 5) ^ (x >> 4);
-#endif
-}
-
-uint_8t inv_affine(const uint_8t x)
-{
-#if defined( HAVE_UINT_32T )
-    uint_32t w = x;
-    w = (w << 1) ^ (w << 3) ^ (w << 6);
-    return 0x05 ^ ((w ^ (w >> 8)) & 0xff);
-#else
-    return 0x05 ^ (x << 1) ^ (x << 3) ^ (x << 6)
-                ^ (x >> 7) ^ (x >> 5) ^ (x >> 2);
-#endif
-}
-
-#define s_box(x)   fwd_affine(gf_inv(x))
-#define is_box(x)  gf_inv(inv_affine(x))
-#define gfm2_sb(x) f2(s_box(x))
-#define gfm3_sb(x) f3(s_box(x))
-#define gfm_9(x)   f9(x)
-#define gfm_b(x)   fb(x)
-#define gfm_d(x)   fd(x)
-#define gfm_e(x)   fe(x)
-
-#endif
-
-#if defined( HAVE_MEMCPY )
-#  define block_copy_nn(d, s, l)    memcpy(d, s, l)
-#  define block_copy(d, s)          memcpy(d, s, N_BLOCK)
-#else
-#  define block_copy_nn(d, s, l)    copy_block_nn(d, s, l)
-#  define block_copy(d, s)          copy_block(d, s)
-#endif
-
-#if !defined( HAVE_MEMCPY )
-static void copy_block( void *d, const void *s )
-{
-#if defined( HAVE_UINT_32T )
-    ((uint_32t*)d)[ 0] = ((uint_32t*)s)[ 0];
-    ((uint_32t*)d)[ 1] = ((uint_32t*)s)[ 1];
-    ((uint_32t*)d)[ 2] = ((uint_32t*)s)[ 2];
-    ((uint_32t*)d)[ 3] = ((uint_32t*)s)[ 3];
-#else
-    ((uint_8t*)d)[ 0] = ((uint_8t*)s)[ 0];
-    ((uint_8t*)d)[ 1] = ((uint_8t*)s)[ 1];
-    ((uint_8t*)d)[ 2] = ((uint_8t*)s)[ 2];
-    ((uint_8t*)d)[ 3] = ((uint_8t*)s)[ 3];
-    ((uint_8t*)d)[ 4] = ((uint_8t*)s)[ 4];
-    ((uint_8t*)d)[ 5] = ((uint_8t*)s)[ 5];
-    ((uint_8t*)d)[ 6] = ((uint_8t*)s)[ 6];
-    ((uint_8t*)d)[ 7] = ((uint_8t*)s)[ 7];
-    ((uint_8t*)d)[ 8] = ((uint_8t*)s)[ 8];
-    ((uint_8t*)d)[ 9] = ((uint_8t*)s)[ 9];
-    ((uint_8t*)d)[10] = ((uint_8t*)s)[10];
-    ((uint_8t*)d)[11] = ((uint_8t*)s)[11];
-    ((uint_8t*)d)[12] = ((uint_8t*)s)[12];
-    ((uint_8t*)d)[13] = ((uint_8t*)s)[13];
-    ((uint_8t*)d)[14] = ((uint_8t*)s)[14];
-    ((uint_8t*)d)[15] = ((uint_8t*)s)[15];
-#endif
-}
-
-static void copy_block_nn( void * d, const void *s, uint_8t nn )
-{
-    while( nn-- )
-        *((uint_8t*)d)++ = *((uint_8t*)s)++;
-}
-#endif
-
-static void xor_block( void *d, const void *s )
-{
-#if defined( HAVE_UINT_32T )
-    ((uint_32t*)d)[ 0] ^= ((uint_32t*)s)[ 0];
-    ((uint_32t*)d)[ 1] ^= ((uint_32t*)s)[ 1];
-    ((uint_32t*)d)[ 2] ^= ((uint_32t*)s)[ 2];
-    ((uint_32t*)d)[ 3] ^= ((uint_32t*)s)[ 3];
-#else
-    ((uint_8t*)d)[ 0] ^= ((uint_8t*)s)[ 0];
-    ((uint_8t*)d)[ 1] ^= ((uint_8t*)s)[ 1];
-    ((uint_8t*)d)[ 2] ^= ((uint_8t*)s)[ 2];
-    ((uint_8t*)d)[ 3] ^= ((uint_8t*)s)[ 3];
-    ((uint_8t*)d)[ 4] ^= ((uint_8t*)s)[ 4];
-    ((uint_8t*)d)[ 5] ^= ((uint_8t*)s)[ 5];
-    ((uint_8t*)d)[ 6] ^= ((uint_8t*)s)[ 6];
-    ((uint_8t*)d)[ 7] ^= ((uint_8t*)s)[ 7];
-    ((uint_8t*)d)[ 8] ^= ((uint_8t*)s)[ 8];
-    ((uint_8t*)d)[ 9] ^= ((uint_8t*)s)[ 9];
-    ((uint_8t*)d)[10] ^= ((uint_8t*)s)[10];
-    ((uint_8t*)d)[11] ^= ((uint_8t*)s)[11];
-    ((uint_8t*)d)[12] ^= ((uint_8t*)s)[12];
-    ((uint_8t*)d)[13] ^= ((uint_8t*)s)[13];
-    ((uint_8t*)d)[14] ^= ((uint_8t*)s)[14];
-    ((uint_8t*)d)[15] ^= ((uint_8t*)s)[15];
-#endif
-}
-
-static void copy_and_key( void *d, const void *s, const void *k )
-{
-#if defined( HAVE_UINT_32T )
-    ((uint_32t*)d)[ 0] = ((uint_32t*)s)[ 0] ^ ((uint_32t*)k)[ 0];
-    ((uint_32t*)d)[ 1] = ((uint_32t*)s)[ 1] ^ ((uint_32t*)k)[ 1];
-    ((uint_32t*)d)[ 2] = ((uint_32t*)s)[ 2] ^ ((uint_32t*)k)[ 2];
-    ((uint_32t*)d)[ 3] = ((uint_32t*)s)[ 3] ^ ((uint_32t*)k)[ 3];
-#elif 1
-    ((uint_8t*)d)[ 0] = ((uint_8t*)s)[ 0] ^ ((uint_8t*)k)[ 0];
-    ((uint_8t*)d)[ 1] = ((uint_8t*)s)[ 1] ^ ((uint_8t*)k)[ 1];
-    ((uint_8t*)d)[ 2] = ((uint_8t*)s)[ 2] ^ ((uint_8t*)k)[ 2];
-    ((uint_8t*)d)[ 3] = ((uint_8t*)s)[ 3] ^ ((uint_8t*)k)[ 3];
-    ((uint_8t*)d)[ 4] = ((uint_8t*)s)[ 4] ^ ((uint_8t*)k)[ 4];
-    ((uint_8t*)d)[ 5] = ((uint_8t*)s)[ 5] ^ ((uint_8t*)k)[ 5];
-    ((uint_8t*)d)[ 6] = ((uint_8t*)s)[ 6] ^ ((uint_8t*)k)[ 6];
-    ((uint_8t*)d)[ 7] = ((uint_8t*)s)[ 7] ^ ((uint_8t*)k)[ 7];
-    ((uint_8t*)d)[ 8] = ((uint_8t*)s)[ 8] ^ ((uint_8t*)k)[ 8];
-    ((uint_8t*)d)[ 9] = ((uint_8t*)s)[ 9] ^ ((uint_8t*)k)[ 9];
-    ((uint_8t*)d)[10] = ((uint_8t*)s)[10] ^ ((uint_8t*)k)[10];
-    ((uint_8t*)d)[11] = ((uint_8t*)s)[11] ^ ((uint_8t*)k)[11];
-    ((uint_8t*)d)[12] = ((uint_8t*)s)[12] ^ ((uint_8t*)k)[12];
-    ((uint_8t*)d)[13] = ((uint_8t*)s)[13] ^ ((uint_8t*)k)[13];
-    ((uint_8t*)d)[14] = ((uint_8t*)s)[14] ^ ((uint_8t*)k)[14];
-    ((uint_8t*)d)[15] = ((uint_8t*)s)[15] ^ ((uint_8t*)k)[15];
-#else
-    block_copy(d, s);
-    xor_block(d, k);
-#endif
-}
-
-static void add_round_key( uint_8t d[N_BLOCK], const uint_8t k[N_BLOCK] )
-{
-    xor_block(d, k);
-}
-
-static void shift_sub_rows( uint_8t st[N_BLOCK] )
-{   uint_8t tt;
-
-    st[ 0] = s_box(st[ 0]); st[ 4] = s_box(st[ 4]);
-    st[ 8] = s_box(st[ 8]); st[12] = s_box(st[12]);
-
-    tt = st[1]; st[ 1] = s_box(st[ 5]); st[ 5] = s_box(st[ 9]);
-    st[ 9] = s_box(st[13]); st[13] = s_box( tt );
-
-    tt = st[2]; st[ 2] = s_box(st[10]); st[10] = s_box( tt );
-    tt = st[6]; st[ 6] = s_box(st[14]); st[14] = s_box( tt );
-
-    tt = st[15]; st[15] = s_box(st[11]); st[11] = s_box(st[ 7]);
-    st[ 7] = s_box(st[ 3]); st[ 3] = s_box( tt );
-}
-
-static void inv_shift_sub_rows( uint_8t st[N_BLOCK] )
-{   uint_8t tt;
-
-    st[ 0] = is_box(st[ 0]); st[ 4] = is_box(st[ 4]);
-    st[ 8] = is_box(st[ 8]); st[12] = is_box(st[12]);
-
-    tt = st[13]; st[13] = is_box(st[9]); st[ 9] = is_box(st[5]);
-    st[ 5] = is_box(st[1]); st[ 1] = is_box( tt );
-
-    tt = st[2]; st[ 2] = is_box(st[10]); st[10] = is_box( tt );
-    tt = st[6]; st[ 6] = is_box(st[14]); st[14] = is_box( tt );
-
-    tt = st[3]; st[ 3] = is_box(st[ 7]); st[ 7] = is_box(st[11]);
-    st[11] = is_box(st[15]); st[15] = is_box( tt );
-}
-
-#if defined( VERSION_1 )
-  static void mix_sub_columns( uint_8t dt[N_BLOCK] )
-  { uint_8t st[N_BLOCK];
-    block_copy(st, dt);
-#else
-  static void mix_sub_columns( uint_8t dt[N_BLOCK], uint_8t st[N_BLOCK] )
-  {
-#endif
-    dt[ 0] = gfm2_sb(st[0]) ^ gfm3_sb(st[5]) ^ s_box(st[10]) ^ s_box(st[15]);
-    dt[ 1] = s_box(st[0]) ^ gfm2_sb(st[5]) ^ gfm3_sb(st[10]) ^ s_box(st[15]);
-    dt[ 2] = s_box(st[0]) ^ s_box(st[5]) ^ gfm2_sb(st[10]) ^ gfm3_sb(st[15]);
-    dt[ 3] = gfm3_sb(st[0]) ^ s_box(st[5]) ^ s_box(st[10]) ^ gfm2_sb(st[15]);
-
-    dt[ 4] = gfm2_sb(st[4]) ^ gfm3_sb(st[9]) ^ s_box(st[14]) ^ s_box(st[3]);
-    dt[ 5] = s_box(st[4]) ^ gfm2_sb(st[9]) ^ gfm3_sb(st[14]) ^ s_box(st[3]);
-    dt[ 6] = s_box(st[4]) ^ s_box(st[9]) ^ gfm2_sb(st[14]) ^ gfm3_sb(st[3]);
-    dt[ 7] = gfm3_sb(st[4]) ^ s_box(st[9]) ^ s_box(st[14]) ^ gfm2_sb(st[3]);
-
-    dt[ 8] = gfm2_sb(st[8]) ^ gfm3_sb(st[13]) ^ s_box(st[2]) ^ s_box(st[7]);
-    dt[ 9] = s_box(st[8]) ^ gfm2_sb(st[13]) ^ gfm3_sb(st[2]) ^ s_box(st[7]);
-    dt[10] = s_box(st[8]) ^ s_box(st[13]) ^ gfm2_sb(st[2]) ^ gfm3_sb(st[7]);
-    dt[11] = gfm3_sb(st[8]) ^ s_box(st[13]) ^ s_box(st[2]) ^ gfm2_sb(st[7]);
-
-    dt[12] = gfm2_sb(st[12]) ^ gfm3_sb(st[1]) ^ s_box(st[6]) ^ s_box(st[11]);
-    dt[13] = s_box(st[12]) ^ gfm2_sb(st[1]) ^ gfm3_sb(st[6]) ^ s_box(st[11]);
-    dt[14] = s_box(st[12]) ^ s_box(st[1]) ^ gfm2_sb(st[6]) ^ gfm3_sb(st[11]);
-    dt[15] = gfm3_sb(st[12]) ^ s_box(st[1]) ^ s_box(st[6]) ^ gfm2_sb(st[11]);
-  }
-
-#if defined( VERSION_1 )
-  static void inv_mix_sub_columns( uint_8t dt[N_BLOCK] )
-  { uint_8t st[N_BLOCK];
-    block_copy(st, dt);
-#else
-  static void inv_mix_sub_columns( uint_8t dt[N_BLOCK], uint_8t st[N_BLOCK] )
-  {
-#endif
-    dt[ 0] = is_box(gfm_e(st[ 0]) ^ gfm_b(st[ 1]) ^ gfm_d(st[ 2]) ^ gfm_9(st[ 3]));
-    dt[ 5] = is_box(gfm_9(st[ 0]) ^ gfm_e(st[ 1]) ^ gfm_b(st[ 2]) ^ gfm_d(st[ 3]));
-    dt[10] = is_box(gfm_d(st[ 0]) ^ gfm_9(st[ 1]) ^ gfm_e(st[ 2]) ^ gfm_b(st[ 3]));
-    dt[15] = is_box(gfm_b(st[ 0]) ^ gfm_d(st[ 1]) ^ gfm_9(st[ 2]) ^ gfm_e(st[ 3]));
-
-    dt[ 4] = is_box(gfm_e(st[ 4]) ^ gfm_b(st[ 5]) ^ gfm_d(st[ 6]) ^ gfm_9(st[ 7]));
-    dt[ 9] = is_box(gfm_9(st[ 4]) ^ gfm_e(st[ 5]) ^ gfm_b(st[ 6]) ^ gfm_d(st[ 7]));
-    dt[14] = is_box(gfm_d(st[ 4]) ^ gfm_9(st[ 5]) ^ gfm_e(st[ 6]) ^ gfm_b(st[ 7]));
-    dt[ 3] = is_box(gfm_b(st[ 4]) ^ gfm_d(st[ 5]) ^ gfm_9(st[ 6]) ^ gfm_e(st[ 7]));
-
-    dt[ 8] = is_box(gfm_e(st[ 8]) ^ gfm_b(st[ 9]) ^ gfm_d(st[10]) ^ gfm_9(st[11]));
-    dt[13] = is_box(gfm_9(st[ 8]) ^ gfm_e(st[ 9]) ^ gfm_b(st[10]) ^ gfm_d(st[11]));
-    dt[ 2] = is_box(gfm_d(st[ 8]) ^ gfm_9(st[ 9]) ^ gfm_e(st[10]) ^ gfm_b(st[11]));
-    dt[ 7] = is_box(gfm_b(st[ 8]) ^ gfm_d(st[ 9]) ^ gfm_9(st[10]) ^ gfm_e(st[11]));
-
-    dt[12] = is_box(gfm_e(st[12]) ^ gfm_b(st[13]) ^ gfm_d(st[14]) ^ gfm_9(st[15]));
-    dt[ 1] = is_box(gfm_9(st[12]) ^ gfm_e(st[13]) ^ gfm_b(st[14]) ^ gfm_d(st[15]));
-    dt[ 6] = is_box(gfm_d(st[12]) ^ gfm_9(st[13]) ^ gfm_e(st[14]) ^ gfm_b(st[15]));
-    dt[11] = is_box(gfm_b(st[12]) ^ gfm_d(st[13]) ^ gfm_9(st[14]) ^ gfm_e(st[15]));
-  }
-
-#if defined( AES_ENC_PREKEYED ) || defined( AES_DEC_PREKEYED )
-
-/*  Set the cipher key for the pre-keyed version */
-
-return_type aes_set_key( const unsigned char key[], length_type keylen, aes_context ctx[1] )
-{
-    uint_8t cc, rc, hi;
-
-    switch( keylen )
-    {
-    case 16:
-    case 128:
-        keylen = 16;
-        break;
-    case 24:
-    case 192:
-        keylen = 24;
-        break;
-    case 32:
-    case 256:
-        keylen = 32;
-        break;
-    default:
-        ctx->rnd = 0;
-        return (return_type)-1;
-    }
-    block_copy_nn(ctx->ksch, key, keylen);
-    hi = (keylen + 28) << 2;
-    ctx->rnd = (hi >> 4) - 1;
-    for( cc = keylen, rc = 1; cc < hi; cc += 4 )
-    {   uint_8t tt, t0, t1, t2, t3;
-
-        t0 = ctx->ksch[cc - 4];
-        t1 = ctx->ksch[cc - 3];
-        t2 = ctx->ksch[cc - 2];
-        t3 = ctx->ksch[cc - 1];
-        if( cc % keylen == 0 )
-        {
-            tt = t0;
-            t0 = s_box(t1) ^ rc;
-            t1 = s_box(t2);
-            t2 = s_box(t3);
-            t3 = s_box(tt);
-            rc = f2(rc);
-        }
-        else if( keylen > 24 && cc % keylen == 16 )
-        {
-            t0 = s_box(t0);
-            t1 = s_box(t1);
-            t2 = s_box(t2);
-            t3 = s_box(t3);
-        }
-        tt = cc - keylen;
-        ctx->ksch[cc + 0] = ctx->ksch[tt + 0] ^ t0;
-        ctx->ksch[cc + 1] = ctx->ksch[tt + 1] ^ t1;
-        ctx->ksch[cc + 2] = ctx->ksch[tt + 2] ^ t2;
-        ctx->ksch[cc + 3] = ctx->ksch[tt + 3] ^ t3;
-    }
-    return 0;
-}
-
-#endif
-
-#if defined( AES_ENC_PREKEYED )
-
-/*  Encrypt a single block of 16 bytes */
-
-return_type aes_encrypt( const unsigned char in[N_BLOCK], unsigned char  out[N_BLOCK], const aes_context ctx[1] )
-{
-    if( ctx->rnd )
-    {
-        uint_8t s1[N_BLOCK], r;
-        copy_and_key( s1, in, ctx->ksch );
-
-        for( r = 1 ; r < ctx->rnd ; ++r )
-#if defined( VERSION_1 )
-        {
-            mix_sub_columns( s1 );
-            add_round_key( s1, ctx->ksch + r * N_BLOCK);
-        }
-#else
-        {   uint_8t s2[N_BLOCK];
-            mix_sub_columns( s2, s1 );
-            copy_and_key( s1, s2, ctx->ksch + r * N_BLOCK);
-        }
-#endif
-        shift_sub_rows( s1 );
-        copy_and_key( out, s1, ctx->ksch + r * N_BLOCK );
-    }
-    else
-        return (return_type)-1;
-    return 0;
-}
-
-/* CBC encrypt a number of blocks (input and return an IV) */
-
-return_type aes_cbc_encrypt( const unsigned char *in, unsigned char *out,
-                         int n_block, unsigned char iv[N_BLOCK], const aes_context ctx[1] )
-{
-
-    while(n_block--)
-    {
-        xor_block(iv, in);
-        if(aes_encrypt(iv, iv, ctx) != EXIT_SUCCESS)
-			return EXIT_FAILURE;
-        memcpy(out, iv, N_BLOCK);
-        in += N_BLOCK;
-        out += N_BLOCK;
-    }
-    return EXIT_SUCCESS;
-}
-
-#endif
-
-#if defined( AES_DEC_PREKEYED )
-
-/*  Decrypt a single block of 16 bytes */
-
-return_type aes_decrypt( const unsigned char in[N_BLOCK], unsigned char out[N_BLOCK], const aes_context ctx[1] )
-{
-    if( ctx->rnd )
-    {
-        uint_8t s1[N_BLOCK], r;
-        copy_and_key( s1, in, ctx->ksch + ctx->rnd * N_BLOCK );
-        inv_shift_sub_rows( s1 );
-
-        for( r = ctx->rnd ; --r ; )
-#if defined( VERSION_1 )
-        {
-            add_round_key( s1, ctx->ksch + r * N_BLOCK );
-            inv_mix_sub_columns( s1 );
-        }
-#else
-        {   uint_8t s2[N_BLOCK];
-            copy_and_key( s2, s1, ctx->ksch + r * N_BLOCK );
-            inv_mix_sub_columns( s1, s2 );
-        }
-#endif
-        copy_and_key( out, s1, ctx->ksch );
-    }
-    else
-        return (return_type)-1;
-    return 0;
-}
-
-/* CBC decrypt a number of blocks (input and return an IV) */
-
-return_type aes_cbc_decrypt( const unsigned char *in, unsigned char *out,
-                         int n_block, unsigned char iv[N_BLOCK], const aes_context ctx[1] )
-{
-    while(n_block--)
-    {   uint_8t tmp[N_BLOCK];
-
-        memcpy(tmp, in, N_BLOCK);
-        if(aes_decrypt(in, out, ctx) != EXIT_SUCCESS)
-			return EXIT_FAILURE;
-        xor_block(out, iv);
-        memcpy(iv, tmp, N_BLOCK);
-        in += N_BLOCK;
-        out += N_BLOCK;
-    }
-    return EXIT_SUCCESS;
-}
-
-#endif
-
-#if defined( AES_ENC_128_OTFK )
-
-/*  The 'on the fly' encryption key update for for 128 bit keys */
-
-static void update_encrypt_key_128( uint_8t k[N_BLOCK], uint_8t *rc )
-{   uint_8t cc;
-
-    k[0] ^= s_box(k[13]) ^ *rc;
-    k[1] ^= s_box(k[14]);
-    k[2] ^= s_box(k[15]);
-    k[3] ^= s_box(k[12]);
-    *rc = f2( *rc );
-
-    for(cc = 4; cc < 16; cc += 4 )
-    {
-        k[cc + 0] ^= k[cc - 4];
-        k[cc + 1] ^= k[cc - 3];
-        k[cc + 2] ^= k[cc - 2];
-        k[cc + 3] ^= k[cc - 1];
-    }
-}
-
-/*  Encrypt a single block of 16 bytes with 'on the fly' 128 bit keying */
-
-void aes_encrypt_128( const unsigned char in[N_BLOCK], unsigned char out[N_BLOCK],
-                     const unsigned char key[N_BLOCK], unsigned char o_key[N_BLOCK] )
-{   uint_8t s1[N_BLOCK], r, rc = 1;
-
-    if(o_key != key)
-        block_copy( o_key, key );
-    copy_and_key( s1, in, o_key );
-
-    for( r = 1 ; r < 10 ; ++r )
-#if defined( VERSION_1 )
-    {
-        mix_sub_columns( s1 );
-        update_encrypt_key_128( o_key, &rc );
-        add_round_key( s1, o_key );
-    }
-#else
-    {   uint_8t s2[N_BLOCK];
-        mix_sub_columns( s2, s1 );
-        update_encrypt_key_128( o_key, &rc );
-        copy_and_key( s1, s2, o_key );
-    }
-#endif
-
-    shift_sub_rows( s1 );
-    update_encrypt_key_128( o_key, &rc );
-    copy_and_key( out, s1, o_key );
-}
-
-#endif
-
-#if defined( AES_DEC_128_OTFK )
-
-/*  The 'on the fly' decryption key update for for 128 bit keys */
-
-static void update_decrypt_key_128( uint_8t k[N_BLOCK], uint_8t *rc )
-{   uint_8t cc;
-
-    for( cc = 12; cc > 0; cc -= 4 )
-    {
-        k[cc + 0] ^= k[cc - 4];
-        k[cc + 1] ^= k[cc - 3];
-        k[cc + 2] ^= k[cc - 2];
-        k[cc + 3] ^= k[cc - 1];
-    }
-    *rc = d2(*rc);
-    k[0] ^= s_box(k[13]) ^ *rc;
-    k[1] ^= s_box(k[14]);
-    k[2] ^= s_box(k[15]);
-    k[3] ^= s_box(k[12]);
-}
-
-/*  Decrypt a single block of 16 bytes with 'on the fly' 128 bit keying */
-
-void aes_decrypt_128( const unsigned char in[N_BLOCK], unsigned char out[N_BLOCK],
-                      const unsigned char key[N_BLOCK], unsigned char o_key[N_BLOCK] )
-{
-    uint_8t s1[N_BLOCK], r, rc = 0x6c;
-    if(o_key != key)
-        block_copy( o_key, key );
-
-    copy_and_key( s1, in, o_key );
-    inv_shift_sub_rows( s1 );
-
-    for( r = 10 ; --r ; )
-#if defined( VERSION_1 )
-    {
-        update_decrypt_key_128( o_key, &rc );
-        add_round_key( s1, o_key );
-        inv_mix_sub_columns( s1 );
-    }
-#else
-    {   uint_8t s2[N_BLOCK];
-        update_decrypt_key_128( o_key, &rc );
-        copy_and_key( s2, s1, o_key );
-        inv_mix_sub_columns( s1, s2 );
-    }
-#endif
-    update_decrypt_key_128( o_key, &rc );
-    copy_and_key( out, s1, o_key );
-}
-
-#endif
-
-#if defined( AES_ENC_256_OTFK )
-
-/*  The 'on the fly' encryption key update for for 256 bit keys */
-
-static void update_encrypt_key_256( uint_8t k[2 * N_BLOCK], uint_8t *rc )
-{   uint_8t cc;
-
-    k[0] ^= s_box(k[29]) ^ *rc;
-    k[1] ^= s_box(k[30]);
-    k[2] ^= s_box(k[31]);
-    k[3] ^= s_box(k[28]);
-    *rc = f2( *rc );
-
-    for(cc = 4; cc < 16; cc += 4)
-    {
-        k[cc + 0] ^= k[cc - 4];
-        k[cc + 1] ^= k[cc - 3];
-        k[cc + 2] ^= k[cc - 2];
-        k[cc + 3] ^= k[cc - 1];
-    }
-
-    k[16] ^= s_box(k[12]);
-    k[17] ^= s_box(k[13]);
-    k[18] ^= s_box(k[14]);
-    k[19] ^= s_box(k[15]);
-
-    for( cc = 20; cc < 32; cc += 4 )
-    {
-        k[cc + 0] ^= k[cc - 4];
-        k[cc + 1] ^= k[cc - 3];
-        k[cc + 2] ^= k[cc - 2];
-        k[cc + 3] ^= k[cc - 1];
-    }
-}
-
-/*  Encrypt a single block of 16 bytes with 'on the fly' 256 bit keying */
-
-void aes_encrypt_256( const unsigned char in[N_BLOCK], unsigned char out[N_BLOCK],
-                      const unsigned char key[2 * N_BLOCK], unsigned char o_key[2 * N_BLOCK] )
-{
-    uint_8t s1[N_BLOCK], r, rc = 1;
-    if(o_key != key)
-    {
-        block_copy( o_key, key );
-        block_copy( o_key + 16, key + 16 );
-    }
-    copy_and_key( s1, in, o_key );
-
-    for( r = 1 ; r < 14 ; ++r )
-#if defined( VERSION_1 )
-    {
-        mix_sub_columns(s1);
-        if( r & 1 )
-            add_round_key( s1, o_key + 16 );
-        else
-        {
-            update_encrypt_key_256( o_key, &rc );
-            add_round_key( s1, o_key );
-        }
-    }
-#else
-    {   uint_8t s2[N_BLOCK];
-        mix_sub_columns( s2, s1 );
-        if( r & 1 )
-            copy_and_key( s1, s2, o_key + 16 );
-        else
-        {
-            update_encrypt_key_256( o_key, &rc );
-            copy_and_key( s1, s2, o_key );
-        }
-    }
-#endif
-
-    shift_sub_rows( s1 );
-    update_encrypt_key_256( o_key, &rc );
-    copy_and_key( out, s1, o_key );
-}
-
-#endif
-
-#if defined( AES_DEC_256_OTFK )
-
-/*  The 'on the fly' encryption key update for for 256 bit keys */
-
-static void update_decrypt_key_256( uint_8t k[2 * N_BLOCK], uint_8t *rc )
-{   uint_8t cc;
-
-    for(cc = 28; cc > 16; cc -= 4)
-    {
-        k[cc + 0] ^= k[cc - 4];
-        k[cc + 1] ^= k[cc - 3];
-        k[cc + 2] ^= k[cc - 2];
-        k[cc + 3] ^= k[cc - 1];
-    }
-
-    k[16] ^= s_box(k[12]);
-    k[17] ^= s_box(k[13]);
-    k[18] ^= s_box(k[14]);
-    k[19] ^= s_box(k[15]);
-
-    for(cc = 12; cc > 0; cc -= 4)
-    {
-        k[cc + 0] ^= k[cc - 4];
-        k[cc + 1] ^= k[cc - 3];
-        k[cc + 2] ^= k[cc - 2];
-        k[cc + 3] ^= k[cc - 1];
-    }
-
-    *rc = d2(*rc);
-    k[0] ^= s_box(k[29]) ^ *rc;
-    k[1] ^= s_box(k[30]);
-    k[2] ^= s_box(k[31]);
-    k[3] ^= s_box(k[28]);
-}
-
-/*  Decrypt a single block of 16 bytes with 'on the fly'
-    256 bit keying
-*/
-void aes_decrypt_256( const unsigned char in[N_BLOCK], unsigned char out[N_BLOCK],
-                      const unsigned char key[2 * N_BLOCK], unsigned char o_key[2 * N_BLOCK] )
-{
-    uint_8t s1[N_BLOCK], r, rc = 0x80;
-
-    if(o_key != key)
-    {
-        block_copy( o_key, key );
-        block_copy( o_key + 16, key + 16 );
-    }
-
-    copy_and_key( s1, in, o_key );
-    inv_shift_sub_rows( s1 );
-
-    for( r = 14 ; --r ; )
-#if defined( VERSION_1 )
-    {
-        if( ( r & 1 ) )
-        {
-            update_decrypt_key_256( o_key, &rc );
-            add_round_key( s1, o_key + 16 );
-        }
-        else
-            add_round_key( s1, o_key );
-        inv_mix_sub_columns( s1 );
-    }
-#else
-    {   uint_8t s2[N_BLOCK];
-        if( ( r & 1 ) )
-        {
-            update_decrypt_key_256( o_key, &rc );
-            copy_and_key( s2, s1, o_key + 16 );
-        }
-        else
-            copy_and_key( s2, s1, o_key );
-        inv_mix_sub_columns( s1, s2 );
-    }
-#endif
-    copy_and_key( out, s1, o_key );
-}
-
-#endif
diff --git a/stack/smp/aes.h b/stack/smp/aes.h
deleted file mode 100644
index dcfde93..0000000
--- a/stack/smp/aes.h
+++ /dev/null
@@ -1,162 +0,0 @@
-/*
- ---------------------------------------------------------------------------
- Copyright (c) 1998-2008, Brian Gladman, Worcester, UK. All rights reserved.
-
- LICENSE TERMS
-
- The redistribution and use of this software (with or without changes)
- is allowed without the payment of fees or royalties provided that:
-
-  1. source code distributions include the above copyright notice, this
-     list of conditions and the following disclaimer;
-
-  2. binary distributions include the above copyright notice, this list
-     of conditions and the following disclaimer in their documentation;
-
-  3. the name of the copyright holder is not used to endorse products
-     built using this software without specific written permission.
-
- DISCLAIMER
-
- This software is provided 'as is' with no explicit or implied warranties
- in respect of its properties, including, but not limited to, correctness
- and/or fitness for purpose.
- ---------------------------------------------------------------------------
- Issue 09/09/2006
-
- This is an AES implementation that uses only 8-bit byte operations on the
- cipher state.
- */
-
-#ifndef AES_H
-#define AES_H
-
-#if 1
-#  define AES_ENC_PREKEYED  /* AES encryption with a precomputed key schedule  */
-#endif
-#if 1
-#  define AES_DEC_PREKEYED  /* AES decryption with a precomputed key schedule  */
-#endif
-#if 1
-#  define AES_ENC_128_OTFK  /* AES encryption with 'on the fly' 128 bit keying */
-#endif
-#if 1
-#  define AES_DEC_128_OTFK  /* AES decryption with 'on the fly' 128 bit keying */
-#endif
-#if 1
-#  define AES_ENC_256_OTFK  /* AES encryption with 'on the fly' 256 bit keying */
-#endif
-#if 1
-#  define AES_DEC_256_OTFK  /* AES decryption with 'on the fly' 256 bit keying */
-#endif
-
-#define N_ROW                   4
-#define N_COL                   4
-#define N_BLOCK   (N_ROW * N_COL)
-#define N_MAX_ROUNDS           14
-
-typedef unsigned char uint_8t;
-
-typedef uint_8t return_type;
-
-/*  Warning: The key length for 256 bit keys overflows a byte
-    (see comment below)
-*/
-
-typedef uint_8t length_type;
-
-typedef struct
-{   uint_8t ksch[(N_MAX_ROUNDS + 1) * N_BLOCK];
-    uint_8t rnd;
-} aes_context;
-
-/*  The following calls are for a precomputed key schedule
-
-    NOTE: If the length_type used for the key length is an
-    unsigned 8-bit character, a key length of 256 bits must
-    be entered as a length in bytes (valid inputs are hence
-    128, 192, 16, 24 and 32).
-*/
-
-#if defined( AES_ENC_PREKEYED ) || defined( AES_DEC_PREKEYED )
-
-return_type aes_set_key( const unsigned char key[],
-                         length_type keylen,
-                         aes_context ctx[1] );
-#endif
-
-#if defined( AES_ENC_PREKEYED )
-
-return_type aes_encrypt( const unsigned char in[N_BLOCK],
-                         unsigned char out[N_BLOCK],
-                         const aes_context ctx[1] );
-
-return_type aes_cbc_encrypt( const unsigned char *in,
-                         unsigned char *out,
-                         int n_block,
-                         unsigned char iv[N_BLOCK],
-                         const aes_context ctx[1] );
-#endif
-
-#if defined( AES_DEC_PREKEYED )
-
-return_type aes_decrypt( const unsigned char in[N_BLOCK],
-                         unsigned char out[N_BLOCK],
-                         const aes_context ctx[1] );
-
-return_type aes_cbc_decrypt( const unsigned char *in,
-                         unsigned char *out,
-                         int n_block,
-                         unsigned char iv[N_BLOCK],
-                         const aes_context ctx[1] );
-#endif
-
-/*  The following calls are for 'on the fly' keying.  In this case the
-    encryption and decryption keys are different.
-
-    The encryption subroutines take a key in an array of bytes in
-    key[L] where L is 16, 24 or 32 bytes for key lengths of 128,
-    192, and 256 bits respectively.  They then encrypts the input
-    data, in[] with this key and put the reult in the output array
-    out[].  In addition, the second key array, o_key[L], is used
-    to output the key that is needed by the decryption subroutine
-    to reverse the encryption operation.  The two key arrays can
-    be the same array but in this case the original key will be
-    overwritten.
-
-    In the same way, the decryption subroutines output keys that
-    can be used to reverse their effect when used for encryption.
-
-    Only 128 and 256 bit keys are supported in these 'on the fly'
-    modes.
-*/
-
-#if defined( AES_ENC_128_OTFK )
-void aes_encrypt_128( const unsigned char in[N_BLOCK],
-                      unsigned char out[N_BLOCK],
-                      const unsigned char key[N_BLOCK],
-                      uint_8t o_key[N_BLOCK] );
-#endif
-
-#if defined( AES_DEC_128_OTFK )
-void aes_decrypt_128( const unsigned char in[N_BLOCK],
-                      unsigned char out[N_BLOCK],
-                      const unsigned char key[N_BLOCK],
-                      unsigned char o_key[N_BLOCK] );
-#endif
-
-#if defined( AES_ENC_256_OTFK )
-void aes_encrypt_256( const unsigned char in[N_BLOCK],
-                      unsigned char out[N_BLOCK],
-                      const unsigned char key[2 * N_BLOCK],
-                      unsigned char o_key[2 * N_BLOCK] );
-#endif
-
-#if defined( AES_DEC_256_OTFK )
-void aes_decrypt_256( const unsigned char in[N_BLOCK],
-                      unsigned char out[N_BLOCK],
-                      const unsigned char key[2 * N_BLOCK],
-                      unsigned char o_key[2 * N_BLOCK] );
-#endif
-
-#endif