Revision 9:8058541a8e2d, committed 2016-09-26

Comitter:

cotigac

Date:

Mon Sep 26 01:37:59 2016 +0000

Parent:

8:2b9b24f3154d

Child:

10:1bed2b28ee18

Commit message:

Updated library so it doesn't require an TX task anymore outside of the library

Changed in this revision

--- a/Hexi_KW40Z.cpp	Sun Sep 25 05:50:22 2016 +0000
+++ b/Hexi_KW40Z.cpp	Mon Sep 26 01:37:59 2016 +0000
@@ -34,12 +34,12 @@
  */
  
 #include "Hexi_KW40Z.h"
-
-#if defined (LIB_DEBUG) 
-RawSerial pc(USBTX, USBRX); // tx, rx
+ 
+KW40Z::KW40Z(PinName txPin,PinName rxPin) : 
+#if defined (LIB_DEBUG)
+pc(USBTX, USBRX),
 #endif
- 
-KW40Z::KW40Z(PinName txPin,PinName rxPin) : device(txPin, rxPin), mainThread(osPriorityNormal,1024), rxThread(osPriorityNormal,1024)
+device(txPin, rxPin), rxThread(osPriorityNormal,1024), mainThread(osPriorityNormal,1024)
 {
 #if defined (LIB_DEBUG) 
     pc.baud(115200);
@@ -71,8 +71,8 @@
     bondPassKey = 0;
     
     /* intialization finalized, signal to start the threads */
-    mainThread.start(this,&KW40Z::mainTask); 
     rxThread.start(this, &KW40Z::rxTask);
+    mainThread.start(this,&KW40Z::mainTask);
 }
 
 KW40Z::~KW40Z(void)
@@ -104,9 +104,9 @@
         osEvent evt = queue.get();
         if (evt.status == osEventMessage) 
         {
-            hostInterface_packet_t *rxPacket = (hostInterface_packet_t*)evt.value.p;
-            ProcessReceivedPacket(rxPacket);
-            mpool.free(rxPacket);
+            hostInterface_packet_t *packet = (hostInterface_packet_t*)evt.value.p;
+            ProcessPacket(packet);
+            mpool.free(packet);
         }
     }
 }
@@ -133,56 +133,61 @@
     }
 }
 
-#if defined (LIB_DEBUG) 
-void KW40Z::DebugPrintTxPacket(hostInterface_packet_t * txPacket)
+void KW40Z::SendPacket(hostInterface_packet_t * txPacket, bool confirmRequested)
 {
-    char * txBuff = (char *)txPacket;
-    uint8_t length = txPacket->length + gHostInterface_headerSize + 1;
+    /* copy txPacket to the mem pool */
+    hostInterface_packet_t *packet = mpool.alloc();
+    memcpy(packet, txPacket, sizeof(hostInterface_packet_t));
+    
+    /* Set the TX bit in the Start 2 byte */
+    packet->start2 |= gHostInterface_txPacketMask;
+    
+    if (true == confirmRequested)
+    {
+        /* Set the confirm requested bit in the Start 2 byte */
+        packet->start2 |= gHostInterface_rxConfirmMask;
+    }
+    
+#if defined (LIB_DEBUG)
+    pc.printf("SendPacket: ");
+    DebugPrintPacket(packet);
+#endif
 
-    pc.printf("Tx: ");    
-    for(uint8_t i = 0; i < length; i++)
-    {
-        pc.printf("%02X ",*txBuff);
-        txBuff++;
-    }
-    pc.printf("\r\n");
+    /* send message to main task */
+    queue.put(packet);
 }
 
-void KW40Z::DebugPrintRxPacket()
-{
-    pc.printf("RX: ");
-    for(uint8_t * i = (uint8_t*)&hostInterface_rxPacket; i<rxBuff; i++)
-    {
-        pc.printf("%02X ",*i);
-    }
-    pc.printf("\r\n");
-}
-#endif
-
-
-void KW40Z::SendPacket(hostInterface_packet_t * txPacket, bool confirmRequested)
+void KW40Z::SendInternal(hostInterface_packet_t * txPacket)
 {
     uint8_t retries = 0;
+    bool confirmRequested = false;
     confirmReceived = false;
     
+#if defined (LIB_DEBUG)
+    pc.printf("SendInternal: ");
+    DebugPrintPacket(txPacket);
+#endif
+    
+    if(gHostInterface_rxConfirmMask == (txPacket->start2 & gHostInterface_rxConfirmMask))
+    {
+        confirmRequested = true;
+        pc.printf("Found confirmRequested\r\n");
+    }
+    
     do
     {
         char * txBuff = (char *)txPacket;
         uint8_t length = txPacket->length + gHostInterface_headerSize + 1;
         
-        if(confirmRequested == true)
-        {
-            txPacket->start2 |= 0x01;
-        }
-        
         for(uint8_t i = 0; i < length; i++)
         {
             device.putc(*txBuff);
             txBuff++;
         }
         
-#if defined (LIB_DEBUG)   
-        DebugPrintTxPacket(txPacket);
+#if defined (LIB_DEBUG)
+        pc.printf("TX: ");
+        DebugPrintPacket(txPacket);
 #endif
 
         retries++;
@@ -210,8 +215,8 @@
            (hostInterface_rxPacket.length > gHostInterface_dataSize))
         {
 #if defined (LIB_DEBUG) 
-            DebugPrintRxPacket();
-            pc.printf("check header failed\r\n");
+            pc.printf("Check header failed: ");
+            DebugPrintPacket(&hostInterface_rxPacket);
 #endif
       
             SearchStartByte();
@@ -225,8 +230,8 @@
                 if(gHostInterface_trailerByte != hostInterface_rxPacket.data[hostInterface_rxPacket.length])
                 {
 #if defined (LIB_DEBUG) 
-                    DebugPrintRxPacket();
-                    pc.printf("trailer byte failed\r\n");
+                    pc.printf("Trailer byte failed: ");
+                    DebugPrintPacket(&hostInterface_rxPacket);
 #endif
 
                     SearchStartByte();
@@ -246,8 +251,9 @@
                     memcpy(rxPacket, &hostInterface_rxPacket, sizeof(hostInterface_packet_t));
                     queue.put(rxPacket);
 
-#if defined (LIB_DEBUG)                     
-                    DebugPrintRxPacket();
+#if defined (LIB_DEBUG)
+                    pc.printf("RX: ");
+                    DebugPrintPacket(&hostInterface_rxPacket);
 #endif 
                     /* reset buffer position */
                     rxBuff = (uint8_t*)&hostInterface_rxPacket;
@@ -287,100 +293,113 @@
     }
     
 #if defined (LIB_DEBUG) 
-    pc.printf("search done\r\n");
-    DebugPrintRxPacket();
+    pc.printf("Search done: ");
+    DebugPrintPacket(&hostInterface_rxPacket);
 #endif
 }
 
-void KW40Z::ProcessReceivedPacket(hostInterface_packet_t * rxPacket)
+void KW40Z::ProcessPacket(hostInterface_packet_t * packet)
 {
-#if defined (LIB_DEBUG) 
-    pc.printf("packet found %d\r\n", rxPacket->type);
+#if defined (LIB_DEBUG)
+    pc.printf("ProcessPacket: ");
+    DebugPrintPacket(packet);
+#endif
+
+    /* check if this is a TX packet */
+    if(gHostInterface_txPacketMask == (packet->start2 & gHostInterface_txPacketMask))
+    {
+        /* revert back the TX bit in Start2 byte */
+        packet->start2 &= ~gHostInterface_txPacketMask;
+        
+        /* This is not a received packet, so call SendInternal */
+        SendInternal(packet);
+    }
+    else
+    {
+#ifdef gHostInterface_TxConfirmationEnable
+        /* acknowledge the packet reception if requested by sender */
+        if(gHostInterface_rxConfirmMask == (packet->start2 & gHostInterface_rxConfirmMask))
+        {
+          SendPacketOK();
+        }
 #endif
 
-#ifdef gHostInterface_TxConfirmationEnable
-      // acknowledge the packet reception
-      if ( 1 == ( rxPacket->start2 & 0x01 ) )
-      {
-          SendPacketOK();
-      }
-#endif
-    
-    switch(rxPacket->type)
-    {
-        /* button presses */
-        case packetType_pressUp:
-            if(buttonUpCb != NULL) buttonUpCb();
-            break;
+        switch(packet->type)
+        {
+            /* button presses */
+            case packetType_pressUp:
+                if(buttonUpCb != NULL) buttonUpCb();
+                break;
+                
+            case packetType_pressDown:
+                if(buttonDownCb != NULL) buttonDownCb();
+                break;
+                
+            case packetType_pressLeft:
+                if(buttonLeftCb != NULL) buttonLeftCb();
+                break;
+                
+            case packetType_pressRight:
+                if(buttonRightCb != NULL) buttonRightCb();
+                break;
             
-        case packetType_pressDown:
-            if(buttonDownCb != NULL) buttonDownCb();
-            break;
-            
-        case packetType_pressLeft:
-            if(buttonLeftCb != NULL) buttonLeftCb();
-            break;
-            
-        case packetType_pressRight:
-            if(buttonRightCb != NULL) buttonRightCb();
-            break;
-        
-        case packetType_slide:
-            if(buttonSlideCb != NULL) buttonSlideCb();
-            break;
-    
-        /* Alert Service */
-        case packetType_alertIn:
-            if(alertCb != NULL) alertCb(&rxPacket->data[0], rxPacket->length);
-            break;
+            case packetType_slide:
+                if(buttonSlideCb != NULL) buttonSlideCb();
+                break;
         
-        /* Passkey for pairing received */
-        case packetType_passDisplay:
-            if(passkeyCb != NULL)
-            {
-                memcpy((uint8_t *)&bondPassKey,&rxPacket->data[0], 3);
-                passkeyCb();
-            }
-            break;
-            
-        /* OTAP messages */
-        case packetType_otapCompleted:
-        case packetType_otapFailed:
-            break;
-            
-        /* TSI Status */
-        case packetType_buttonsGroupSendActive:
-            activeTsiGroup = rxPacket->data[0];
-            break;
+            /* Alert Service */
+            case packetType_alertIn:
+                if(alertCb != NULL) alertCb(&packet->data[0], packet->length);
+                break;
             
-        /* Advertisement Mode Info */
-        case packetType_advModeSend:
-            advertisementMode = rxPacket->data[0];
-            break;
-        
-        /* Link State */
-        case packetType_linkStateSend:
-            linkState = rxPacket->data[0];
-            break;
+            /* Passkey for pairing received */
+            case packetType_passDisplay:
+                if(passkeyCb != NULL)
+                {
+                    memcpy((uint8_t *)&bondPassKey,&packet->data[0], 3);
+                    passkeyCb();
+                }
+                break;
+                
+            /* OTAP messages */
+            case packetType_otapCompleted:
+            case packetType_otapFailed:
+                break;
+                
+            /* TSI Status */
+            case packetType_buttonsGroupSendActive:
+                activeTsiGroup = packet->data[0];
+                break;
+                
+            /* Advertisement Mode Info */
+            case packetType_advModeSend:
+                advertisementMode = packet->data[0];
+                break;
             
-        /* ANCS Service Notification Received */
-        case packetType_notification:
-            if(notificationsCb != NULL) notificationsCb(rxPacket->data[0], rxPacket->data[1]);
-            break;
-            
-        /* Build version */
-        case packetType_buildVersion:
-            kw40_version.ver_patchNumber = rxPacket->data[2];
-            kw40_version.ver_minorNumber = rxPacket->data[1];
-            kw40_version.ver_majorNumber = rxPacket->data[0];
-            break;
-      
-        case packetType_OK:
-            /* do nothing, the flag is set in the RxTask */
-            break;
-    
-        default:
-            break;
+            /* Link State */
+            case packetType_linkStateSend:
+                linkState = packet->data[0];
+                break;
+                
+            /* ANCS Service Notification Received */
+            case packetType_notification:
+                if(notificationsCb != NULL) notificationsCb(packet->data[0], packet->data[1]);
+                break;
+                
+            /* Build version */
+            case packetType_buildVersion:
+                kw40_version.ver_patchNumber = packet->data[2];
+                kw40_version.ver_minorNumber = packet->data[1];
+                kw40_version.ver_majorNumber = packet->data[0];
+                break;
+          
+            case packetType_OK:
+                /* do nothing, passthrough, the flag is set in the RxTask */
+                break;
+                
+            default:
+                break;
+        }
     }
 }
 
@@ -660,7 +679,7 @@
     txPacket.data[2]    = HEXIWEAR_VERSION_PATCH;
     txPacket.data[3]    = gHostInterface_trailerByte;
     
-    SendPacket(&txPacket, false);
+    SendPacket(&txPacket, true);
 }
 
 void KW40Z::SendPacketOK(void)
@@ -699,4 +718,19 @@
 uint8_t KW40Z::GetTsiGroup(void)
 {
     return activeTsiGroup;
-}   
+}
+
+#if defined (LIB_DEBUG) 
+void KW40Z::DebugPrintPacket(hostInterface_packet_t * packet)
+{
+    char * idx = (char *)packet;
+    uint8_t length = packet->length + gHostInterface_headerSize + 1;
+
+    for(uint8_t i = 0; i < length; i++)
+    {
+        pc.printf("%02X ",*idx);
+        idx++;
+    }
+    pc.printf("\r\n");
+}
+#endif

--- a/Hexi_KW40Z.cpp.orig	Sun Sep 25 05:50:22 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,692 +0,0 @@
-/** BLE KW40Z Driver for Hexiwear
- *  This file contains BLE and Touch Buttons driver functionality for Hexiwear
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this list
- * of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice, this
- * list of conditions and the following disclaimer in the documentation and/or
- * other materials provided with the distribution.
- *
- * Neither the name of NXP, nor the names of its
- * contributors may be used to endorse or promote products derived from this
- * software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
- * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
- * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * visit: http://www.mikroe.com and http://www.nxp.com
- *
- * get support at: http://www.mikroe.com/forum and https://community.nxp.com
- *
- * Project HEXIWEAR, 2015
- */
- 
-#include "Hexi_KW40Z.h"
-
-#if defined (LIB_DEBUG) 
-RawSerial pc(USBTX, USBRX); // tx, rx
-#endif
- 
-KW40Z::KW40Z(PinName txPin,PinName rxPin) : device(txPin, rxPin), mainThread(osPriorityNormal,1024), rxThread(osPriorityNormal,1024)
-{
-#if defined (LIB_DEBUG) 
-    pc.baud(115200);
-    pc.printf("Initializing\r\n");
-#endif
-
-    device.baud(230400);
-    device.format(8, Serial::None, 2);
-    
-    rxBuff = (uint8_t*)&hostInterface_rxPacket;
-    
-    /* initialize callbacks */
-    buttonUpCb = NULL;
-    buttonDownCb = NULL;
-    buttonLeftCb = NULL;
-    buttonRightCb = NULL;
-    buttonSlideCb = NULL;
-    alertCb = NULL;
-    passkeyCb = NULL;
-    notificationsCb = NULL;
-
-    kw40_version.ver_patchNumber = 0;
-    kw40_version.ver_minorNumber = 0;
-    kw40_version.ver_majorNumber = 0;
-    
-    activeTsiGroup = 0;
-    advertisementMode = 0;
-    linkState = 0;
-    bondPassKey = 0;
-    
-    /* intialization finalized, signal to start the threads */
-    mainThread.start(this,&KW40Z::mainTask); 
-    rxThread.start(this, &KW40Z::rxTask);
-}
-
-KW40Z::~KW40Z(void)
-{
-}
-
-void KW40Z::attach_buttonUp(button_t btnFct)    { buttonUpCb = btnFct;    }
-void KW40Z::attach_buttonDown(button_t btnFct)  { buttonDownCb = btnFct;  }
-void KW40Z::attach_buttonLeft(button_t btnFct)  { buttonLeftCb = btnFct;  }
-void KW40Z::attach_buttonRight(button_t btnFct) { buttonRightCb = btnFct; }
-void KW40Z::attach_buttonSlide(button_t btnFct) { buttonSlideCb = btnFct; }
-void KW40Z::attach_alert(alert_t alertFct)      { alertCb = alertFct;     }
-void KW40Z::attach_passkey(passkey_t passkeyFct){ passkeyCb = passkeyFct; }
-void KW40Z::attach_notifications(notifications_t notFct) { notificationsCb = notFct; }
-
-void KW40Z::mainTask(void)
-{
-#if defined (LIB_DEBUG)     
-    pc.printf("MainTask Stared\r\n");
-#endif
-
-    SendGetActiveTsiGroup();
-    SendGetAdvertisementMode();
-    SendGetLinkState();
-    SendGetVersion();
-
-    while(1)
-    {
-        osEvent evt = queue.get();
-        if (evt.status == osEventMessage) 
-        {
-            hostInterface_packet_t *rxPacket = (hostInterface_packet_t*)evt.value.p;
-            ProcessReceivedPacket(rxPacket);
-            mpool.free(rxPacket);
-        }
-    }
-}
-
-void KW40Z::rxTask(void)
-{
-#if defined (LIB_DEBUG)     
-    pc.printf("RxTask Stared\r\n");
-#endif
-
-    while(1)
-    {
-        if(device.readable()) 
-        {   
-            *rxBuff++ = device.getc();
-            ProcessBuffer();
-            
-            /* check for buffer overflow */
-            if(rxBuff >= ((uint8_t*)&hostInterface_rxPacket + sizeof(hostInterface_rxPacket)))
-            {
-                rxBuff = (uint8_t*)&hostInterface_rxPacket;
-            }
-        }
-    }
-}
-
-#if defined (LIB_DEBUG) 
-void KW40Z::DebugPrintTxPacket(hostInterface_packet_t * txPacket)
-{
-    char * txBuff = (char *)txPacket;
-    uint8_t length = txPacket->length + gHostInterface_headerSize + 1;
-
-    pc.printf("Tx: ");    
-    for(uint8_t i = 0; i < length; i++)
-    {
-        pc.printf("%02X ",*txBuff);
-        txBuff++;
-    }
-    pc.printf("\r\n");
-}
-
-void KW40Z::DebugPrintRxPacket()
-{
-    pc.printf("RX: ");
-    for(uint8_t * i = (uint8_t*)&hostInterface_rxPacket; i<rxBuff; i++)
-    {
-        pc.printf("%02X ",*i);
-    }
-    pc.printf("\r\n");
-}
-#endif
-
-
-void KW40Z::SendPacket(hostInterface_packet_t * txPacket, bool confirmRequested)
-{
-    uint8_t retries = 0;
-    confirmReceived = false;
-    
-    do
-    {
-        char * txBuff = (char *)txPacket;
-        uint8_t length = txPacket->length + gHostInterface_headerSize + 1;
-        
-        if(confirmRequested == true)
-        {
-            txPacket->start2 |= 0x01;
-        }
-        
-        for(uint8_t i = 0; i < length; i++)
-        {
-            device.putc(*txBuff);
-            txBuff++;
-        }
-        
-#if defined (LIB_DEBUG)   
-        DebugPrintTxPacket(txPacket);
-#endif
-
-        retries++;
-        
-        #if defined (gHostInterface_RxConfirmationEnable)
-        if((confirmRequested == true) && (confirmReceived == false))
-        {
-            Thread::wait(gHostInterface_retransmitTimeout);
-        }
-        #endif
-    }
-    while((confirmRequested == true) && 
-          (confirmReceived == false) && 
-          (retries < gHostInterface_retransmitCount));
-}
-
-void KW40Z::ProcessBuffer()
-{    
-    /* check if header has been received */
-    if(rxBuff > ((uint8_t*)&hostInterface_rxPacket + gHostInterface_headerSize))
-    {
-        /* check packet header */
-        if((gHostInterface_startByte1 != hostInterface_rxPacket.start1)||
-           (gHostInterface_startByte2 != (hostInterface_rxPacket.start2 & 0xFE))||
-           (hostInterface_rxPacket.length > gHostInterface_dataSize))
-        {
-#if defined (LIB_DEBUG) 
-            DebugPrintRxPacket();
-            pc.printf("check header failed\r\n");
-#endif
-      
-            SearchStartByte();
-        }
-        else
-        {
-            /* check data length */
-            if(rxBuff > ((uint8_t*)&hostInterface_rxPacket + gHostInterface_headerSize + hostInterface_rxPacket.length)) 
-            {
-                /* check trailer byte */
-                if(gHostInterface_trailerByte != hostInterface_rxPacket.data[hostInterface_rxPacket.length])
-                {
-#if defined (LIB_DEBUG) 
-                    DebugPrintRxPacket();
-                    pc.printf("trailer byte failed\r\n");
-#endif
-
-                    SearchStartByte();
-                }
-                else
-                {   
-                
-                    #if defined (gHostInterface_RxConfirmationEnable)
-                    if(hostInterface_rxPacket.type == packetType_OK)
-                    {
-                        confirmReceived = true;
-                    }
-                    #endif
-                    
-                    /* send message to main task */
-                    hostInterface_packet_t *rxPacket = mpool.alloc();
-                    memcpy(rxPacket, &hostInterface_rxPacket, sizeof(hostInterface_packet_t));
-                    queue.put(rxPacket);
-
-#if defined (LIB_DEBUG)                     
-                    DebugPrintRxPacket();
-#endif 
-                    /* reset buffer position */
-                    rxBuff = (uint8_t*)&hostInterface_rxPacket;
-                } 
-            }      
-        }
-    }
-}
-
-void KW40Z::SearchStartByte()
-{
-    bool found = false;
-    uint8_t * rdIdx = (uint8_t*)&hostInterface_rxPacket + 1;
-    
-    while(rdIdx < rxBuff)
-    {
-        if(*rdIdx == gHostInterface_startByte1)
-        {
-            uint32_t len = rxBuff - rdIdx;
-            
-            memcpy(&hostInterface_rxPacket,rdIdx,len);
-            rxBuff -= len;
-            found = true;
-            
-#if defined (LIB_DEBUG) 
-            pc.printf("moving ");
-#endif
-            break;
-        }
-        rdIdx++;
-    }
-    
-    if(!found)
-    {
-        /* reset buffer position */
-        rxBuff = (uint8_t*)&hostInterface_rxPacket;
-    }
-    
-#if defined (LIB_DEBUG) 
-    pc.printf("search done\r\n");
-    DebugPrintRxPacket();
-#endif
-}
-
-void KW40Z::ProcessReceivedPacket(hostInterface_packet_t * rxPacket)
-{
-#if defined (LIB_DEBUG) 
-    pc.printf("packet found %d\r\n", rxPacket->type);
-#endif
-
-#ifdef gHostInterface_TxConfirmationEnable
-      // acknowledge the packet reception
-      if ( 1 == ( rxPacket->start2 & 0x01 ) )
-      {
-          SendPacketOK();
-      }
-#endif
-    
-    switch(rxPacket->type)
-    {
-        /* button presses */
-        case packetType_pressUp:
-            if(buttonUpCb != NULL) buttonUpCb();
-            break;
-            
-        case packetType_pressDown:
-            if(buttonDownCb != NULL) buttonDownCb();
-            break;
-            
-        case packetType_pressLeft:
-            if(buttonLeftCb != NULL) buttonLeftCb();
-            break;
-            
-        case packetType_pressRight:
-            if(buttonRightCb != NULL) buttonRightCb();
-            break;
-        
-        case packetType_slide:
-            if(buttonSlideCb != NULL) buttonSlideCb();
-            break;
-    
-        /* Alert Service */
-        case packetType_alertIn:
-            if(alertCb != NULL) alertCb(&rxPacket->data[0], rxPacket->length);
-            break;
-        
-        /* Passkey for pairing received */
-        case packetType_passDisplay:
-            if(passkeyCb != NULL)
-            {
-                memcpy((uint8_t *)&bondPassKey,&rxPacket->data[0], 3);
-                passkeyCb();
-            }
-            break;
-            
-        /* OTAP messages */
-        case packetType_otapCompleted:
-        case packetType_otapFailed:
-            break;
-            
-        /* TSI Status */
-        case packetType_buttonsGroupSendActive:
-            activeTsiGroup = rxPacket->data[0];
-            break;
-            
-        /* Advertisement Mode Info */
-        case packetType_advModeSend:
-            advertisementMode = rxPacket->data[0];
-            break;
-        
-        /* Link State */
-        case packetType_linkStateSend:
-            linkState = rxPacket->data[0];
-            break;
-            
-        /* ANCS Service Notification Received */
-        case packetType_notification:
-            if(notificationsCb != NULL) notificationsCb(rxPacket->data[0], rxPacket->data[1]);
-            break;
-            
-        /* Build version */
-        case packetType_buildVersion:
-            kw40_version.ver_patchNumber = rxPacket->data[2];
-            kw40_version.ver_minorNumber = rxPacket->data[1];
-            kw40_version.ver_majorNumber = rxPacket->data[0];
-            break;
-      
-        case packetType_OK:
-            /* do nothing, the flag is set in the RxTask */
-            break;
-    
-        default:
-            break;
-    }
-}
-
-void KW40Z::SendBatteryLevel(uint8_t percentage)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_batteryLevel;
-    txPacket.length     = 1;
-    txPacket.data[0]    = percentage;
-    txPacket.data[1]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendAccel(uint8_t x, uint8_t y, uint8_t z)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_accel;
-    txPacket.length     = 3;
-    txPacket.data[0]    = x;
-    txPacket.data[1]    = y;
-    txPacket.data[2]    = z;    
-    txPacket.data[3]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendGyro(uint8_t x, uint8_t y, uint8_t z)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_gyro;
-    txPacket.length     = 3;
-    txPacket.data[0]    = x;
-    txPacket.data[1]    = y;
-    txPacket.data[2]    = z;    
-    txPacket.data[3]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendMag(uint8_t x, uint8_t y, uint8_t z)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_magnet;
-    txPacket.length     = 3;
-    txPacket.data[0]    = x;
-    txPacket.data[1]    = y;
-    txPacket.data[2]    = z;    
-    txPacket.data[3]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendAmbientLight(uint8_t percentage)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_ambiLight;
-    txPacket.length     = 1;
-    txPacket.data[0]    = percentage;
-    txPacket.data[1]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendTemperature(uint16_t celsius)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_temperature;
-    txPacket.length     = 2;
-    memcpy(&txPacket.data[0],(uint8_t*)&celsius,txPacket.length);
-    txPacket.data[2]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendHumidity(uint16_t percentage)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_humidity;
-    txPacket.length     = 2;
-    memcpy(&txPacket.data[0],(uint8_t*)&percentage,txPacket.length);
-    txPacket.data[2]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendPressure(uint16_t pascal)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_pressure;
-    txPacket.length     = 2;
-    memcpy(&txPacket.data[0],(uint8_t*)&pascal,txPacket.length);
-    txPacket.data[2]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendHeartRate(uint8_t rate)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_steps;
-    txPacket.length     = 1;
-    txPacket.data[0]    = rate;
-    txPacket.data[1]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendSteps(uint16_t steps)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_steps;
-    txPacket.length     = 2;
-    memcpy(&txPacket.data[0],(uint8_t*)&steps,txPacket.length);
-    txPacket.data[2]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendCalories(uint16_t calories)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_calories;
-    txPacket.length     = 2;
-    memcpy(&txPacket.data[0],(uint8_t*)&calories,txPacket.length);
-    txPacket.data[2]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendAlert(uint8_t *pData, uint8_t length)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_alertOut;
-    txPacket.length     = length;
-    memcpy(&txPacket.data[0],pData,length);
-    txPacket.data[length] = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::ToggleTsiGroup(void)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_buttonsGroupToggleActive;
-    txPacket.length     = 0;
-    txPacket.data[0]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::ToggleAdvertisementMode(void)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_advModeToggle;
-    txPacket.length     = 0;
-    txPacket.data[0]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendSetApplicationMode(gui_current_app_t mode)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_appMode;
-    txPacket.length     = 1;
-    txPacket.data[0]    = (uint8_t)mode;
-    txPacket.data[1]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, true);
-}
-
-void KW40Z::SendGetActiveTsiGroup(void)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_buttonsGroupGetActive;
-    txPacket.length     = 0;
-    txPacket.data[0]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, false);
-}
-
-void KW40Z::SendGetAdvertisementMode(void)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_advModeGet;
-    txPacket.length     = 0;
-    txPacket.data[0]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, false);
-}
-
-void KW40Z::SendGetLinkState(void)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_linkStateGet;
-    txPacket.length     = 0;
-    txPacket.data[0]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, false);
-}
-
-void KW40Z::SendGetVersion(void)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_buildVersion;
-    txPacket.length     = 3;
-    txPacket.data[0]    = HEXIWEAR_VERSION_MAJOR;
-    txPacket.data[1]    = HEXIWEAR_VERSION_MINOR;
-    txPacket.data[2]    = HEXIWEAR_VERSION_PATCH;
-    txPacket.data[3]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, false);
-}
-
-void KW40Z::SendPacketOK(void)
-{
-    hostInterface_packet_t txPacket = {0};
-    
-    txPacket.start1     = gHostInterface_startByte1;
-    txPacket.start2     = gHostInterface_startByte2;
-    txPacket.type       = packetType_OK;
-    txPacket.length     = 0;
-    txPacket.data[0]    = gHostInterface_trailerByte;
-    
-    SendPacket(&txPacket, false);    
-}
-
-uint8_t KW40Z::GetAdvertisementMode(void)
-{
-    return advertisementMode;
-}
-
-uint32_t KW40Z::GetPassKey(void)
-{
-    return bondPassKey;
-}
-   
-uint8_t KW40Z::GetLinkState(void)
-{
-    return linkState;
-}
-
-hexiwear_version_t KW40Z::GetVersion(void)
-{
-    return kw40_version;
-}
-
-uint8_t KW40Z::GetTsiGroup(void)
-{
-    return activeTsiGroup;
-}   

--- a/Hexi_KW40Z.h	Sun Sep 25 05:50:22 2016 +0000
+++ b/Hexi_KW40Z.h	Mon Sep 26 01:37:59 2016 +0000
@@ -39,11 +39,13 @@
 #include "mbed.h"
 #include "rtos.h"
 
-//#define LIB_DEBUG                               1
+#define LIB_DEBUG                               1
 
 #define gHostInterface_startByte1               0x55
 #define gHostInterface_startByte2               0xAA
 #define gHostInterface_trailerByte              0x45
+#define gHostInterface_rxConfirmMask            0x01
+#define gHostInterface_txPacketMask             0x10
 
 #define gHostInterface_dataSize                 23
 #define gHostInterface_headerSize               4
@@ -204,6 +206,7 @@
     void SendCalories(uint16_t calories);
     void SendAlert(uint8_t *pData, uint8_t length);
     void SendSetApplicationMode(gui_current_app_t mode);
+    void SendGetVersion(void);
     
     void ToggleTsiGroup(void);
     void ToggleAdvertisementMode(void);
@@ -216,9 +219,13 @@
     uint32_t GetPassKey(void); 
 
 private:
-    RawSerial device;    
+#if defined (LIB_DEBUG) 
+    RawSerial pc;
+#endif
+
+    RawSerial device;
+    Thread rxThread;  
     Thread mainThread;
-    Thread rxThread;
     
     hostInterface_packet_t hostInterface_rxPacket;
     hostInterface_packet_t hostInterface_txPacket;
@@ -249,19 +256,18 @@
     void rxTask(void);
     
     void ProcessBuffer();
-    void ProcessReceivedPacket(hostInterface_packet_t * rxPacket);
+    void ProcessPacket(hostInterface_packet_t * packet);
     void SendPacket(hostInterface_packet_t * txPacket, bool confirmRequested);
+    void SendInternal(hostInterface_packet_t * txPacket);
     void SearchStartByte();
     
     void SendPacketOK(void);
     void SendGetActiveTsiGroup(void);
     void SendGetAdvertisementMode(void);
     void SendGetLinkState(void);
-    void SendGetVersion(void);
     
-#if defined (LIB_DEBUG) 
-    void DebugPrintRxPacket();
-    void DebugPrintTxPacket(hostInterface_packet_t * txPacket);
+#if defined (LIB_DEBUG)
+    void DebugPrintPacket(hostInterface_packet_t * packet);
 #endif
 };